Plane: add flight stage LAND_PREFLARE

ArduPlane/ArduPlane.cpp

@@ -844,6 +844,7 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
         gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted via throttle. Climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
         break;
 
+    case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
     case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
     case AP_SpdHgtControl::FLIGHT_NORMAL:
     case AP_SpdHgtControl::FLIGHT_VTOL:

ArduPlane/Attitude.cpp

@@ -217,7 +217,9 @@ void Plane::stabilize_yaw(float speed_scaler)
         // are below the GROUND_STEER_ALT
         steering_control.ground_steering = (channel_roll->control_in == 0 && 
                                             fabsf(relative_altitude()) < g.ground_steer_alt);
-        if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
+        if (control_mode == AUTO &&
+                (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE)) {
             // don't use ground steering on landing approach
             steering_control.ground_steering = false;
         }
@@ -987,6 +989,7 @@ void Plane::set_servos(void)
                 }
                 break;
             case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
+            case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
             case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
                 if (g.land_flap_percent != 0) {
                     auto_flap_percent = g.land_flap_percent;

ArduPlane/GCS_Mavlink.cpp

@@ -1390,7 +1390,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             //Not allowing go around at FLIGHT_LAND_FINAL stage on purpose --
             //if plane is close to the ground a go around coudld be dangerous.
-            if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
+            if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
+                plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
                 // Initiate an aborted landing. This will trigger a pitch-up and
                 // climb-out to a safe altitude holding heading then one of the
                 // following actions will occur, check for in this order:

ArduPlane/altitude.cpp

@@ -34,7 +34,8 @@ void Plane::adjust_altitude_target()
         // in land final TECS uses TECS_LAND_SINK as a target sink
         // rate, and ignores the target altitude
         set_target_altitude_location(next_WP_loc);
-    } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
+    } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+            flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
         setup_landing_glide_slope();
     } else if (nav_controller->reached_loiter_target()) {
         // once we reach a loiter target then lock to the final
@@ -357,7 +358,8 @@ void Plane::set_offset_altitude_location(const Location &loc)
     }
 #endif
 
-    if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+    if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
+        flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
         flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
         // if we are within GLIDE_SLOPE_MIN meters of the target altitude
         // then reset the offset to not use a glide slope. This allows for
@@ -536,6 +538,7 @@ float Plane::rangefinder_correction(void)
     bool using_rangefinder = (g.rangefinder_landing &&
                               control_mode == AUTO && 
                               (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                               flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
                                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
     if (!using_rangefinder) {
         return 0;
@@ -576,7 +579,9 @@ void Plane::rangefinder_height_update(void)
         } else {
             rangefinder_state.in_range = true;
             if (!rangefinder_state.in_use &&
-                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+                (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
+                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) &&
                 g.rangefinder_landing) {
                 rangefinder_state.in_use = true;
                 gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate);

ArduPlane/events.cpp

@@ -138,6 +138,7 @@ void Plane::low_battery_event(void)
     gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
                       (double)battery.voltage(), (double)battery.current_total_mah());
     if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
+        flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
         flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
     	set_mode(RTL);
     	aparm.throttle_cruise.load();

ArduPlane/is_flying.cpp

@@ -93,6 +93,7 @@ void Plane::update_is_flying_5Hz(void)
                 }
                 break;
 
+            case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
             case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
                 break;
 
@@ -218,6 +219,7 @@ void Plane::crash_detection_update(void)
             // a crash into a tree would be caught here.
             break;
 
+        case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
         case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
             // We should be nice and level-ish in this flight stage. If not, we most
             // likely had a crazy landing. Throttle is inhibited already at the flare

ArduPlane/landing.cpp

@@ -314,6 +314,7 @@ float Plane::tecs_hgt_afe(void)
     */
     float hgt_afe;
     if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
+        flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
         flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
         hgt_afe = height_above_target();
         hgt_afe -= rangefinder_correction();

ArduPlane/sensors.cpp

@@ -36,6 +36,7 @@ void Plane::read_rangefinder(void)
     {
         // use the best available alt estimate via baro above home
         if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+            flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
             flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
             // ensure the rangefinder is powered-on when land alt is higher than home altitude.
             // This is done using the target alt which we know is below us and we are sinking to it

ArduPlane/system.cpp

@@ -528,7 +528,9 @@ void Plane::check_long_failsafe()
     uint32_t tnow = millis();
     // only act on changes
     // -------------------
-    if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
+    if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS &&
+            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
+            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
             flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
         if (failsafe.state == FAILSAFE_SHORT &&
                    (tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
@@ -562,8 +564,10 @@ void Plane::check_short_failsafe()
 {
     // only act on changes
     // -------------------
-    if(failsafe.state == FAILSAFE_NONE && (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
-            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH)) {
+    if(failsafe.state == FAILSAFE_NONE &&
+            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
+            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
+            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
         if(failsafe.ch3_failsafe) {                                              // The condition is checked and the flag ch3_failsafe is set in radio.pde
             failsafe_short_on_event(FAILSAFE_SHORT);
         }