diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp index c145615fdd..30c1d9329b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp @@ -58,6 +58,9 @@ #define REGG_FIFO_CONFIG_1 0x3E #define REGG_FIFO_DATA 0x3F +#define ACCEL_BACKEND_SAMPLE_RATE 2000 +#define GYRO_BACKEND_SAMPLE_RATE 2000 + extern const AP_HAL::HAL& hal; #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) @@ -98,17 +101,17 @@ AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu, void AP_InertialSensor_BMI055::start() { - accel_instance = _imu.register_accel(2000, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)); - gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055)); + accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)); + gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055)); // setup sensor rotations from probe() set_gyro_orientation(gyro_instance, rotation); set_accel_orientation(accel_instance, rotation); // setup callbacks - dev_accel->register_periodic_callback(1000, + dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_accel, void)); - dev_gyro->register_periodic_callback(1000, + dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_gyro, void)); } @@ -156,7 +159,6 @@ bool AP_InertialSensor_BMI055::accel_init() goto failed; } - hal.console->printf("BMI055: found accel\n"); dev_accel->get_semaphore()->give(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index a3fa2c143f..f1859a13e3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -56,6 +56,9 @@ #define REGG_FIFO_CONFIG_1 0x3E #define REGG_FIFO_DATA 0x3F +#define ACCEL_BACKEND_SAMPLE_RATE 1600 +#define GYRO_BACKEND_SAMPLE_RATE 2000 + extern const AP_HAL::HAL& hal; AP_InertialSensor_BMI088::AP_InertialSensor_BMI088(AP_InertialSensor &imu, @@ -94,17 +97,17 @@ AP_InertialSensor_BMI088::probe(AP_InertialSensor &imu, void AP_InertialSensor_BMI088::start() { - accel_instance = _imu.register_accel(1600, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)); - gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088)); + accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)); + gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088)); // setup sensor rotations from probe() set_gyro_orientation(gyro_instance, rotation); set_accel_orientation(accel_instance, rotation); - + // setup callbacks - dev_accel->register_periodic_callback(1000000UL / 1600, + dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void)); - dev_gyro->register_periodic_callback(1000000UL / 2000, + dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void)); }