Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: removed 1D accel calibration 

it is finally time to move on from this. We want to push people
towards better calibration and removing the 1D accel cal is the first
step
This commit is contained in:
Andrew Tridgell 2015-03-11 10:16:04 +11:00
parent a975520033
commit 786172aa4e
3 changed files with 1 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -388,17 +388,6 @@ AP_InertialSensor::_detect_backends(void)
_product_id.set(_backends[0]->product_id());
}
void
AP_InertialSensor::init_accel()
{
_init_accel();
// save calibration
_save_parameters();
check_3D_calibration();
}
#if !defined( __AVR_ATmega1280__ )
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
@ -631,117 +620,6 @@ bool AP_InertialSensor::get_accel_health_all(void) const
return (get_accel_count() > 0);
}
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));
// exit immediately if calibration is already in progress
if (_calibrating) {
return;
}
// record we are calibrating
_calibrating = true;
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// cold start
hal.scheduler->delay(100);
hal.console->print_P(PSTR("Init Accel"));
// 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];
}
// record calibration complete
_calibrating = false;
// stop flashing the leds
AP_Notify::flags.initialising = false;
hal.console->print_P(PSTR(" "));
}
void
AP_InertialSensor::_init_gyro()
{

11
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -70,14 +70,6 @@ public:
void init( Start_style style,
Sample_rate sample_rate);
/// Perform cold startup initialisation for just the accelerometers.
///
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work.
///
void init_accel();
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro(void);
@ -218,8 +210,7 @@ private:
void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &));
void _detect_backends(void);
// accel and gyro initialisation
void _init_accel();
// gyro initialisation
void _init_gyro();
#if !defined( __AVR_ATmega1280__ )

30
libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde
View File

@ -97,11 +97,6 @@ void loop(void)
display_offsets_and_scaling();
}
if( user_input == 'l' || user_input == 'L' ) {
run_level();
display_offsets_and_scaling();
}
if( user_input == 't' || user_input == 'T' ) {
run_test();
}
@ -153,31 +148,6 @@ void display_offsets_and_scaling()
gyro_offsets.z);
}
void run_level()
{
// clear off any input in the buffer
while( hal.console->available() ) {
hal.console->read();
}
// display message to user
hal.console->print("Place APM on a level surface and press any key..\n");
// wait for user input
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
while( hal.console->available() ) {
hal.console->read();
}
// run accel level
ins.init_accel();
// display results
display_offsets_and_scaling();
}
void run_test()
{
Vector3f accel;