AP_InertialSensor: simplify sensor backends

use common code for filtering and update, allowing each sensor driver
to be simpler and more consistent

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -368,22 +368,24 @@ AP_InertialSensor *AP_InertialSensor::get_instance()
 /*
   register a new gyro instance
  */
-uint8_t AP_InertialSensor::register_gyro(void)
+uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz)
 {
     if (_gyro_count == INS_MAX_INSTANCES) {
         hal.scheduler->panic("Too many gyros");
     }
+    _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
     return _gyro_count++;
 }
 
 /*
   register a new accel instance
  */
-uint8_t AP_InertialSensor::register_accel(void)
+uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz)
 {
     if (_accel_count == INS_MAX_INSTANCES) {
         hal.scheduler->panic("Too many accels");
     }
+    _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
     return _accel_count++;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -23,6 +23,7 @@
 #include <AP_Math/AP_Math.h>
 #include "AP_InertialSensor_UserInteract.h"
 #include <Filter/LowPassFilter.h>
+#include <Filter/LowPassFilter2p.h>
 
 class AP_InertialSensor_Backend;
 class AuxiliaryBus;
@@ -73,8 +74,8 @@ public:
 
     /// Register a new gyro/accel driver, allocating an instance
     /// number
-    uint8_t register_gyro(void);
-    uint8_t register_accel(void);
+    uint8_t register_gyro(uint16_t raw_sample_rate_hz);
+    uint8_t register_accel(uint16_t raw_sample_rate_hz);
 
     // perform accelerometer calibration including providing user instructions
     // and feedback
@@ -283,6 +284,14 @@ private:
     // time accumulator for delta velocity accumulator
     float _delta_velocity_acc_dt[INS_MAX_INSTANCES];
 
+    // Low Pass filters for gyro and accel
+    LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
+    LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
+    Vector3f _accel_filtered[INS_MAX_INSTANCES];
+    Vector3f _gyro_filtered[INS_MAX_INSTANCES];
+    bool _new_accel_data[INS_MAX_INSTANCES];
+    bool _new_gyro_data[INS_MAX_INSTANCES];
+    
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
     Vector3f _delta_angle[INS_MAX_INSTANCES];
@@ -303,8 +312,8 @@ private:
     float _accel_max_abs_offsets[INS_MAX_INSTANCES];
 
     // accelerometer and gyro raw sample rate in units of Hz
-    uint32_t _accel_raw_sample_rates[INS_MAX_INSTANCES];
-    uint32_t _gyro_raw_sample_rates[INS_MAX_INSTANCES];
+    uint16_t _accel_raw_sample_rates[INS_MAX_INSTANCES];
+    uint16_t _gyro_raw_sample_rates[INS_MAX_INSTANCES];
 
     // temperatures for an instance if available
     float _temperature[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -4,6 +4,8 @@
 #include "AP_InertialSensor.h"
 #include "AP_InertialSensor_Backend.h"
 
+const extern AP_HAL::HAL& hal;
+
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
     _imu(imu),
     _product_id(AP_PRODUCT_ID_NONE)
@@ -87,6 +89,10 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
     // save previous delta angle for coning correction
     _imu._last_delta_angle[instance] = delta_angle;
     _imu._last_raw_gyro[instance] = gyro;
+
+    _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
+
+    _imu._new_gyro_data[instance] = true;
 }
 
 /*
@@ -123,6 +129,10 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     // delta velocity
     _imu._delta_velocity_acc[instance] += accel * dt;
     _imu._delta_velocity_acc_dt[instance] += dt;
+
+    _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
+
+    _imu._new_accel_data[instance] = true;
 }
 
 void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
@@ -131,24 +141,12 @@ void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
     _imu._accel_max_abs_offsets[instance] = max_offset;
 }
 
-void AP_InertialSensor_Backend::_set_accel_raw_sample_rate(uint8_t instance,
-                                                           uint32_t rate)
-{
-    _imu._accel_raw_sample_rates[instance] = rate;
-}
-
 // set accelerometer error_count
 void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
 {
     _imu._accel_error_count[instance] = error_count;
 }
 
-void AP_InertialSensor_Backend::_set_gyro_raw_sample_rate(uint8_t instance,
-                                                          uint32_t rate)
-{
-    _imu._gyro_raw_sample_rates[instance] = rate;
-}
-
 // set gyro error_count
 void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count)
 {
@@ -169,3 +167,45 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem
 {
     _imu._temperature[instance] = temperature;
 }
+
+/*
+  common gyro update function for all backends
+ */
+void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
+{    
+    hal.scheduler->suspend_timer_procs();
+
+    if (_imu._new_gyro_data[instance]) {
+        _publish_gyro(instance, _imu._gyro_filtered[instance]);
+        _imu._new_gyro_data[instance] = false;
+    }
+
+    // possibly update filter frequency
+    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
+        _imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
+        _last_gyro_filter_hz = _gyro_filter_cutoff();
+    }
+
+    hal.scheduler->resume_timer_procs();
+}
+
+/*
+  common accel update function for all backends
+ */
+void AP_InertialSensor_Backend::update_accel(uint8_t instance)
+{    
+    hal.scheduler->suspend_timer_procs();
+
+    if (_imu._new_accel_data[instance]) {
+        _publish_accel(instance, _imu._accel_filtered[instance]);
+        _imu._new_accel_data[instance] = false;
+    }
+    
+    // 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();
+    }
+
+    hal.scheduler->resume_timer_procs();
+}

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -101,14 +101,12 @@ protected:
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);
 
