diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 2dd6998e40..1802dbcdc6 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -269,8 +269,12 @@ void Plane::one_second_loop() adsb.set_stall_speed_cm(aparm.airspeed_min); adsb.set_max_speed(aparm.airspeed_max); #endif - ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2), - (float)((aparm.airspeed_max - aparm.airspeed_min)/2)); + + if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) { + // use average of min and max airspeed as default airspeed fusion with high variance + ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2), + (float)((aparm.airspeed_max - aparm.airspeed_min)/2)); + } // sync MAVLink system ID mavlink_system.sysid = g.sysid_this_mav; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index be30eb011b..1cfb056ba5 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1105,7 +1105,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options // @Description: Flight mode specific options - // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor. + // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor, 7:EnableDefaultAirspeed for takeoff // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 769c8f155a..5e2ac140af 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -159,6 +159,7 @@ enum FlightOptions { CLIMB_BEFORE_TURN = (1 << 4), ACRO_YAW_DAMPER = (1 << 5), SURPRESS_TKOFF_SCALING = (1<<6), + ENABLE_DEFAULT_AIRSPEED = (1<<7), }; enum CrowFlapOptions {