Plane: added quadplane version of is_flying()

ArduPlane/ArduPlane.cpp

@@ -486,7 +486,7 @@ void Plane::handle_auto_mode(void)
         nav_cmd_id = auto_rtl_command.id;
     }
 
-    if (auto_state.vtol_mode) {
+    if (quadplane.in_vtol_auto()) {
         quadplane.control_auto(next_WP_loc);
     } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
         (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
@@ -541,7 +541,7 @@ void Plane::update_flight_mode(void)
     if (effective_mode == QSTABILIZE ||
         effective_mode == QHOVER ||
         effective_mode == QLOITER ||
-        (effective_mode == AUTO && auto_state.vtol_mode)) {
+        quadplane.in_vtol_auto()) {
         ahrs.set_fly_forward(false);
     } else {
         ahrs.set_fly_forward(true);
@@ -867,7 +867,7 @@ void Plane::update_flight_stage(void)
     // Update the speed & height controller states
     if (auto_throttle_mode && !throttle_suppressed) {        
         if (control_mode==AUTO) {
-            if (auto_state.vtol_mode) {
+            if (quadplane.in_vtol_auto()) {
                 set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
             } else if (auto_state.takeoff_complete == false) {
                 set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);

ArduPlane/Attitude.cpp

@@ -638,6 +638,11 @@ bool Plane::suppress_throttle(void)
         }
     }
 
+    if (quadplane.is_flying()) {
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled VTOL");
+        throttle_suppressed = false;
+    }
+
     // throttle remains suppressed
     return true;
 }
@@ -931,7 +936,7 @@ void Plane::set_servos(void)
         } else if (control_mode == QSTABILIZE ||
                    control_mode == QHOVER ||
                    control_mode == QLOITER ||
-                   (control_mode == AUTO && auto_state.vtol_mode)) {
+                   quadplane.in_vtol_auto()) {
             // no forward throttle for now
             channel_throttle->servo_out = 0;
             channel_throttle->calc_pwm();

ArduPlane/is_flying.cpp

@@ -125,6 +125,10 @@ void Plane::update_is_flying_5Hz(void)
         isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
     }
 
+    if (quadplane.is_flying()) {
+        is_flying_bool = true;
+    }
+    
     /*
       update last_flying_ms so we always know how long we have not
       been flying for. This helps for crash detection and auto-disarm

ArduPlane/quadplane.cpp

@@ -440,6 +440,19 @@ void QuadPlane::init_loiter(void)
     init_throttle_wait();
 }
 
+
+// helper for is_flying()
+bool QuadPlane::is_flying(void)
+{
+    if (!available()) {
+        return false;
+    }
+    if (motors->get_throttle() > 200 && !motors->limit.throttle_lower) {
+        return true;
+    }
+    return false;
+}
+
 // crude landing detector to prevent tipover
 bool QuadPlane::should_relax(void)
 {
@@ -677,7 +690,7 @@ void QuadPlane::update(void)
     bool quad_mode = (plane.control_mode == QSTABILIZE ||
                       plane.control_mode == QHOVER ||
                       plane.control_mode == QLOITER ||
-                      (plane.control_mode == AUTO && plane.auto_state.vtol_mode));
+                      in_vtol_auto());
     
     if (!quad_mode) {
         update_transition();
@@ -793,6 +806,26 @@ bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet)
     return MAV_RESULT_FAILED;
 }
 
+/*
+  are we in a VTOL auto state?
+ */
+bool QuadPlane::in_vtol_auto(void)
+{
+    if (plane.control_mode != AUTO) {
+        return false;
+    }
+    if (plane.auto_state.vtol_mode) {
+        return true;
+    }
+    switch (plane.mission.get_current_nav_cmd().id) {
+    case MAV_CMD_NAV_VTOL_LAND:
+    case MAV_CMD_NAV_VTOL_TAKEOFF:
+        return true;
+    default:
+        return false;
+    }
+}
+
 /*
   handle auto-mode when auto_state.vtol_mode is true
  */
@@ -828,7 +861,13 @@ void QuadPlane::control_auto(const Location &loc)
 
     switch (plane.mission.get_current_nav_cmd().id) {
     case MAV_CMD_NAV_VTOL_LAND:
-        pos_control->set_alt_target_from_climb_rate_ff(-50, plane.G_Dt, true);
+        if (plane.auto_state.wp_distance > 2) {
+            pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, true);
+        } else if (plane.current_loc.alt > plane.next_WP_loc.alt+5*100) {
+            pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t());
+        } else {
+            pos_control->set_alt_target_from_climb_rate_ff(-30, plane.G_Dt, false);            
+        }
         break;
     case MAV_CMD_NAV_VTOL_TAKEOFF:
         pos_control->set_alt_target_with_slew(wp_nav->get_loiter_target().z, plane.ins.get_loop_delta_t());
@@ -852,7 +891,6 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
     plane.prev_WP_loc = plane.current_loc;
     plane.next_WP_loc = cmd.content.location;
     plane.next_WP_loc.alt = plane.current_loc.alt + cmd.content.location.alt;
-    plane.auto_state.vtol_mode = true;
     throttle_wait = false;
 
     // also update nav_controller for status output
@@ -871,7 +909,6 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
     }
     plane.prev_WP_loc = plane.current_loc;
     plane.next_WP_loc = cmd.content.location;
-    plane.auto_state.vtol_mode = true;
     throttle_wait = false;    
     land_complete = false;
 
@@ -892,7 +929,6 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
     if (plane.current_loc.alt+threshold < plane.next_WP_loc.alt) {
         return false;
     }
-    plane.auto_state.vtol_mode = false;
     transition_state = TRANSITION_AIRSPEED_WAIT;
     return true;
 }

ArduPlane/quadplane.h

@@ -46,6 +46,10 @@ public:
     bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
     bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
     bool verify_vtol_land(void);
+    bool in_vtol_auto(void);
+
+    // vtol help for is_flying()
+    bool is_flying(void);
     
 private:
     AP_AHRS_NavEKF &ahrs;