AP_InertialSensor: move gyro and accel instance ids into AP_InertialS…

…ensor_Backend
ArduPilot · Jun 26, 2024 · a86f4fd · a86f4fd
1 parent 5911b87
commit a86f4fd
Show file tree

Hide file tree

Showing 24 changed files with 160 additions and 188 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.h
@@ -66,8 +66,6 @@ class AP_InertialSensor_ADIS1647x : public AP_InertialSensor_Backend {
         Delta32    =3
     } opmode;
 
-    uint8_t accel_instance;
-    uint8_t gyro_instance;
     enum Rotation rotation;
     uint8_t drdy_pin;
 

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h
@@ -62,8 +62,6 @@ class AP_InertialSensor_BMI055 : public AP_InertialSensor_Backend {
     AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
     AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
 
-    uint8_t accel_instance;
-    uint8_t gyro_instance;
     enum Rotation rotation;
     uint8_t temperature_counter;
 };
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
@@ -79,8 +79,6 @@ class AP_InertialSensor_BMI088 : public AP_InertialSensor_Backend {
     AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
     AP_HAL::Device::PeriodicHandle gyro_periodic_handle;
 
-    uint8_t accel_instance;
-    uint8_t gyro_instance;
     enum Rotation rotation;
     uint8_t temperature_counter;
     enum DevTypes _accel_devtype;

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
@@ -201,8 +201,8 @@ void AP_InertialSensor_BMI160::start()
 
     _dev->get_semaphore()->give();
 
-    if (!_imu.register_accel(_accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) ||
-        !_imu.register_gyro(_gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR),   _dev->get_bus_id_devtype(DEVTYPE_BMI160))) {
+    if (!_imu.register_accel(accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) ||
+        !_imu.register_gyro(gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR),   _dev->get_bus_id_devtype(DEVTYPE_BMI160))) {
         return;
     }
 
@@ -213,8 +213,8 @@ void AP_InertialSensor_BMI160::start()
 
 bool AP_InertialSensor_BMI160::update()
 {
-    update_accel(_accel_instance);
-    update_gyro(_gyro_instance);
+    update_accel(accel_instance);
+    update_gyro(gyro_instance);
     return true;
 }
 
@@ -420,11 +420,11 @@ void AP_InertialSensor_BMI160::_read_fifo()
         accel *= _accel_scale;
         gyro *= _gyro_scale;
 
-        _rotate_and_correct_accel(_accel_instance, accel);
-        _rotate_and_correct_gyro(_gyro_instance, gyro);
+        _rotate_and_correct_accel(accel_instance, accel);
+        _rotate_and_correct_gyro(gyro_instance, gyro);
 