-    // set accelerometer raw sample rate
-    void _set_accel_raw_sample_rate(uint8_t instance, uint32_t rate);
+    // get accelerometer raw sample rate
     uint32_t _accel_raw_sample_rate(uint8_t instance) const {
         return _imu._accel_raw_sample_rates[instance];
     }
 
-    // set gyroscope raw sample rate
-    void _set_gyro_raw_sample_rate(uint8_t instance, uint32_t rate);
+    // get gyroscope raw sample rate
     uint32_t _gyro_raw_sample_rate(uint8_t instance) const {
         return _imu._gyro_raw_sample_rates[instance];
     }
@@ -142,6 +140,16 @@ protected:
         return _imu._log_raw_data? _imu._dataflash : NULL; 
     }
 
+    // common gyro update function for all backends
+    void update_gyro(uint8_t instance);
+
+    // common accel update function for all backends
+    void update_accel(uint8_t instance);
+    
+    // support for updating filter at runtime
+    int8_t _last_accel_filter_hz;
+    int8_t _last_gyro_filter_hz;
+    
     // note that each backend is also expected to have a static detect()
     // function which instantiates an instance of the backend sensor
     // driver if the sensor is available

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -145,8 +145,8 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
     // give back i2c semaphore
     i2c_sem->give();
 
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
+    _gyro_instance = _imu.register_gyro(raw_sample_rate_hz);
+    _accel_instance = _imu.register_accel(raw_sample_rate_hz);
 
     _product_id = AP_PRODUCT_ID_FLYMAPLE;
 
@@ -166,22 +166,11 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
 // This takes about 20us to run
 bool AP_InertialSensor_Flymaple::update(void) 
 {
-    Vector3f accel, gyro;
-
-    hal.scheduler->suspend_timer_procs();
-    accel = _accel_filtered;
-    gyro = _gyro_filtered;
     _have_gyro_sample = false;
     _have_accel_sample = false;
-    hal.scheduler->resume_timer_procs();
-
-    _publish_accel(_accel_instance, accel);
-    _publish_gyro(_gyro_instance, gyro);
 
-    if (_last_filter_hz != _accel_filter_cutoff()) {
-        _set_filter_frequency(_accel_filter_cutoff());
-        _last_filter_hz = _accel_filter_cutoff();
-    }
+    update_accel(_accel_instance);
+    update_gyro(_gyro_instance);
 
     return true;
 }
@@ -226,7 +215,6 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
         _rotate_and_correct_accel(_accel_instance, accel);
         _notify_new_accel_raw_sample(_accel_instance, accel);
-        _accel_filtered = _accel_filter.apply(accel);
         _have_accel_sample = true;
         _last_accel_timestamp = now;
     }
