AP_InertialSensor: added wait_for_sample() API call

this waits for a new INS sample to arrive, using whatever method is
most efficient on each INS type

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -364,16 +364,16 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         samples[i] = Vector3f();
         uint8_t num_samples = 0;
         while (num_samples < 32) {
-            if (sample_available()) {
-                // read samples from ins
-                update();
-                // capture sample
-                samples[i] += get_accel();
-                hal.scheduler->delay(10);
-                num_samples++;
-            } else {
-                hal.scheduler->delay_microseconds(500);
+            if (!wait_for_sample(1000)) {
+                interact->printf_P(PSTR("Failed to get INS sample\n"));
+                return false;
             }
+            // read samples from ins
+            update();
+            // capture sample
+            samples[i] += get_accel();
+            hal.scheduler->delay(10);
+            num_samples++;
         }
         samples[i] /= num_samples;
     }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -121,6 +121,9 @@ public:
     // 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;
+
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -175,8 +175,8 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
 // This takes about 20us to run
 bool AP_InertialSensor_Flymaple::update(void) 
 {
-    while (sample_available() == false) {
-        hal.scheduler->delay(1);
+    if (!wait_for_sample(100)) {
+        return false;
     }
     Vector3f accel_scale = _accel_scale.get();
 
@@ -304,5 +304,20 @@ bool AP_InertialSensor_Flymaple::sample_available(void)
     return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
 }
 
+bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms)
+{
+    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()) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -23,6 +23,7 @@ public:
     uint32_t        get_last_sample_time_micros();
     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 );

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -52,3 +52,18 @@ bool AP_InertialSensor_HIL::sample_available()
     
     return ret > 0;
 }
+
+bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
+{
+    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()) {
+            return true;
+        }
+    }
+    return false;
+}

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -18,6 +18,7 @@ public:
     uint32_t        get_last_sample_time_micros();
     float           get_gyro_drift_rate();
     bool            sample_available();
+    bool            wait_for_sample(uint16_t timeout_ms);
 
 protected:
     uint16_t        _init_sensor( Sample_rate sample_rate );

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -229,8 +229,8 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz)
 
 bool AP_InertialSensor_L3G4200D::update(void) 
 {
-    while (sample_available() == false) {
-        hal.scheduler->delay(1);
+    if (!wait_for_sample(1000)) {
+        return false;
     }
     Vector3f accel_scale = _accel_scale.get();
 
@@ -349,5 +349,20 @@ bool AP_InertialSensor_L3G4200D::sample_available(void)
     return ((hal.scheduler->micros() - _last_sample_time) >= _sample_period_usec);
 }
 
+bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms)
+{
+    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()) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h

@@ -22,6 +22,7 @@ public:
     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 );

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -245,18 +245,19 @@ static volatile uint16_t _count;
 
 /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
-void AP_InertialSensor_MPU6000::wait_for_sample()
+bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms)
 {
-    uint32_t tstart = hal.scheduler->micros();
-    while (sample_available() == false) {
-        uint32_t now = hal.scheduler->micros();
-        uint32_t dt = now - tstart;
-        if (dt > 50000) {
-            hal.scheduler->panic(
-                    PSTR("PANIC: AP_InertialSensor_MPU6000::update "
-                        "waited 50ms for data from interrupt"));
+    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()) {
+            return true;
         }
     }
+    return false;
 }
 
 bool AP_InertialSensor_MPU6000::update( void )
@@ -266,7 +267,9 @@ bool AP_InertialSensor_MPU6000::update( void )
     Vector3f accel_scale = _accel_scale.get();
 
     // wait for at least 1 sample
-    wait_for_sample();
+    if (!wait_for_sample(1000)) {
+        return false;
+    }
 
     // disable timer procs for mininum time
     hal.scheduler->suspend_timer_procs();

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -27,6 +27,9 @@ public:
     // 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);
+
     // get_delta_time returns the time period in seconds overwhich the sensor data was collected
     float            	get_delta_time();
 
@@ -43,7 +46,6 @@ private:
     uint8_t              _register_read( uint8_t reg );
     bool _register_read_from_timerprocess( uint8_t reg, uint8_t *val );
     void                 register_write( uint8_t reg, uint8_t val );
-    void                        wait_for_sample();
     bool                        hardware_init(Sample_rate sample_rate);
 
     AP_HAL::SPIDeviceDriver *_spi;

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -4,6 +4,8 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #include "AP_InertialSensor_Oilpan.h"
 
+const extern AP_HAL::HAL& hal;
+
 // ADC channel mappings on for the APM Oilpan
 // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
 const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
@@ -126,5 +128,21 @@ 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()) {
+        return true;
+    }
+    uint32_t start = hal.scheduler->millis();
+    while ((hal.scheduler->millis() - start) < timeout_ms) {
+        hal.scheduler->delay_microseconds(100);
+        if (sample_available()) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -24,6 +24,9 @@ public:
     // 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);
+
 protected:
     uint16_t        _init_sensor(Sample_rate sample_rate);
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -152,5 +152,20 @@ bool AP_InertialSensor_PX4::sample_available(void)
     return _num_samples_available > 0;
 }
 
+bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
+{
+    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()) {
+            return true;
+        }
+    }
+    return false;
+}
+
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -24,6 +24,7 @@ public:
     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 );