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 <SPI.h>
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include <wiring.h>
-#endif
+#include <AP_Common.h>
#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__