@@ -246,7 +234,6 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         gyro *= FLYMAPLE_GYRO_SCALE_R_S;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
-        _gyro_filtered = _gyro_filter.apply(gyro);
         _have_gyro_sample = true;
         _last_gyro_timestamp = now;
     }

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -27,19 +27,9 @@ public:
 private:
     bool        _init_sensor(void);
     void        _accumulate(void);
-    Vector3f	_accel_filtered;
-    Vector3f	_gyro_filtered;
     bool        _have_gyro_sample;
     bool        _have_accel_sample;
 
-    // support for updating filter at runtime
-    uint8_t _last_filter_hz;
-
-    void _set_filter_frequency(uint8_t filter_hz);
-    // Low Pass filters for gyro and accel 
-    LowPassFilter2pVector3f _accel_filter;
-    LowPassFilter2pVector3f _gyro_filter;
-
     uint8_t _gyro_instance;
     uint8_t _accel_instance;
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -29,8 +29,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu
 bool AP_InertialSensor_HIL::_init_sensor(void) 
 {
     // grab the used instances
-    _imu.register_gyro();
-    _imu.register_accel();
+    _imu.register_gyro(1200);
+    _imu.register_accel(1200);
 
     _product_id = AP_PRODUCT_ID_NONE;
     _imu.set_hil_mode();

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -117,9 +117,7 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
     _data_idx(0),
     _have_gyro_sample(false),
     _have_accel_sample(false),
-    _have_sample(false),
-    _accel_filter(800, 10),
-    _gyro_filter(800, 10)
+    _have_sample(false)
 {
     pthread_spin_init(&_data_lock, PTHREAD_PROCESS_PRIVATE);
 }
@@ -231,53 +229,33 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
     hal.scheduler->delay(1);
                            
 
-    // Set up the filter desired
-    _set_filter_frequency(_accel_filter_cutoff());
-
     // give back i2c semaphore
     i2c_sem->give();
 
     // start the timer process to read samples
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void));
 
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
+    _gyro_instance = _imu.register_gyro(800);
+    _accel_instance = _imu.register_accel(800);
 
     _product_id = AP_PRODUCT_ID_L3G4200D;
 
     return true;
 }
 
-/*
-  set the filter frequency
- */
-void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
-{
-    _accel_filter.set_cutoff_frequency(800, filter_hz);
-    _gyro_filter.set_cutoff_frequency(800, filter_hz);
-}
-
 /*
   copy filtered data to the frontend
  */
 bool AP_InertialSensor_L3G4200D::update(void) 
 {
-    Vector3f accel, gyro;
-
     pthread_spin_lock(&_data_lock);
     unsigned int idx = !_data_idx;
-    accel = _data[idx].accel_filtered;
-    gyro = _data[idx].gyro_filtered;
-    _have_sample = false;
-    pthread_spin_unlock(&_data_lock);
 
-    _publish_accel(_accel_instance, accel);
-    _publish_gyro(_gyro_instance, gyro);
+    update_gyro(_gyro_instance);
+    update_accel(_accel_instance);
 
-    if (_last_filter_hz != _accel_filter_cutoff()) {
-        _set_filter_frequency(_accel_filter_cutoff());
-        _last_filter_hz = _accel_filter_cutoff();
-    }
+    _have_sample = false;
+    pthread_spin_unlock(&_data_lock);
 
     return true;
 }
@@ -322,7 +300,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                 gyro *= L3G4200D_GYRO_SCALE_R_S;
                 _rotate_and_correct_gyro(_gyro_instance, gyro);
                 _notify_new_gyro_raw_sample(_gyro_instance, gyro);
-                _data[_data_idx].gyro_filtered = _gyro_filter.apply(gyro);
                 _have_gyro_sample = true;
             }
         }
@@ -347,7 +324,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
                 accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
                 _rotate_and_correct_accel(_accel_instance, accel);
                 _notify_new_accel_raw_sample(_accel_instance, accel);
-                _data[_data_idx].accel_filtered = _accel_filter.apply(accel);
                 _have_accel_sample = true;
             }
         }

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -35,10 +35,6 @@ private:
     bool            _init_sensor(void);
     void            _accumulate(void);
 
