Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Quadplane: add option_is_set helper for Q_OPTIONS

This commit is contained in:
Iampete1 2022-08-05 16:08:59 +01:00 committed by Peter Barker
parent 1f819e11fb
commit ee778dbd3a
14 changed files with 59 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/AP_Arming.cpp
View File

@ -173,7 +173,7 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure)
ret = false;
}
if ((plane.quadplane.options & QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO) != 0) {
if (plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {
if (!plane.control_mode->is_vtol_mode() && (plane.control_mode != &plane.mode_auto) && (plane.control_mode != &plane.mode_guided)) {
check_failed(display_failure,"not in Q mode");
ret = false;

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -982,7 +982,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
const bool attempt_go_around =
(!plane.quadplane.available()) ||
((!plane.quadplane.in_vtol_auto()) &&
(!(plane.quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)));
!plane.quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH));
#else
const bool attempt_go_around = true;
#endif

2
ArduPlane/altitude.cpp
View File

@ -180,7 +180,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_land_descent() &&
!(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)) {
!quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) {
// when doing a VTOL landing we can use the waypoint height as
// ground height. We can't do this if using the
// LAND_FW_APPROACH as that uses the wp height as the approach

4
ArduPlane/commands_logic.cpp
View File

@ -98,7 +98,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
return quadplane.do_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
if (quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) {
if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) {
// the user wants to approach the landing in a fixed wing flight mode
// the waypoint will be used as a loiter_to_alt
// after which point the plane will compute the optimal into the wind direction
@ -286,7 +286,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) {
if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) && !verify_landing_vtol_approach(cmd)) {
// verify_landing_vtol_approach will return true once we have completed the approach,
// in which case we fall over to normal vtol landing code
return false;

8
ArduPlane/events.cpp
View File

@ -57,9 +57,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
case Mode::Number::QAUTOTUNE:
#endif
case Mode::Number::QACRO:
if (quadplane.options & QuadPlane::OPTION_FS_RTL) {
if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
set_mode(mode_rtl, reason);
} else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
} else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
set_mode(mode_qrtl, reason);
} else {
set_mode(mode_qland, reason);
@ -152,9 +152,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
#if QAUTOTUNE_ENABLED
case Mode::Number::QAUTOTUNE:
#endif
if (quadplane.options & QuadPlane::OPTION_FS_RTL) {
if (quadplane.option_is_set(QuadPlane::OPTION::FS_RTL)) {
set_mode(mode_rtl, reason);
} else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) {
} else if (quadplane.option_is_set(QuadPlane::OPTION::FS_QRTL)) {
set_mode(mode_qrtl, reason);
} else {
set_mode(mode_qland, reason);

2
ArduPlane/mode_qloiter.cpp
View File

@ -99,7 +99,7 @@ void ModeQLoiter::run()
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground);
if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && (quadplane.options & QuadPlane::OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
quadplane.ahrs.set_touchdown_expected(true);
}

2
ArduPlane/mode_qstabilize.cpp
View File

@ -27,7 +27,7 @@ void ModeQStabilize::update()
return;
}
if ((plane.quadplane.options & QuadPlane::OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES) == 0) {
if (!plane.quadplane.option_is_set(QuadPlane::OPTION::INGORE_FW_ANGLE_LIMITS_IN_Q_MODES)) {
// by default angles are also constrained by forward flight limits
set_limited_roll_pitch(roll_input, pitch_input);
} else {

2
ArduPlane/navigation.cpp
View File

@ -113,7 +113,7 @@ void Plane::navigate()
float Plane::mode_auto_target_airspeed_cm()
{
#if HAL_QUADPLANE_ENABLED
if ((quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
if (quadplane.option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
const float land_airspeed = TECS_controller.get_land_airspeed();

34
ArduPlane/quadplane.cpp
View File

@ -762,7 +762,7 @@ bool QuadPlane::setup(void)
hal.rcout->set_failsafe_pwm(mask, plane.quadplane.motors->get_pwm_output_min());
// default QAssist state as set with Q_OPTIONS
if ((options & OPTION_Q_ASSIST_FORCE_ENABLE) != 0) {
if (option_is_set(QuadPlane::OPTION::Q_ASSIST_FORCE_ENABLE)) {
q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_FORCE;
}
@ -1190,7 +1190,7 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground)
height_above_ground,
land_final_alt, land_final_alt+6);
if ((options & OPTION_THR_LANDING_CONTROL) != 0) {
if (option_is_set(QuadPlane::OPTION::THR_LANDING_CONTROL)) {
// allow throttle control for landing speed
const float thr_in = get_pilot_land_throttle();
if (thr_in > THR_CTRL_LAND_THRESH) {
@ -1409,7 +1409,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed)
// assistance due to Q_ASSIST_SPEED
// if option bit is enabled only allow assist with real airspeed sensor
if ((have_airspeed && aspeed < assist_speed) &&
(((options & OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST) == 0) || ahrs.airspeed_sensor_enabled())) {
(!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.airspeed_sensor_enabled())) {
in_angle_assist = false;
angle_error_start_ms = 0;
return true;
@ -1568,7 +1568,7 @@ void SLT_Transition::update()
// if option is set and ground speed> 1/2 arspd_fbw_min for non-tiltrotors, then complete transition, otherwise QLAND.
// tiltrotors will immediately transition
const bool tiltrotor_with_ground_speed = quadplane.tiltrotor.enabled() && (plane.ahrs.groundspeed() > plane.aparm.airspeed_min * 0.5);
if ((quadplane.options & QuadPlane::OPTION_TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) {
if (quadplane.option_is_set(QuadPlane::OPTION::TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) {
transition_state = TRANSITION_TIMER;
in_forced_transition = true;
} else {
@ -1604,7 +1604,7 @@ void SLT_Transition::update()
// otherwise the plane can end up in high-alpha flight with
// low VTOL thrust and may not complete a transition
float climb_rate_cms = quadplane.assist_climb_rate_cms();
if ((quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION) && !quadplane.tiltrotor.enabled()) {
if (quadplane.option_is_set(QuadPlane::OPTION::LEVEL_TRANSITION) && !quadplane.tiltrotor.enabled()) {
climb_rate_cms = MIN(climb_rate_cms, 0.0f);
}
quadplane.hold_hover(climb_rate_cms);
@ -1957,7 +1957,7 @@ void QuadPlane::motors_output(bool run_rate_controller)
1) for safety (OPTION_DELAY_ARMING)
2) to allow motors to return to vertical (OPTION_DISARMED_TILT)
*/
if ((options & OPTION_DISARMED_TILT) || (options & OPTION_DELAY_ARMING)) {
if (option_is_set(QuadPlane::OPTION::DISARMED_TILT) || option_is_set(QuadPlane::OPTION::DELAY_ARMING)) {
if (plane.arming.get_delay_arming()) {
// delay motor start after arming
set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
@ -2143,7 +2143,7 @@ bool QuadPlane::in_vtol_posvel_mode(void) const
*/
void QuadPlane::update_land_positioning(void)
{
if ((options & OPTION_REPOSITION_LANDING) == 0) {
if (!option_is_set(QuadPlane::OPTION::REPOSITION_LANDING)) {
// not enabled
poscontrol.pilot_correction_active = false;
poscontrol.target_vel_cms.zero();
@ -2195,7 +2195,7 @@ void QuadPlane::run_xy_controller(float accel_limit)
void QuadPlane::poscontrol_init_approach(void)
{
const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
if ((options & OPTION_DISABLE_APPROACH) != 0) {
if (option_is_set(QuadPlane::OPTION::DISABLE_APPROACH)) {
// go straight to QPOS_POSITION1
poscontrol.set_state(QPOS_POSITION1);
gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
@ -2801,7 +2801,7 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_LAND_FINAL: {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
if (poscontrol.get_state() == QPOS_LAND_FINAL) {
if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
if (!option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
ahrs.set_touchdown_expected(true);
}
}
@ -3070,7 +3070,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
loc.lat = 0;
loc.lng = 0;
plane.set_next_WP(loc);
if (options & OPTION_RESPECT_TAKEOFF_FRAME) {
if (option_is_set(QuadPlane::OPTION::RESPECT_TAKEOFF_FRAME)) {
if (plane.current_loc.alt >= plane.next_WP_loc.alt) {
// we are above the takeoff already, no need to do anything
return false;
@ -3164,7 +3164,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
}
if (now - takeoff_start_time_ms < 3000 &&
(options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
!option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
ahrs.set_takeoff_expected(true);
}
@ -3657,7 +3657,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
guided_start();
guided_takeoff = true;
guided_wait_takeoff = false;
if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
if (!option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) {
ahrs.set_takeoff_expected(true);
}
return true;
@ -3691,7 +3691,7 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const
if (id == MAV_CMD_NAV_VTOL_TAKEOFF) {
return true;
}
if (id == MAV_CMD_NAV_TAKEOFF && available() && (options & OPTION_ALLOW_FW_TAKEOFF) == 0) {
if (id == MAV_CMD_NAV_TAKEOFF && available() && !option_is_set(QuadPlane::OPTION::ALLOW_FW_TAKEOFF)) {
// treat fixed wing takeoff as VTOL takeoff
return true;
}
@ -3704,13 +3704,13 @@ bool QuadPlane::is_vtol_takeoff(uint16_t id) const
bool QuadPlane::is_vtol_land(uint16_t id) const
{
if (id == MAV_CMD_NAV_VTOL_LAND) {
if (options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) {
if (option_is_set(QuadPlane::OPTION::MISSION_LAND_FW_APPROACH)) {
return plane.vtol_approach_s.approach_stage == Plane::Landing_ApproachStage::VTOL_LANDING;
} else {
return true;
}
}
if (id == MAV_CMD_NAV_LAND && available() && (options & OPTION_ALLOW_FW_LAND) == 0) {
if (id == MAV_CMD_NAV_LAND && available() && !option_is_set(QuadPlane::OPTION::ALLOW_FW_LAND)) {
// treat fixed wing land as VTOL land
return true;
}
@ -4075,7 +4075,7 @@ QuadPlane *QuadPlane::_singleton = nullptr;
bool SLT_Transition::set_FW_roll_limit(int32_t& roll_limit_cd)
{
if (quadplane.assisted_flight && (transition_state == TRANSITION_AIRSPEED_WAIT || transition_state == TRANSITION_TIMER) &&
(quadplane.options & QuadPlane::OPTION_LEVEL_TRANSITION)) {
quadplane.option_is_set(QuadPlane::OPTION::LEVEL_TRANSITION)) {
// the user wants transitions to be kept level to within LEVEL_ROLL_LIMIT
roll_limit_cd = MIN(roll_limit_cd, plane.g.level_roll_limit*100);
return true;
@ -4290,7 +4290,7 @@ bool QuadPlane::allow_servo_auto_trim()
// In forward flight and VTOL motors not active
return true;
}
if (tailsitter.enabled() && ((options & QuadPlane::OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0)) {
if (tailsitter.enabled() && option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) {
// Tailsitter in forward flight, motors providing active stabalisation with motors only option
// Control surfaces are running as normal with I term active, motor I term is zeroed
return true;

47
ArduPlane/quadplane.h
View File

@ -535,29 +535,32 @@ private:
// additional options
AP_Int32 options;
enum {
OPTION_LEVEL_TRANSITION=(1<<0),
OPTION_ALLOW_FW_TAKEOFF=(1<<1),
OPTION_ALLOW_FW_LAND=(1<<2),
OPTION_RESPECT_TAKEOFF_FRAME=(1<<3),
OPTION_MISSION_LAND_FW_APPROACH=(1<<4),
OPTION_FS_QRTL=(1<<5),
OPTION_IDLE_GOV_MANUAL=(1<<6),
OPTION_Q_ASSIST_FORCE_ENABLE=(1<<7),
OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
OPTION_AIRMODE_UNUSED=(1<<9),
OPTION_DISARMED_TILT=(1<<10),
OPTION_DELAY_ARMING=(1<<11),
OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12),
OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13),
OPTION_INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14),
OPTION_THR_LANDING_CONTROL=(1<<15),
OPTION_DISABLE_APPROACH=(1<<16),
OPTION_REPOSITION_LANDING=(1<<17),
OPTION_ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18),
OPTION_TRANS_FAIL_TO_FW=(1<<19),
OPTION_FS_RTL=(1<<20),
enum class OPTION {
LEVEL_TRANSITION=(1<<0),
ALLOW_FW_TAKEOFF=(1<<1),
ALLOW_FW_LAND=(1<<2),
RESPECT_TAKEOFF_FRAME=(1<<3),
MISSION_LAND_FW_APPROACH=(1<<4),
FS_QRTL=(1<<5),
IDLE_GOV_MANUAL=(1<<6),
Q_ASSIST_FORCE_ENABLE=(1<<7),
TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
AIRMODE_UNUSED=(1<<9),
DISARMED_TILT=(1<<10),
DELAY_ARMING=(1<<11),
DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12),
DISABLE_GROUND_EFFECT_COMP=(1<<13),
INGORE_FW_ANGLE_LIMITS_IN_Q_MODES=(1<<14),
THR_LANDING_CONTROL=(1<<15),
DISABLE_APPROACH=(1<<16),
REPOSITION_LANDING=(1<<17),
ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18),
TRANS_FAIL_TO_FW=(1<<19),
FS_RTL=(1<<20),
};
bool option_is_set(OPTION option) const {
return (options.get() & int32_t(option)) != 0;
}
AP_Float takeoff_failure_scalar;
AP_Float maximum_takeoff_airspeed;

2
ArduPlane/servos.cpp
View File

@ -388,7 +388,7 @@ void Plane::set_servos_manual_passthrough(void)
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() && (quadplane.options & QuadPlane::OPTION_IDLE_GOV_MANUAL)) {
if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) {
// for quadplanes it can be useful to run the idle governor in MANUAL mode
// as it prevents the VTOL motors from running
int8_t min_throttle = aparm.throttle_min.get();

2
ArduPlane/system.cpp
View File

@ -36,7 +36,7 @@ void Plane::init_ardupilot()
// initialise rc channels including setting mode
#if HAL_QUADPLANE_ENABLED
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, (quadplane.enabled() && (quadplane.options & QuadPlane::OPTION_AIRMODE_UNUSED) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) ? RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE : RC_Channel::AUX_FUNC::ARMDISARM);
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, (quadplane.enabled() && quadplane.option_is_set(QuadPlane::OPTION::AIRMODE_UNUSED) && (rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRMODE) == nullptr)) ? RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE : RC_Channel::AUX_FUNC::ARMDISARM);
#else
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
#endif

4
ArduPlane/tailsitter.cpp
View File

@ -236,7 +236,7 @@ void Tailsitter::setup()
// Do not allow arming in forward flight modes
// motors will become active due to assisted flight airmode, the vehicle will try very hard to get level
quadplane.options.set(quadplane.options.get() | QuadPlane::OPTION_ONLY_ARM_IN_QMODE_OR_AUTO);
quadplane.options.set(quadplane.options.get() | int32_t(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO));
}
transition = new Tailsitter_Transition(quadplane, motors, *this);
@ -361,7 +361,7 @@ void Tailsitter::output(void)
quadplane.hold_stabilize(throttle);
quadplane.motors_output(true);
if ((quadplane.options & QuadPlane::OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY) != 0) {
if (quadplane.option_is_set(QuadPlane::OPTION::TAILSIT_Q_ASSIST_MOTORS_ONLY)) {
// only use motors for Q assist, control surfaces remain under plane control
// zero copter I terms and use plane
quadplane.attitude_control->reset_rate_controller_I_terms();

2
ArduPlane/tiltrotor.cpp
View File

@ -508,7 +508,7 @@ void Tiltrotor::vectoring(void)
// Wait TILT_DELAY_MS after disarming to allow props to spin down first.
constexpr uint32_t TILT_DELAY_MS = 3000;
uint32_t now = AP_HAL::millis();
if (!hal.util->get_soft_armed() && (plane.quadplane.options & QuadPlane::OPTION_DISARMED_TILT)) {
if (!hal.util->get_soft_armed() && plane.quadplane.option_is_set(QuadPlane::OPTION::DISARMED_TILT)) {
// this test is subject to wrapping at ~49 days, but the consequences are insignificant
if ((now - hal.util->get_last_armed_change()) > TILT_DELAY_MS) {
if (quadplane.in_vtol_mode()) {