AP_InertialSensor: fixed watchdog on AHRS trim gyro wait

This commit is contained in:
Andrew Tridgell 2019-08-18 07:05:12 +10:00
parent 914e6c5497
commit 992016069a

View File

@ -1127,8 +1127,6 @@ AP_InertialSensor::_init_gyro()
// cold start // cold start
hal.console->printf("Init Gyro"); hal.console->printf("Init Gyro");
EXPECT_DELAY_MS(60000);
/* /*
we do the gyro calibration with no board rotation. This avoids we do the gyro calibration with no board rotation. This avoids
having to rotate readings during the calibration having to rotate readings during the calibration
@ -1164,6 +1162,8 @@ AP_InertialSensor::_init_gyro()
float diff_norm[INS_MAX_INSTANCES]; float diff_norm[INS_MAX_INSTANCES];
uint8_t i; uint8_t i;
EXPECT_DELAY_MS(1000);
memset(diff_norm, 0, sizeof(diff_norm)); memset(diff_norm, 0, sizeof(diff_norm));
hal.console->printf("*"); hal.console->printf("*");