mirror of https://github.com/ArduPilot/ardupilot
DCM: reset the DCM matrix after a ground start
This commit is contained in:
parent
621f21e4f5
commit
e8617fc929
|
@ -458,6 +458,7 @@ static void startup_IMU_ground(void)
|
|||
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
|
||||
imu.init_accel(mavlink_delay, flash_leds);
|
||||
dcm.set_centripetal(1);
|
||||
dcm.matrix_reset();
|
||||
|
||||
// read Baro pressure at ground
|
||||
//-----------------------------
|
||||
|
|
|
@ -502,6 +502,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
//Serial.printf_P(PSTR("Calibrating."));
|
||||
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
|
||||
dcm.matrix_reset();
|
||||
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
@ -563,6 +564,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
// we need the DCM initialised for this test
|
||||
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
|
||||
dcm.matrix_reset();
|
||||
|
||||
int counter = 0;
|
||||
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
|
||||
|
|
Loading…
Reference in New Issue