-    struct {
-        Vector3f accel_filtered;
-        Vector3f gyro_filtered;
-    } _data[2];
     int _data_idx;
     pthread_spinlock_t _data_lock;
 
@@ -46,15 +42,6 @@ private:
     bool _have_accel_sample;
     volatile bool _have_sample;
 
-    // support for updating filter at runtime
-    uint8_t         _last_filter_hz;
-
-    void _set_filter_frequency(uint8_t filter_hz);
-
-    // Low Pass filters for gyro and accel 
-    LowPassFilter2pVector3f _accel_filter;
-    LowPassFilter2pVector3f _gyro_filter;
-
     // gyro and accel instances
     uint8_t _gyro_instance;
     uint8_t _accel_instance;

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -376,10 +376,6 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
     AP_InertialSensor_Backend(imu),
     _drdy_pin_a(NULL),
     _drdy_pin_g(NULL),
-    _last_accel_filter_hz(-1),
-    _last_gyro_filter_hz(-1),
-    _accel_filter(800, 15),
-    _gyro_filter(760, 15),
     _gyro_sample_available(false),
     _accel_sample_available(false),
     _drdy_pin_num_a(drdy_pin_num_a),
@@ -480,8 +476,8 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
 
     hal.scheduler->resume_timer_procs();
 
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
+    _gyro_instance = _imu.register_gyro(760);
+    _accel_instance = _imu.register_accel(800);
 
     _set_accel_max_abs_offset(_accel_instance, 5.0f);
 
@@ -742,7 +738,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
     accel_data *= _accel_scale;
     _rotate_and_correct_accel(_accel_instance, accel_data);
     _notify_new_accel_raw_sample(_accel_instance, accel_data);
-    _accel_filtered = _accel_filter.apply(accel_data);
     _accel_sample_available = true;
 }
 
@@ -758,7 +753,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
     gyro_data *= _gyro_scale;
     _rotate_and_correct_gyro(_gyro_instance, gyro_data);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
-    _gyro_filtered = _gyro_filter.apply(gyro_data);
     _gyro_sample_available = true;
 }
 
@@ -767,38 +761,12 @@ bool AP_InertialSensor_LSM9DS0::update()
     _accel_sample_available = false;
     _gyro_sample_available = false;
 
-    _publish_gyro(_gyro_instance, _gyro_filtered);
-    _publish_accel(_accel_instance, _accel_filtered);
-
-    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
-        _set_accel_filter(_accel_filter_cutoff());
-        _last_accel_filter_hz = _accel_filter_cutoff();
-    }
-
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
-        _set_gyro_filter(_gyro_filter_cutoff());
-        _last_gyro_filter_hz = _gyro_filter_cutoff();
-    }
+    update_gyro(_gyro_instance);
+    update_accel(_accel_instance);
 
     return true;
 }
 
-/*
- *  set the accel filter frequency
- */
-void AP_InertialSensor_LSM9DS0::_set_accel_filter(uint8_t filter_hz)
-{
-    _accel_filter.set_cutoff_frequency(800, filter_hz);
-}
-
-/*
- *  set the gyro filter frequency
- */
-void AP_InertialSensor_LSM9DS0::_set_gyro_filter(uint8_t filter_hz)
-{
-    _gyro_filter.set_cutoff_frequency(760, filter_hz);
-}
-
  #if LSM9DS0_DEBUG
 /* dump all config registers - used for debug */
 void AP_InertialSensor_LSM9DS0::_dump_registers(void)

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h

@@ -97,21 +97,6 @@ private:
     void                            _read_data_transaction_a();
     void                            _read_data_transaction_g();
 
-    /* support for updating filter at runtime */
-    int16_t         _last_gyro_filter_hz;
-    int16_t         _last_accel_filter_hz;
-
-    /* change the filter frequency */
-    void            _set_accel_filter(uint8_t filter_hz);
-    void            _set_gyro_filter(uint8_t filter_hz);
-
-    Vector3f        _accel_filtered;
-    Vector3f        _gyro_filtered;
-
-    /* Low Pass filters for gyro and accel */
-    LowPassFilter2pVector3f         _accel_filter;
-    LowPassFilter2pVector3f         _gyro_filter;
-
 #if LSM9DS0_DEBUG
     void        _dump_registers();
 #endif

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -439,12 +439,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
     _drdy_pin(NULL),
     _bus(bus),
     _bus_sem(NULL),
