AP_InertialSensor: removed "have sample" logic from drivers

the frontend has all the information it needs, so we can simplify the
drivers some more

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1394,8 +1394,11 @@ check_sample:
         bool accel_available = false;
         while (!gyro_available || !accel_available) {
             for (uint8_t i=0; i<_backend_count; i++) {
-                gyro_available |= _backends[i]->gyro_sample_available();
-                accel_available |= _backends[i]->accel_sample_available();
+                _backends[i]->accumulate();
+            }
+            for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+                gyro_available |= _new_gyro_data[i];
+                accel_available |= _new_accel_data[i];
             }
             if (!gyro_available || !accel_available) {
                 hal.scheduler->delay_microseconds(100);

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -43,17 +43,10 @@ public:
      */
     virtual bool update() = 0;
 
-    /* 
-     * return true if at least one accel sample is available in the backend
-     * since the last call to update()
-     */
-    virtual bool accel_sample_available() = 0;
-
-    /* 
-     * return true if at least one gyro sample is available in the backend
-     * since the last call to update()
+    /*
+     * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
      */
-    virtual bool gyro_sample_available() = 0;
+    virtual void accumulate() {}
 
     /*
      * Configure and start all sensors. The empty implementation allows

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -186,7 +186,7 @@ bool AP_InertialSensor_Flymaple::update(void)
 // operations take too long
 // So we are stuck with a suboptimal solution. The results are not so
 // good in terms of timing. It may be better with the FIFOs enabled
-void AP_InertialSensor_Flymaple::_accumulate(void)
+void AP_InertialSensor_Flymaple::accumulate(void)
 {
     // get pointer to i2c bus semaphore
     AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -18,15 +18,14 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; }
-    bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; }
+    // accumulate samples
+    void accumulate(void) override;
 
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
     bool        _init_sensor(void);
-    void        _accumulate(void);
     bool        _have_gyro_sample;
     bool        _have_accel_sample;
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -13,9 +13,6 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { return true; }
-    bool accel_sample_available(void) { return true; }
-
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -114,17 +114,13 @@ const extern AP_HAL::HAL& hal;
 // constructor
 AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
     AP_InertialSensor_Backend(imu),
-    _data_idx(0),
     _have_gyro_sample(false),
-    _have_accel_sample(false),
-    _have_sample(false)
+    _have_accel_sample(false)
 {
-    pthread_spin_init(&_data_lock, PTHREAD_PROCESS_PRIVATE);
 }
 
 AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D()
 {
-    pthread_spin_destroy(&_data_lock);
 }
 
 /*
@@ -232,14 +228,13 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
     // 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(800);
     _accel_instance = _imu.register_accel(800);
-
     _product_id = AP_PRODUCT_ID_L3G4200D;
 
+    // start the timer process to read samples
+    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void));
+
     return true;
 }
 
@@ -248,15 +243,9 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
  */
 bool AP_InertialSensor_L3G4200D::update(void) 
 {
-    pthread_spin_lock(&_data_lock);
-    unsigned int idx = !_data_idx;
-
     update_gyro(_gyro_instance);
     update_accel(_accel_instance);
 
-    _have_sample = false;
-    pthread_spin_unlock(&_data_lock);
-
     return true;
 }
 
@@ -335,11 +324,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
     if (_have_accel_sample && _have_gyro_sample) {
         _have_gyro_sample = false;
         _have_accel_sample = false;
-
-        pthread_spin_lock(&_data_lock);
-        _data_idx = !_data_idx;
-        _have_sample = true;
-        pthread_spin_unlock(&_data_lock);
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -22,9 +22,6 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { return _have_sample; }
-    bool accel_sample_available(void) { return _have_sample; }
-
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
@@ -35,12 +32,8 @@ private:
     bool            _init_sensor(void);
     void            _accumulate(void);
 
-    int _data_idx;
-    pthread_spinlock_t _data_lock;
-
     bool _have_gyro_sample;
     bool _have_accel_sample;
-    volatile bool _have_sample;
 
     // gyro and accel instances
     uint8_t _gyro_instance;

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -376,8 +376,6 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
     AP_InertialSensor_Backend(imu),
     _drdy_pin_a(NULL),
     _drdy_pin_g(NULL),
-    _gyro_sample_available(false),
-    _accel_sample_available(false),
     _drdy_pin_num_a(drdy_pin_num_a),
     _drdy_pin_num_g(drdy_pin_num_g)
 {
@@ -736,9 +734,9 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a()
 
     Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z);
     accel_data *= _accel_scale;
+
     _rotate_and_correct_accel(_accel_instance, accel_data);
     _notify_new_accel_raw_sample(_accel_instance, accel_data);
-    _accel_sample_available = true;
 }
 
 /*
@@ -751,16 +749,13 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
 
     Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
     gyro_data *= _gyro_scale;
+
     _rotate_and_correct_gyro(_gyro_instance, gyro_data);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro_data);
-    _gyro_sample_available = true;
 }
 
 bool AP_InertialSensor_LSM9DS0::update()
 {
-    _accel_sample_available = false;
-    _gyro_sample_available = false;
-
     update_gyro(_gyro_instance);
     update_accel(_accel_instance);
 

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h

@@ -32,14 +32,6 @@ public:
 
     bool        update();
 
-    bool        gyro_sample_available() {
-        return _gyro_sample_available;
-    };
-
-    bool        accel_sample_available() {
-        return _accel_sample_available;
-    };
-
     static AP_InertialSensor_Backend *      detect(AP_InertialSensor &imu);
 
 private:
@@ -64,9 +56,6 @@ private:
     int                             _drdy_pin_num_g;
     AP_HAL::DigitalSource *         _drdy_pin_g;
 
-    bool                            _gyro_sample_available;
-    bool                            _accel_sample_available;
-
     bool                            _accel_data_ready();
     bool                            _gyro_data_ready();
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -537,8 +537,6 @@ void AP_InertialSensor_MPU6000::start()
     _samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
     hal.scheduler->delay(1);
 
-    _sample_count = 1;
-
     // disable sensor filtering
     _set_filter_register(256);
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -63,9 +63,6 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
-    bool accel_sample_available(void) { return _sum_count >= _sample_count; }
-
     /*
      * Return an AuxiliaryBus if the bus driver allows it
      */
@@ -88,7 +85,6 @@ private:
 
     AP_HAL::DigitalSource *_drdy_pin;
     bool    _init_sensor(void);
-    bool    _sample_available();
     void    _read_data_transaction();
     bool    _data_ready();
     void    _poll_data(void);
@@ -108,14 +104,10 @@ private:
 
     void _set_filter_register(uint16_t filter_hz);
 
-    // how many hardware samples before we report a sample to the caller
-    uint8_t _sample_count;
-
     float    _temp_filtered;
 
     LowPassFilter2pFloat    _temp_filter;
 
-    volatile uint16_t _sum_count;
     bool _fifo_mode;
     uint8_t *_samples = nullptr;
 };

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp

