Plane: added QLAND mode

for VTOL landing. Use for failsafe as well

ArduPlane/ArduPlane.cpp

@@ -544,10 +544,7 @@ void Plane::update_flight_mode(void)
     }
 
     // ensure we are fly-forward
-    if (effective_mode == QSTABILIZE ||
-        effective_mode == QHOVER ||
-        effective_mode == QLOITER ||
-        quadplane.in_vtol_auto()) {
+    if (quadplane.in_vtol_mode()) {
         ahrs.set_fly_forward(false);
     } else {
         ahrs.set_fly_forward(true);
@@ -709,7 +706,8 @@ void Plane::update_flight_mode(void)
 
     case QSTABILIZE:
     case QHOVER:
-    case QLOITER: {
+    case QLOITER:
+    case QLAND: {
         // set nav_roll and nav_pitch using sticks
         nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
         nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@@ -789,6 +787,7 @@ void Plane::update_navigation()
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
+    case QLAND:
         // nothing to do
         break;
     }
@@ -936,9 +935,7 @@ void Plane::update_flight_stage(void)
         } else {
             set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
         }
-    } else if (control_mode == QSTABILIZE ||
-               control_mode == QHOVER ||
-               control_mode == QLOITER ||
+    } else if (quadplane.in_vtol_mode() ||
                quadplane.in_assisted_flight()) {
         set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
     } else {

ArduPlane/Attitude.cpp

@@ -143,6 +143,7 @@ void Plane::stabilize_stick_mixing_direct()
         control_mode == QSTABILIZE ||
         control_mode == QHOVER ||
         control_mode == QLOITER ||
+        control_mode == QLAND ||
         control_mode == TRAINING) {
         return;
     }
@@ -165,6 +166,7 @@ void Plane::stabilize_stick_mixing_fbw()
         control_mode == QSTABILIZE ||
         control_mode == QHOVER ||
         control_mode == QLOITER ||
+        control_mode == QLAND ||
         control_mode == TRAINING ||
         (control_mode == AUTO && g.auto_fbw_steer)) {
         return;
@@ -364,7 +366,8 @@ void Plane::stabilize()
         stabilize_acro(speed_scaler);
     } else if (control_mode == QSTABILIZE ||
                control_mode == QHOVER ||
-               control_mode == QLOITER) {
+               control_mode == QLOITER ||
+               control_mode == QLAND) {
         quadplane.control_run();
     } else {
         if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
@@ -994,10 +997,7 @@ void Plane::set_servos(void)
                    guided_throttle_passthru) {
             // manual pass through of throttle while in GUIDED
             channel_throttle->radio_out = channel_throttle->radio_in;
-        } else if (control_mode == QSTABILIZE ||
-                   control_mode == QHOVER ||
-                   control_mode == QLOITER ||
-                   quadplane.in_vtol_auto()) {
+        } else if (quadplane.in_vtol_mode()) {
             // no forward throttle for now
             channel_throttle->servo_out = 0;
             channel_throttle->calc_pwm();

ArduPlane/GCS_Mavlink.cpp

@@ -42,6 +42,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
+    case QLAND:
     case CRUISE:
         base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
         break;
@@ -178,6 +179,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
     case AUTOTUNE:
     case QSTABILIZE:
     case QHOVER:
+    case QLAND:
     case QLOITER:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation

ArduPlane/Plane.h

@@ -204,6 +204,7 @@ private:
         float initial_correction;
         uint32_t last_correction_time_ms;
         uint8_t in_range_count;
+        float height_estimate;
     } rangefinder_state;
 #endif
 

ArduPlane/altitude.cpp

@@ -565,7 +565,7 @@ void Plane::rangefinder_height_update(void)
         }
         // correct the range for attitude (multiply by DCM.c.z, which
         // is cos(roll)*cos(pitch))
-        height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
+        rangefinder_state.height_estimate = distance * ahrs.get_rotation_body_to_ned().c.z;
 
         // we consider ourselves to be fully in range when we have 10
         // good samples (0.2s) that are different by 5% of the maximum
@@ -580,11 +580,12 @@ void Plane::rangefinder_height_update(void)
             rangefinder_state.in_range = true;
             if (!rangefinder_state.in_use &&
                 (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
-                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
-                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) &&
+                 flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE ||
+                 flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL ||
+                 control_mode == QLAND) &&
                 g.rangefinder_landing) {
                 rangefinder_state.in_use = true;
-                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)height_estimate);
+                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);
             }
         }
     } else {

ArduPlane/defines.h

@@ -66,7 +66,8 @@ enum FlightMode {
     INITIALISING  = 16,
     QSTABILIZE    = 17,
     QHOVER        = 18,
-    QLOITER       = 19
+    QLOITER       = 19,
+    QLAND         = 20
 };
 
 // type of stick mixing enabled

ArduPlane/events.cpp

@@ -29,9 +29,10 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
 
     case QSTABILIZE:
     case QLOITER:
+    case QHOVER:
         failsafe.saved_mode = control_mode;
         failsafe.saved_mode_set = 1;
-        set_mode(QHOVER);
+        set_mode(QLAND);
         break;
         
     case AUTO:
@@ -50,7 +51,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
 
     case CIRCLE:
     case RTL:
-    case QHOVER:
+    case QLAND:
     default:
         break;
     }
