Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: QuadPlane: move assistane into its own class

Browse Source
This commit is contained in:
Iampete1 2024-02-24 23:18:19 +00:00 committed by Andrew Tridgell
parent 8125f47a15
commit 39c86a46e0
5 changed files with 80 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/AP_Arming.cpp
View File

@ -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 Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
*/ */
if (check_enabled(ARMING_CHECK_PARAMETERS) && if (check_enabled(ARMING_CHECK_PARAMETERS) &&
is_zero(plane.quadplane.assist_speed) && is_zero(plane.quadplane.assist.speed) &&
!plane.quadplane.tailsitter.enabled()) { !plane.quadplane.tailsitter.enabled()) {
check_failed(display_failure,"Q_ASSIST_SPEED is not set"); check_failed(display_failure,"Q_ASSIST_SPEED is not set");
ret = false; ret = false;

12
ArduPlane/RC_Channel.cpp
View File

@ -61,17 +61,17 @@ void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
switch(ch_flag) { switch(ch_flag) {
case AuxSwitchPos::HIGH: case AuxSwitchPos::HIGH:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled"); 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(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
break; break;
case AuxSwitchPos::MIDDLE: case AuxSwitchPos::MIDDLE:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled"); 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(QuadPlane::VTOL_Assist::STATE::ASSIST_ENABLED);
break; break;
case AuxSwitchPos::LOW: case AuxSwitchPos::LOW:
gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled"); 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(QuadPlane::VTOL_Assist::STATE::ASSIST_DISABLED);
break; break;
} }
} }
@ -122,18 +122,18 @@ void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
case AuxSwitchPos::HIGH: case AuxSwitchPos::HIGH:
plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET; plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET;
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED); plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_DISABLED);
#endif #endif
break; break;
case AuxSwitchPos::MIDDLE: case AuxSwitchPos::MIDDLE:
plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET; plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET;
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED); plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_DISABLED);
#endif #endif
break; break;
case AuxSwitchPos::LOW: case AuxSwitchPos::LOW:
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED); plane.quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::ASSIST_ENABLED);
#endif #endif
plane.flare_mode = Plane::FlareMode::FLARE_DISABLED; plane.flare_mode = Plane::FlareMode::FLARE_DISABLED;
break; break;

54
ArduPlane/quadplane.cpp
View File