@@ -325,8 +325,7 @@ static struct gyro_state_s st = {
  *  @brief      Constructor
  */
 AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) :
-    AP_InertialSensor_Backend(imu),
-    _have_sample_available(false)
+    AP_InertialSensor_Backend(imu)
 {
 }
 
@@ -1076,8 +1075,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
         gyro *= MPU9150_GYRO_SCALE_2000;
         _rotate_and_correct_gyro(_gyro_instance, gyro);
         _notify_new_gyro_raw_sample(_gyro_instance, gyro);
-
-        _have_sample_available = true;
     }
 
     // give back i2c semaphore
@@ -1086,7 +1083,6 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
 
 bool AP_InertialSensor_MPU9150::update(void) 
 {
-    _have_sample_available = false;
     update_gyro(_gyro_instance);
     update_accel(_accel_instance);
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h

@@ -19,16 +19,12 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { return _have_sample_available; }
-    bool accel_sample_available(void) { return _have_sample_available; }
-
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
 private:
     bool            _init_sensor();
     void             _accumulate(void);
-    bool            _have_sample_available;
 
     int16_t mpu_set_gyro_fsr(uint16_t fsr);
     int16_t mpu_set_accel_fsr(uint8_t fsr);

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -362,7 +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),
-    _have_sample_available(false),
     _bus(bus),
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
     _default_rotation(ROTATION_ROLL_180_YAW_270)
