Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 07:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: add Qassist switch

Browse Source
This commit is contained in:
Peter Hall 2020-01-02 19:06:39 +00:00 committed by Andrew Tridgell
parent bfd71fdae1
commit 2467d7b5c7
4 changed files with 61 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduPlane/RC_Channel.cpp
View File

@ -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;

2
ArduPlane/RC_Channel.h
View File

@ -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

36
ArduPlane/quadplane.cpp
View File

@ -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;
}

14
ArduPlane/quadplane.h
View File

@ -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,