@ -108,7 +108,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Range: 0 100 // @Range: 0 100
// @Increment: 0.1 // @Increment: 0.1
// @User: Standard // @User: Standard
AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist_speed, 0), AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist.speed, 0),
// @Param: YAW_RATE_MAX // @Param: YAW_RATE_MAX
// @DisplayName: Maximum yaw rate // @DisplayName: Maximum yaw rate
@ -236,7 +236,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Range: 0 90 // @Range: 0 90
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist_angle, 30), AP_GROUPINFO("ASSIST_ANGLE", 45, QuadPlane, assist.angle, 30),
// 47: TILT_TYPE // 47: TILT_TYPE
// 48: TAILSIT_ANGLE // 48: TAILSIT_ANGLE
@ -405,7 +405,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @Range: 0 120 // @Range: 0 120
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0), AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist.alt, 0),
// 17: TAILSIT_GSCMSK // 17: TAILSIT_GSCMSK
// 18: TAILSIT_GSCMIN // 18: TAILSIT_GSCMIN
@ -417,7 +417,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @Range: 0 2 // @Range: 0 2
// @Increment: 0.1 // @Increment: 0.1
// @User: Standard // @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 // @Param: FWD_MANTHR_MAX
// @DisplayName: VTOL manual forward throttle max percent // @DisplayName: VTOL manual forward throttle max percent
@ -825,7 +825,7 @@ bool QuadPlane::setup(void)
// default QAssist state as set with Q_OPTIONS // default QAssist state as set with Q_OPTIONS
if (option_is_set(QuadPlane::OPTION::Q_ASSIST_FORCE_ENABLE)) { 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(); setup_defaults();
@ -1454,16 +1454,16 @@ float QuadPlane::desired_auto_yaw_rate_cds(void) const
/* /*
return true if the quadplane should provide stability assistance return true if the quadplane should provide stability assistance
*/ */
bool QuadPlane::should_assist(float aspeed, bool have_airspeed) bool QuadPlane::VTOL_Assist::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()) { 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 // disarmed or disabled by aux switch or because a control surface tailsitter
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
return false; return false;
} }
if (!tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed) if (!quadplane.tailsitter.enabled() && !( (plane.control_mode->does_auto_throttle() && !plane.throttle_suppressed)
|| is_positive(plane.get_throttle_input()) || is_positive(plane.get_throttle_input())
|| plane.is_flying() ) ) { || plane.is_flying() ) ) {
// not in a flight mode and condition where it would be safe to turn on vertial lift motors // not in a flight mode and condition where it would be safe to turn on vertial lift motors
@ -1473,14 +1473,14 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
return false; return false;
} }
if (q_assist_state == Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE) { if (state == STATE::FORCE_ENABLED) {
// force enabled, no need to check thresholds // force enabled, no need to check thresholds
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
return true; return true;
} }
if (assist_speed <= 0) { if (speed <= 0) {
// disabled via speed threshold // disabled via speed threshold
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
@ -1489,8 +1489,8 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
// assistance due to Q_ASSIST_SPEED // assistance due to Q_ASSIST_SPEED
// if option bit is enabled only allow assist with real airspeed sensor // if option bit is enabled only allow assist with real airspeed sensor
if ((have_airspeed && aspeed < assist_speed) && if ((have_airspeed && aspeed < speed) &&
(!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.using_airspeed_sensor())) { (!quadplane.option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || plane.ahrs.using_airspeed_sensor())) {
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
return true; return true;
@ -1501,13 +1501,13 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
/* /*
optional assistance when altitude is too close to the ground optional assistance when altitude is too close to the ground
*/ */
if (assist_alt > 0) { if (alt > 0) {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (height_above_ground < assist_alt) { if (height_above_ground < alt) {
if (alt_error_start_ms == 0) { if (alt_error_start_ms == 0) {
alt_error_start_ms = now; alt_error_start_ms = now;
} }
if (now - alt_error_start_ms > assist_delay*1000) { if (now - alt_error_start_ms > delay*1000) {
// we've been below assistant alt for Q_ASSIST_DELAY seconds // we've been below assistant alt for Q_ASSIST_DELAY seconds
if (!in_alt_assist) { if (!in_alt_assist) {
in_alt_assist = true; in_alt_assist = true;
@ -1521,7 +1521,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
} }
} }
if (assist_angle <= 0) { if (angle <= 0) {
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
return false; return false;
@ -1532,18 +1532,18 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
*/ */
const uint16_t allowed_envelope_error_cd = 500U; const uint16_t allowed_envelope_error_cd = 500U;
if (labs(ahrs.roll_sensor) <= plane.aparm.roll_limit*100 + allowed_envelope_error_cd && if (labs(plane.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 && plane.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)) { plane.ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min*100)) {
// we are inside allowed attitude envelope // we are inside allowed attitude envelope
in_angle_assist = false; in_angle_assist = false;
angle_error_start_ms = 0; angle_error_start_ms = 0;
return false; return false;
} }
int32_t max_angle_cd = 100U*assist_angle; int32_t max_angle_cd = 100U*angle;
if ((labs(ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd && if ((labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) < max_angle_cd &&
labs(ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd)) { labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) < max_angle_cd)) {
// not beyond angle error // not beyond angle error
angle_error_start_ms = 0; angle_error_start_ms = 0;
in_angle_assist = false; in_angle_assist = false;
@ -1553,12 +1553,12 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
if (angle_error_start_ms == 0) { if (angle_error_start_ms == 0) {
angle_error_start_ms = now; angle_error_start_ms = now;
} }
bool ret = (now - angle_error_start_ms) >= assist_delay*1000; bool ret = (now - angle_error_start_ms) >= delay*1000;
if (ret && !in_angle_assist) { if (ret && !in_angle_assist) {
in_angle_assist = true; in_angle_assist = true;
gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d", gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
(int)(ahrs.roll_sensor/100), (int)(plane.ahrs.roll_sensor/100),
(int)(ahrs.pitch_sensor/100)); (int)(plane.ahrs.pitch_sensor/100));
} }
return ret; return ret;
} }
@ -1581,7 +1581,7 @@ void SLT_Transition::update()
/* /*
see if we should provide some assistance 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 // the quad should provide some assistance to the plane
quadplane.assisted_flight = true; quadplane.assisted_flight = true;
// update transition state for vehicles using airspeed wait // update transition state for vehicles using airspeed wait
@ -2468,7 +2468,7 @@ void QuadPlane::vtol_position_controller(void)
} }
// speed for crossover to POSITION1 controller // 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 // run fixed wing navigation
plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc); plane.nav_controller->update_waypoint(plane.auto_state.crosstrack ? plane.prev_WP_loc : plane.current_loc, loc);

59
ArduPlane/quadplane.h
View File

@ -162,13 +162,6 @@ public:
MAV_TYPE get_mav_type(void) const; 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) // called when we change mode (for any mode, not just Q modes)
void mode_enter(void); void mode_enter(void);
@ -232,9 +225,6 @@ private:
// return true if airmode should be active // return true if airmode should be active
bool air_mode_active() const; bool air_mode_active() const;
// check for quadplane assistance needed
bool should_assist(float aspeed, bool have_airspeed);
// check for an EKF yaw reset // check for an EKF yaw reset
void check_yaw_reset(void); void check_yaw_reset(void);
@ -336,19 +326,52 @@ private:
AP_Int16 rc_speed; 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);
// speed below which quad assistance is given // speed below which quad assistance is given
AP_Float assist_speed; AP_Float speed;
// angular error at which quad assistance is given // angular error at which quad assistance is given
AP_Int8 assist_angle; AP_Int8 angle;
uint32_t angle_error_start_ms;
AP_Float assist_delay;
// altitude to trigger assistance // altitude to trigger assistance
AP_Int16 assist_alt; 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; }
private:
// Default to enabled
STATE state = STATE::ASSIST_ENABLED;
// true when in angle assist
uint32_t angle_error_start_ms;
bool in_angle_assist;
uint32_t alt_error_start_ms; uint32_t alt_error_start_ms;
bool in_alt_assist; bool in_alt_assist;
// Reference to access quadplane
QuadPlane& quadplane;
} assist {*this};
// landing speed in m/s // landing speed in m/s
AP_Float land_final_speed; AP_Float land_final_speed;
@ -459,9 +482,6 @@ private:
// true when quad is assisting a fixed wing mode // true when quad is assisting a fixed wing mode
bool assisted_flight:1; bool assisted_flight:1;
// true when in angle assist
bool in_angle_assist:1;
// are we in a guided takeoff? // are we in a guided takeoff?
bool guided_takeoff:1; bool guided_takeoff:1;
@ -685,9 +705,6 @@ private:
// returns true if the vehicle should currently be doing a spiral landing // returns true if the vehicle should currently be doing a spiral landing
bool landing_with_fixed_wing_spiral_approach(void) const; 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 return true if we should use the fixed wing attitude control loop
*/ */

6
ArduPlane/tailsitter.cpp
View File

@ -233,7 +233,7 @@ void Tailsitter::setup()
// Setup for control surface less operation // Setup for control surface less operation
if (enable == 2) { if (enable == 2) {
quadplane.q_assist_state = QuadPlane::Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE; quadplane.assist.set_state(QuadPlane::VTOL_Assist::STATE::FORCE_ENABLED);
quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY; quadplane.air_mode = AirMode::ASSISTED_FLIGHT_ONLY;
// Do not allow arming in forward flight modes // Do not allow arming in forward flight modes
@ -819,7 +819,7 @@ void Tailsitter_Transition::update()
float aspeed; float aspeed;
bool have_airspeed = quadplane.ahrs.airspeed_estimate(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) { if (transition_state < TRANSITION_DONE) {
// during transition we ask TECS to use a synthetic // during transition we ask TECS to use a synthetic
@ -885,7 +885,7 @@ void Tailsitter_Transition::VTOL_update()
float aspeed; float aspeed;
bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed); bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
// provide assistance in forward flight portion of tailsitter transition // 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()) { if (!quadplane.tailsitter.transition_vtol_complete()) {
return; return;
} }