From 50ae5b2519443209bff453c5f71499e754cfad0a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 1 Sep 2014 15:28:07 +0900
Subject: [PATCH] InertialSensor: reorder .cpp file to match .h

No functional changes
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 440 +++++++++---------
 1 file changed, 220 insertions(+), 220 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index d1bbac8333..3cb39d5b35 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -131,126 +131,6 @@ AP_InertialSensor::init( Start_style style,
     }
 }
 
-// save parameters to eeprom
-void AP_InertialSensor::_save_parameters()
-{
-    _product_id.save();
-    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
-        _accel_scale[i].save();
-        _accel_offset[i].save();
-        _gyro_offset[i].save();
-    }
-}
-
-void
-AP_InertialSensor::init_gyro()
-{
-    _init_gyro();
-
-    // save calibration
-    _save_parameters();
-}
-
-void
-AP_InertialSensor::_init_gyro()
-{
-    uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
-    Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
-    float best_diff[INS_MAX_INSTANCES];
-    bool converged[INS_MAX_INSTANCES];
-
-    // cold start
-    hal.console->print_P(PSTR("Init Gyro"));
-
-    // flash leds to tell user to keep the IMU still
-    AP_Notify::flags.initialising = true;
-
-    // remove existing gyro offsets
-    for (uint8_t k=0; k<num_gyros; k++) {
-        _gyro_offset[k] = Vector3f(0,0,0);
-        best_diff[k] = 0;
-        last_average[k].zero();
-        converged[k] = false;
-    }
-
-    for(int8_t c = 0; c < 5; c++) {
-        hal.scheduler->delay(5);
-        update();
-    }
-
-    // the strategy is to average 50 points over 0.5 seconds, then do it
-    // again and see if the 2nd average is within a small margin of
-    // the first
-
-    uint8_t num_converged = 0;
-
-    // we try to get a good calibration estimate for up to 10 seconds
-    // if the gyros are stable, we should get it in 1 second
-    for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) {
-        Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
-        float diff_norm[INS_MAX_INSTANCES];
-        uint8_t i;
-
-        memset(diff_norm, 0, sizeof(diff_norm));
-
-        hal.console->print_P(PSTR("*"));
-
-        for (uint8_t k=0; k<num_gyros; k++) {
-            gyro_sum[k].zero();
-        }
-        for (i=0; i<50; i++) {
-            update();
-            for (uint8_t k=0; k<num_gyros; k++) {
-                gyro_sum[k] += get_gyro(k);
-            }
-            hal.scheduler->delay(5);
-        }
-        for (uint8_t k=0; k<num_gyros; k++) {
-            gyro_avg[k] = gyro_sum[k] / i;
-            gyro_diff[k] = last_average[k] - gyro_avg[k];
-            diff_norm[k] = gyro_diff[k].length();
-        }
-
-        for (uint8_t k=0; k<num_gyros; k++) {
-            if (converged[k]) continue;
-            if (j == 0) {
-                best_diff[k] = diff_norm[k];
-                best_avg[k] = gyro_avg[k];
-            } else if (gyro_diff[k].length() < ToRad(0.1f)) {
-                // we want the average to be within 0.1 bit, which is 0.04 degrees/s
-                last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
-                _gyro_offset[k] = last_average[k];            
-                converged[k] = true;
-                num_converged++;
-            } else if (diff_norm[k] < best_diff[k]) {
-                best_diff[k] = diff_norm[k];
-                best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
-            }
-            last_average[k] = gyro_avg[k];
-        }
-    }
-
-    // stop flashing leds
-    AP_Notify::flags.initialising = false;
-
-    if (num_converged == num_gyros) {
-        // all OK
-        return;
-    }
-
-    // we've kept the user waiting long enough - use the best pair we
-    // found so far
-    hal.console->println();
-    for (uint8_t k=0; k<num_gyros; k++) {
-        if (!converged[k]) {
-            hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"), 
-                                  (unsigned)k, ToDeg(best_diff[k]));
-            _gyro_offset[k] = best_avg[k];
-        }
-    }
-}
-
-
 void
 AP_InertialSensor::init_accel()
 {
@@ -260,106 +140,6 @@ AP_InertialSensor::init_accel()
     _save_parameters();
 }
 
-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));
-
-    // cold start
-    hal.scheduler->delay(100);
-
-    hal.console->print_P(PSTR("Init Accel"));
-
-    // flash leds to tell user to keep the IMU still
-    AP_Notify::flags.initialising = true;
-
-    // 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];
-    }
-
-    // stop flashing the leds
-    AP_Notify::flags.initialising = false;
-
-    hal.console->print_P(PSTR(" "));
-
-}
-
 #if !defined( __AVR_ATmega1280__ )
 // calibrate_accel - perform accelerometer calibration including providing user
 // instructions and feedback Gauss-Newton accel calibration routines borrowed
