From 05484c39873d8251ca9e2b00eaccb79f8e15c4c2 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Fri, 16 Nov 2018 15:27:45 -0700 Subject: [PATCH] Plane: Support a different landing airspeed for fw quadplane landings --- ArduPlane/commands_logic.cpp | 2 -- ArduPlane/navigation.cpp | 12 +++++++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index c1e9ee5b98..d65abef3cc 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -430,8 +430,6 @@ void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd) } vtol_approach_s.approach_stage = LOITER_TO_ALT; - - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); } void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 1c860e8304..3322a9857e 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -139,7 +139,17 @@ void Plane::calc_airspeed_errors() } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { // Landing airspeed target target_airspeed_cm = landing.get_target_airspeed_cm(); - + } else if ((control_mode == AUTO) && + (quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && + ((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) || + (vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) { + float land_airspeed = SpdHgt_Controller->get_land_airspeed(); + if (is_positive(land_airspeed)) { + target_airspeed_cm = SpdHgt_Controller->get_land_airspeed() * 100; + } else { + // fallover to normal airspeed + target_airspeed_cm = aparm.airspeed_cruise_cm; + } } else { // Normal airspeed target target_airspeed_cm = aparm.airspeed_cruise_cm;