From ec82aa68c409ab7b14ee4cbf010bd691550d82a1 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 17 Sep 2015 15:42:41 +0900
Subject: [PATCH] InertialSensor: add GYR_CAL to control when gyro calibration
 occurs

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 15 ++++++++----
 .../AP_InertialSensor/AP_InertialSensor.h     | 23 ++++++++-----------
 2 files changed, 21 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 2fdeb60514..6768e29ca4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -303,6 +303,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold,  DEFAULT_STILL_THRESH),
 #endif
 
+    // @Param: GYR_CAL
+    // @DisplayName: Gyro Calibration scheme
+    // @Description: Conrols when automatic gyro calibration is performed
+    // @Values: 0:Never, 1:Start-up only, 2:Start-up and first arming (Copter only)
+    // @User: Advanced
+    AP_GROUPINFO("GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -339,6 +346,7 @@ AP_InertialSensor::AP_InertialSensor() :
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         _accel_error_count[i] = 0;
         _gyro_error_count[i] = 0;
+        _gyro_cal_ok[i] = true;
 #if INS_VIBRATION_CHECK
         _accel_clip_count[i] = 0;
 #endif
@@ -424,8 +432,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id)
 }
 
 void
-AP_InertialSensor::init( Start_style style,
-                         Sample_rate sample_rate)
+AP_InertialSensor::init(Sample_rate sample_rate)
 {
     // remember the sample rate
     _sample_rate = sample_rate;
@@ -442,8 +449,8 @@ AP_InertialSensor::init( Start_style style,
         }
     }
 
-    if (WARM_START != style) {
-        // do cold-start calibration for gyro only
+    // calibrate gyros unless gyro calibration has been disabled
+    if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
         _init_gyro();
     }
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index e15ded11f7..52e11eb4fd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -56,11 +56,6 @@ public:
     AP_InertialSensor();
     static AP_InertialSensor *get_instance();
 
-    enum Start_style {
-        COLD_START = 0,
-        WARM_START
-    };
-
     // the rate that updates will be available to the application
     enum Sample_rate {
         RATE_50HZ  = 50,
@@ -69,21 +64,21 @@ public:
         RATE_400HZ = 400
     };
 
+    enum Gyro_Calibration_Timing {
+        GYRO_CAL_NEVER = 0,
+        GYRO_CAL_STARTUP_ONLY = 1,
+        GYRO_CAL_STARTUP_AND_FIRST_BOOT = 2
+    };
+
     /// Perform startup initialisation.
     ///
     /// Called to initialise the state of the IMU.
     ///
-    /// For COLD_START, implementations using real sensors can assume
-    /// that the airframe is stationary and nominally oriented.
-    ///
-    /// For WARM_START, no assumptions should be made about the
-    /// orientation or motion of the airframe.  Calibration should be
-    /// as for the previous COLD_START call.
+    /// Gyros will be calibrated unless INS_GYRO_CAL is zero
     ///
     /// @param style	The initialisation startup style.
     ///
-    void init( Start_style style,
-               Sample_rate sample_rate);
+    void init(Sample_rate sample_rate);
 
     /// Register a new gyro/accel driver, allocating an instance
     /// number
@@ -147,6 +142,7 @@ public:
     bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
     bool gyro_calibrated_ok_all() const;
     bool use_gyro(uint8_t instance) const;
+    Gyro_Calibration_Timing gyro_calibration_timing() { return (Gyro_Calibration_Timing)_gyro_cal_timing.get(); }
 
     bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
     bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
@@ -317,6 +313,7 @@ private:
     // filtering frequency (0 means default)
     AP_Int8     _accel_filter_cutoff;
     AP_Int8     _gyro_filter_cutoff;
+    AP_Int8     _gyro_cal_timing;
 
     // use for attitude, velocity, position estimates
     AP_Int8     _use[INS_MAX_INSTANCES];