AP_InertialSensor: correct loop rate on BMI055, clean up setting of loop rate on BMI055 and BMI088

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();

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));
 }