From 5ba171af9aa88fd6d853a4792419579074bc0d69 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 May 2013 13:38:28 +1000 Subject: [PATCH] Plane: fixed mavlink levelling this was broken by a recent commit (after 2.72 was released) --- ArduPlane/GCS_Mavlink.pde | 2 +- ArduPlane/setup.pde | 2 +- ArduPlane/system.pde | 8 ++++++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 5047fa974a..7968b10515 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1136,7 +1136,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_PREFLIGHT_CALIBRATION: if (packet.param1 == 1 || packet.param2 == 1) { - startup_INS_ground(); + startup_INS_ground(true); } else if (packet.param3 == 1) { init_barometer(); if (airspeed.enabled()) { diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 58fd921b29..136ecff4e3 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -376,7 +376,7 @@ setup_erase(uint8_t argc, const Menu::arg *argv) static int8_t setup_level(uint8_t argc, const Menu::arg *argv) { - startup_INS_ground(); + startup_INS_ground(true); return 0; } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 2d428bfaaf..a16a71730e 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -281,7 +281,7 @@ static void startup_ground(void) //INS ground start //------------------------ // - startup_INS_ground(); + startup_INS_ground(false); // read the radio to set trims // --------------------------- @@ -427,7 +427,7 @@ static void check_short_failsafe() } -static void startup_INS_ground(void) +static void startup_INS_ground(bool do_accel_init) { #if HIL_MODE != HIL_MODE_DISABLED while (!barometer.healthy) { @@ -453,6 +453,10 @@ static void startup_INS_ground(void) ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, flash_leds); + if (do_accel_init) { + ins.init_accel(flash_leds); + ahrs.set_trim(Vector3f(0, 0, 0)); + } ahrs.reset(); // read Baro pressure at ground