@@ -489,6 +269,7 @@ failed:
     }
     return false;
 }
+#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
@@ -504,6 +285,215 @@ bool AP_InertialSensor::calibrated()
     return true;
 }
 
+void
+AP_InertialSensor::init_gyro()
+{
+    _init_gyro();
+
+    // save calibration
+    _save_parameters();
+}
+
+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));
+
+    // cold start
+    hal.scheduler->delay(100);
+
+    hal.console->print_P(PSTR("Init Accel"));
+
+    // flash leds to tell user to keep the IMU still
+    AP_Notify::flags.initialising = true;
+
+    // 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];
+    }
+
+    // stop flashing the leds
+    AP_Notify::flags.initialising = false;
+
+    hal.console->print_P(PSTR(" "));
+
+}
+
+void
+AP_InertialSensor::_init_gyro()
+{
+    uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
+    Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
+    float best_diff[INS_MAX_INSTANCES];
+    bool converged[INS_MAX_INSTANCES];
+
+    // cold start
+    hal.console->print_P(PSTR("Init Gyro"));
+
+    // flash leds to tell user to keep the IMU still
+    AP_Notify::flags.initialising = true;
+
+    // remove existing gyro offsets
+    for (uint8_t k=0; k<num_gyros; k++) {
+        _gyro_offset[k] = Vector3f(0,0,0);
+        best_diff[k] = 0;
+        last_average[k].zero();
+        converged[k] = false;
+    }
+
+    for(int8_t c = 0; c < 5; c++) {
+        hal.scheduler->delay(5);
+        update();
+    }
+
+    // the strategy is to average 50 points over 0.5 seconds, then do it
+    // again and see if the 2nd average is within a small margin of
+    // the first
+
+    uint8_t num_converged = 0;
+
+    // we try to get a good calibration estimate for up to 10 seconds
+    // if the gyros are stable, we should get it in 1 second
+    for (int16_t j = 0; j <= 20 && num_converged < num_gyros; j++) {
+        Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
+        float diff_norm[INS_MAX_INSTANCES];
+        uint8_t i;
+
+        memset(diff_norm, 0, sizeof(diff_norm));
+
+        hal.console->print_P(PSTR("*"));
+
+        for (uint8_t k=0; k<num_gyros; k++) {
+            gyro_sum[k].zero();
+        }
+        for (i=0; i<50; i++) {
+            update();
+            for (uint8_t k=0; k<num_gyros; k++) {
+                gyro_sum[k] += get_gyro(k);
+            }
+            hal.scheduler->delay(5);
+        }
+        for (uint8_t k=0; k<num_gyros; k++) {
+            gyro_avg[k] = gyro_sum[k] / i;
+            gyro_diff[k] = last_average[k] - gyro_avg[k];
+            diff_norm[k] = gyro_diff[k].length();
+        }
+
+        for (uint8_t k=0; k<num_gyros; k++) {
+            if (converged[k]) continue;
+            if (j == 0) {
+                best_diff[k] = diff_norm[k];
+                best_avg[k] = gyro_avg[k];
+            } else if (gyro_diff[k].length() < ToRad(0.1f)) {
+                // we want the average to be within 0.1 bit, which is 0.04 degrees/s
+                last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
+                _gyro_offset[k] = last_average[k];
+                converged[k] = true;
+                num_converged++;
+            } else if (diff_norm[k] < best_diff[k]) {
+                best_diff[k] = diff_norm[k];
+                best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
+            }
+            last_average[k] = gyro_avg[k];
+        }
+    }
+
+    // stop flashing leds
+    AP_Notify::flags.initialising = false;
+
+    if (num_converged == num_gyros) {
+        // all OK
+        return;
+    }
+
+    // we've kept the user waiting long enough - use the best pair we
+    // found so far
+    hal.console->println();
+    for (uint8_t k=0; k<num_gyros; k++) {
+        if (!converged[k]) {
+            hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
+                                  (unsigned)k, ToDeg(best_diff[k]));
+            _gyro_offset[k] = best_avg[k];
+        }
+    }
+}
+
+#if !defined( __AVR_ATmega1280__ )
 // _calibrate_model - perform low level accel calibration
 // accel_sample are accelerometer samples collected in 6 different positions
 // accel_offsets are output from the calibration routine
@@ -679,3 +669,13 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll,
 
 #endif // __AVR_ATmega1280__
 
+// save parameters to eeprom
+void AP_InertialSensor::_save_parameters()
+{
+    _product_id.save();
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        _accel_scale[i].save();
+        _accel_offset[i].save();
+        _gyro_offset[i].save();
+    }
+}