InertialSensor: accel_calibrated_all_ok replaces calibrated

This checks that the current number of accelerometers matches the number
of calibrated accels in order to catch accel failures at boot

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -283,7 +283,6 @@ AP_InertialSensor::AP_InertialSensor() :
     _primary_gyro(0),
     _primary_accel(0),
     _hil_mode(false),
-    _have_3D_calibration(false),
     _calibrating(false),
     _log_raw_data(false),
     _dataflash(NULL)
@@ -343,10 +342,6 @@ AP_InertialSensor::init( Start_style style,
         }
     }
 
-    // remember whether we have 3D calibration so this can be used for
-    // AHRS health
-    check_3D_calibration();
-
     if (WARM_START != style) {
         // do cold-start calibration for gyro only
         _init_gyro();
@@ -575,10 +570,13 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
             _accel_offset[k].set(new_offsets[k]);
             _accel_scale[k].set(new_scaling[k]);
         }
+        for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
+            // clear unused accelerometer's scaling and offsets
+            _accel_offset[k] = Vector3f(0,0,0);
+            _accel_scale[k] = Vector3f(0,0,0);
+        }
         _save_parameters();
 
-        check_3D_calibration();
-
         /*
           calculate the trims as well from primary accels 
           We use the original board rotation for this sample
@@ -607,39 +605,6 @@ failed:
 }
 #endif
 
-/*
-  check if the accelerometers are calibrated in 3D. Called on startup
-  and any accel cal
- */
-void AP_InertialSensor::check_3D_calibration()
-{
-    _have_3D_calibration = false;
-    // check each accelerometer has offsets saved
-    for (uint8_t i=0; i<get_accel_count(); i++) {
-        // exactly 0.0 offset is extremely unlikely
-        if (_accel_offset[i].get().is_zero()) {
-            return;
-        }
-        // exactly 1.0 scaling is extremely unlikely
-        const Vector3f &scaling = _accel_scale[i].get();
-        if (fabsf(scaling.x - 1.0f) < 0.00001f &&
-            fabsf(scaling.y - 1.0f) < 0.00001f &&
-            fabsf(scaling.z - 1.0f) < 0.00001f) {
-            return;
-        }
-    }
-    // if we got this far the accelerometers must have been calibrated
-    _have_3D_calibration = true;
-}
-
-/*
-  return true if we have 3D calibration values
- */
-bool AP_InertialSensor::calibrated() const
-{
-    return _have_3D_calibration;
-}
-
 void
 AP_InertialSensor::init_gyro()
 {
@@ -684,6 +649,44 @@ bool AP_InertialSensor::get_accel_health_all(void) const
     return (get_accel_count() > 0);
 }
 
+/*
+  check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
+ */
+bool AP_InertialSensor::accel_calibrated_ok_all() const
+{
+    // check each accelerometer has offsets saved
+    for (uint8_t i=0; i<get_accel_count(); i++) {
+        // exactly 0.0 offset is extremely unlikely
+        if (_accel_offset[i].get().is_zero()) {
+            return false;
+        }
+        // exactly 1.0 scaling is extremely unlikely
+        const Vector3f &scaling = _accel_scale[i].get();
+        if (is_equal(scaling.x,1.0f) && is_equal(scaling.y,1.0f) && is_equal(scaling.z,1.0f)) {
+            return false;
+        }
+        // zero scaling also indicates not calibrated
+        if (_accel_scale[i].get().is_zero()) {
+            return false;
+        }
+    }
+
+    // check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
+    if (get_accel_count() < INS_MAX_INSTANCES) {
+        for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
+            const Vector3f &scaling = _accel_scale[i].get();
+            bool have_scaling = (!is_zero(scaling.x) && !is_equal(scaling.x,1.0f)) || (!is_zero(scaling.y) && !is_equal(scaling.y,1.0f)) || (!is_zero(scaling.z) && !is_equal(scaling.z,1.0f));
+            bool have_offsets = !_accel_offset[i].get().is_zero();
+            if (have_scaling || have_offsets) {
+                return false;
+            }
+        }
+    }
+
+    // if we got this far the accelerometers must have been calibrated
+    return true;
+}
+
 void
 AP_InertialSensor::_init_gyro()
 {

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -89,12 +89,6 @@ public:
                          float& trim_pitch);
 #endif
 
-    /// calibrated - returns true if the accelerometers have been calibrated
-    ///
-    /// @note this should not be called while flying because it reads from the eeprom which can be slow
-    ///
-    bool calibrated() const;
-
     /// calibrating - returns true if the gyros or accels are currently being calibrated
     bool calibrating() const { return _calibrating; }
 
@@ -160,6 +154,7 @@ public:
     bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
     bool get_accel_health_all(void) const;
     uint8_t get_accel_count(void) const { return _accel_count; };
+    bool accel_calibrated_ok_all() const;
 
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
@@ -244,9 +239,6 @@ private:
     void _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
 #endif
 
-    // check if we have 3D accel calibration
-    void check_3D_calibration(void);
-
     // save parameters to eeprom
     void  _save_parameters();
 
@@ -305,9 +297,6 @@ private:
     // are we in HIL mode?
     bool _hil_mode:1;
 
-    // do we have offsets/scaling from a 3D calibration?
-    bool _have_3D_calibration:1;
-
     // are gyros or accels currently being calibrated
     bool _calibrating:1;