From 745b739ab7f07df904a9691417063bd72ab955a2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 2 Jan 2015 17:38:28 +1100
Subject: [PATCH] AP_InertialSensor: make calibrated() function fast enough to
 call in flight

this will be used in plane to make AHRS SYS_STATUS unhealthy if a user
tries to fly with EKF enabled without a full 3D accel cal.

Note that it doesn't rely on using AP_Param load() to detect that a
value has been set, as some users are first doing a 3D cal then later
doing a 1D cal. In that case load() was returning true and would give
a false positive
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 42 +++++++++++++++----
 .../AP_InertialSensor/AP_InertialSensor.h     |  6 +++
 2 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 9cf1e4412b..f53572ba7b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -223,7 +223,8 @@ AP_InertialSensor::AP_InertialSensor() :
     _accel(),
     _gyro(),
     _board_orientation(ROTATION_NONE),
-    _hil_mode(false)
+    _hil_mode(false),
+    _have_3D_calibration(false)
 {
     AP_Param::setup_object_defaults(this, var_info);        
     for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {
@@ -280,6 +281,10 @@ 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();
@@ -369,6 +374,8 @@ AP_InertialSensor::init_accel()
 
     // save calibration
     _save_parameters();
+
+    check_3D_calibration();
 }
 
 #if !defined( __AVR_ATmega1280__ )
@@ -482,6 +489,8 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact
         }
         _save_parameters();
 
+        check_3D_calibration();
+
         // calculate the trims as well from primary accels and pass back to caller
         _calculate_trim(samples[0][0], trim_roll, trim_pitch);
 
@@ -499,18 +508,37 @@ failed:
 }
 #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 AP_InertialSensor::calibrated()
+/*
+  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++) {
-        if (!_accel_offset[i].load()) {
-            return false;
+        // 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
-    return true;
+    _have_3D_calibration = true;
+}
+
+/*
+  return true if we have 3D calibration values
+ */
+bool AP_InertialSensor::calibrated()
+{
+    return _have_3D_calibration;
 }
 
 void
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index ad86fbeb4a..12004e1ede 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -215,6 +215,9 @@ private:
     void _calculate_trim(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();
 
@@ -264,6 +267,9 @@ private:
     // are we in HIL mode?
     bool _hil_mode:1;
 
+    // do we have offsets/scaling from a 3D calibration?
+    bool _have_3D_calibration:1;
+
     // the delta time in seconds for the last sample
     float _delta_time;