Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: changes after review with Tridge.

Browse Source 
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.
This commit is contained in:
rmackay9 2012-11-07 15:20:22 +09:00
parent 0868917ff4
commit e2b1cb7e8d
12 changed files with 96 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Parameters.h
View File

@ -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,

2
ArduCopter/setup.pde
View File

@ -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);
}

3
ArduPlane/Parameters.h
View File

@ -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
//

2
ArduPlane/setup.pde
View File

@ -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);
}

126
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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];
}
}
}

20
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -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;

2
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -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;

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -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);

2
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -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);

4
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -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;

2
libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
View File

@ -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;
}

5
libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
View File

@ -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__