ArduCopter: clear ahrs roll and pitch trims after an accel level command

This commit is contained in:
rmackay9 2012-12-12 17:17:09 +09:00 committed by Andrew Tridgell
parent 7ea34d4fb7
commit 0ab82f8c4e
2 changed files with 2 additions and 0 deletions

View File

@ -1082,6 +1082,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.param2 == 1 ||
packet.param3 == 1) {
ins.init_accel(flash_leds);
ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim
}
if (packet.param4 == 1) {
trim_radio();

View File

@ -251,6 +251,7 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
ins_sample_rate,
flash_leds);
ins.init_accel(flash_leds);
ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim
report_ins();
return(0);
}