/* IMU Class specific functions */
int IMU::start() {
if(!bno055_driver_bound) {
bno055_driver_bind();
bno055_driver_bound = true;
}
if(!bno055_initialized) {
s8 bno055_init_result = bno055_init(&bno055);
if((int) bno055_init_result != 0) {
std::cerr << TAG_ERROR << "Failed to bno055_init => " << bno055_init_result << std::endl;
return IMC_FAIL;
}
bno055_initialized = true;
}
if(!bno055_power_mode_normal) {
int bno055_power_mode_result = bno055_set_power_mode(POWER_MODE_NORMAL);
if(bno055_power_mode_result != 0) {
std::cerr << TAG_ERROR << "Failed to set bno055 power mode to normal" << std::endl;
return IMC_FAIL;
}
bno055_power_mode_normal = true;
}
if(!bno055_operation_mode_ndof) {
int bno055_set_operation_mode_result = bno055_set_operation_mode(OPERATION_MODE_NDOF);
if(bno055_set_operation_mode_result != 0) {
std::cerr << TAG_ERROR << "Failed to set bno055 operation mode to ndof" << std::endl;
return IMC_FAIL;
}
bno055_operation_mode_ndof = true;
}
return IMC_SUCCESS;
}
/////////////////
// main routine
/////////////////
void main (void) {
int8_t ret1, ret2;
int16_t accX1, accY1, accZ1;
int16_t accX2, accY2, accZ2;
char buf[100];
uint8_t count=0, len=0;
/////////////////
// init peripherals
/////////////////
// disable interrupts
DISABLE_INTERRUPTS;
// switch to 16MHz (default is 2MHz)
CLK.CKDIVR.byte = 0x00;
// set default option bytes to assert bootloader is running
flash_OPT_default();
// init timer TIM3 for sleep and timeout (required by I2C)
tim3_init();
// init timer TIM4 for 1ms clock with interrupts
tim4_init();
// init I2C bus
i2c_init();
// init and reset LCD display
lcd_init();
// init pins for UART1 Rx(=PA4) and Tx(=PA5)
gpio_init(&PORT_A, PIN_4, INPUT_PULLUP_NOEXINT);
gpio_init(&PORT_A, PIN_5, OUTPUT_PUSHPULL_FAST);
// init UART1 to high speed (connected to PC on muBoard)
uart1_init(230400L);
// init LEDs on muBoard for visual feedback
GPIO_SET(PORT_H,PIN_2|PIN_3, 1);
gpio_init(&PORT_H, PIN_2|PIN_3, OUTPUT_PUSHPULL_FAST);
// enable interrupts
ENABLE_INTERRUPTS;
// init I2C routine pointers
I2C_routine();
// initialize sensors
do {
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_init(&bno055);
ret1 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret1 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret1 && USE_I2C_ADDR1);
do {
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_init(&bno055);
ret2 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret2 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret2 && USE_I2C_ADDR2);
/////////////////
// main loop
/////////////////
while (1) {
// every 1ms do
if (g_flagClock) {
g_flagClock = 0;
// every 10ms do
if (g_clock > 10) {
g_clock = 0;
// just to be sure
accX1 = accY1 = accZ1 = ret1 = 0;
accX2 = accY2 = accZ2 = ret2 = 0;
// read data from sensor 1
#if USE_I2C_ADDR1
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_read_accel_x(&accX1);
ret1 |= bno055_read_accel_y(&accY1);
ret1 |= bno055_read_accel_z(&accZ1);
if (ret1 != 0) {
accX1 = accY1 = accZ1 = 0;
}
#endif // USE_I2C_ADDR1
// read data from sensor 2
#if USE_I2C_ADDR2
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_read_accel_x(&accX2);
ret2 |= bno055_read_accel_y(&accY2);
ret2 |= bno055_read_accel_z(&accZ2);
if (ret2 != 0) {
accX2 = accY2 = accZ2 = 0;
}
#endif // USE_I2C_ADDR2
// send data to PC via UART1. Use SW FIFO for background operation
len = 0;
buf[len++] = (uint8_t)(accX1 >> 8); // x1-acc (MSB first)
buf[len++] = (uint8_t) accX1;
buf[len++] = (uint8_t)(accY1 >> 8); // y1-acc (MSB first)
buf[len++] = (uint8_t) accY1;
buf[len++] = (uint8_t)(accZ1 >> 8); // z1-acc (MSB first)
buf[len++] = (uint8_t) accZ1;
buf[len++] = (uint8_t)(accX2 >> 8); // x2-acc (MSB first)
buf[len++] = (uint8_t) accX2;
buf[len++] = (uint8_t)(accY2 >> 8); // y2-acc (MSB first)
buf[len++] = (uint8_t) accY2;
buf[len++] = (uint8_t)(accZ2 >> 8); // z2-acc (MSB first)
buf[len++] = (uint8_t) accZ2;
uart1_send_buf(len, buf);
// indicate I2C status via red LED (on=ok)
GPIO_SET(PORT_H,PIN_3, ret1|ret2);
// show life beat via green LED
if (++count > 20) {
count = 0;
GPIO_TOGGLE(PORT_H,PIN_2);
// print to LCD
sprintf(buf, "%02d %03d %03d %03d", (int) ret1, (int) accX1, (int) accY1, (int) accZ1);
lcd_print(1, 1, buf);
sprintf(buf, "%02d %03d %03d %03d", (int) ret2, (int) accX2, (int) accY2, (int) accZ2);
lcd_print(2, 1, buf);
}
} // loop 10ms
} // loop 1ms
} // main loop
