diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 376a91fbbc..78c17577a1 100644 --- a/ArduPlane/Plane.h +++ b/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; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 3d0caaec35..338e5582c8 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/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; } } diff --git a/ArduPlane/VTOL_Assist.cpp b/ArduPlane/VTOL_Assist.cpp new file mode 100644 index 0000000000..f071b5865d --- /dev/null +++ b/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 diff --git a/ArduPlane/VTOL_Assist.h b/ArduPlane/VTOL_Assist.h new file mode 100644 index 0000000000..c358223650 --- /dev/null +++ b/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; +}; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 70b57605f2..35a4c39d15 100644 --- a/ArduPlane/quadplane.cpp +++ b/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 */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index cd52905df4..2ced92498b 100644 --- a/ArduPlane/quadplane.h +++ b/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 { - 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}; + VTOL_Assist assist {*this}; // landing speed in m/s AP_Float land_final_speed; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index e4a7fe6f27..3df68e3bc3 100644 --- a/ArduPlane/tailsitter.cpp +++ b/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