mirror of https://github.com/ArduPilot/ardupilot
Plane: add AirMode RC option for quadplanes
add Q_OPTION for AirMode (auto-enabled if RCx_OPTION ARMDISARM assigned) bugfix: manual throttle mix for qacro qualify auto airmode on/off add Air Mode to Plane RC_OPTION metadata restrict airmode to manual throttle modes add qhover to manual throttle mix move air_mode from Plane to QuadPlane add Mode::is_vtol_man_throttle()
This commit is contained in:
parent
c722367c6c
commit
e22d9398d3
|
@ -179,6 +179,13 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
|
|||
return false;
|
||||
}
|
||||
|
||||
if ((method == Method::AUXSWITCH) && (plane.quadplane.options & QuadPlane::OPTION_AIRMODE)) {
|
||||
// if no airmode switch assigned, honour the QuadPlane option bit:
|
||||
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr) {
|
||||
plane.quadplane.air_mode = AirMode::ON;
|
||||
}
|
||||
}
|
||||
|
||||
change_arm_state();
|
||||
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed");
|
||||
|
@ -202,6 +209,11 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method)
|
|||
// suppress the throttle in auto-throttle modes
|
||||
plane.throttle_suppressed = plane.auto_throttle_mode;
|
||||
|
||||
// if no airmode switch assigned, ensure airmode is off:
|
||||
if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr) {
|
||||
plane.quadplane.air_mode = AirMode::OFF;
|
||||
}
|
||||
|
||||
//only log if disarming was successful
|
||||
change_arm_state();
|
||||
|
||||
|
|
|
@ -116,7 +116,6 @@ void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
|
|||
plane.g2.soaring_controller.set_pilot_desired_state(desired_state);
|
||||
#endif
|
||||
}
|
||||
|
||||
void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
|
||||
const RC_Channel::AuxSwitchPos ch_flag)
|
||||
{
|
||||
|
@ -136,6 +135,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option,
|
|||
|
||||
case AUX_FUNC::Q_ASSIST:
|
||||
case AUX_FUNC::SOARING:
|
||||
case AUX_FUNC::AIRMODE:
|
||||
do_aux_function(ch_option, ch_flag);
|
||||
break;
|
||||
|
||||
|
@ -244,6 +244,19 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
|
|||
do_aux_function_crow_mode(ch_flag);
|
||||
break;
|
||||
|
||||
case AUX_FUNC::AIRMODE:
|
||||
switch (ch_flag) {
|
||||
case AuxSwitchPos::HIGH:
|
||||
plane.quadplane.air_mode = AirMode::ON;
|
||||
break;
|
||||
case AuxSwitchPos::MIDDLE:
|
||||
break;
|
||||
case AuxSwitchPos::LOW:
|
||||
plane.quadplane.air_mode = AirMode::OFF;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
RC_Channel::do_aux_function(ch_option, ch_flag);
|
||||
break;
|
||||
|
|
|
@ -165,3 +165,9 @@ enum guided_heading_type_t {
|
|||
GUIDED_HEADING_COG, // maintain ground track
|
||||
GUIDED_HEADING_HEADING, // maintain a heading
|
||||
};
|
||||
|
||||
|
||||
enum class AirMode {
|
||||
OFF,
|
||||
ON,
|
||||
};
|
||||
|
|
|
@ -68,6 +68,7 @@ public:
|
|||
|
||||
// true for all q modes
|
||||
virtual bool is_vtol_mode() const { return false; }
|
||||
virtual bool is_vtol_man_throttle() const { return false; }
|
||||
|
||||
// true if mode can have terrain following disabled by switch
|
||||
virtual bool allows_terrain_disable() const { return false; }
|
||||
|
@ -344,6 +345,7 @@ public:
|
|||
const char *name4() const override { return "QSTB"; }
|
||||
|
||||
bool is_vtol_mode() const override { return true; }
|
||||
bool is_vtol_man_throttle() const override { return true; }
|
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override;
|
||||
|
@ -436,6 +438,7 @@ public:
|
|||
const char *name4() const override { return "QACRO"; }
|
||||
|
||||
bool is_vtol_mode() const override { return true; }
|
||||
bool is_vtol_man_throttle() const override { return true; }
|
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override;
|
||||
|
|
|
@ -328,8 +328,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
|
|||
|
||||
// @Param: OPTIONS
|
||||
// @DisplayName: quadplane options
|
||||
// @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight, and the vehicle will not use the vertical lift motors to climb during the transition. If AllowFWTakeoff bit is not set then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand bit is not set then fixed wing land on quadplanes will instead perform a VTOL land. If respect takeoff frame is not set the vehicle will interpret all takeoff waypoints as an altitude above the corrent position. When Use QRTL is set it will replace QLAND with QRTL for failsafe actions when in VTOL modes.
|
||||
// @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types,4:Use a fixed wing approach for VTOL landings,5:Use QRTL instead of QLAND for failsafe when in VTOL modes,6:Use idle governor in MANUAL,7:QAssist force enabled,8:Tailsitter QAssist motors only
|
||||
// @Description: This provides a set of additional control options for quadplanes. LevelTransition means that the wings should be held level to within LEVEL_ROLL_LIMIT degrees during transition to fixed wing flight, and the vehicle will not use the vertical lift motors to climb during the transition. If AllowFWTakeoff bit is not set then fixed wing takeoff on quadplanes will instead perform a VTOL takeoff. If AllowFWLand bit is not set then fixed wing land on quadplanes will instead perform a VTOL land. If respect takeoff frame is not set the vehicle will interpret all takeoff waypoints as an altitude above the current position. When Use QRTL is set it will replace QLAND with QRTL for failsafe actions when in VTOL modes. When AIRMODE is set AirMode is automatically enabled if arming by RC channel.
|
||||
// @Bitmask: 0:LevelTransition,1:AllowFWTakeoff,2:AllowFWLand,3:Respect takeoff frame types,4:Use a fixed wing approach for VTOL landings,5:Use QRTL instead of QLAND for failsafe when in VTOL modes,6:Use idle governor in MANUAL,7:QAssist force enabled,8:Tailsitter QAssist motors only,9:enable AirMode if arming by aux switch
|
||||
AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
|
||||
|
||||
AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
|
||||
|
@ -917,7 +917,7 @@ void QuadPlane::hold_stabilize(float throttle_in)
|
|||
// call attitude controller
|
||||
multicopter_attitude_rate_update(get_desired_yaw_rate_cds());
|
||||
|
||||
if (throttle_in <= 0) {
|
||||
if ((throttle_in <= 0) && (air_mode == AirMode::OFF)) {
|
||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
||||
attitude_control->set_throttle_out(0, true, 0);
|
||||
if (!is_tailsitter()) {
|
||||
|
@ -1920,6 +1920,11 @@ void QuadPlane::update_throttle_suppression(void)
|
|||
return;
|
||||
}
|
||||
|
||||
// if in a VTOL manual throttle mode and air_mode is on then allow motors to run
|
||||
if (plane.control_mode->is_vtol_man_throttle() && air_mode == AirMode::ON) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if we are in a fixed wing auto throttle mode and we have
|
||||
// unsuppressed the throttle then allow motors to run
|
||||
if (plane.auto_throttle_mode && !plane.throttle_suppressed) {
|
||||
|
@ -3291,9 +3296,9 @@ void QuadPlane::update_throttle_mix(void)
|
|||
return;
|
||||
}
|
||||
|
||||
if (plane.control_mode == &plane.mode_qstabilize) {
|
||||
if (plane.control_mode->is_vtol_man_throttle()) {
|
||||
// manual throttle
|
||||
if (plane.get_throttle_input() <= 0) {
|
||||
if ((plane.get_throttle_input() <= 0) && (air_mode == AirMode::OFF)) {
|
||||
attitude_control->set_throttle_mix_min();
|
||||
} else {
|
||||
attitude_control->set_throttle_mix_man();
|
||||
|
|
|
@ -11,6 +11,7 @@
|
|||
#include <AC_Avoidance/AC_Avoid.h>
|
||||
#include <AP_Proximity/AP_Proximity.h>
|
||||
#include "qautotune.h"
|
||||
#include "defines.h"
|
||||
|
||||
/*
|
||||
QuadPlane specific functionality
|
||||
|
@ -24,6 +25,8 @@ public:
|
|||
friend class AP_AdvancedFailsafe_Plane;
|
||||
friend class QAutoTune;
|
||||
friend class AP_Arming_Plane;
|
||||
friend class RC_Channel_Plane;
|
||||
friend class RC_Channel;
|
||||
|
||||
friend class Mode;
|
||||
friend class ModeAuto;
|
||||
|
@ -188,7 +191,10 @@ private:
|
|||
// vertical acceleration the pilot may request
|
||||
AP_Int16 pilot_accel_z;
|
||||
|
||||
// check for quadplane assistance needed
|
||||
// air mode state: OFF, ON
|
||||
AirMode air_mode;
|
||||
|
||||
// check for quadplane assistance needed
|
||||
bool assistance_needed(float aspeed, bool have_airspeed);
|
||||
|
||||
// check if it is safe to provide assistance
|
||||
|
@ -556,6 +562,7 @@ private:
|
|||
OPTION_IDLE_GOV_MANUAL=(1<<6),
|
||||
OPTION_Q_ASSIST_FORCE_ENABLE=(1<<7),
|
||||
OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
|
||||
OPTION_AIRMODE=(1<<9),
|
||||
};
|
||||
|
||||
AP_Float takeoff_failure_scalar;
|
||||
|
|
Loading…
Reference in New Issue