-        _notify_new_accel_raw_sample(_accel_instance, accel);
-        _notify_new_gyro_raw_sample(_gyro_instance, gyro);
+        _notify_new_accel_raw_sample(accel_instance, accel);
+        _notify_new_gyro_raw_sample(gyro_instance, gyro);
     }
 
     if (excess) {

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
@@ -120,10 +120,7 @@ class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend {
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
     enum Rotation _rotation;
 
-    uint8_t _accel_instance;
     float _accel_scale;
-
-    uint8_t _gyro_instance;
     float _gyro_scale;
 
     AP_HAL::DigitalSource *_int1_pin;

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp
@@ -212,23 +212,23 @@ void AP_InertialSensor_BMI270::start()
 
     _dev->get_semaphore()->give();
 
-    if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||
-        !_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {
+    if (!_imu.register_accel(accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||
+        !_imu.register_gyro(gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {
         return;
     }
 
     // setup sensor rotations from probe()
-    set_gyro_orientation(_gyro_instance, _rotation);
-    set_accel_orientation(_accel_instance, _rotation);
+    set_gyro_orientation(gyro_instance, _rotation);
+    set_accel_orientation(accel_instance, _rotation);
 
     /* Call read_fifo() at 1600Hz */
     periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
 }
 
 bool AP_InertialSensor_BMI270::update()
 {
-    update_accel(_accel_instance);
-    update_gyro(_gyro_instance);
+    update_accel(accel_instance);
+    update_gyro(gyro_instance);
     return true;
 }
 
@@ -364,8 +364,8 @@ void AP_InertialSensor_BMI270::fifo_reset()
     // flush and reset FIFO
     write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH);
 
-    notify_accel_fifo_reset(_accel_instance);
-    notify_gyro_fifo_reset(_gyro_instance);
+    notify_accel_fifo_reset(accel_instance);
+    notify_gyro_fifo_reset(gyro_instance);
 }
 
 /*
@@ -383,8 +383,8 @@ void AP_InertialSensor_BMI270::read_fifo(void)
 
     uint8_t len[2];
     if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) {
-        _inc_accel_error_count(_accel_instance);
-        _inc_gyro_error_count(_gyro_instance);
+        _inc_accel_error_count(accel_instance);
+        _inc_gyro_error_count(gyro_instance);
         return;
     }
     uint16_t fifo_length = len[0] + (len[1]<<8);
@@ -403,8 +403,8 @@ void AP_InertialSensor_BMI270::read_fifo(void)
 
     uint8_t data[fifo_length];
     if (!read_registers(BMI270_REG_FIFO_DATA, data, fifo_length)) {
-        _inc_accel_error_count(_accel_instance);
-        _inc_gyro_error_count(_gyro_instance);
+        _inc_accel_error_count(accel_instance);
+        _inc_gyro_error_count(gyro_instance);
         return;
     }
 
@@ -462,14 +462,14 @@ void AP_InertialSensor_BMI270::read_fifo(void)
         temperature_counter = 0;
         uint8_t tbuf[2];
         if (!read_registers(BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) {
-            _inc_accel_error_count(_accel_instance);
-            _inc_gyro_error_count(_gyro_instance);
+            _inc_accel_error_count(accel_instance);
+            _inc_gyro_error_count(gyro_instance);
         } else {
             uint16_t tval = tbuf[0] | (tbuf[1] << 8);
             if (tval != 0x8000) {   // 0x8000 is invalid
                 int16_t klsb = static_cast<int16_t>(tval);
                 float temp_degc = klsb * 0.002f + 23.0f;
-                _publish_temperature(_accel_instance, temp_degc);
+                _publish_temperature(accel_instance, temp_degc);
             }
         }
     }
@@ -487,8 +487,8 @@ void AP_InertialSensor_BMI270::parse_accel_frame(const uint8_t* d)
 
     accel *= scale;
 
-    _rotate_and_correct_accel(_accel_instance, accel);
-    _notify_new_accel_raw_sample(_accel_instance, accel);
+    _rotate_and_correct_accel(accel_instance, accel);
+    _notify_new_accel_raw_sample(accel_instance, accel);
 }
 
 void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
@@ -502,8 +502,8 @@ void AP_InertialSensor_BMI270::parse_gyro_frame(const uint8_t* d)
     Vector3f gyro(xyz[0], xyz[1], xyz[2]);
     gyro *= scale;
 
-    _rotate_and_correct_gyro(_gyro_instance, gyro);
-    _notify_new_gyro_raw_sample(_gyro_instance, gyro);
+    _rotate_and_correct_gyro(gyro_instance, gyro);
+    _notify_new_gyro_raw_sample(gyro_instance, gyro);
 }
 
 bool AP_InertialSensor_BMI270::hardware_init()

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h
@@ -115,8 +115,6 @@ class AP_InertialSensor_BMI270 : public AP_InertialSensor_Backend {
 
     enum Rotation _rotation;
 
-    uint8_t _accel_instance;
-    uint8_t _gyro_instance;
     uint8_t temperature_counter;
 
     static const uint8_t maximum_fifo_config_file[];

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -141,6 +141,10 @@ class AP_InertialSensor_Backend
     //Default Clip Limit
     float _clip_limit = 15.5f * GRAVITY_MSS;
 
+    // instance numbers of accel and gyro data
+    uint8_t gyro_instance;
+    uint8_t accel_instance;
+
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
 
@@ -281,6 +285,14 @@ class AP_InertialSensor_Backend
         _imu._accel_orientation[instance] = rotation;
     }
 
+    uint8_t get_gyro_instance() const {
+        return gyro_instance;
+    }
+
+    uint8_t get_accel_instance() const {
+        return accel_instance;
+    }
+
     // increment clipping counted. Used by drivers that do decimation before supplying
     // samples to the frontend
     void increment_clip_count(uint8_t instance) {

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h
@@ -21,8 +21,6 @@ class AP_InertialSensor_ExternalAHRS : public AP_InertialSensor_Backend
     bool get_output_banner(char* banner, uint8_t banner_len) override;
 
 private:
-    uint8_t gyro_instance;
-    uint8_t accel_instance;
     const uint8_t serial_port;
     bool started;
 };