AP_InertialSensor: added support for multiple accel/gyro devices

this makes it possible to ask for the gyro and accel vectors from
secondary INS devices.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -588,5 +588,42 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll,
     }
 }
 
+/**
+   default versions of multi-device accessor functions
+ */
+bool AP_InertialSensor::get_gyro_instance_health(uint8_t instance) const
+{
+    if (instance != 0) {
+        return false;
+    }
+    return healthy();
+}
+
+bool AP_InertialSensor::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
+{
+    if (instance != 0) {
+        return false;
+    }
+    gyro = get_gyro();
+    return true;
+}
+
+bool AP_InertialSensor::get_accel_instance_health(uint8_t instance) const
+{
+    if (instance != 0) {
+        return false;
+    }
+    return healthy();
+}
+
+bool AP_InertialSensor::get_accel_instance(uint8_t instance, Vector3f &accel) const
+{
+    if (instance != 0) {
+        return false;
+    }
+    accel = get_accel();
+    return true;
+}
+
 #endif // __AVR_ATmega1280__
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -97,6 +97,15 @@ public:
     const Vector3f     &get_accel(void) const { return _accel; }
     virtual void       set_accel(const Vector3f &accel) {}
 
+    // multi-device interface
+    virtual bool get_gyro_instance_health(uint8_t instance) const;
+    virtual uint8_t get_gyro_count(void) const { return 1; };
+    virtual bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
+
+    virtual bool get_accel_instance_health(uint8_t instance) const;
+    virtual uint8_t get_accel_count(void) const { return 1; };
+    virtual bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
+
     // get accel offsets in m/s/s
     Vector3f get_accel_offsets() { return _accel_offset; }
     void     set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
@@ -118,9 +127,6 @@ public:
     // depends on what gyro chips are being used
     virtual float get_gyro_drift_rate(void) = 0;
 
-    // true if a new sample is available from the sensors
-    virtual bool sample_available() = 0;
-
     // wait for a sample to be available, with timeout in milliseconds
     virtual bool wait_for_sample(uint16_t timeout_ms) = 0;
 
@@ -140,7 +146,7 @@ public:
     }
 
     virtual uint16_t error_count(void) const { return 0; }
-    virtual bool healthy(void) { return true; }
+    virtual bool healthy(void) const { return true; }
 
 protected:
 

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -295,7 +295,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_Flymaple::sample_available(void)
+bool AP_InertialSensor_Flymaple::_sample_available(void)
 {
     _accumulate();
     return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
@@ -303,13 +303,13 @@ bool AP_InertialSensor_Flymaple::sample_available(void)
 
 bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         return true;
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
         hal.scheduler->delay_microseconds(100);
-        if (sample_available()) {
+        if (_sample_available()) {
             return true;
         }
     }

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -21,12 +21,12 @@ public:
     bool            update();
     float        	get_delta_time();
     float           get_gyro_drift_rate();
-    bool            sample_available();
     bool            wait_for_sample(uint16_t timeout_ms);
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );
     static          void _accumulate(void);
+    bool            _sample_available();
     uint64_t        _last_update_usec;
     float           _delta_time;
     static Vector3f	_accel_filtered;

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -41,7 +41,7 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
     return ToRad(0.5/60);
 }
 
