fischertechnik community forum

Code: Alles auswählen

       /*!   RoboPro element
	 * initialisation of the TBO055 sensor software.
	 * This set also the I2C communication.
	 * error workflow -1 = SLI is not init, -2 = sensor is not initialized.
	 */
	int	setInitSensorShort(short v)     //RoboPro set element name =InitSensor, type= Short,  RoboPro data input (v) is not used
	{
		if (!IsInit)
		{
			fprintf(stderr,	"libSliTxtBNO055.setOpearationModeShort: Not initialized!\n");
			return -1; //make use of the RoboPro element error workflow continuation
		}
		int res = sensor->initSensor(user_delay_ms);
		return res;  //make use of the RoboPro element error workflow continuation
			}

Code: Alles auswählen

s32 TxtBNO055::initSensor(void(*f)(uint32_t))
{
	bno.bus_read = &(TxtBNO055::user_i2c_read);
   	bno.bus_write = &(TxtBNO055::user_i2c_write);
        bno.delay_msec = &(*f);//user_delay_ms;
	bno.dev_addr = BNO055_I2C_ADDR1;
        int8_t ret = bno055_init(&bno);
	if (ret != BNO055_SUCCESS) {
            printf("error: exit bno055_init");
	    return -2;   //make use of the RoboPro element error workflow continuation
	}
	return 0;
};

Code: Alles auswählen

int8_t TxtBNO055::user_i2c_read(uint8_t dev_id, uint8_t reg_addr,	uint8_t *reg_data, uint8_t len)
 {
	int8_t rslt = 0; /* Return 0 for Success, non-zero for failure */
	//Adr, Anz Wr, Wr-Data, Anz Read, Rd-Data, Speed
	uint32_t u32RetValue = KeLibI2cTransfer(dev_id, 1, &reg_addr, len,	reg_data, I2C_SPEED_400_KHZ);
	rslt = (u32RetValue == 0 ? 0 : -1); //0=success, -1 error
	return rslt;
}