-    _last_accel_filter_hz(-1),
-    _last_gyro_filter_hz(-1),
-    _accel_filter(1000, 15),
-    _gyro_filter(1000, 15),
     _temp_filter(1000, 1),
-    _sum_count(0),
     _samples(NULL)
 {
 
@@ -590,11 +585,8 @@ void AP_InertialSensor_MPU6000::start()
     _bus_sem->give();
 
     // grab the used instances
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
-
-    _set_accel_raw_sample_rate(_accel_instance, 1000);
-    _set_gyro_raw_sample_rate(_gyro_instance, 1000);
+    _gyro_instance = _imu.register_gyro(1000);
+    _accel_instance = _imu.register_accel(1000);
 
     hal.scheduler->resume_timer_procs();
 
@@ -608,38 +600,13 @@ void AP_InertialSensor_MPU6000::start()
  */
 bool AP_InertialSensor_MPU6000::update( void )
 {    
-    // we have a full set of samples
-    uint16_t num_samples;
-    Vector3f accel, gyro;
-    float temp;
-
-    hal.scheduler->suspend_timer_procs();
-    gyro = _gyro_filtered;
-    accel = _accel_filtered;
-    temp = _temp_filtered;
-    num_samples = 1;
-    _sum_count = 0;
-    hal.scheduler->resume_timer_procs();
-
-    gyro /= num_samples;
-    accel /= num_samples;
-    temp /= num_samples;
+    update_accel(_accel_instance);
+    update_gyro(_gyro_instance);
 
-    _publish_accel(_accel_instance, accel);
-    _publish_gyro(_gyro_instance, gyro);
-    _publish_temperature(_accel_instance, temp);
+    _publish_temperature(_accel_instance, _temp_filtered);
+    
     /* give the temperature to the control loop in order to keep it constant*/
-    hal.util->set_imu_temp(temp);
-
-    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
-        _accel_filter.set_cutoff_frequency(1000, _accel_filter_cutoff());
-        _last_accel_filter_hz = _accel_filter_cutoff();
-    }
-
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
-        _gyro_filter.set_cutoff_frequency(1000, _gyro_filter_cutoff());
-        _last_gyro_filter_hz = _gyro_filter_cutoff();
-    }
+    hal.util->set_imu_temp(_temp_filtered);
 
     return true;
 }
@@ -724,10 +691,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
         _notify_new_accel_raw_sample(_accel_instance, accel);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
-        _accel_filtered = _accel_filter.apply(accel);
-        _gyro_filtered = _gyro_filter.apply(gyro);
         _temp_filtered = _temp_filter.apply(temp);
-        _sum_count++;
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -106,22 +106,13 @@ private:
 
     static const float   _gyro_scale;
 
-    // support for updating filter at runtime
-    int8_t _last_accel_filter_hz;
-    int8_t _last_gyro_filter_hz;
-
     void _set_filter_register(uint16_t filter_hz);
 
     // how many hardware samples before we report a sample to the caller
     uint8_t _sample_count;
 
-    Vector3f _accel_filtered;
-    Vector3f _gyro_filtered;
     float    _temp_filtered;
 
-    // Low Pass filters for gyro and accel
-    LowPassFilter2pVector3f _accel_filter;
-    LowPassFilter2pVector3f _gyro_filter;
     LowPassFilter2pFloat    _temp_filter;
 
     volatile uint16_t _sum_count;

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp

@@ -326,9 +326,7 @@ static struct gyro_state_s st = {
  */
 AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
     AP_InertialSensor_Backend(imu),
-    _have_sample_available(false),
-    _accel_filter(800, 10),
-    _gyro_filter(800, 10)
+    _have_sample_available(false)
 {
 }
 
@@ -349,22 +347,6 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &
     return sensor;
 }
 
