Plane: Quadplane: rework assistance check

ArduPlane/AP_Arming.cpp

@@ -214,7 +214,7 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure)
       Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
      */
     if (check_enabled(ARMING_CHECK_PARAMETERS) &&
-        is_zero(plane.quadplane.assist_speed) &&
+        is_zero(plane.quadplane.assist.speed) &&
         !plane.quadplane.tailsitter.enabled()) {
         check_failed(display_failure,"Q_ASSIST_SPEED is not set");
         ret = false;

ArduPlane/Log.cpp

@@ -389,7 +389,8 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: CRt: climb rate
 // @Field: TMix: transition throttle mix value
 // @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done
-// @Field: Ast: Q assist active
+// @Field: Ast: bitmask of assistance flags
+// @FieldBitmaskEnum: Ast: log_assistance_flags
 #if HAL_QUADPLANE_ENABLED
     { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
       "QTUN", "QffffffeccfBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Trn,Ast", "s----mmmnn---", "F----00000---" , true },

ArduPlane/Plane.h

@@ -143,6 +143,7 @@ class Plane : public AP_Vehicle {
     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.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE);
+            plane.quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
             break;
 
         case AuxSwitchPos::MIDDLE:
             gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
-            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_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.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
+            plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_DISABLED);
             break;
     }
 }
@@ -121,20 +121,11 @@ void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
         switch(ch_flag) {
         case AuxSwitchPos::HIGH:
             plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET;
-#if HAL_QUADPLANE_ENABLED
-            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
-#endif
             break;
         case AuxSwitchPos::MIDDLE:
             plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET;
-#if HAL_QUADPLANE_ENABLED
-            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED);
-#endif
             break;
         case AuxSwitchPos::LOW:
-#if HAL_QUADPLANE_ENABLED
-            plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED);
-#endif
             plane.flare_mode = Plane::FlareMode::FLARE_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;
+};