From e2b1cb7e8d0c2152fbd2af220d3cb646d597b706 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 7 Nov 2012 15:20:22 +0900 Subject: [PATCH] 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 | 3 +- ArduCopter/setup.pde | 2 +- ArduPlane/Parameters.h | 3 +- ArduPlane/setup.pde | 2 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 126 ++++++++++-------- .../AP_InertialSensor/AP_InertialSensor.h | 20 ++- .../AP_InertialSensor_MPU6000.cpp | 2 +- .../AP_InertialSensor_MPU6000.h | 4 +- .../AP_InertialSensor_Oilpan.cpp | 2 +- .../AP_InertialSensor_Oilpan.h | 4 +- .../AP_InertialSensor_Stub.cpp | 2 +- .../AP_InertialSensor_Stub.h | 5 +- 12 files changed, 96 insertions(+), 79 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8a5494f90b..3c1ae38321 100644 --- a/ArduCopter/Parameters.h +++ b/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, diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index aad2284f9c..9066032185 100644 --- a/ArduCopter/setup.pde +++ b/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); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 643dcaa25f..a145c2ded7 100644 --- a/ArduPlane/Parameters.h +++ b/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 // diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index e2b3d0f613..0c7a60a23b 100644 --- a/ArduPlane/setup.pde +++ b/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); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index cda72d4bbe..0d962bff82 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -4,11 +4,7 @@ #include "AP_InertialSensor.h" #include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include -#endif +#include #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("ACCOFF", 2, AP_InertialSensor, _accel_offset, 0), - AP_GROUPINFO("GYROFF", 3, AP_InertialSensor, _gyro_offset, 0), + AP_GROUPINFO("ACCSCAL", 1, AP_InertialSensor, _accel_scale, 0), + AP_GROUPINFO("ACCOFFS", 2, AP_InertialSensor, _accel_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) { - load_parameters(); - } else { - - // do cold-start calibration for both accel and gyro + if (WARM_START != style) { + // do cold-start calibration for gyro only _init_gyro(delay_cb, flash_leds_cb); - - // save calibration - save_parameters(); } } // save parameters to eeprom -void AP_InertialSensor::load_parameters() -{ - _product_id.load(); - _accel_scale.load(); - _accel_offset.load(); -} - -// save parameters to eeprom -void AP_InertialSensor::save_parameters() +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")); - break; - default: - // should never happen + msg = PSTR("on it's back"); 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 - return true; + // sanity check scale + 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]; } -} \ No newline at end of file +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 8f8b839464..6113bffdc5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 94ac27318e..a71580b3dd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 60b4e0b749..dd616c506e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/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); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 36b60a04ec..9c1da869f5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/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); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index 483c918259..5ed61762df 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp index 181aff36bb..42d5372b40 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp +++ b/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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h index b2187c23ab..284a3629c3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h +++ b/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__