diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 061cb476a07bd..a774b778f5f59 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/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; diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c712b2db1756b..bcca30e38ae8e 100644 --- a/ArduPlane/Log.cpp +++ b/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 }, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 376a91fbbc796..78c17577a1aae 100644 --- a/ArduPlane/Plane.h +++ b/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; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 1fc5b0ad222a1..338e5582c8781 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.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; } diff --git a/ArduPlane/VTOL_Assist.cpp b/ArduPlane/VTOL_Assist.cpp new file mode 100644 index 0000000000000..f071b5865d49b --- /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 0000000000000..c358223650173 --- /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 a96d70aeb50f8..35a4c39d15f0b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -108,7 +108,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0 100 // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist_speed, 0), + AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist.speed, 0), // @Param: YAW_RATE_MAX // @DisplayName: Maximum yaw rate @@ -236,7 +236,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0 90 // @Increment: 1 // @User: Standard - AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist_angle, 30), + AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist.angle, 30), // 47: TILT_TYPE // 48: TAILSIT_ANGLE @@ -405,7 +405,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Range: 0 120 // @Increment: 1 // @User: Standard - AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0), + AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist.alt, 0), // 17: TAILSIT_GSCMSK // 18: TAILSIT_GSCMIN @@ -417,7 +417,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Range: 0 2 // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("ASSIST_DELAY", 19, QuadPlane, assist_delay, 0.5), + AP_GROUPINFO("ASSIST_DELAY", 19, QuadPlane, assist.delay, 0.5), // @Param: FWD_MANTHR_MAX // @DisplayName: VTOL manual forward throttle max percent @@ -825,7 +825,7 @@ bool QuadPlane::setup(void) // default QAssist state as set with Q_OPTIONS if (option_is_set(QuadPlane::OPTION::Q_ASSIST_FORCE_ENABLE)) { - q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE; + assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED); } setup_defaults(); @@ -1451,118 +1451,6 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const return yaw_rate; } -/* - return true if the quadplane should provide stability assistance - */ -bool QuadPlane::should_assist(float aspeed, bool have_airspeed) -{ - if (!plane.arming.is_armed_and_safety_off() || (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED) || tailsitter.is_control_surface_tailsitter()) { - // disarmed or disabled by aux switch or because a control surface tailsitter - in_angle_assist = false; - angle_error_start_ms = 0; - return false; - } - - if (!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 - in_angle_assist = false; - angle_error_start_ms = 0; - return false; - } - - if (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE) { - // force enabled, no need to check thresholds - in_angle_assist = false; - angle_error_start_ms = 0; - return true; - } - - if (assist_speed <= 0) { - // disabled via speed threshold - in_angle_assist = false; - angle_error_start_ms = 0; - return false; - } - - // assistance due to Q_ASSIST_SPEED - // if option bit is enabled only allow assist with real airspeed sensor - if ((have_airspeed && aspeed < assist_speed) && - (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.using_airspeed_sensor())) { - in_angle_assist = false; - angle_error_start_ms = 0; - return true; - } - - const uint32_t now = AP_HAL::millis(); - - /* - optional assistance when altitude is too close to the ground - */ - if (assist_alt > 0) { - float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); - if (height_above_ground < assist_alt) { - if (alt_error_start_ms == 0) { - alt_error_start_ms = now; - } - if (now - alt_error_start_ms > assist_delay*1000) { - // we've been below assistant alt for Q_ASSIST_DELAY seconds - if (!in_alt_assist) { - in_alt_assist = true; - gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground); - } - return true; - } - } else { - in_alt_assist = false; - alt_error_start_ms = 0; - } - } - - if (assist_angle <= 0) { - in_angle_assist = false; - angle_error_start_ms = 0; - return false; - } - - /* - now check if we should provide assistance due to attitude error - */ - - const uint16_t allowed_envelope_error_cd = 500U; - if (labs(ahrs.roll_sensor) <= plane.aparm.roll_limit*100 + allowed_envelope_error_cd && - ahrs.pitch_sensor < plane.aparm.pitch_limit_max*100+allowed_envelope_error_cd && - ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min*100)) { - // we are inside allowed attitude envelope - in_angle_assist = false; - angle_error_start_ms = 0; - return false; - } - - int32_t max_angle_cd = 100U*assist_angle; - if ((labs(ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd && - labs(ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd)) { - // not beyond angle error - angle_error_start_ms = 0; - in_angle_assist = false; - return false; - } - - if (angle_error_start_ms == 0) { - angle_error_start_ms = now; - } - bool ret = (now - angle_error_start_ms) >= assist_delay*1000; - if (ret && !in_angle_assist) { - in_angle_assist = true; - gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d", - (int)(ahrs.roll_sensor/100), - (int)(ahrs.pitch_sensor/100)); - } - return ret; -} - /* update for transition from quadplane to fixed wing mode */ @@ -1581,7 +1469,7 @@ void SLT_Transition::update() /* see if we should provide some assistance */ - if (quadplane.should_assist(aspeed, have_airspeed)) { + if (quadplane.assist.should_assist(aspeed, have_airspeed)) { // the quad should provide some assistance to the plane quadplane.assisted_flight = true; // update transition state for vehicles using airspeed wait @@ -1790,6 +1678,9 @@ void SLT_Transition::VTOL_update() transition_state = TRANSITION_AIRSPEED_WAIT; } last_throttle = motors->get_throttle(); + + // Keep assistance reset while not checking + quadplane.assist.reset(); } /* @@ -2468,7 +2359,7 @@ void QuadPlane::vtol_position_controller(void) } // speed for crossover to POSITION1 controller - const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist_speed); + const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist.speed); // run fixed wing navigation plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc); @@ -3721,6 +3612,32 @@ void QuadPlane::Log_Write_QControl_Tuning() target_climb_rate_cms = pos_control->get_vel_target_z_cms(); } + // Asemble assistance bitmask, defintion here is used to generate log documentation + enum class log_assistance_flags { + in_assisted_flight = 1U<<0, // true if VTOL assist is active + forced = 1U<<1, // true if assistance is forced + speed = 1U<<2, // true if assistance due to low airspeed + alt = 1U<<3, // true if assistance due to low altitude + angle = 1U<<4, // true if assistance due to attitude error + }; + + uint8_t assist_flags = 0; + if (assisted_flight) { + assist_flags |= (uint8_t)log_assistance_flags::in_assisted_flight; + } + if (assist.in_force_assist()) { + assist_flags |= (uint8_t)log_assistance_flags::forced; + } + if (assist.in_speed_assist()) { + assist_flags |= (uint8_t)log_assistance_flags::speed; + } + if (assist.in_alt_assist()) { + assist_flags |= (uint8_t)log_assistance_flags::alt; + } + if (assist.in_angle_assist()) { + assist_flags |= (uint8_t)log_assistance_flags::angle; + } + struct log_QControl_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG), time_us : AP_HAL::micros64(), @@ -3735,7 +3652,7 @@ void QuadPlane::Log_Write_QControl_Tuning() climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()), throttle_mix : attitude_control->get_throttle_mix(), transition_state : transition->get_log_transition_state(), - assist : assisted_flight, + assist : assist_flags, }; plane.logger.WriteBlock(&pkt, sizeof(pkt)); @@ -4567,6 +4484,9 @@ void SLT_Transition::force_transition_complete() { transition_start_ms = 0; transition_low_airspeed_ms = 0; set_last_fw_pitch(); + + // Keep assistance reset while not checking + quadplane.assist.reset(); } MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5b2b35fa92dff..2ced92498b423 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 @@ class QuadPlane friend class Tiltrotor; friend class SLT_Transition; friend class Tailsitter_Transition; + friend class VTOL_Assist; friend class Mode; friend class ModeManual; @@ -162,13 +164,6 @@ class QuadPlane MAV_TYPE get_mav_type(void) const; - enum Q_ASSIST_STATE_ENUM { - Q_ASSIST_DISABLED, - Q_ASSIST_ENABLED, - Q_ASSIST_FORCE, - }; - void set_q_assist_state(Q_ASSIST_STATE_ENUM state) {q_assist_state = state;}; - // called when we change mode (for any mode, not just Q modes) void mode_enter(void); @@ -232,9 +227,6 @@ class QuadPlane // return true if airmode should be active bool air_mode_active() const; - // check for quadplane assistance needed - bool should_assist(float aspeed, bool have_airspeed); - // check for an EKF yaw reset void check_yaw_reset(void); @@ -336,18 +328,8 @@ class QuadPlane AP_Int16 rc_speed; - // speed below which quad assistance is given - AP_Float assist_speed; - - // angular error at which quad assistance is given - AP_Int8 assist_angle; - uint32_t angle_error_start_ms; - AP_Float assist_delay; - - // altitude to trigger assistance - AP_Int16 assist_alt; - uint32_t alt_error_start_ms; - bool in_alt_assist; + // VTOL assistance in a forward flight mode + VTOL_Assist assist {*this}; // landing speed in m/s AP_Float land_final_speed; @@ -459,9 +441,6 @@ class QuadPlane // true when quad is assisting a fixed wing mode bool assisted_flight:1; - // true when in angle assist - bool in_angle_assist:1; - // are we in a guided takeoff? bool guided_takeoff:1; @@ -685,9 +664,6 @@ class QuadPlane // returns true if the vehicle should currently be doing a spiral landing bool landing_with_fixed_wing_spiral_approach(void) const; - // Q assist state, can be enabled, disabled or force. Default to enabled - Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED; - /* return true if we should use the fixed wing attitude control loop */ diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 4e28d534440a3..3df68e3bc3483 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.q_assist_state = QuadPlane::Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE; + quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED); quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY; // Do not allow arming in forward flight modes @@ -819,7 +819,7 @@ void Tailsitter_Transition::update() float aspeed; bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed); - quadplane.assisted_flight = quadplane.should_assist(aspeed, have_airspeed); + quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed); if (transition_state < TRANSITION_DONE) { // during transition we ask TECS to use a synthetic @@ -885,7 +885,7 @@ void Tailsitter_Transition::VTOL_update() float aspeed; bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed); // provide assistance in forward flight portion of tailsitter transition - quadplane.assisted_flight = quadplane.should_assist(aspeed, have_airspeed); + quadplane.assisted_flight = quadplane.assist.should_assist(aspeed, have_airspeed); if (!quadplane.tailsitter.transition_vtol_complete()) { return; } @@ -894,6 +894,9 @@ void Tailsitter_Transition::VTOL_update() vtol_limit_start_ms = now; vtol_limit_initial_pitch = quadplane.ahrs_view->pitch_sensor; } + } else { + // Keep assistance reset while not checking + quadplane.assist.reset(); } restart(); } @@ -1006,6 +1009,8 @@ void Tailsitter_Transition::force_transition_complete() vtol_transition_start_ms = AP_HAL::millis(); vtol_transition_initial_pitch = constrain_float(plane.nav_pitch_cd,-8500,8500); fw_limit_start_ms = 0; + + quadplane.assist.reset(); } MAV_VTOL_STATE Tailsitter_Transition::get_mav_vtol_state() const diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 8cd4bc80826a2..98628a45d71b6 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -808,7 +808,6 @@ def CPUFailsafe(self): def QAssist(self): '''QuadPlane Assist tests''' - # find a motor peak self.takeoff(10, mode="QHOVER") self.set_rc(3, 1800) self.change_mode("FBWA") @@ -841,6 +840,7 @@ def QAssist(self): comparator=operator.eq) self.set_rc(3, 1300) + # Test angle assist self.context_push() self.progress("Rolling over to %.0f degrees" % -lim_roll_deg) self.set_rc(1, 1000) @@ -856,9 +856,31 @@ def QAssist(self): self.wait_roll(lim_roll_deg, 5) self.context_pop() self.set_rc(1, 1500) - self.set_parameter("Q_RTL_MODE", 1) - self.change_mode("RTL") - self.wait_disarmed(timeout=300) + + # Test alt assist, climb to 60m and set assist alt to 50m + self.context_push() + guided_loc = self.home_relative_loc_ne(0, 0) + guided_loc.alt = 60 + self.change_mode("GUIDED") + self.do_reposition(guided_loc) + self.wait_altitude(58, 62, relative=True) + self.set_parameter("Q_ASSIST_ALT", 50) + + # Try and descent to 40m + guided_loc.alt = 40 + self.do_reposition(guided_loc) + + # Expect alt assist to kick in, eg "Alt assist 48.9m" + self.wait_statustext(r"Alt assist \d*.\d*m", regex=True, timeout=100) + + # Test transition timeout, should switch to QRTL + self.set_parameter("Q_TRANS_FAIL_ACT", 1) + self.set_parameter("Q_TRANS_FAIL", 10) + self.wait_mode("QRTL") + + self.context_pop() + + self.wait_disarmed(timeout=200) def LoiterAltQLand(self): '''test loitering and qland with terrain involved'''