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

INS: protect against two calibrations running at the same time

This commit is contained in:
Randy Mackay 2015-03-09 15:37:11 +09:00 committed by Andrew Tridgell
parent 72d1113501
commit 5f26a36060
1 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -610,10 +610,10 @@ AP_InertialSensor::_init_accel()
memset(max_offset, 0, sizeof(max_offset));
memset(total_change, 0, sizeof(total_change));
// cold start
hal.scheduler->delay(100);
hal.console->print_P(PSTR("Init Accel"));
// exit immediately if calibration is already in progress
if (_calibrating) {
return;
}
// record we are calibrating
_calibrating = true;
@ -621,6 +621,11 @@ AP_InertialSensor::_init_accel()
// 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);
@ -711,8 +716,10 @@ AP_InertialSensor::_init_gyro()
float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
// cold start
hal.console->print_P(PSTR("Init Gyro"));
// exit immediately if calibration is already in progress
if (_calibrating) {
return;
}
// record we are calibrating
_calibrating = true;
@ -720,6 +727,9 @@ AP_InertialSensor::_init_gyro()
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// cold start
hal.console->print_P(PSTR("Init Gyro"));
// remove existing gyro offsets
for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k] = Vector3f(0,0,0);