AP_InertialSensor: allow backend filters to be updated independently …

…from a separate thread

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1896,6 +1896,7 @@ void AP_InertialSensor::update(void)
             _delta_velocity_valid[i] = false;
             _delta_angle_valid[i] = false;
         }
+
         for (uint8_t i=0; i<_backend_count; i++) {
             _backends[i]->update();
         }
@@ -2798,6 +2799,13 @@ void AP_InertialSensor::force_save_calibration(void)
     }
 }
 
+void AP_InertialSensor::update_backend_filters()
+{
+    for (uint8_t i=0; i<_backend_count; i++) {
+        _backends[i]->update_filters();
+    }
+}
+
 namespace AP {
 
 AP_InertialSensor &ins()

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -263,6 +263,7 @@ class AP_InertialSensor : AP_AccelCal_Client
     AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
 
     void detect_backends(void);
+    void update_backend_filters();
 
     // accel peak hold detector
     void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -17,6 +17,13 @@
 #define AP_HEATER_IMU_INSTANCE 0
 #endif
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+// hal.console can be accessed from bus threads on ChibiOS
+#define debug(fmt, args ...)  do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0)
+#else
+#define debug(fmt, args ...)  do {printf("IMU: " fmt "\n", ## args); } while(0)
+#endif
+
 const extern AP_HAL::HAL& hal;
 
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
@@ -781,6 +788,14 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
         _imu._new_gyro_data[instance] = false;
     }
 
+    update_gyro_filters(instance);
+}
+
+/*
+  propagate filter changes from front end to backend
+ */
+void AP_InertialSensor_Backend::update_gyro_filters(uint8_t instance) /* front end */
+{
     // possibly update filter frequency
     const float gyro_rate = _gyro_raw_sample_rate(instance);
 
@@ -815,14 +830,31 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
         _publish_accel(instance, _imu._accel_filtered[instance]);
         _imu._new_accel_data[instance] = false;
     }
-
+
+    update_accel_filters(instance);
+}
+
+
+/*
+  propagate filter changes from front end to backend
+ */
+void AP_InertialSensor_Backend::update_accel_filters(uint8_t instance) /* front end */
+{
     // possibly update filter frequency
     if (_last_accel_filter_hz != _accel_filter_cutoff()) {
         _imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
         _last_accel_filter_hz = _accel_filter_cutoff();
     }
 }
 
+void AP_InertialSensor_Backend::update_filters()
+{
+    WITH_SEMAPHORE(_sem);
+
+    update_accel_filters(accel_instance);
+    update_gyro_filters(gyro_instance);
+}
+
 #if HAL_LOGGING_ENABLED
 bool AP_InertialSensor_Backend::should_log_imu_raw() const
 {

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -53,6 +53,13 @@ class AP_InertialSensor_Backend
      */
     virtual bool update() = 0; /* front end */
 
+    /*
+     * Update the filter parameters. Called by the frontend to propagate
+     * filter parameters to the frontend structure via the
+     * update_gyro_filters() and update_accel_filters() functions
+     */
+    void update_filters() __RAMFUNC__; /* front end */
+
     /*
      * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
      */
@@ -276,9 +283,11 @@ class AP_InertialSensor_Backend
 
     // common gyro update function for all backends
     void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
+    void update_gyro_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
     // common accel update function for all backends
     void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
+    void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -46,6 +46,7 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
 
     /* update accel and gyro state */
     bool update() override __RAMFUNC__; /* front end */
+
     void accumulate() override; /* front end */
 
     /*

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h

@@ -42,7 +42,7 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
                                             enum Rotation rotation);
 
     /* update accel and gyro state */
-    bool update() override;
+    bool update() override __RAMFUNC__;
     void accumulate() override;
 
     /*

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -24,7 +24,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
                                             enum Rotation rotation);
 
     /* update accel and gyro state */
-    bool update() override;
+    bool update() override __RAMFUNC__;
     void accumulate() override;
 
     void start() override;

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -193,10 +193,7 @@ void AP_InertialSensor::write_notch_log_messages() const
                 notches[0]);
         }
 
-        // ask the HarmonicNotchFilter object for primary gyro to
-        // log the actual notch centers
-        const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
-        notch.filter[primary_gyro].log_notch_centers(i, now_us);
+        notch.filter[AP::ahrs().get_primary_gyro_index()].log_notch_centers(i, now_us);
     }
 }
 #endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED