AP_InertialSensor: changes after review with Tridge.

sanity checking added to accelerometer calibration routine.
user feedback is sent using gcs_send_text_fmt instead of Serial.printf.
moved ins parameters to new eeprom number to avoid conflicts with older parameters.
other small changes including renaming of functions and parameters.

ArduCopter/Parameters.h

@@ -52,7 +52,8 @@ public:
         //
         k_param_format_version = 0,
         k_param_software_type,
-        k_param_ins,
+        k_param_ins_old,                        // *** Deprecated, remove with next eeprom number change
+        k_param_ins,                            // libraries/AP_InertialSensor variables
 
         // simulation
         k_param_sitl = 10,

ArduCopter/setup.pde

@@ -258,7 +258,7 @@ static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
     ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
-    ins.calibrate_accel(delay, flash_leds);
+    ins.calibrate_accel(delay, flash_leds, gcs_send_text_fmt);
     report_ins();
     return(0);
 }

ArduPlane/Parameters.h

@@ -68,7 +68,7 @@ public:
         k_param_reset_switch_chan,
         k_param_manual_level,
         k_param_land_pitch_cd,
-        k_param_ins,
+        k_param_ins_old,            // *** Deprecated, remove with next eeprom number change
         k_param_stick_mixing,
         k_param_reset_mission_chan,
         k_param_land_flare_alt,
@@ -77,6 +77,7 @@ public:
         k_param_rudder_steer,
         k_param_throttle_nudge,
         k_param_alt_offset,
+        k_param_ins,                // libraries/AP_InertialSensor variables
 
         // 110: Telemetry control
         //

ArduPlane/setup.pde

@@ -303,7 +303,7 @@ static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
     ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
-    ins.calibrate_accel(delay, flash_leds);
+    ins.calibrate_accel(delay, flash_leds, gcs_send_text_fmt);
     report_ins();
     return(0);
 }

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -4,11 +4,7 @@
 #include "AP_InertialSensor.h"
 
 #include <SPI.h>
-#if defined(ARDUINO) && ARDUINO >= 100
+#include <AP_Common.h>
- #include "Arduino.h"
-#else
- #include <wiring.h>
-#endif
 
 #define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0)
 
@@ -17,9 +13,9 @@
 // Class level parameters
 const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     AP_GROUPINFO("PRODUCT_ID",  0, AP_InertialSensor, _product_id,   0),
-    AP_GROUPINFO("ACCSCA",      1, AP_InertialSensor, _accel_scale,  0),
+    AP_GROUPINFO("ACCSCAL",     1, AP_InertialSensor, _accel_scale,  0),
-    AP_GROUPINFO("ACCOFF",      2, AP_InertialSensor, _accel_offset, 0),
+    AP_GROUPINFO("ACCOFFS",     2, AP_InertialSensor, _accel_offset, 0),
-    AP_GROUPINFO("GYROFF",      3, AP_InertialSensor, _gyro_offset,  0),
+    AP_GROUPINFO("GYROFFS",     3, AP_InertialSensor, _gyro_offset,  0),
     AP_GROUPEND
 };
 
@@ -33,7 +29,7 @@ AP_InertialSensor::init( Start_style style,
                          void (*flash_leds_cb)(bool on),
                          AP_PeriodicProcess *  scheduler )
 {
-    _product_id = _init(scheduler);
+    _product_id = _init_sensor(scheduler);
 
     // check scaling
     Vector3f accel_scale = _accel_scale.get();
@@ -42,33 +38,19 @@ AP_InertialSensor::init( Start_style style,
         _accel_scale.set(accel_scale);
     }
 
-    // if we are warm-starting, load the calibration data from EEPROM and go
+    if (WARM_START != style) {
-    if (WARM_START == style) {
+        // do cold-start calibration for gyro only
-        load_parameters();
-    } else {
-
-        // do cold-start calibration for both accel and gyro
         _init_gyro(delay_cb, flash_leds_cb);
-
-        // save calibration
-        save_parameters();
     }
 }
 
 // save parameters to eeprom
-void AP_InertialSensor::load_parameters()
+void AP_InertialSensor::_save_parameters()
-{
-    _product_id.load();
-    _accel_scale.load();
-    _accel_offset.load();
-}
-
-// save parameters to eeprom
-void AP_InertialSensor::save_parameters()
 {
     _product_id.save();
     _accel_scale.save();
     _accel_offset.save();
+    _gyro_offset.save();
 }
 
 void
@@ -77,7 +59,7 @@ AP_InertialSensor::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_led
     _init_gyro(delay_cb, flash_leds_cb);
 
     // save calibration
-    save_parameters();
+    _save_parameters();
 }
 
 void
@@ -170,13 +152,13 @@ AP_InertialSensor::init_accel(void (*delay_cb)(unsigned long t), void (*flash_le
     _init_accel(delay_cb, flash_leds_cb);
 
     // save calibration
-    save_parameters();
+    _save_parameters();
 }
 
 void
 AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
 {
-    int16_t flashcount = 0;
+    int8_t flashcount = 0;
     Vector3f ins_accel;
     Vector3f prev;
     Vector3f accel_offset;
@@ -184,7 +166,7 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
     float max_offset;
 
     // cold start
-    delay_cb(500);
+    delay_cb(100);
 
     Serial.printf_P(PSTR("Init Accel"));
 
@@ -209,7 +191,7 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
         accel_offset = ins_accel;
 
         // We take some readings...
-        for(int16_t i = 0; i < 50; i++) {
+        for(int8_t i = 0; i < 50; i++) {
 
             delay_cb(20);
             update();
@@ -250,49 +232,56 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
 }
 
 // perform accelerometer calibration including providing user instructions and feedback
-void AP_InertialSensor::calibrate_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on))
+void AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
 {
     Vector3f samples[6];
     Vector3f new_offsets;
     Vector3f new_scaling;
+    Vector3f orig_offset;
+    Vector3f orig_scale;
+
+    // backup original offsets and scaling
+    orig_offset = _accel_offset.get();
+    orig_scale = _accel_scale.get();
 
     // clear accelerometer offsets and scaling
     _accel_offset = Vector3f(0,0,0);
     _accel_scale = Vector3f(1,1,1);
 
     // capture data from 6 positions
-    for(int16_t i=0; i<6; i++ ) {
+    for (int8_t i=0; i<6; i++) {
+        const prog_char_t *msg;
 
         // display message to user
-        Serial.print_P(PSTR("Place APM "));
+        switch ( i ) {
-        switch( i ) {
             case 0:
-                Serial.print_P(PSTR("level"));
+                msg = PSTR("level");
                 break;
             case 1:
-                Serial.print_P(PSTR("on it's left side"));
+                msg = PSTR("on it's left side");
                 break;
             case 2:
-                Serial.print_P(PSTR("on it's right side"));
+                msg = PSTR("on it's right side");
                 break;
             case 3:
-                Serial.print_P(PSTR("nose down"));
+                msg = PSTR("nose down");
                 break;
             case 4:
-                Serial.print_P(PSTR("nose up"));
+                msg = PSTR("nose up");
                 break;
             case 5:
-                Serial.print_P(PSTR("on it's back"));
+                msg = PSTR("on it's back");
-                break;
-            default:
-                // should never happen
                 break;
         }
-        Serial.print_P(PSTR(" and press any key.\n"));
+        if (send_msg == NULL) {
+            Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
+        }else{
+            send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
+        }
 
         // wait for user input
         while( !Serial.available() ) {
-            delay(20);
+            delay_cb(20);
         }
         // clear unput buffer
         while( Serial.available() ) {
@@ -304,7 +293,7 @@ void AP_InertialSensor::calibrate_accel(void (*callback)(unsigned long t), void
 
         // wait until we have 32 samples
         while( num_samples_available() < 32 * SAMPLE_UNIT ) {
-            delay(1);
+            delay_cb(1);
         }
 
         // read samples from ins
@@ -312,20 +301,33 @@ void AP_InertialSensor::calibrate_accel(void (*callback)(unsigned long t), void
 
         // capture sample
         samples[i] = get_accel();
-
-        // display sample
-        Serial.printf_P(PSTR("Acc X:%4.2f\tY:%4.2f\tZ:%4.2f\n"),samples[i].x,samples[i].y,samples[i].z);
     }
 
     // run the calibration routine
     if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
+
+        if (send_msg == NULL) {
+            Serial.printf_P(PSTR("Calibration successful\n"));
+        }else{
+            send_msg(PSTR("Calibration successful\n"));
+        }
+
+        // set and save calibration
         _accel_offset.set(new_offsets);
         _accel_scale.set(new_scaling);
-        // save calibration
+        _save_parameters();
-        save_parameters();
     }else{
-        Serial.printf_P(PSTR("Accel calibration failed"));
+        if (send_msg == NULL) {
+            Serial.printf_P(PSTR("Calibration failed\n"));
+        }else{
+            send_msg(PSTR("Calibration failed\n"));
+        }
+
+        // restore original scaling and offsets
+        _accel_offset.set(orig_offset);
+        _accel_scale.set(orig_scale);
     }
+    delay_cb(100);
 }
 
 // _calibrate_model - perform low level accel calibration
@@ -344,6 +346,7 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& ac
     float delta[6];
     float ds[6];
     float JS[6][6];
+    bool success = true;
 
     // reset
     beta[0] = beta[1] = beta[2] = 0;
@@ -384,8 +387,17 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& ac
     accel_offsets.y = beta[1] * accel_scale.y;
     accel_offsets.z = beta[2] * accel_scale.z;
 
-    // always return success
+    // sanity check scale
-    return true;
+    if( accel_scale.is_nan() || fabs(accel_scale.x-1.0) > 0.1 || fabs(accel_scale.y-1.0) > 1.1 || fabs(accel_scale.z-1.0) > 1.1 ) {
+        success = false;
+    }
+    // sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G)
+    if( accel_offsets.is_nan() || fabs(accel_offsets.x) > 2.0 || fabs(accel_offsets.y) > 2.0 || fabs(accel_offsets.z) > 5.0 ) {
+        success = false;
+    }
+
+    // return success or failure
+    return success;
 }
 
 void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3])
@@ -460,4 +472,4 @@ void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float
     for( i=0; i<6; i++ ) {
         delta[i] = dS[i];
     }
-}
+}

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -47,10 +47,12 @@ public:
     /// @note This should not be called unless ::init has previously
     ///       been called, as ::init may perform other work.
     ///
-    virtual void        init_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
+    virtual void        init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on));
 
     // perform accelerometer calibration including providing user instructions and feedback
-    virtual void        calibrate_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
+    virtual void        calibrate_accel(void (*delay_cb)(unsigned long t),
+                                        void (*flash_leds_cb)(bool on) = NULL,
+                                        void (*send_msg)(const prog_char_t *, ...) = NULL);
 
     /// Perform cold-start initialisation for just the gyros.
     ///
@@ -82,10 +84,6 @@ public:
     // get accel scale
     Vector3f            get_accel_scale() { return _accel_scale; }
 
-    // sensor specific init to be overwritten by descendant classes
-    // To-Do: should be combined with init above?
-    virtual uint16_t        _init( AP_PeriodicProcess * scheduler ) = 0;
-
     /* Update the sensor data, so that getters are nonblocking.
      * Returns a bool of whether data was updated or not.
      */
@@ -133,6 +131,9 @@ public:
 
 protected:
 
+    // sensor specific init to be overwritten by descendant classes
+    virtual uint16_t        _init_sensor( AP_PeriodicProcess * scheduler ) = 0;
+
     // no-save implementations of accel and gyro initialisation routines
     virtual void            _init_accel(void    (*delay_cb)(unsigned long t),
                                         void    (*flash_leds_cb)(bool on) = NULL);
