AP_InertialSensor: changed num_samples_available() to sample_available()

this makes the interface clearer. It also fixes a 3D accel cal bug.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -360,16 +360,20 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         // clear out any existing samples from ins
         update();
 
-        // wait until we have 32 samples
-        while( num_samples_available() < 32 * SAMPLE_UNIT ) {
-            hal.scheduler->delay(10);
+        // average 32 samples
+        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++;
+            }
         }
-
-        // read samples from ins
-        update();
-
-        // capture sample
-        samples[i] = get_accel();
+        samples[i] /= num_samples;
     }
 
     // run the calibration routine

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -118,8 +118,8 @@ public:
     // depends on what gyro chips are being used
     virtual float get_gyro_drift_rate(void) = 0;
 
-    // get number of samples read from the sensors
-    virtual uint16_t num_samples_available() = 0;
+    // true if a new sample is available from the sensors
+    virtual bool sample_available() = 0;
 
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -120,7 +120,7 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
 
 bool AP_InertialSensor_Flymaple::update(void) 
 {
-    while (num_samples_available() == 0) {
+    while (sample_available() == false) {
         hal.scheduler->delay(1);
     }
     Vector3f accel_scale = _accel_scale.get();
@@ -221,10 +221,10 @@ void AP_InertialSensor_Flymaple::_ins_timer(uint32_t now)
     _accumulate();
 }
 
-uint16_t AP_InertialSensor_Flymaple::num_samples_available(void)
+bool AP_InertialSensor_Flymaple::sample_available(void)
 {
     _accumulate();
-    return min(_accel_sum_count, _gyro_sum_count) / _sample_divider;
+    return (min(_accel_sum_count, _gyro_sum_count) / _sample_divider) > 0;
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -20,7 +20,7 @@ public:
     float        	get_delta_time();
     uint32_t        get_last_sample_time_micros();
     float           get_gyro_drift_rate();
-    uint16_t        num_samples_available();
+    bool            sample_available();
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -279,7 +279,7 @@ static volatile uint16_t _count;
 void AP_InertialSensor_MPU6000::wait_for_sample()
 {
     uint32_t tstart = hal.scheduler->micros();
-    while (num_samples_available() == 0) {
+    while (sample_available() == false) {
         uint32_t now = hal.scheduler->micros();
         uint32_t dt = now - tstart;
         if (dt > 50000) {
@@ -417,7 +417,7 @@ void AP_InertialSensor_MPU6000::_read_data_from_timerprocess()
     if (!_spi_sem->take_nonblocking()) {
         /*
           the semaphore being busy is an expected condition when the
-          mainline code is calling num_samples_available() which will
+          mainline code is calling sample_available() which will
           grab the semaphore. We return now and rely on the mainline
           code grabbing the latest sample.
          */
@@ -629,11 +629,11 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void)
     return ToRad(0.5/60);
 }
 
-// get number of samples read from the sensors
-uint16_t AP_InertialSensor_MPU6000::num_samples_available()
+// return true if a sample is available
+bool AP_InertialSensor_MPU6000::sample_available()
 {
     _poll_data(0);
-    return _count >> _sample_shift;
+    return (_count >> _sample_shift) > 0;
 }
 
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -39,8 +39,8 @@ public:
     void                push_accel_offsets_to_dmp();
     void                set_dmp_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
 
-    // num_samples_available - get number of samples read from the sensors
-    uint16_t            num_samples_available();
+    // sample_available - true when a new sample is available
+    bool                sample_available();
 
     // get_delta_time returns the time period in seconds overwhich the sensor data was collected
     float            	get_delta_time();

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -121,10 +121,10 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
     return ToRad(3.0/60);
 }
 
-// get number of samples read from the sensors
-uint16_t AP_InertialSensor_Oilpan::num_samples_available()
+// return true if a new sample is available
+bool AP_InertialSensor_Oilpan::sample_available()
 {
-    return _adc->num_samples_available(_sensors) / _sample_threshold;
+    return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0;
 }
 #endif // CONFIG_HAL_BOARD
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -21,8 +21,8 @@ public:
     //uint32_t        get_last_sample_time_micros();  // last_sample_time - get time (in microseconds) that last sample was captured
     float           get_gyro_drift_rate();
 
-    // get number of samples read from the sensors
-    uint16_t        num_samples_available();
+    // sample_available() - true when a new sample is available
+    bool            sample_available();
 
 protected:
     uint16_t        _init_sensor(Sample_rate sample_rate);

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -142,14 +142,14 @@ void AP_InertialSensor_PX4::_get_sample(void)
 	}
 }
 
-uint16_t AP_InertialSensor_PX4::num_samples_available(void)
+bool AP_InertialSensor_PX4::sample_available(void)
 {
     uint64_t tnow = hrt_absolute_time();
     if (tnow - _last_sample_timestamp > _sample_time_usec) {
         _num_samples_available++;
         _last_sample_timestamp = tnow;
     }
-    return _num_samples_available;
+    return _num_samples_available > 0;
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -23,7 +23,7 @@ public:
     bool            update();
     float        	get_delta_time();
     float           get_gyro_drift_rate();
-    uint16_t        num_samples_available();
+    bool            sample_available();
 
 private:
     uint16_t _init_sensor( Sample_rate sample_rate );

libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp

@@ -44,10 +44,11 @@ float AP_InertialSensor_Stub::get_gyro_drift_rate(void) {
     // 0.5 degrees/second/minute
     return ToRad(0.5/60);
 }
-uint16_t AP_InertialSensor_Stub::num_samples_available()
+
+bool AP_InertialSensor_Stub::sample_available()
 {
     uint16_t ret = (hal.scheduler->millis() - _last_update_ms) 
         / _sample_period_ms;
     
-    return ret;
+    return ret > 0;
 }

libraries/AP_InertialSensor/AP_InertialSensor_Stub.h

@@ -17,7 +17,7 @@ public:
     float	        get_delta_time();
     uint32_t        get_last_sample_time_micros();
     float           get_gyro_drift_rate();
-    uint16_t        num_samples_available();
+    bool            sample_available();
 
 protected:
     uint16_t        _init_sensor( Sample_rate sample_rate );

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.num_samples_available() == 0) /* noop */ ;
+        while (ins.sample_available() == false) /* noop */ ;
 
         // 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.num_samples_available() == 0) /* noop */ ;
+        while (ins.sample_available() == false) /* noop */ ;
 
         // read samples from ins
         ins.update();

libraries/AP_InertialSensor/examples/PX4/PX4.pde

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