-bool AP_InertialSensor_HIL::sample_available()
+bool AP_InertialSensor_HIL::_sample_available()
 {
     uint16_t ret = (hal.scheduler->millis() - _last_update_ms) 
         / _sample_period_ms;
@@ -51,13 +51,13 @@ bool AP_InertialSensor_HIL::sample_available()
 
 bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         return true;
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
         hal.scheduler->delay(1);
-        if (sample_available()) {
+        if (_sample_available()) {
             return true;
         }
     }
@@ -80,7 +80,7 @@ void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
 /**
    try to detect bad accel/gyro sensors
  */
-bool AP_InertialSensor_HIL::healthy(void)
+bool AP_InertialSensor_HIL::healthy(void) const
 {
     uint32_t tnow = hal.scheduler->micros();
     if ((tnow - _last_accel_usec) > 40000) {

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -16,13 +16,13 @@ public:
     bool            update();
     float	        get_delta_time();
     float           get_gyro_drift_rate();
-    bool            sample_available();
     bool            wait_for_sample(uint16_t timeout_ms);
     void            set_accel(const Vector3f &accel);
     void            set_gyro(const Vector3f &gyro);
-    bool            healthy(void);
+    bool            healthy(void) const;
 
-protected:
+private:
+    bool            _sample_available();
     uint16_t        _init_sensor( Sample_rate sample_rate );
     uint32_t        _sample_period_ms;
     uint32_t        _last_update_ms;

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -344,14 +344,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
     i2c_sem->give();
 }
 
-bool AP_InertialSensor_L3G4200D::sample_available(void)
+bool AP_InertialSensor_L3G4200D::_sample_available(void)
 {
     return (_gyro_samples_available >= _gyro_samples_needed);
 }
 
 bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         _last_sample_time = hal.scheduler->micros();
         return true;
     }
@@ -359,7 +359,7 @@ bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
     while ((hal.scheduler->millis() - start) < timeout_ms) {
         hal.scheduler->delay_microseconds(100);
         _accumulate();
-        if (sample_available()) {
+        if (_sample_available()) {
             _last_sample_time = hal.scheduler->micros();
             return true;
         }

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -21,12 +21,12 @@ public:
     bool            update();
     float        	get_delta_time();
     float           get_gyro_drift_rate();
-    bool            sample_available();
     bool            wait_for_sample(uint16_t timeout_ms);
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );
     void             _accumulate(void);
+    bool            _sample_available();
     uint64_t        _last_update_usec;
     Vector3f        _accel_filtered;
     Vector3f        _gyro_filtered;

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -235,13 +235,13 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate )
 
 bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         return true;
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
         hal.scheduler->delay_microseconds(100);
-        if (sample_available()) {
+        if (_sample_available()) {
             return true;
         }
     }
@@ -319,7 +319,7 @@ void AP_InertialSensor_MPU6000::_poll_data(void)
         if (!_spi_sem->take_nonblocking()) {
             /*
               the semaphore being busy is an expected condition when the
-              mainline code is calling sample_available() which will
+              mainline code is calling wait_for_sample() which will
               grab the semaphore. We return now and rely on the mainline
               code grabbing the latest sample.
             */
@@ -563,7 +563,7 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
 }
 
 // return true if a sample is available
-bool AP_InertialSensor_MPU6000::sample_available()
+bool AP_InertialSensor_MPU6000::_sample_available()
 {
     _poll_data();
     return (_sum_count >> _sample_shift) > 0;

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -24,9 +24,6 @@ public:
     bool                update();
     float               get_gyro_drift_rate();
 
-    // sample_available - true when a new sample is available
-    bool                sample_available();
-
     // wait for a sample to be available, with timeout in milliseconds
     bool                wait_for_sample(uint16_t timeout_ms);
 
@@ -34,7 +31,7 @@ public:
     float            	get_delta_time();
 
     uint16_t error_count(void) const { return _error_count; }
-    bool healthy(void) { return _error_count <= 4; }
+    bool healthy(void) const { return _error_count <= 4; }
 
 protected:
     uint16_t                    _init_sensor( Sample_rate sample_rate );
@@ -42,6 +39,7 @@ protected:
 private:
     AP_HAL::DigitalSource *_drdy_pin;
 
+    bool                 _sample_available();
     void                 _read_data_transaction();
     bool                 _data_ready();
     void                 _poll_data(void);

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -124,20 +124,20 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
 }
 
 // return true if a new sample is available
-bool AP_InertialSensor_Oilpan::sample_available()
+bool AP_InertialSensor_Oilpan::_sample_available()
 {
     return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0;
 }
 
 bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms)
 {
-    if (sample_available()) {
+    if (_sample_available()) {
         return true;
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
         hal.scheduler->delay_microseconds(100);
-        if (sample_available()) {
+        if (_sample_available()) {
             return true;
         }
     }

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -20,9 +20,6 @@ public:
     float        	get_delta_time();    // get_delta_time returns the time period in seconds overwhich the sensor data was collected    
     float           get_gyro_drift_rate();
 
-    // sample_available() - true when a new sample is available
-    bool            sample_available();
-
     // wait for a sample to be available, with timeout in milliseconds
     bool            wait_for_sample(uint16_t timeout_ms);
 
@@ -31,6 +28,8 @@ protected:
 
 private:
 
+    bool            			_sample_available();
+
     AP_ADC *                    _adc;
 
     float                       _temp;

libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde

@@ -165,7 +165,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        while (ins.sample_available() == false) /* noop */ ;
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();

libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde

@@ -166,7 +166,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        while (ins.sample_available() == false) hal.scheduler->delay(1);
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();

libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde

@@ -170,7 +170,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        while (ins.sample_available() == false) /* noop */ ;
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();

libraries/AP_InertialSensor/examples/OilPan/OilPan.pde

@@ -174,7 +174,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        while (ins.sample_available() == false) /* noop */ ;
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();

libraries/AP_InertialSensor/examples/PX4/PX4.pde

@@ -164,9 +164,7 @@ void run_test()
     while( !hal.console->available() ) {
 
         // wait until we have a sample
-        while (ins.sample_available() == false) {
-			hal.scheduler->delay(1);
-		}
+        ins.wait_for_sample();
 
         // read samples from ins
         ins.update();