-/*
-  set the accel filter frequency
- */
-void AP_InertialSensor_MPU9150::_set_accel_filter_frequency(uint8_t filter_hz)
-{
-    _accel_filter.set_cutoff_frequency(800, filter_hz);
-}
-
-/*
-  set the gyro filter frequency
- */
-void AP_InertialSensor_MPU9150::_set_gyro_filter_frequency(uint8_t filter_hz)
-{
-    _gyro_filter.set_cutoff_frequency(800, filter_hz);
-}
-
 /**
  *  Init method
  */
@@ -458,15 +440,11 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
 
     mpu_set_sensors(sensors);
 
-    // Set the filter frecuency
-    _set_accel_filter_frequency(_accel_filter_cutoff());
-    _set_gyro_filter_frequency(_gyro_filter_cutoff());
-
     // give back i2c semaphore
     i2c_sem->give();
 
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
+    _gyro_instance = _imu.register_gyro(800);
+    _accel_instance = _imu.register_accel(800);
 
     // start the timer process to read samples    
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9150::_accumulate, void));
@@ -1093,13 +1071,11 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
         accel *= MPU9150_ACCEL_SCALE_2G;
         _rotate_and_correct_accel(_accel_instance, accel);
         _notify_new_accel_raw_sample(_accel_instance, accel);
-        _accel_filtered = _accel_filter.apply(accel);
 
         gyro = Vector3f(gyro_x, gyro_y, gyro_z);
         gyro *= MPU9150_GYRO_SCALE_2000;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
-        _gyro_filtered = _gyro_filter.apply(gyro);
 
         _have_sample_available = true;
     }
@@ -1110,26 +1086,9 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
 
 bool AP_InertialSensor_MPU9150::update(void) 
 {
-    Vector3f accel, gyro;
-
-    hal.scheduler->suspend_timer_procs();
-    accel = _accel_filtered;
-    gyro = _gyro_filtered;
     _have_sample_available = false;
-    hal.scheduler->resume_timer_procs();
-
-    _publish_accel(_accel_instance, accel);
-    _publish_gyro(_gyro_instance, gyro);
-
-    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
-        _set_accel_filter_frequency(_accel_filter_cutoff());
-        _last_accel_filter_hz = _accel_filter_cutoff();
-    }
-
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
-        _set_gyro_filter_frequency(_gyro_filter_cutoff());
-        _last_gyro_filter_hz = _gyro_filter_cutoff();
-    }
+    update_gyro(_gyro_instance);
+    update_accel(_accel_instance);
 
     return true;
 }

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h

@@ -28,14 +28,8 @@ public:
 private:
     bool            _init_sensor();
     void             _accumulate(void);
-    Vector3f        _accel_filtered;
-    Vector3f        _gyro_filtered;
     bool            _have_sample_available;
 
-    // // support for updating filter at runtime
-    uint8_t         _last_accel_filter_hz;
-    uint8_t         _last_gyro_filter_hz;
-
     int16_t mpu_set_gyro_fsr(uint16_t fsr);
     int16_t mpu_set_accel_fsr(uint8_t fsr);
     int16_t mpu_set_lpf(uint16_t lpf);
@@ -48,13 +42,6 @@ private:
     int16_t mpu_set_int_latched(uint8_t enable);
     int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t *timestamp, uint8_t *sensors, uint8_t *more);
 
-    void _set_accel_filter_frequency(uint8_t filter_hz);
-    void _set_gyro_filter_frequency(uint8_t filter_hz);
-
-    // Low Pass filters for gyro and accel 
-    LowPassFilter2pVector3f _accel_filter;
-    LowPassFilter2pVector3f _gyro_filter;
-
     uint8_t _gyro_instance;
     uint8_t _accel_instance;
 };

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -362,11 +362,6 @@ bool AP_MPU9250_BusDriver_I2C::has_auxiliary_bus()
 
 AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) :
 	AP_InertialSensor_Backend(imu),
-    _last_accel_filter_hz(-1),
-    _last_gyro_filter_hz(-1),
-    _shared_data_idx(0),
-    _accel_filter(DEFAULT_SAMPLE_RATE, 15),
-    _gyro_filter(DEFAULT_SAMPLE_RATE, 15),
     _have_sample_available(false),
     _bus(bus),
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
@@ -437,17 +432,14 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
     if (!_hardware_init())
         return false;
 
