C++ err_SetMsg函数代码示例

RBAPI(bool) com_FlushWFIFO(int com) {
    #if defined(RB_MSVC_WIN32) || defined(RB_MSVC_WINCE)
        if (FlushFileBuffers(COM_info[com].fp) == FALSE)
        {
            err_SetMsg(ERROR_COM_FAIL, "FlushFileBuffers() fails");
            return false;
        }
    #elif defined(RB_LINUX)
        if (COM_duplex[com] != COM_HDUPLEX_RTS)
        {
            if (tcdrain(COM_info[com].fp) < 0)
            {
                err_SetMsg(ERROR_COM_FAIL, "tcdrain() fails");
                return false;
            }
        }
        else  // tcdrain() is too slow in the purpose of switching RTS to read servos in readtime
        {
            //use "ioctl(fd, FIONWRITE, &nBytes)" to wait write-FIFO empty...
        }
    #endif
    
    if (COM_duplex[com] == COM_HDUPLEX_RTS)  // ensure that all bytes in 16550 FIFO have been sent
        while((io_inpb(COM_baseaddr[com] + 5) & 0x60) != 0x60);
    
    return true;
}

RBAPI(bool) i2cslave_Write(int dev, unsigned char val) {
    unsigned long nowtime;

    if (I2C_action[dev] != I2CACT_SLAVEWRITEREQ)
    {
        err_SetMsg(ERROR_I2CWRONGUSAGE, "slave must write data only when being requested");
        return false;
    }
    
    i2cslave_ClearSlaveWREQ(dev);
    i2c_WriteDataREG(dev, val);

    for (nowtime=0; nowtime<100000L; nowtime++) //trick for performance; timer_nowtime() is time-consuming in some OS
        if (i2c_CheckTXDone(dev) == true) goto TX_SUCCESS;

    nowtime = timer_nowtime();
    while ((i2c_CheckTXDone(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));

    if (i2c_CheckTXDone(dev) == false)
    {
        err_SetMsg(ERROR_I2CFAIL, "the I2C module doesn't respond");
        I2C_action[dev] = I2CACT_IDLE;
        return false;
    }

TX_SUCCESS:
    i2c_ClearTXDone(dev);
    I2C_action[dev] = I2CACT_SLAVE;
    return true;
}

_RB_INLINE bool check_RX_done(int dev) {
    unsigned long nowtime;
    unsigned char statreg;

    for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
        if (i2c_CheckRXRdy(dev) == true) goto RX_SUCCESS;

    nowtime = timer_nowtime();
    while ((i2c_CheckRXRdy(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));

RX_SUCCESS:
    statreg = i2c_ReadStatREG(dev); //ugly code for speed:p
    i2c_ClearRXRdy(dev);

    if ((statreg & 0x08) != 0) //if (i2cmaster_CheckARLoss(dev) == true)
    {
        err_SetMsg(ERROR_I2CARLOSS, "arbitration loss for I2C bus");
        i2cmaster_ClearARLoss(dev);
        return false;
    }
    if ((statreg & 0x01) == 0) //if (i2c_IsMaster(dev) == false)
    {
        err_SetMsg(ERROR_I2CFAIL, "I2C%d module isn't in Master Mode", dev);
        return false;
    }
    if ((statreg & 0x40) == 0) //if (i2c_CheckRXRdy(dev) == false)
    {
        err_SetMsg(ERROR_I2CFAIL, "I2C%d module doesn't respond", dev);
        return false;
    }

    return true;
}

RBAPI(bool) spi_Init2(unsigned baseaddr, int clkmode) {
    int i;

	if(spi_InUse() == true)
	{
		err_SetMsg(ERROR_SPI_INUSE, "SPI was already opened");
		return false;
	}
	
	if ((SPI_ioSection = io_Init()) == -1) return false;

	//NOTE: base address should be selected carefully to avoid conflicts with other devices!
	if (baseaddr != 0xffff)
        spi_SetBaseAddress(baseaddr);
	else
	{
        baseaddr = spi_SetDefaultBaseAddress();
        if ((baseaddr == 0x0000) || (baseaddr == 0xffff)) spi_SetBaseAddress(0xfc00);
    }

    spidx_DisableCS();
    spi_ClearErrors();

    // enable FIFO and set clock divisor
    //if (spi_SetControlREG(0x10 + SPI_CLKMODE[clkmode]) == false)
    if (spi_SetControlREG(0x10 + get_spidivisor(clkmode)) == false)
    {
        spi_Close();
        err_SetMsg(ERROR_SPI_INITFAIL, "fail to write SPI Control Register");
        return false;
    }
    
    // clear the input buffer if it is not empty
    for (i=0; i<20; i++)
    {
        if (spi_InputReady() == false) break;
        if (spidx_Read() == 0xffff)
        {
            spi_Close();
            err_SetMsg(ERROR_SPI_INITFAIL, "fail to clear SPI input buffer");
            return false;
        }
    }

    // switch GPIO3[3:0] to external SPI interface
    OLD_SPIGPIO3FLAG = read_sb_reg(SB_MULTIFUNC_REG);
    write_sb_reg(SB_MULTIFUNC_REG, OLD_SPIGPIO3FLAG | 1L);
    OLD_SPIGPIO3FLAG = OLD_SPIGPIO3FLAG & 1L;

    return true;
}

RBAPI(unsigned) ad79x8_RawRead(void) {
    unsigned val1, val2;

    #ifdef ROBOIO
        if (roboio_GetRBVer() == RB_110)
        {
            io_outpb(GPIO2_DIR, io_inpb(GPIO2_DIR) | 0x20);
            io_outpb(GPIO2_DATA, io_inpb(GPIO2_DATA) & 0xdf);
        }
    #endif
    spi_EnableCS();

    val1 = spi_Read2();
    val2 = spi_Read2();

    spi_DisableCS();
    #ifdef ROBOIO
        if (roboio_GetRBVer() == RB_110)
        {
            io_outpb(GPIO2_DIR, io_inpb(GPIO2_DIR) | 0x20);
            io_outpb(GPIO2_DATA, io_inpb(GPIO2_DATA) | 0x20);
        }
    #endif

    if ((val1 == 0xffff) || (val2 == 0xffff))
    {
        err_SetMsg(ERROR_ADCFAIL, "fail to read ADC because SPI fails");
        return AD79x8_READFAIL;
    }

    return (val1 << 8) + val2;
}

static bool i2csw_Write(int dev, unsigned char val) {
    int i, databit;

    // send data byte
    for(i=0; i<8; i++)
    {
        databit = ((val & 0x80) == 0)? 0 : 1;
        set_pins(dev, 0, databit); delay_us(I2CSW_delay[dev]);  // set data bit
        set_pins(dev, 1, databit); delay_us(I2CSW_delay[dev]);  // send a clock
        set_pins(dev, 0, databit);
        val = val << 1;
    }
    
    // read ACK
    set_pins(dev, 0, 1); delay_us(I2CSW_delay[dev]);  // set SDA as input
    set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // send a clock
    if (read_sda(dev) == 1)
    {
		err_SetMsg(ERROR_I2CACKERR, "receive no ACK after transmitting");
        return false;
    }
    set_pins(dev, 0, 1);
    
    return true;
}

static bool i2csw_Start(int dev, unsigned char addr, unsigned char rwbit, bool restart) {
    int i, databit;
    
    if (restart == false)
    {   // send START signal
        set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 1
        set_pins(dev, 1, 0); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 0
        set_pins(dev, 0, 0);                              // SCL = 0, SDA = 0
    }
    else
    if (I2C_swMode[dev] == I2CSW_LEGO)
    {   // send LEGO NXT's I2C RESTART signal
        set_pins(dev, 0, 0); delay_us(I2CSW_delay[dev]);  // SCL = 0, SDA = 0
        set_pins(dev, 1, 0); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 0
        set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 1

        set_pins(dev, 0, 1); delay_us(I2CSW_delay[dev]);  // LEGO NXT special: SCL = 0, SDA = 1
        
        set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 1
        set_pins(dev, 1, 0); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 0
        set_pins(dev, 0, 0);                              // SCL = 0, SDA = 0
    }
    else
    {   // send normal I2C RESTART signal
        set_pins(dev, 0, 0); delay_us(I2CSW_delay[dev]);  // SCL = 0, SDA = 0
        set_pins(dev, 0, 1); delay_us(I2CSW_delay[dev]);  // SCL = 0, SDA = 1
        set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 1
        set_pins(dev, 1, 0); delay_us(I2CSW_delay[dev]);  // SCL = 1, SDA = 0
        set_pins(dev, 0, 0);                              // SCL = 0, SDA = 0
    }

    // send data byte
    addr = (addr << 1) | rwbit;
    for(i=0; i<8; i++)
    {
        databit = ((addr & 0x80) == 0)? 0 : 1;
        set_pins(dev, 0, databit); delay_us(I2CSW_delay[dev]);  // set data bit
        set_pins(dev, 1, databit); delay_us(I2CSW_delay[dev]);  // send a clock
        set_pins(dev, 0, databit);
        addr = addr << 1;
    }
    
    // read ACK
    set_pins(dev, 0, 1); delay_us(I2CSW_delay[dev]);  // set SDA as input
    set_pins(dev, 1, 1); delay_us(I2CSW_delay[dev]);  // send a clock
    if (read_sda(dev) == 1)
    {
		err_SetMsg(ERROR_I2CACKERR, "receive no ACK after transmitting");
        return false;
    }
    set_pins(dev, 0, 1);
    
    return true;
}

_RB_INLINE bool i2cmaster_Start1(int dev, unsigned char addr, unsigned char rwbit) {
    if (i2c_IsMaster(dev) == false)
    {
        err_SetMsg(ERROR_I2CWRONGUSAGE, "can't start a transaction in Slave state");
        return false;
    }

    i2cmaster_WriteAddrREG(dev, addr, rwbit);

    //Remarks: if I2C master sends "Start" twice sequently, ARLoss will occur according to H/W design
    return check_TX_done(dev);
}

DMP_INLINE(bool) check_TX_done(int dev) {
    unsigned long nowtime;
    unsigned char statreg;

    for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
        if (i2c_CheckTXDone(dev) == true) goto TX_SUCCESS;

    nowtime = timer_nowtime();
    while ((i2c_CheckTXDone(dev) == false) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));

TX_SUCCESS:
    statreg = i2c_ReadStatREG(dev); //ugly code for speed:p
    i2c_ClearTXDone(dev);

    if ((statreg & 0x08) != 0) //if (i2cmaster_CheckARLoss(dev) == true)
    {
        err_SetMsg(ERROR_I2CARLOSS, "arbitration loss for I2C bus");
        i2cmaster_ClearARLoss(dev);
    }
    if ((statreg & 0x10) != 0) //if (i2cmaster_CheckAckErr(dev) == true)
    {
        err_SetMsg(ERROR_I2CACKERR, "receive no ACK after transmitting");
        i2cmaster_ClearAckErr(dev);
    }
    if ((statreg & (0x10 + 0x08)) != 0) return false;

    if ((statreg & 0x01) == 0) //if (i2c_IsMaster(dev) == false)
    {
        err_SetMsg(ERROR_I2CFAIL, "I2C%d module isn't in Master Mode", dev);
        return false;
    }
    if ((statreg & 0x20) == 0) //if (i2c_CheckTXDone(dev) == false)
    {
        err_SetMsg(ERROR_I2CFAIL, "I2C%d module doesn't respond", dev);
        return false;
    }
    
    return true;
}

RBAPI(bool) i2cmaster_Write(int dev, unsigned char val) {
    if (I2C_action[dev] != I2CACT_MASTERWRITE)
    {
        err_SetMsg(ERROR_I2CWRONGUSAGE, "must start a correct transaction before writing data");
        return false;
    }

    i2c_WriteDataREG(dev, val);

    if (check_TX_done(dev) == true) return true;

    I2C_action[dev] = I2CACT_IDLE;
    return false;
}

_RB_INLINE bool i2cmaster_Start1(int dev, unsigned char addr, unsigned char rwbit) {
    if (i2c_IsMaster(dev) == false)
    {
        err_SetMsg(ERROR_I2CWRONGUSAGE, "can't start a transaction in Slave state");
        return false;
    }

    i2cmaster_WriteAddrREG(dev, addr, rwbit);

    //Remarks: if I2C master sends "Start" twice sequently, ARLoss will occur according to H/W design
    if (check_TX_done(dev) == false) return false;
    
    I2C_action[dev] = (rwbit == I2C_WRITE)? I2CACT_MASTERWRITE : I2CACT_MASTERREAD;
    I2C_curAddr = addr;
    return true;
}

DMPAPI(bool) i2c_Reset(int dev) {
    unsigned long nowtime;
    
    io_outpb(I2C_EXCTRL_REG(dev), io_inpb(I2C_EXCTRL_REG(dev)) | 0x80);

    nowtime = timer_nowtime();
    while (((io_inpb(I2C_EXCTRL_REG(dev)) & 0x80) != 0) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));

    if ((io_inpb(I2C_EXCTRL_REG(dev)) & 0x80) != 0)
    {
        err_SetMsg(ERROR_I2CFAIL, "fail to reset I2C module %d", dev);
        return false;
    }
    
    //Remarks: due to Vortex86DX's poor I2C design, RESET bit may become 0 when dummy clocks are still being output
    delay_ms(20L); //lazy trick to tackle the above issue
    return true;
}

_RB_INLINE bool check_STOP_done(int dev) {
    unsigned long nowtime;

    for (nowtime=0; nowtime<100000L; nowtime++) //trick for speed; timer_nowtime() is time-consuming in some OS
        if (i2cmaster_CheckStopBit(dev) == false) goto STOP_SUCCESS;

    nowtime = timer_nowtime();
    while ((i2cmaster_CheckStopBit(dev) == true) && ((timer_nowtime() - nowtime) < I2C_TIMEOUT));

    if (i2cmaster_CheckStopBit(dev) == true)
    {
        err_SetMsg(ERROR_I2CFAIL, "fail to stop the transaction");
        return false;
    }

STOP_SUCCESS:
    return true;
}