diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 7f884ada3d..1a6780eccf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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; kprint_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