@@ -87,8 +88,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
         break;
 
     case QSTABILIZE:
+    case QHOVER:
     case QLOITER:
-        set_mode(QHOVER);
+        set_mode(QLAND);
         break;
         
     case AUTO:
@@ -106,7 +108,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
         break;
 
     case RTL:
-    case QHOVER:
+    case QLAND:
     default:
         break;
     }

ArduPlane/quadplane.cpp

@@ -569,6 +569,14 @@ void QuadPlane::init_loiter(void)
     init_throttle_wait();
 }
 
+void QuadPlane::init_land(void)
+{
+    init_loiter();
+    throttle_wait = false;
+    land_state = QLAND_DESCEND;
+    motors_lower_limit_start_ms = 0;
+}
+
 
 // helper for is_flying()
 bool QuadPlane::is_flying(void)
@@ -644,8 +652,23 @@ void QuadPlane::control_loiter()
     plane.nav_roll_cd = wp_nav->get_roll();
     plane.nav_pitch_cd = wp_nav->get_pitch();
 
-    // update altitude target and call position controller
-    pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
+    if (plane.control_mode == QLAND) {
+        if (land_state == QLAND_DESCEND) {
+            if (plane.g.rangefinder_landing && plane.rangefinder_state.in_range) {
+                if (plane.rangefinder_state.height_estimate < land_final_alt) {
+                    land_state = QLAND_FINAL;
+                }
+            } else if (plane.adjusted_relative_altitude_cm() < land_final_alt*100) {
+                land_state = QLAND_FINAL;                
+            }
+        }
+        float descent_rate = (land_state == QLAND_FINAL)?land_speed_cms:wp_nav->get_speed_down();
+        pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
+        check_land_complete();
+    } else {
+        // update altitude target and call position controller
+        pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
+    }
     pos_control->update_z_controller();
 }
 
@@ -848,12 +871,7 @@ void QuadPlane::update(void)
         return;
     }
 
-    bool quad_mode = (plane.control_mode == QSTABILIZE ||
-                      plane.control_mode == QHOVER ||
-                      plane.control_mode == QLOITER ||
-                      in_vtol_auto());
-    
-    if (!quad_mode) {
+    if (!in_vtol_mode()) {
         update_transition();
     } else {
         assisted_flight = false;
@@ -898,8 +916,8 @@ void QuadPlane::control_run(void)
         control_hover();
         break;
     case QLOITER:
+    case QLAND:
         control_loiter();
-        break;
     default:
         break;
     }
@@ -931,6 +949,9 @@ bool QuadPlane::init_mode(void)
     case QLOITER:
         init_loiter();
         break;
+    case QLAND:
+        init_land();
+        break;
     default:
         break;
     }
@@ -997,6 +1018,7 @@ bool QuadPlane::in_vtol_mode(void)
     return (plane.control_mode == QSTABILIZE ||
             plane.control_mode == QHOVER ||
             plane.control_mode == QLOITER ||
+            plane.control_mode == QLAND ||
             in_vtol_auto());
 }
 
@@ -1220,6 +1242,17 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
     return true;
 }
 
+void QuadPlane::check_land_complete(void)
+{
+    if (land_state == QLAND_FINAL &&
+        (motors_lower_limit_start_ms != 0 &&
+         millis() - motors_lower_limit_start_ms > 5000)) {
+        plane.disarm_motors();
+        land_state = QLAND_COMPLETE;
+        plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
+    }
+}
+
 /*
   check if a VTOL landing has completed
  */
@@ -1247,12 +1280,6 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
         plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started");
     }
 
-    if (land_state == QLAND_FINAL &&
-        (motors_lower_limit_start_ms != 0 &&
-         millis() - motors_lower_limit_start_ms > 5000)) {
-        plane.disarm_motors();
-        land_state = QLAND_COMPLETE;
-        plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
-    }
+    check_land_complete();
     return false;
 }

ArduPlane/quadplane.h

@@ -118,7 +118,9 @@ private:
     void control_hover(void);
 
     void init_loiter(void);
+    void init_land(void);
     void control_loiter(void);
+    void check_land_complete(void);
 
     float assist_climb_rate_cms(void);
 

ArduPlane/system.cpp

@@ -461,6 +461,7 @@ void Plane::set_mode(enum FlightMode mode)
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
+    case QLAND:
         if (!quadplane.init_mode()) {
             control_mode = previous_mode;
         } else {
@@ -505,6 +506,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
     case QSTABILIZE:
     case QHOVER:
     case QLOITER:
+    case QLAND:
         set_mode((enum FlightMode)mode);
         return true;
     }
@@ -704,6 +706,9 @@ void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case QLOITER:
         port->print("QLoiter");
         break;
+    case QLAND:
+        port->print("QLand");
+        break;
     default:
         port->printf("Mode(%u)", (unsigned)mode);
         break;