From 786172aa4e99c5aac78a53a30b1ca9b8b69df04f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 11 Mar 2015 10:16:04 +1100
Subject: [PATCH] AP_InertialSensor: removed 1D accel calibration

it is finally time to move on from this. We want to push people
towards better calibration and removing the 1D accel cal is the first
step
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 122 ------------------
 .../AP_InertialSensor/AP_InertialSensor.h     |  11 +-
 .../examples/INS_generic/INS_generic.pde      |  30 -----
 3 files changed, 1 insertion(+), 162 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index f494f71f89..668d896639 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -388,17 +388,6 @@ AP_InertialSensor::_detect_backends(void)
     _product_id.set(_backends[0]->product_id());
 }
 
-void
-AP_InertialSensor::init_accel()
-{
-    _init_accel();
-
-    // save calibration
-    _save_parameters();
-
-    check_3D_calibration();
-}
-
 #if !defined( __AVR_ATmega1280__ )
 // calibrate_accel - perform accelerometer calibration including providing user
 // instructions and feedback Gauss-Newton accel calibration routines borrowed
@@ -631,117 +620,6 @@ bool AP_InertialSensor::get_accel_health_all(void) const
     return (get_accel_count() > 0);
 }
 
-void
-AP_InertialSensor::_init_accel()
-{
-    uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
-    uint8_t flashcount = 0;
-    Vector3f prev[INS_MAX_INSTANCES];
-    Vector3f accel_offset[INS_MAX_INSTANCES];
-    float total_change[INS_MAX_INSTANCES];
-    float max_offset[INS_MAX_INSTANCES];
-
-    memset(max_offset, 0, sizeof(max_offset));
-    memset(total_change, 0, sizeof(total_change));
-
-    // exit immediately if calibration is already in progress
-    if (_calibrating) {
-        return;
-    }
-
-    // record we are calibrating
-    _calibrating = true;
-
-    // flash leds to tell user to keep the IMU still
-    AP_Notify::flags.initialising = true;
-
-    // cold start
-    hal.scheduler->delay(100);
-
-    hal.console->print_P(PSTR("Init Accel"));
-
-    // clear accelerometer offsets and scaling
-    for (uint8_t k=0; k<num_accels; k++) {
-        _accel_offset[k] = Vector3f(0,0,0);
-        _accel_scale[k] = Vector3f(1,1,1);
-
-        // initialise accel offsets to a large value the first time
-        // this will force us to calibrate accels at least twice
-        accel_offset[k] = Vector3f(500, 500, 500);
-    }
-
-    // loop until we calculate acceptable offsets
-    while (true) {
-        // get latest accelerometer values
-        update();
-
-        for (uint8_t k=0; k<num_accels; k++) {
-            // store old offsets
-            prev[k] = accel_offset[k];
-
-            // get new offsets
-            accel_offset[k] = get_accel(k);
-        }
-
-        // We take some readings...
-        for(int8_t i = 0; i < 50; i++) {
-
-            hal.scheduler->delay(20);
-            update();
-
-            // low pass filter the offsets
-            for (uint8_t k=0; k<num_accels; k++) {
-                accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
-            }
-
-            // display some output to the user
-            if(flashcount >= 10) {
-                hal.console->print_P(PSTR("*"));
-                flashcount = 0;
-            }
-            flashcount++;
-        }
-
-        for (uint8_t k=0; k<num_accels; k++) {
-            // null gravity from the Z accel
-            accel_offset[k].z += GRAVITY_MSS;
-
-            total_change[k] = 
-                fabsf(prev[k].x - accel_offset[k].x) + 
-                fabsf(prev[k].y - accel_offset[k].y) + 
-                fabsf(prev[k].z - accel_offset[k].z);
-            max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
-            max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
-        }
-
-        uint8_t num_converged = 0;
-        for (uint8_t k=0; k<num_accels; k++) {
-            if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE && 
-                max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
-                num_converged++;
-            }
-        }
-
-        if (num_converged == num_accels) break;
-
-        hal.scheduler->delay(500);
-    }
-
-    // set the global accel offsets
-    for (uint8_t k=0; k<num_accels; k++) {
-        _accel_offset[k] = accel_offset[k];
-    }
-
-    // record calibration complete
-    _calibrating = false;
-
-    // stop flashing the leds
-    AP_Notify::flags.initialising = false;
-
-    hal.console->print_P(PSTR(" "));
-
-}
-
 void
 AP_InertialSensor::_init_gyro()
 {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 033efc17ac..d1f3a49ba0 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -70,14 +70,6 @@ public:
     void init( Start_style style,
                Sample_rate sample_rate);
 
-    /// Perform cold startup initialisation for just the accelerometers.
-    ///
-    /// @note This should not be called unless ::init has previously
-    ///       been called, as ::init may perform other work.
-    ///
-    void init_accel();
-
-
     /// Register a new gyro/accel driver, allocating an instance
     /// number
     uint8_t register_gyro(void);
@@ -218,8 +210,7 @@ private:
     void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
     void _detect_backends(void);
 
-    // accel and gyro initialisation
-    void _init_accel();
+    // gyro initialisation
     void _init_gyro();
 
 #if !defined( __AVR_ATmega1280__ )
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
index 2c97524bd8..5253ec436b 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
+++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
@@ -97,11 +97,6 @@ void loop(void)
             display_offsets_and_scaling();
         }
 
-        if( user_input == 'l' || user_input == 'L' ) {
-            run_level();
-            display_offsets_and_scaling();
-        }
-
         if( user_input == 't' || user_input == 'T' ) {
             run_test();
         }
@@ -153,31 +148,6 @@ void display_offsets_and_scaling()
                     gyro_offsets.z);
 }
 
-void run_level()
-{
-    // clear off any input in the buffer
-    while( hal.console->available() ) {
-        hal.console->read();
-    }
-
-    // display message to user
-    hal.console->print("Place APM on a level surface and press any key..\n");
-
-    // wait for user input
-    while( !hal.console->available() ) {
-        hal.scheduler->delay(20);
-    }
-    while( hal.console->available() ) {
-        hal.console->read();
-    }
-
-    // run accel level
-    ins.init_accel();
-
-    // display results
-    display_offsets_and_scaling();
-}
-
 void run_test()
 {
     Vector3f accel;