From 2467d7b5c7b4f9728603068478b175771de09012 Mon Sep 17 00:00:00 2001 From: Peter Hall <33176108+IamPete1@users.noreply.github.com> Date: Thu, 2 Jan 2020 19:06:39 +0000 Subject: [PATCH] Plane: add Qassist switch --- ArduPlane/RC_Channel.cpp | 28 ++++++++++++++++++++++++++++ ArduPlane/RC_Channel.h | 2 ++ ArduPlane/quadplane.cpp | 36 +++++++++++++++++++----------------- ArduPlane/quadplane.h | 14 ++++++++++++-- 4 files changed, 61 insertions(+), 19 deletions(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 9e9f88c7d0..5b77dde42b 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -58,6 +58,26 @@ void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number, } } +void RC_Channel_Plane::do_aux_function_q_assist_state(aux_switch_pos_t ch_flag) +{ + switch(ch_flag) { + case 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); + break; + + case MIDDLE: + gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled"); + plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED); + break; + + case LOW: + gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled"); + plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED); + break; + } +} + void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, const RC_Channel::aux_switch_pos_t ch_flag) { @@ -74,6 +94,10 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::TAKEOFF: break; + case AUX_FUNC::Q_ASSIST: + do_aux_function(ch_option, ch_flag); + break; + case AUX_FUNC::REVERSE_THROTTLE: plane.have_reverse_throttle_rc_option = true; // setup input throttle as a range. This is needed as init_aux_function is called @@ -136,6 +160,10 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const aux_swi case AUX_FUNC::FLAP: break; // flap input label, nothing to do + case AUX_FUNC::Q_ASSIST: + do_aux_function_q_assist_state(ch_flag); + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index d7fd045696..142a063e6b 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -17,6 +17,8 @@ private: void do_aux_function_change_mode(Mode::Number number, aux_switch_pos_t ch_flag); + + void do_aux_function_q_assist_state(aux_switch_pos_t ch_flag); }; class RC_Channels_Plane : public RC_Channels diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 44ea9b36f3..8f35a8231b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1436,7 +1436,7 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const /* return true if the quadplane should provide stability assistance */ -bool QuadPlane::assistance_needed(float aspeed) +bool QuadPlane::assistance_needed(float aspeed, bool have_airspeed) { if (assist_speed <= 0) { // assistance disabled @@ -1445,7 +1445,7 @@ bool QuadPlane::assistance_needed(float aspeed) return false; } - if (aspeed < assist_speed) { + if (have_airspeed && aspeed < assist_speed) { // assistance due to Q_ASSIST_SPEED in_angle_assist = false; angle_error_start_ms = 0; @@ -1559,26 +1559,28 @@ void QuadPlane::update_transition(void) if (is_tailsitter() && transition_state == TRANSITION_AIRSPEED_WAIT) { transition_state = TRANSITION_ANGLE_WAIT_FW; } - + /* see if we should provide some assistance */ - if (have_airspeed && - assistance_needed(aspeed) && - !is_tailsitter() && - hal.util->get_soft_armed() && - ((plane.auto_throttle_mode && !plane.throttle_suppressed) || - plane.get_throttle_input()>0 || - plane.is_flying())) { + bool q_assist_safe = hal.util->get_soft_armed() && ((plane.auto_throttle_mode && !plane.throttle_suppressed) || + plane.get_throttle_input()>0 || + plane.is_flying()); + if (q_assist_safe && + (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE || + (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED && assistance_needed(aspeed, have_airspeed)))) { // the quad should provide some assistance to the plane - if (transition_state != TRANSITION_AIRSPEED_WAIT) { - gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); - } - transition_state = TRANSITION_AIRSPEED_WAIT; - if (transition_start_ms == 0) { - transition_start_ms = now; - } assisted_flight = true; + if (!is_tailsitter()) { + // update tansition state for vehicles using airspeed wait + if (transition_state != TRANSITION_AIRSPEED_WAIT) { + gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed); + } + transition_state = TRANSITION_AIRSPEED_WAIT; + if (transition_start_ms == 0) { + transition_start_ms = now; + } + } } else { assisted_flight = false; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 35d25e6a4c..eec82784b3 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -155,6 +155,13 @@ public: 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;}; + private: AP_AHRS_NavEKF &ahrs; AP_Vehicle::MultiCopter aparm; @@ -179,7 +186,7 @@ private: AP_Int16 pilot_accel_z; // check for quadplane assistance needed - bool assistance_needed(float aspeed); + bool assistance_needed(float aspeed, bool have_airspeed); // update transition handling void update_transition(void); @@ -579,7 +586,10 @@ private: are we in any of the phases of a VTOL landing? */ bool in_vtol_land_sequence(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; + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,