@@ -140,16 +141,13 @@ protected:
                                        void (*flash_leds_cb)(bool on) = NULL);
 
     // _calibrate_accel - perform low level accel calibration
-    virtual bool            _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale );
+    virtual bool            _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale);
     virtual void            _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
     virtual void            _calibrate_reset_matrices(float dS[6], float JS[6][6]);
     virtual void            _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
 
-    // load parameters from eeprom
-    void                    load_parameters();
-
     // save parameters to eeprom
-    void                    save_parameters();
+    void                    _save_parameters();
 
     // Most recent accelerometer reading obtained by ::update
     Vector3f                _accel;

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -210,7 +210,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
     _dmp_initialised = false;
 }
 
-uint16_t AP_InertialSensor_MPU6000::_init( AP_PeriodicProcess * scheduler )
+uint16_t AP_InertialSensor_MPU6000::_init_sensor( AP_PeriodicProcess * scheduler )
 {
     if (_initialised) return _mpu6000_product_id;
     _initialised = true;

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -22,7 +22,6 @@ public:
 
     AP_InertialSensor_MPU6000();
 
-    uint16_t            _init( AP_PeriodicProcess * scheduler );
     static void         dmp_init(); // Initialise MPU6000's DMP
     static void         dmp_reset();    // Reset the DMP (required for changes in gains or offsets to take effect)
 
@@ -52,6 +51,9 @@ public:
     // get_delta_time returns the time period in seconds overwhich the sensor data was collected
     uint32_t            get_delta_time_micros();
 
+protected:
+    uint16_t                    _init_sensor( AP_PeriodicProcess * scheduler );
+
 private:
 
     static void                 read(uint32_t);

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -41,7 +41,7 @@ AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
 {
 }
 
-uint16_t AP_InertialSensor_Oilpan::_init( AP_PeriodicProcess * scheduler)
+uint16_t AP_InertialSensor_Oilpan::_init_sensor( AP_PeriodicProcess * scheduler)
 {
     _adc->Init(scheduler);
 

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -17,7 +17,6 @@ public:
     AP_InertialSensor_Oilpan( AP_ADC * adc );
 
     /* Concrete implementation of AP_InertialSensor functions: */
-    uint16_t        _init(AP_PeriodicProcess * scheduler);
     bool            update();
     bool            new_data_available();
     float           gx();
@@ -34,6 +33,9 @@ public:
     // get number of samples read from the sensors
     uint16_t        num_samples_available();
 
+protected:
+    uint16_t        _init_sensor(AP_PeriodicProcess * scheduler);
+
 private:
 
     AP_ADC *                    _adc;

libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp

@@ -2,7 +2,7 @@
 
 #include "AP_InertialSensor_Stub.h"
 
-uint16_t AP_InertialSensor_Stub::_init( AP_PeriodicProcess * scheduler ) {
+uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler ) {
     return AP_PRODUCT_ID_NONE;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Stub.h

@@ -16,8 +16,6 @@ public:
     AP_InertialSensor_Stub() {
     }
 
-    uint16_t        _init( AP_PeriodicProcess * scheduler );
-
     /* Concrete implementation of AP_InertialSensor functions: */
     bool            update();
     bool            new_data_available();
@@ -32,6 +30,9 @@ public:
     uint32_t        get_last_sample_time_micros();
     float           get_gyro_drift_rate();
     uint16_t        num_samples_available();
+
+protected:
+    uint16_t        _init_sensor( AP_PeriodicProcess * scheduler );
 };
 
 #endif // __AP_INERTIAL_SENSOR_STUB_H__