@@ -451,8 +450,6 @@ bool AP_InertialSensor_MPU9250::_init_sensor()
  */
 bool AP_InertialSensor_MPU9250::update( void )
 {
-    _have_sample_available = false;
-
     update_gyro(_gyro_instance);
     update_accel(_accel_instance);
 
@@ -508,10 +505,9 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
                     -int16_val(rx, 6));
     gyro *= GYRO_SCALE;
     gyro.rotate(_default_rotation);
+
     _rotate_and_correct_gyro(_gyro_instance, gyro);
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
-
-    _have_sample_available = true;
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -44,9 +44,6 @@ public:
     /* update accel and gyro state */
     bool update();
 
-    bool gyro_sample_available(void) { return _have_sample_available; }
-    bool accel_sample_available(void) { return _have_sample_available; }
-
     AuxiliaryBus *get_auxiliary_bus();
 
     static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) {
@@ -71,15 +68,11 @@ private:
     uint8_t              _register_read( uint8_t reg );
     void                 _register_write( uint8_t reg, uint8_t val );
     bool                 _hardware_init(void);
-    bool                 _sample_available();
 
     AP_MPU9250_BusDriver *_bus;
     AP_HAL::Semaphore *_bus_sem;
     AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr;
 
-    // do we currently have a sample pending?
-    bool _have_sample_available;
-
     // gyro and accel instances
     uint8_t _gyro_instance;
     uint8_t _accel_instance;

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -23,9 +23,7 @@ const extern AP_HAL::HAL& hal;
 AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
     AP_InertialSensor_Backend(imu),
     _last_get_sample_timestamp(0),
-    _last_sample_timestamp(0),
-    _last_gyro_filter_hz(-1),
-    _last_accel_filter_hz(-1)
+    _last_sample_timestamp(0)
 {
 }
 
@@ -164,9 +162,6 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
         _accel_sample_time[i] = 1.0f / samplerate;
     }
 
-    _set_accel_filter_frequency(_accel_filter_cutoff());
-    _set_gyro_filter_frequency(_gyro_filter_cutoff());
-
 #if  CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     _product_id = AP_PRODUCT_ID_VRBRAIN;
 #else
@@ -179,28 +174,6 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
     return true;
 }
 
-/*
-  set the accel filter frequency
- */
-void AP_InertialSensor_PX4::_set_accel_filter_frequency(uint8_t filter_hz)
-{
-    for (uint8_t i=0; i<_num_accel_instances; i++) {
-        float samplerate = _accel_raw_sample_rate(_accel_instance[i]);
-        _accel_filter[i].set_cutoff_frequency(samplerate, filter_hz);
-    }
-}
-
-/*
-  set the gyro filter frequency
- */
-void AP_InertialSensor_PX4::_set_gyro_filter_frequency(uint8_t filter_hz)
-{
-    for (uint8_t i=0; i<_num_gyro_instances; i++) {
-        float samplerate = _gyro_raw_sample_rate(_gyro_instance[i]);
-        _gyro_filter[i].set_cutoff_frequency(samplerate, filter_hz);
-    }
-}
-
 bool AP_InertialSensor_PX4::update(void) 
 {
     // get the latest sample from the sensor drivers
@@ -369,27 +342,5 @@ bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro
     return false;
 }
 
-bool AP_InertialSensor_PX4::gyro_sample_available(void)
-{
-    _get_sample();
-    for (uint8_t i=0; i<_num_gyro_instances; i++) {
-        if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) {
-            return true;
-        }
-    }    
-    return false;
-}
-
-bool AP_InertialSensor_PX4::accel_sample_available(void)
-{
-    _get_sample();
-    for (uint8_t i=0; i<_num_accel_instances; i++) {
-        if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) {
-            return true;
-        }
-    }    
-    return false;
-}
-
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -27,13 +27,12 @@ public:
     // detect the sensor
     static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
 
-    bool gyro_sample_available(void);
-    bool accel_sample_available(void);
+    // accumulate more samples
+    void accumulate(void) override { _get_sample(); }
 
 private:
     bool     _init_sensor(void);
     void     _get_sample(void);
-    bool     _sample_available(void);
     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];