From 2cc0cd65b4032d9c1b083b1a11f8ec5a19aa53db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Nov 2011 12:05:43 +1100 Subject: [PATCH] zero airspeed on ground start when initiated by MAVLink If the users asks for a new calibration, that should include the airspeed sensor --- ArduPlane/system.pde | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 237a473e54..6e352014a1 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -286,20 +286,6 @@ static void startup_ground(void) // --------------------------- trim_radio(); // This was commented out as a HACK. Why? I don't find a problem. -#if HIL_MODE != HIL_MODE_ATTITUDE -if (g.airspeed_enabled == true) - { - // initialize airspeed sensor - // -------------------------- - zero_airspeed(); - gcs_send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); - } -else - { - gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); - } -#endif - // Save the settings for in-air restart // ------------------------------------ //save_EEPROM_groundstart(); @@ -439,6 +425,15 @@ static void startup_IMU_ground(void) //----------------------------- init_barometer(); + if (g.airspeed_enabled == true) { + // initialize airspeed sensor + // -------------------------- + zero_airspeed(); + gcs_send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); + } else { + gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); + } + #endif // HIL_MODE_ATTITUDE digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready