Plane: Quadplane: move Q Assist check into new VTOL assist files.

ArduPlane/Plane.h

@@ -143,6 +143,7 @@ public:
     friend class Tiltrotor;
     friend class SLT_Transition;
     friend class Tailsitter_Transition;
+    friend class VTOL_Assist;
 
     friend class Mode;
     friend class ModeCircle;

ArduPlane/RC_Channel.cpp

@@ -61,17 +61,17 @@ void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
     switch(ch_flag) {
         case AuxSwitchPos::HIGH:
             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled");
-            plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
+            plane.quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
             break;
 
         case AuxSwitchPos::MIDDLE:
             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
-            plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_ENABLED);
+            plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_ENABLED);
             break;
 
         case AuxSwitchPos::LOW:
             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled");
-            plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_DISABLED);
+            plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_DISABLED);
             break;
     }
 }

ArduPlane/VTOL_Assist.cpp

@@ -0,0 +1,143 @@
+#include "Plane.h"
+
+#if HAL_QUADPLANE_ENABLED
+
+// Assistance hysteresis helpers
+
+// Reset state
+void VTOL_Assist::Assist_Hysteresis::reset()
+{
+    start_ms = 0;
+    last_ms = 0;
+    active = false;
+}
+
+// Update state, return true when first triggered
+bool VTOL_Assist::Assist_Hysteresis::update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms)
+{
+    bool ret = false;
+
+    if (trigger) {
+        last_ms = now_ms;
+        if (start_ms == 0) {
+            start_ms = now_ms;
+        }
+        if ((now_ms - start_ms) > trigger_delay_ms) {
+            // trigger delay has elapsed
+            if (!active) {
+                // return true on first trigger
+                ret = true;
+            }
+            active = true;
+        }
+
+    } else if (active) {
+        if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {
+            // Clear delay passed
+            reset();
+        }
+
+    } else {
+        reset();
+    }
+
+    return ret;
+}
+
+// Assistance not needed, reset any state
+void VTOL_Assist::reset()
+{
+    force_assist = false;
+    speed_assist = false;
+    angle_error.reset();
+    alt_error.reset();
+}
+
+/*
+  return true if the quadplane should provide stability assistance
+ */
+bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
+{
+    if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) {
+        // disarmed or disabled by aux switch or because a control surface tailsitter
+        reset();
+        return false;
+    }
+
+    if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
+                                                                      || is_positive(plane.get_throttle_input()) 
+                                                                      || plane.is_flying() ) ) {
+        // not in a flight mode and condition where it would be safe to turn on vertial lift motors
+        // skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes
+        reset();
+        return false;
+    }
+
+    if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {
+        // Never active in fixed wing flare
+        reset();
+        return false;
+    }
+
+    force_assist = state == STATE::FORCE_ENABLED;
+
+    if (speed <= 0) {
+        // all checks disabled via speed threshold, still allow force enabled
+        speed_assist = false;
+        alt_error.reset();
+        angle_error.reset();
+        return force_assist;
+    }
+
+    // assistance due to Q_ASSIST_SPEED
+    // if option bit is enabled only allow assist with real airspeed sensor
+    speed_assist = (have_airspeed && aspeed < speed) && 
+       (!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());
+
+    const uint32_t now_ms = AP_HAL::millis();
+    const uint32_t tigger_delay_ms = delay * 1000;
+    const uint32_t clear_delay_ms = tigger_delay_ms * 2;
+
+    /*
+      optional assistance when altitude is too close to the ground
+     */
+    if (alt <= 0) {
+        // Alt assist disabled
+        alt_error.reset();
+
+    } else {
+        const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+        if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
+        }
+    }
+
+    if (angle <= 0) {
+        // Angle assist disabled
+        angle_error.reset();
+
+    } else {
+
+        /*
+        now check if we should provide assistance due to attitude error
+        */
+        const uint16_t allowed_envelope_error_cd = 500U;
+        const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) &&
+                                     (plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) &&
+                                     (plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd));
+
+        const int32_t max_angle_cd = 100U*angle;
+        const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) &&
+                                        (labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd);
+
+        if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
+                                         (int)(plane.ahrs.roll_sensor/100),
+                                         (int)(plane.ahrs.pitch_sensor/100));
+        }
+    }
+
+    return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
+}
+
+#endif  // HAL_QUADPLANE_ENABLED

ArduPlane/VTOL_Assist.h

@@ -0,0 +1,72 @@
+#pragma once
+
+// VTOL assistance in a forward flight mode
+
+class QuadPlane;
+class VTOL_Assist {
+public:
+    VTOL_Assist(QuadPlane& _quadplane):quadplane(_quadplane) {};
+
+    // check for assistance needed
+    bool should_assist(float aspeed, bool have_airspeed);
+
+    // Assistance not needed, reset any state
+    void reset();
+
+    // speed below which quad assistance is given
+    AP_Float speed;
+
+    // angular error at which quad assistance is given
+    AP_Int8 angle;
+
+    // altitude to trigger assistance
+    AP_Int16 alt;
+
+    // Time hysteresis for triggering of assistance
+    AP_Float delay;
+
+    // State from pilot
+    enum class STATE {
+        ASSIST_DISABLED,
+        ASSIST_ENABLED,
+        FORCE_ENABLED,
+    };
+    void set_state(STATE _state) { state = _state; }
+
+    // Logging getters for assist types
+    bool in_force_assist() const { return force_assist; }
+    bool in_speed_assist() const { return speed_assist; }
+    bool in_alt_assist() const { return alt_error.is_active(); }
+    bool in_angle_assist() const { return angle_error.is_active(); }
+
+private:
+
+    // Default to enabled
+    STATE state = STATE::ASSIST_ENABLED;
+
+    class Assist_Hysteresis {
+    public:
+        // Reset state
+        void reset();
+
+        // Update state, return true when first triggered
+        bool update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms);
+
+        // Return true if the output is active
+        bool is_active() const { return active; }
+
+    private:
+        uint32_t start_ms;
+        uint32_t last_ms;
+        bool active;
+    };
+    Assist_Hysteresis angle_error;
+    Assist_Hysteresis alt_error;
+
+    // Force and speed assist have no hysteresis
+    bool force_assist;
+    bool speed_assist;
+
+    // Reference to access quadplane
+    QuadPlane& quadplane;
+};

ArduPlane/quadplane.cpp

@@ -1451,143 +1451,6 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const
     return yaw_rate;
 }
 
-// Assistance not needed, reset any state
-void QuadPlane::VTOL_Assist::reset()
-{
-    force_assist = false;
-    speed_assist = false;
-    angle_error.reset();
-    alt_error.reset();
-}
-
-// Assistance hysteresis helpers
-
-// Reset state
-void QuadPlane::VTOL_Assist::Assist_Hysteresis::reset()
-{
-    start_ms = 0;
-    last_ms = 0;
-    active = false;
-}
-
-bool QuadPlane::VTOL_Assist::Assist_Hysteresis::update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms)
-{
-    bool ret = false;
-
-    if (trigger) {
-        last_ms = now_ms;
-        if (start_ms == 0) {
-            start_ms = now_ms;
-        }
-        if ((now_ms - start_ms) > trigger_delay_ms) {
-            // trigger delay has elapsed
-            if (!active) {
-                // return true on first trigger
-                ret = true;
-            }
-            active = true;
-        }
-
-    } else if (active) {
-        if ((last_ms == 0) || ((now_ms - last_ms) > clear_delay_ms)) {
-            // Clear delay passed
-            reset();
-        }
-
-    } else {
-        reset();
-    }
-
-    return ret;
-}
-
-/*
-  return true if the quadplane should provide stability assistance
- */
-bool QuadPlane::VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
-{
-    if (!plane.arming.is_armed_and_safety_off() || (state == STATE::ASSIST_DISABLED) || quadplane.tailsitter.is_control_surface_tailsitter()) {
-        // disarmed or disabled by aux switch or because a control surface tailsitter
-        reset();
-        return false;
-    }
-
-    if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
-                                                                      || is_positive(plane.get_throttle_input()) 
-                                                                      || plane.is_flying() ) ) {
-        // not in a flight mode and condition where it would be safe to turn on vertial lift motors
-        // skip this check for tailsitters because the forward and vertial motors are the same and are controled directly by throttle imput unlike other quadplanes
-        reset();
-        return false;
-    }
-
-    if (plane.flare_mode != Plane::FlareMode::FLARE_DISABLED) {
-        // Never active in fixed wing flare
-        reset();
-        return false;
-    }
-
-    force_assist = state == STATE::FORCE_ENABLED;
-
-    if (speed <= 0) {
-        // all checks disabled via speed threshold, still allow force enabled
-        speed_assist = false;
-        alt_error.reset();
-        angle_error.reset();
-        return force_assist;
-    }
-
-    // assistance due to Q_ASSIST_SPEED
-    // if option bit is enabled only allow assist with real airspeed sensor
-    speed_assist = (have_airspeed && aspeed < speed) && 
-       (!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor());
-
-    const uint32_t now_ms = AP_HAL::millis();
-    const uint32_t tigger_delay_ms = delay * 1000;
-    const uint32_t clear_delay_ms = tigger_delay_ms * 2;
-
-    /*
-      optional assistance when altitude is too close to the ground
-     */
-    if (alt <= 0) {
-        // Alt assist disabled
-        alt_error.reset();
-
-    } else {
-        const float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
-        if (alt_error.update(height_above_ground < alt, now_ms, tigger_delay_ms, clear_delay_ms)) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground);
-        }
-    }
-
-    if (angle <= 0) {
-        // Angle assist disabled
-        angle_error.reset();
-
-    } else {
-
-        /*
-        now check if we should provide assistance due to attitude error
-        */
-        const uint16_t allowed_envelope_error_cd = 500U;
-        const bool inside_envelope = (labs(plane.ahrs.roll_sensor) <= (plane.aparm.roll_limit*100 + allowed_envelope_error_cd)) &&
-                                     (plane.ahrs.pitch_sensor < (plane.aparm.pitch_limit_max*100 + allowed_envelope_error_cd)) &&
-                                     (plane.ahrs.pitch_sensor > (plane.aparm.pitch_limit_min*100 - allowed_envelope_error_cd));
-
-        const int32_t max_angle_cd = 100U*angle;
-        const bool inside_angle_error = (labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd) &&
-                                        (labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd);
-
-        if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
-                                         (int)(plane.ahrs.roll_sensor/100),
-                                         (int)(plane.ahrs.pitch_sensor/100));
-        }
-    }
-
-    return force_assist || speed_assist || alt_error.is_active() || angle_error.is_active();
-}
-
 /*
   update for transition from quadplane to fixed wing mode
  */

ArduPlane/quadplane.h

@@ -26,6 +26,7 @@
 #include "tailsitter.h"
 #include "tiltrotor.h"
 #include "transition.h"
+#include "VTOL_Assist.h"
 
 /*
   QuadPlane specific functionality
@@ -45,6 +46,7 @@ public:
     friend class Tiltrotor;
     friend class SLT_Transition;
     friend class Tailsitter_Transition;
+    friend class VTOL_Assist;
 
     friend class Mode;
     friend class ModeManual;
@@ -326,75 +328,8 @@ private:
 
     AP_Int16 rc_speed;
 
-
     // VTOL assistance in a forward flight mode
-    class VTOL_Assist {
+    VTOL_Assist assist {*this};
-    public:
-        VTOL_Assist(QuadPlane& _quadplane):quadplane(_quadplane) {};
-
-        // check for assistance needed
-        bool should_assist(float aspeed, bool have_airspeed);
-
-        // Assistance not needed, reset any state
-        void reset();
-
-        // speed below which quad assistance is given
-        AP_Float speed;
-
-        // angular error at which quad assistance is given
-        AP_Int8 angle;
-
-        // altitude to trigger assistance
-        AP_Int16 alt;
-
-        // Time hysteresis for triggering of assistance
-        AP_Float delay;
-
-        // State from pilot
-        enum class STATE {
-            ASSIST_DISABLED,
-            ASSIST_ENABLED,
-            FORCE_ENABLED,
-        };
-        void set_state(STATE _state) { state = _state; }
-
-        // Logging getters for assist types
-        bool in_force_assist() const { return force_assist; }
-        bool in_speed_assist() const { return speed_assist; }
-        bool in_alt_assist() const { return alt_error.is_active(); }
-        bool in_angle_assist() const { return angle_error.is_active(); }
-
-    private:
-
-        // Default to enabled
-        STATE state = STATE::ASSIST_ENABLED;
-
-        class Assist_Hysteresis {
-        public:
-            // Reset state
-            void reset();
-
-            // Update state, return true when first triggered
-            bool update(const bool trigger, const uint32_t &now_ms, const uint32_t &trigger_delay_ms, const uint32_t &clear_delay_ms);
-
-            // Return true if the output is active
-            bool is_active() const { return active; }
-
-        private:
-            uint32_t start_ms;
-            uint32_t last_ms;
-            bool active;
-        };
-        Assist_Hysteresis angle_error;
-        Assist_Hysteresis alt_error;
-
-        // Force and speed assist have no hysteresis
-        bool force_assist;
-        bool speed_assist;
-
-        // Reference to access quadplane
-        QuadPlane& quadplane;
-    } assist {*this};
 
     // landing speed in m/s
     AP_Float land_final_speed;

ArduPlane/tailsitter.cpp

@@ -233,7 +233,7 @@ void Tailsitter::setup()
 
     // Setup for control surface less operation
     if (enable == 2) {
-        quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
+        quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
         quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY;
 
         // Do not allow arming in forward flight modes