-    _gyro_instance = _imu.register_gyro();
-    _accel_instance = _imu.register_accel();
+    _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE);
+    _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE);
 
     _product_id = AP_PRODUCT_ID_MPU9250;
 
     // start the timer process to read samples
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void));
 
-    _set_accel_raw_sample_rate(_accel_instance, DEFAULT_SAMPLE_RATE);
-    _set_gyro_raw_sample_rate(_gyro_instance, DEFAULT_SAMPLE_RATE);
-
 #if MPU9250_DEBUG
     _dump_registers();
 #endif
@@ -459,25 +451,10 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
  */
 bool AP_InertialSensor_MPU9250::update( void )
 {
-    // pull the data from the timer shared data buffer
-    uint8_t idx = _shared_data_idx;
-    Vector3f gyro = _shared_data[idx]._gyro_filtered;
-    Vector3f accel = _shared_data[idx]._accel_filtered;
-
     _have_sample_available = false;
 
-    _publish_gyro(_gyro_instance, gyro);
-    _publish_accel(_accel_instance, accel);
-
-    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
-        _set_accel_filter(_accel_filter_cutoff());
-        _last_accel_filter_hz = _accel_filter_cutoff();
-    }
-
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
-        _set_gyro_filter(_gyro_filter_cutoff());
-        _last_gyro_filter_hz = _gyro_filter_cutoff();
-    }
+    update_gyro(_gyro_instance);
+    update_accel(_accel_instance);
 
     return true;
 }
@@ -534,13 +511,6 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
     _rotate_and_correct_gyro(_gyro_instance, gyro);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 
-
-    // update the shared buffer
-    uint8_t idx = _shared_data_idx ^ 1;
-    _shared_data[idx]._accel_filtered = _accel_filter.apply(accel);
-    _shared_data[idx]._gyro_filtered = _gyro_filter.apply(gyro);
-    _shared_data_idx = idx;
-
     _have_sample_available = true;
 }
 
@@ -562,23 +532,6 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
     _bus->write8(reg, val);
 }
 
-/*
-  set the accel filter frequency
- */
-void AP_InertialSensor_MPU9250::_set_accel_filter(uint8_t filter_hz)
-{
-    _accel_filter.set_cutoff_frequency(DEFAULT_SAMPLE_RATE, filter_hz);
-}
-
-/*
-  set the gyro filter frequency
- */
-void AP_InertialSensor_MPU9250::_set_gyro_filter(uint8_t filter_hz)
-{
-    _gyro_filter.set_cutoff_frequency(DEFAULT_SAMPLE_RATE, filter_hz);
-}
-
-
 /*
   initialise the sensor configuration registers
  */

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -77,28 +77,6 @@ private:
     AP_HAL::Semaphore *_bus_sem;
     AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr;
 
-    // support for updating filter at runtime
-    int16_t _last_gyro_filter_hz;
-    int16_t _last_accel_filter_hz;
-
-    // change the filter frequency
-    void _set_accel_filter(uint8_t filter_hz);
-    void _set_gyro_filter(uint8_t filter_hz);
-
-    // This structure is used to pass data from the timer which reads
-    // the sensor to the main thread. The _shared_data_idx is used to
-    // prevent race conditions by ensuring the data is fully updated
-    // before being used by the consumer
-    struct {
-        Vector3f _accel_filtered;
-        Vector3f _gyro_filtered;
-    } _shared_data[2];
-    volatile uint8_t _shared_data_idx;
-
-    // Low Pass filters for gyro and accel 
-    LowPassFilter2pVector3f _accel_filter;
-    LowPassFilter2pVector3f _gyro_filter;
-
     // do we currently have a sample pending?
     bool _have_sample_available;
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -73,11 +73,9 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         if (_accel_fd[i] >= 0) {
             _num_accel_instances = i+1;
-            _accel_instance[i] = _imu.register_accel();
         }
         if (_gyro_fd[i] >= 0) {
             _num_gyro_instances = i+1;
-            _gyro_instance[i] = _imu.register_gyro();
         }
     }
     if (_num_accel_instances == 0) {
@@ -122,7 +120,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
         if (samplerate < 100 || samplerate > 10000) {
             hal.scheduler->panic("Invalid gyro sample rate");
         }
-        _set_gyro_raw_sample_rate(_gyro_instance[i], (uint32_t)samplerate);
+        _gyro_instance[i] = _imu.register_gyro(samplerate);
         _gyro_sample_time[i] = 1.0f / samplerate;
     }
 
@@ -162,7 +160,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
         if (samplerate < 100 || samplerate > 10000) {
             hal.scheduler->panic("Invalid accel sample rate");
         }
-        _set_accel_raw_sample_rate(_accel_instance[i], (uint32_t) samplerate);
+        _accel_instance[i] = _imu.register_accel(samplerate);
         _accel_sample_time[i] = 1.0f / samplerate;
     }
 
@@ -209,33 +207,11 @@ bool AP_InertialSensor_PX4::update(void)
     _get_sample();
 
     for (uint8_t k=0; k<_num_accel_instances; k++) {
-        Vector3f accel = _accel_in[k];
-        // calling _publish_accel sets the sensor healthy,
-        // so we only want to do this if we have new data from it
-        if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) {
-            _publish_accel(_accel_instance[k], accel);
-            _last_accel_update_timestamp[k] = _last_accel_timestamp[k];
-        }
+        update_accel(_accel_instance[k]);
     }
 
     for (uint8_t k=0; k<_num_gyro_instances; k++) {
-        Vector3f gyro = _gyro_in[k];
-        // calling _publish_accel sets the sensor healthy,
-        // so we only want to do this if we have new data from it
-        if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) {
-            _publish_gyro(_gyro_instance[k], gyro);
-            _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k];
-        }
-    }
-
-    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
-        _set_accel_filter_frequency(_accel_filter_cutoff());
-        _last_accel_filter_hz = _accel_filter_cutoff();
-    }
-
-    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
-        _set_gyro_filter_frequency(_gyro_filter_cutoff());
-        _last_gyro_filter_hz = _gyro_filter_cutoff();
+        update_gyro(_gyro_instance[k]);
     }
     
     return true;
@@ -250,9 +226,6 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
     _rotate_and_correct_accel(frontend_instance, accel);
     _notify_new_accel_raw_sample(frontend_instance, accel);
 
-    // apply filter for control path
-    _accel_in[i] = _accel_filter[i].apply(accel);
-
     // save last timestamp
     _last_accel_timestamp[i] = accel_report.timestamp;
 
@@ -291,9 +264,6 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report
     _rotate_and_correct_gyro(frontend_instance, gyro);
     _notify_new_gyro_raw_sample(frontend_instance, gyro);
 
-    // apply filter for control path
-    _gyro_in[i] = _gyro_filter[i].apply(gyro);
-
     // save last timestamp
     _last_gyro_timestamp[i] = gyro_report.timestamp;
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -34,8 +34,6 @@ private:
     bool     _init_sensor(void);
     void     _get_sample(void);
     bool     _sample_available(void);
-    Vector3f _accel_in[INS_MAX_INSTANCES];
-    Vector3f _gyro_in[INS_MAX_INSTANCES];
     uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
     uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
     uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES];
@@ -54,13 +52,6 @@ private:
     // calculate right queue depth for a sensor
     uint8_t _queue_depth(uint16_t sensor_sample_rate) const;
 
-    // support for updating filter at runtime (-1 means unset)
-    int8_t _last_gyro_filter_hz;
-    int8_t _last_accel_filter_hz;
-
-    void _set_gyro_filter_frequency(uint8_t filter_hz);
-    void _set_accel_filter_frequency(uint8_t filter_hz);
-
     // accelerometer and gyro driver handles
     uint8_t _num_accel_instances;
     uint8_t _num_gyro_instances;
@@ -73,10 +64,6 @@ private:
     uint8_t _accel_instance[INS_MAX_INSTANCES];
     uint8_t _gyro_instance[INS_MAX_INSTANCES];
 
-    // Low Pass filters for gyro and accel
-    LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
-    LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
-
 #ifdef AP_INERTIALSENSOR_PX4_DEBUG
     uint32_t _gyro_meas_count[INS_MAX_INSTANCES];
     uint32_t _accel_meas_count[INS_MAX_INSTANCES];