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

Plane: massive refactor and creation of Mode class

This commit is contained in:
Tom Pittenger 2019-01-15 10:46:13 -07:00 committed by Andrew Tridgell
parent 49efe539fc
commit 0270c57530
55 changed files with 1688 additions and 996 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/APM_Config.h.reference
View File

@ -291,12 +291,12 @@
//
//#define FLIGHT_MODE_CHANNEL 8
//
//#define FLIGHT_MODE_1 RTL
//#define FLIGHT_MODE_2 RTL
//#define FLIGHT_MODE_3 STABILIZE
//#define FLIGHT_MODE_4 STABILIZE
//#define FLIGHT_MODE_5 MANUAL
//#define FLIGHT_MODE_6 MANUAL
//#define FLIGHT_MODE_1 Mode::Number::RTL
//#define FLIGHT_MODE_2 Mode::Number::RTL
//#define FLIGHT_MODE_3 Mode::Number::STABILIZE
//#define FLIGHT_MODE_4 Mode::Number::STABILIZE
//#define FLIGHT_MODE_5 Mode::Number::MANUAL
//#define FLIGHT_MODE_6 Mode::Number::MANUAL
//
//////////////////////////////////////////////////////////////////////////////

2
ArduPlane/AP_Arming.cpp
View File

@ -76,7 +76,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
}
}
if (plane.control_mode == AUTO && plane.mission.num_commands() <= 1) {
if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
check_failed(ARMING_CHECK_NONE, display_failure, "No mission loaded");
ret = false;
}

311
ArduPlane/ArduPlane.cpp
View File

@ -36,7 +36,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(read_radio, 50, 100),
SCHED_TASK(check_short_failsafe, 50, 100),
SCHED_TASK(update_speed_height, 50, 200),
SCHED_TASK(update_flight_mode, 400, 100),
SCHED_TASK(update_control_mode, 400, 100),
SCHED_TASK(stabilize, 400, 100),
SCHED_TASK(set_servos, 400, 100),
SCHED_TASK(update_throttle_hover, 100, 90),
@ -415,65 +415,16 @@ void Plane::update_GPS_10Hz(void)
}
/*
main handling for AUTO mode
main control mode dependent update code
*/
void Plane::handle_auto_mode(void)
void Plane::update_control_mode(void)
{
uint16_t nav_cmd_id;
if (mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
set_mode(RTL, MODE_REASON_MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
Mode *effective_mode = control_mode;
if (control_mode == &mode_auto && g.auto_fbw_steer == 42) {
effective_mode = &mode_fbwa;
}
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
takeoff_calc_roll();
takeoff_calc_pitch();
calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
calc_nav_roll();
calc_nav_pitch();
// allow landing to restrict the roll limits
nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL);
if (landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
steer_state.hold_course_cd = -1;
}
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
}
}
/*
main flight mode dependent update code
*/
void Plane::update_flight_mode(void)
{
enum FlightMode effective_mode = control_mode;
if (control_mode == AUTO && g.auto_fbw_steer == 42) {
effective_mode = FLY_BY_WIRE_A;
}
if (effective_mode != AUTO) {
if (effective_mode != &mode_auto) {
// hold_course is only used in takeoff and landing
steer_state.hold_course_cd = -1;
}
@ -490,199 +441,7 @@ void Plane::update_flight_mode(void)
ahrs.set_fly_forward(true);
}
switch (effective_mode)
{
case AUTO:
handle_auto_mode();
break;
case AVOID_ADSB:
case GUIDED:
if (auto_state.vtol_loiter && quadplane.available()) {
quadplane.guided_update();
break;
}
FALLTHROUGH;
case RTL:
case LOITER:
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case TRAINING: {
training_manual_roll = false;
training_manual_pitch = false;
update_load_factor();
// if the roll is past the set roll limit, then
// we set target roll to the limit
if (ahrs.roll_sensor >= roll_limit_cd) {
nav_roll_cd = roll_limit_cd;
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
nav_roll_cd = -roll_limit_cd;
} else {
training_manual_roll = true;
nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
nav_pitch_cd = pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
}
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
break;
}
case ACRO: {
// handle locked/unlocked control
if (acro_state.locked_roll) {
nav_roll_cd = acro_state.locked_roll_err;
} else {
nav_roll_cd = ahrs.roll_sensor;
}
if (acro_state.locked_pitch) {
nav_pitch_cd = acro_state.locked_pitch_cd;
} else {
nav_pitch_cd = ahrs.pitch_sensor;
}
break;
}
case AUTOTUNE:
case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
update_load_factor();
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
}
adjust_nav_pitch_throttle();
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
if (fly_inverted()) {
nav_pitch_cd = -nav_pitch_cd;
}
if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// FBWA failsafe glide
nav_roll_cd = 0;
nav_pitch_cd = 0;
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
if (g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
bool tdrag_mode = (RC_Channels::get_radio_in(g.fbwa_tdrag_chan-1) > 1700);
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
auto_state.fbwa_tdrag_takeoff_mode = true;
gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
}
}
}
break;
}
case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code!
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
update_load_factor();
update_fbwb_speed_height();
break;
case CRUISE:
/*
in CRUISE mode we use the navigation code to control
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if (channel_roll->get_control_in() != 0 || channel_rudder->get_control_in() != 0) {
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
}
if (!cruise_state.locked_heading) {
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
update_load_factor();
} else {
calc_nav_roll();
}
update_fbwb_speed_height();
break;
case STABILIZE:
nav_roll_cd = 0;
nav_pitch_cd = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we
// switched into the mode
nav_roll_cd = roll_limit_cd / 3;
update_load_factor();
calc_nav_pitch();
calc_throttle();
break;
case MANUAL:
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz());
steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
case QAUTOTUNE: {
// set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max);
float pitch_input = channel_pitch->norm_input();
// Scale from normalized input [-1,1] to centidegrees
if (quadplane.tailsitter_active()) {
// separate limit for tailsitter roll, if set
if (quadplane.tailsitter.max_roll_angle > 0) {
roll_limit = quadplane.tailsitter.max_roll_angle * 100.0f;
}
// angle max for tailsitter pitch
nav_pitch_cd = pitch_input * quadplane.aparm.angle_max;
} else {
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
// tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max);
} else {
nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}
nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
break;
}
case QACRO:
case INITIALISING:
// handled elsewhere
break;
}
effective_mode->update();
}
void Plane::update_navigation()
@ -696,14 +455,14 @@ void Plane::update_navigation()
qrtl_radius = abs(aparm.loiter_radius);
}
switch(control_mode) {
case AUTO:
switch (control_mode->mode_number()) {
case Mode::Number::AUTO:
if (ahrs.home_is_set()) {
mission.update();
}
break;
case RTL:
case Mode::Number::RTL:
if (quadplane.available() && quadplane.rtl_mode == 1 &&
(nav_controller->reached_loiter_target() ||
location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
@ -714,7 +473,7 @@ void Plane::update_navigation()
are within the maximum of the stopping distance and the
RTL_RADIUS
*/
set_mode(QRTL, MODE_REASON_UNKNOWN);
set_mode(mode_qrtl, MODE_REASON_UNKNOWN);
break;
} else if (g.rtl_autoland == 1 &&
!auto_state.checked_for_autoland &&
@ -723,7 +482,7 @@ void Plane::update_navigation()
// we've reached the RTL point, see if we have a landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
set_mode(AUTO, MODE_REASON_UNKNOWN);
set_mode(mode_auto, MODE_REASON_UNKNOWN);
}
// prevent running the expensive jump_to_landing_sequence
@ -735,7 +494,7 @@ void Plane::update_navigation()
// Go directly to the landing sequence
if (mission.jump_to_landing_sequence()) {
// switch from RTL -> AUTO
set_mode(AUTO, MODE_REASON_UNKNOWN);
set_mode(mode_auto, MODE_REASON_UNKNOWN);
}
// prevent running the expensive jump_to_landing_sequence
@ -749,32 +508,32 @@ void Plane::update_navigation()
// fall through to LOITER
FALLTHROUGH;
case LOITER:
case AVOID_ADSB:
case GUIDED:
case Mode::Number::LOITER:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
update_loiter(radius);
break;
case CRUISE:
case Mode::Number::CRUISE:
update_cruise();
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case INITIALISING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
case QAUTOTUNE:
case QACRO:
case Mode::Number::MANUAL:
case Mode::Number::STABILIZE:
case Mode::Number::TRAINING:
case Mode::Number::INITIALISING:
case Mode::Number::ACRO:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CIRCLE:
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::QAUTOTUNE:
case Mode::Number::QACRO:
// nothing to do
break;
}
@ -855,7 +614,7 @@ void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (control_mode == &mode_auto) {
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {

92
ArduPlane/Attitude.cpp
View File

@ -89,7 +89,7 @@ void Plane::stabilize_roll(float speed_scaler)
}
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) {
if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
@ -113,7 +113,7 @@ void Plane::stabilize_pitch(float speed_scaler)
}
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) {
if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
@ -145,19 +145,19 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
void Plane::stabilize_stick_mixing_direct()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == QACRO ||
control_mode == TRAINING ||
control_mode == QAUTOTUNE) {
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
control_mode == &mode_training ||
control_mode == &mode_qautotune) {
return;
}
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@ -176,20 +176,20 @@ void Plane::stabilize_stick_mixing_direct()
void Plane::stabilize_stick_mixing_fbw()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == QACRO ||
control_mode == TRAINING ||
control_mode == QAUTOTUNE ||
(control_mode == AUTO && g.auto_fbw_steer == 42)) {
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
control_mode == &mode_training ||
control_mode == &mode_qautotune ||
(control_mode == &mode_auto && g.auto_fbw_steer == 42)) {
return;
}
// do FBW style stick mixing. We don't treat it linearly
@ -374,7 +374,7 @@ void Plane::stabilize_acro(float speed_scaler)
*/
void Plane::stabilize()
{
if (control_mode == MANUAL) {
if (control_mode == &mode_manual) {
// nothing to do
return;
}
@ -389,26 +389,26 @@ void Plane::stabilize()
nav_roll_cd = 0;
}
if (control_mode == TRAINING) {
if (control_mode == &mode_training) {
stabilize_training(speed_scaler);
} else if (control_mode == ACRO) {
} else if (control_mode == &mode_acro) {
stabilize_acro(speed_scaler);
} else if ((control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == QACRO ||
control_mode == QAUTOTUNE) &&
} else if ((control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
control_mode == &mode_qautotune) &&
!quadplane.in_tailsitter_vtol_transition()) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) {
stabilize_stick_mixing_fbw();
}
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) {
if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == &mode_stabilize) {
stabilize_stick_mixing_direct();
}
stabilize_yaw(speed_scaler);
@ -449,7 +449,7 @@ void Plane::calc_throttle()
int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand();
// Received an external msg that guides throttle in the last 3 seconds?
if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
commanded_throttle = plane.guided_state.forced_throttle;
@ -473,12 +473,12 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
int16_t commanded_rudder;
// Received an external msg that guides yaw in the last 3 seconds?
if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
plane.guided_state.last_forced_rpy_ms.z > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
commanded_rudder = plane.guided_state.forced_rpy_cd.z;
} else {
if (control_mode == STABILIZE && rudder_in != 0) {
if (control_mode == &mode_stabilize && rudder_in != 0) {
disable_integrator = true;
}
@ -561,7 +561,7 @@ void Plane::calc_nav_pitch()
int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand();
// Received an external msg that guides roll in the last 3 seconds?
if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
commanded_pitch = plane.guided_state.forced_rpy_cd.y;
@ -579,7 +579,7 @@ void Plane::calc_nav_roll()
int32_t commanded_roll = nav_controller->nav_roll_cd();
// Received an external msg that guides roll in the last 3 seconds?
if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
commanded_roll = plane.guided_state.forced_rpy_cd.x;

119
ArduPlane/GCS_Mavlink.cpp
View File

@ -19,39 +19,39 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
switch (plane.control_mode) {
case MANUAL:
case TRAINING:
case ACRO:
case QACRO:
switch (plane.control_mode->mode_number()) {
case Mode::Number::MANUAL:
case Mode::Number::TRAINING:
case Mode::Number::ACRO:
case Mode::Number::QACRO:
_base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case CRUISE:
case QAUTOTUNE:
case Mode::Number::STABILIZE:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::CRUISE:
case Mode::Number::QAUTOTUNE:
_base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case AUTO:
case RTL:
case LOITER:
case AVOID_ADSB:
case GUIDED:
case CIRCLE:
case QRTL:
case Mode::Number::AUTO:
case Mode::Number::RTL:
case Mode::Number::LOITER:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::QRTL:
_base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
MAV_MODE_FLAG_STABILIZE_ENABLED;
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
// APM does in any mode, as that is defined as "system finds its own goal
// positions", which APM does not currently do
break;
case INITIALISING:
case Mode::Number::INITIALISING:
break;
}
@ -59,12 +59,12 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
_base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (plane.control_mode != MANUAL && plane.control_mode != INITIALISING) {
if (plane.control_mode != &plane.mode_manual && plane.control_mode != &plane.mode_initializing) {
// stabiliser of some form is enabled
_base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != INITIALISING) {
if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != &plane.mode_initializing) {
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
@ -77,7 +77,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
#endif
// we are armed if we are not initialising
if (plane.control_mode != INITIALISING && plane.arming.is_armed()) {
if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) {
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@ -89,12 +89,12 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const
uint32_t GCS_Plane::custom_mode() const
{
return plane.control_mode;
return plane.control_mode->mode_number();
}
MAV_STATE GCS_MAVLINK_Plane::system_status() const
{
if (plane.control_mode == INITIALISING) {
if (plane.control_mode == &plane.mode_initializing) {
return MAV_STATE_CALIBRATING;
}
if (plane.any_failsafe_triggered()) {
@ -156,7 +156,7 @@ void Plane::send_fence_status(mavlink_channel_t chan)
void GCS_MAVLINK_Plane::send_nav_controller_output() const
{
if (plane.control_mode == MANUAL) {
if (plane.control_mode == &plane.mode_manual) {
return;
}
const QuadPlane &quadplane = plane.quadplane;
@ -171,7 +171,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
targets.z * 1.0e-2f,
wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0,
(plane.control_mode != QSTABILIZE) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
plane.airspeed_error * 100,
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
} else {
@ -191,7 +191,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
void GCS_MAVLINK_Plane::send_position_target_global_int()
{
if (plane.control_mode == MANUAL) {
if (plane.control_mode == &plane.mode_manual) {
return;
}
Location &next_WP_loc = plane.next_WP_loc;
@ -334,7 +334,7 @@ void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info,
*/
void GCS_MAVLINK_Plane::send_pid_tuning()
{
if (plane.control_mode == MANUAL) {
if (plane.control_mode == &plane.mode_manual) {
// no PIDs should be used in manual
return;
}
@ -655,7 +655,7 @@ bool GCS_MAVLINK_Plane::in_hil_mode() const
*/
bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
if (plane.control_mode != GUIDED) {
if (plane.control_mode != &plane.mode_guided) {
// only accept position updates when in GUIDED mode
return false;
}
@ -783,8 +783,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
// location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == GUIDED)) {
plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND);
(plane.control_mode == &plane.mode_guided)) {
plane.set_mode(plane.mode_guided, MODE_REASON_GCS_COMMAND);
plane.guided_WP_loc = requested_position;
// add home alt if needed
@ -814,7 +814,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
// controlled modes (e.g., MANUAL, TRAINING)
// this command should be ignored since it comes in from GCS
// or a companion computer:
if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) {
if ((plane.control_mode != &plane.mode_guided) &&
(plane.control_mode != &plane.mode_auto) &&
(plane.control_mode != &plane.mode_avoidADSB)) {
// failed
return MAV_RESULT_FAILED;
}
@ -828,11 +830,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
case MAV_CMD_NAV_LOITER_UNLIM:
plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND);
plane.set_mode(plane.mode_loiter, MODE_REASON_GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
plane.set_mode(RTL, MODE_REASON_GCS_COMMAND);
plane.set_mode(plane.mode_rtl, MODE_REASON_GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
case MAV_CMD_NAV_TAKEOFF: {
@ -846,7 +848,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
case MAV_CMD_MISSION_START:
plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
plane.set_mode(plane.mode_auto, MODE_REASON_GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
case MAV_CMD_COMPONENT_ARM_DISARM:
@ -868,7 +870,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
case MAV_CMD_DO_LAND_START:
// attempt to switch to next DO_LAND_START command in the mission
if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(AUTO, MODE_REASON_UNKNOWN);
plane.set_mode(plane.mode_auto, MODE_REASON_UNKNOWN);
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
@ -1185,7 +1187,8 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
// in e.g., RTL, CICLE. Specifying a single mode for companion
// computer control is more safe (even more so when using
// FENCE_ACTION = 4 for geofence failures).
if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { // don't screw up failsafes
if ((plane.control_mode != &plane.mode_guided) &&
(plane.control_mode != &plane.mode_avoidADSB)) { // don't screw up failsafes
break;
}
@ -1270,7 +1273,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
// exit if vehicle is not in Guided mode
if (plane.control_mode != GUIDED) {
if (plane.control_mode != &plane.mode_guided) {
break;
}
@ -1295,7 +1298,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
// in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode
// for companion computer control is more safe (provided
// one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) {
if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_avoidADSB) {
//don't screw up failsafes
break;
}
@ -1397,7 +1400,7 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_
{
plane.auto_state.next_wp_crosstrack = false;
GCS_MAVLINK::handle_mission_set_current(mission, msg);
if (plane.control_mode == AUTO && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
plane.mission.resume();
}
}
@ -1412,31 +1415,11 @@ AP_AdvancedFailsafe *GCS_MAVLINK_Plane::get_advanced_failsafe() const
*/
bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode)
{
switch (mode) {
case MANUAL:
case CIRCLE:
case STABILIZE:
case TRAINING:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case AVOID_ADSB:
case GUIDED:
case AUTO:
case RTL:
case LOITER:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
case QAUTOTUNE:
plane.set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND);
return true;
Mode *new_mode = plane.mode_from_mode_num((enum Mode::Number)mode);
if (new_mode == nullptr) {
return false;
}
return false;
return plane.set_mode(*new_mode, MODE_REASON_GCS_COMMAND);
}
uint64_t GCS_MAVLINK_Plane::capabilities() const

46
ArduPlane/GCS_Plane.cpp
View File

@ -50,43 +50,43 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
bool rate_controlled = false;
bool attitude_stabilized = false;
switch (plane.control_mode) {
case MANUAL:
switch (plane.control_mode->mode_number()) {
case Mode::Number::MANUAL:
break;
case ACRO:
case QACRO:
case Mode::Number::ACRO:
case Mode::Number::QACRO:
rate_controlled = true;
break;
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
case QLAND:
case QLOITER:
case QAUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case Mode::Number::STABILIZE:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLAND:
case Mode::Number::QLOITER:
case Mode::Number::QAUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
rate_controlled = true;
attitude_stabilized = true;
break;
case TRAINING:
case Mode::Number::TRAINING:
if (!plane.training_manual_roll || !plane.training_manual_pitch) {
rate_controlled = true;
attitude_stabilized = true;
}
break;
case AUTO:
case RTL:
case LOITER:
case AVOID_ADSB:
case GUIDED:
case CIRCLE:
case QRTL:
case Mode::Number::AUTO:
case Mode::Number::RTL:
case Mode::Number::LOITER:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::CIRCLE:
case Mode::Number::QRTL:
rate_controlled = true;
attitude_stabilized = true;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
@ -97,7 +97,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case INITIALISING:
case Mode::Number::INITIALISING:
break;
}

2
ArduPlane/Log.cpp
View File

@ -299,7 +299,7 @@ void Plane::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
logger.Write_Mode(control_mode, control_mode_reason);
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();
}

2
ArduPlane/Parameters.cpp
View File

@ -582,7 +582,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver.
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE
// @User: Advanced
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL),
// @Param: LIM_ROLL_CD
// @DisplayName: Maximum Bank Angle

67
ArduPlane/Plane.h
View File

@ -106,6 +106,7 @@
// Local modules
#include "defines.h"
#include "mode.h"
#ifdef ENABLE_SCRIPTING
#include <AP_Scripting/AP_Scripting.h>
@ -136,7 +137,7 @@ protected:
void setup_IO_failsafe(void);
// return the AFS mapped control mode
enum control_mode afs_mode(void);
enum AP_AdvancedFailsafe::control_mode afs_mode(void);
};
/*
@ -157,6 +158,30 @@ public:
friend class RC_Channel_Plane;
friend class RC_Channels_Plane;
friend class Mode;
friend class ModeCircle;
friend class ModeStabilize;
friend class ModeTraining;
friend class ModeAcro;
friend class ModeFBWA;
friend class ModeFBWB;
friend class ModeCruise;
friend class ModeAutoTune;
friend class ModeAuto;
friend class ModeRTL;
friend class ModeLoiter;
friend class ModeAvoidADSB;
friend class ModeGuided;
friend class ModeInitializing;
friend class ModeManual;
friend class ModeQStabilize;
friend class ModeQHover;
friend class ModeQLoiter;
friend class ModeQLand;
friend class ModeQRTL;
friend class ModeQAcro;
friend class ModeQAutotune;
Plane(void);
// HAL::Callbacks implementation.
@ -299,11 +324,34 @@ private:
AP_OSD osd;
#endif
ModeCircle mode_circle;
ModeStabilize mode_stabilize;
ModeTraining mode_training;
ModeAcro mode_acro;
ModeFBWA mode_fbwa;
ModeFBWB mode_fbwb;
ModeCruise mode_cruise;
ModeAutoTune mode_autotune;
ModeAuto mode_auto;
ModeRTL mode_rtl;
ModeLoiter mode_loiter;
ModeAvoidADSB mode_avoidADSB;
ModeGuided mode_guided;
ModeInitializing mode_initializing;
ModeManual mode_manual;
ModeQStabilize mode_qstabilize;
ModeQHover mode_qhover;
ModeQLoiter mode_qloiter;
ModeQLand mode_qland;
ModeQRTL mode_qrtl;
ModeQAcro mode_qacro;
ModeQAutotune mode_qautotune;
// This is the state of the flight control system
// There are multiple states defined such as MANUAL, FBW-A, AUTO
enum FlightMode control_mode = INITIALISING;
Mode *control_mode = &mode_initializing;
mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN;
enum FlightMode previous_mode = INITIALISING;
Mode *previous_mode = &mode_initializing;
mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN;
// time of last mode change
@ -329,7 +377,7 @@ private:
bool adsb:1;
// saved flight mode
enum FlightMode saved_mode;
enum Mode::Number saved_mode_number;
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
@ -904,9 +952,9 @@ private:
void rpm_update(void);
void init_ardupilot();
void startup_ground(void);
enum FlightMode get_previous_mode();
void set_mode(enum FlightMode mode, mode_reason_t reason);
void exit_mode(enum FlightMode mode);
bool set_mode(Mode& new_mode, const mode_reason_t reason);
bool set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason);
Mode *mode_from_mode_num(const enum Mode::Number num);
void check_long_failsafe();
void check_short_failsafe();
void startup_INS_ground(void);
@ -938,7 +986,7 @@ private:
void update_logging1(void);
void update_logging2(void);
void avoidance_adsb_update(void);
void update_flight_mode(void);
void update_control_mode(void);
void stabilize();
void set_servos_idle(void);
void set_servos();
@ -957,7 +1005,6 @@ private:
void update_is_flying_5Hz(void);
void crash_detection_update(void);
bool in_preLaunch_flight_stage(void);
void handle_auto_mode(void);
void calc_throttle();
void calc_nav_roll();
void calc_nav_pitch();
@ -1007,7 +1054,7 @@ private:
void do_set_home(const AP_Mission::Mission_Command& cmd);
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void notify_flight_mode(enum FlightMode mode);
void notify_mode(const Mode& mode);
void log_init();
void parachute_check();
#if PARACHUTE == ENABLED

2
ArduPlane/afs_plane.cpp
View File

@ -86,7 +86,7 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
if (plane.auto_throttle_mode) {
return AP_AdvancedFailsafe::AFS_AUTO;
}
if (plane.control_mode == MANUAL) {
if (plane.control_mode == &plane.mode_manual) {
return AP_AdvancedFailsafe::AFS_MANUAL;
}
return AP_AdvancedFailsafe::AFS_STABILIZED;

24
ArduPlane/altitude.cpp
View File

@ -27,8 +27,8 @@ void Plane::adjust_altitude_target()
{
Location target_location;
if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) {
if (control_mode == &mode_fbwb ||
control_mode == &mode_cruise) {
return;
}
if (landing.is_flaring()) {
@ -75,10 +75,10 @@ void Plane::setup_glide_slope(void)
work out if we will gradually change altitude, or try to get to
the new altitude as quickly as possible.
*/
switch (control_mode) {
case RTL:
case AVOID_ADSB:
case GUIDED:
switch (control_mode->mode_number()) {
case Mode::Number::RTL:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
/* glide down slowly if above target altitude, but ascend more
rapidly if below it. See
https://github.com/ArduPilot/ardupilot/issues/39
@ -90,7 +90,7 @@ void Plane::setup_glide_slope(void)
}
break;
case AUTO:
case Mode::Number::AUTO:
// we only do glide slide handling in AUTO when above 20m or
// when descending. The 20 meter threshold is arbitrary, and
// is basically to prevent situations where we try to slowly
@ -466,7 +466,7 @@ int32_t Plane::adjusted_relative_altitude_cm(void)
float Plane::mission_alt_offset(void)
{
float ret = g.alt_offset;
if (control_mode == AUTO &&
if (control_mode == &mode_auto &&
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) {
// when landing after an aborted landing due to too high glide
// slope we use an offset from the last landing attempt
@ -506,7 +506,7 @@ float Plane::lookahead_adjustment(void)
int32_t bearing_cd;
int16_t distance;
// work out distance and bearing to target
if (control_mode == FLY_BY_WIRE_B) {
if (control_mode == &mode_fbwb) {
// there is no target waypoint in FBWB, so use yaw as an approximation
bearing_cd = ahrs.yaw_sensor;
distance = g.terrain_lookahead;
@ -611,9 +611,9 @@ void Plane::rangefinder_height_update(void)
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
control_mode == QLAND ||
control_mode == QRTL ||
(control_mode == AUTO && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) &&
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
(control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) &&
g.rangefinder_landing) {
rangefinder_state.in_use = true;
gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate);

30
ArduPlane/avoidance_adsb.cpp
View File

@ -19,15 +19,15 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
plane.failsafe.adsb = true;
failsafe_state_change = true;
// record flight mode in case it's required for the recovery
prev_control_mode = plane.control_mode;
prev_control_mode_number = plane.control_mode->mode_number();
}
// take no action in some flight modes
if (plane.control_mode == MANUAL ||
(plane.control_mode == AUTO && !plane.auto_state.takeoff_complete) ||
if (plane.control_mode == &plane.mode_manual ||
(plane.control_mode == &plane.mode_auto && !plane.auto_state.takeoff_complete) ||
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || // TODO: consider allowing action during approach
plane.control_mode == AUTOTUNE ||
plane.control_mode == QLAND) {
plane.control_mode == &plane.mode_autotune ||
plane.control_mode == &plane.mode_qland) {
actual_action = MAV_COLLISION_ACTION_NONE;
}
@ -36,16 +36,16 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
case MAV_COLLISION_ACTION_RTL:
if (failsafe_state_change) {
plane.set_mode(RTL, MODE_REASON_AVOIDANCE);
plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE);
}
break;
case MAV_COLLISION_ACTION_HOVER:
if (failsafe_state_change) {
if (plane.quadplane.is_flying()) {
plane.set_mode(QLOITER, MODE_REASON_AVOIDANCE);
plane.set_mode(plane.mode_qloiter, MODE_REASON_AVOIDANCE);
} else {
plane.set_mode(LOITER, MODE_REASON_AVOIDANCE);
plane.set_mode(plane.mode_loiter, MODE_REASON_AVOIDANCE);
}
}
break;
@ -113,16 +113,16 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action)
break;
case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE:
plane.set_mode(prev_control_mode, MODE_REASON_AVOIDANCE_RECOVERY);
plane.set_mode_by_number(prev_control_mode_number, MODE_REASON_AVOIDANCE_RECOVERY);
break;
case AP_AVOIDANCE_RECOVERY_RTL:
plane.set_mode(RTL, MODE_REASON_AVOIDANCE_RECOVERY);
plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE_RECOVERY);
break;
case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER:
if (prev_control_mode == AUTO) {
plane.set_mode(AUTO, MODE_REASON_AVOIDANCE_RECOVERY);
if (prev_control_mode_number == Mode::Number::AUTO) {
plane.set_mode(plane.mode_auto, MODE_REASON_AVOIDANCE_RECOVERY);
}
// else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER
break;
@ -138,12 +138,12 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action)
bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change)
{
// ensure plane is in avoid_adsb mode
if (allow_mode_change && plane.control_mode != AVOID_ADSB) {
plane.set_mode(AVOID_ADSB, MODE_REASON_AVOIDANCE);
if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) {
plane.set_mode(plane.mode_avoidADSB, MODE_REASON_AVOIDANCE);
}
// check flight mode
return (plane.control_mode == AVOID_ADSB);
return (plane.control_mode == &plane.mode_avoidADSB);
}
bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)

2
ArduPlane/avoidance_adsb.h
View File

@ -34,5 +34,5 @@ protected:
bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change);
// control mode before avoidance began
FlightMode prev_control_mode = RTL;
enum Mode::Number prev_control_mode_number = Mode::Number::RTL;
};

12
ArduPlane/commands_logic.cpp
View File

@ -75,7 +75,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(RTL, MODE_REASON_UNKNOWN);
set_mode(mode_rtl, MODE_REASON_UNKNOWN);
break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
@ -321,7 +321,7 @@ void Plane::do_RTL(int32_t rtl_altitude)
setup_glide_slope();
setup_turn_angle();
logger.Write_Mode(control_mode, control_mode_reason);
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
}
/*
@ -930,7 +930,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
{
if (control_mode == AUTO) {
if (control_mode == &mode_auto) {
return start_command(cmd);
}
return true;
@ -940,7 +940,7 @@ bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
{
if (control_mode == AUTO) {
if (control_mode == &mode_auto) {
bool cmd_complete = verify_command(cmd);
// send message to GCS
@ -957,8 +957,8 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
void Plane::exit_mission_callback()
{
if (control_mode == AUTO) {
set_mode(RTL, MODE_REASON_MISSION_END);
if (control_mode == &mode_auto) {
set_mode(mode_rtl, MODE_REASON_MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
}
}

12
ArduPlane/config.h
View File

@ -104,22 +104,22 @@
#endif
#if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 RTL
# define FLIGHT_MODE_1 Mode::Number::RTL
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 RTL
# define FLIGHT_MODE_2 Mode::Number::RTL
#endif
#if !defined(FLIGHT_MODE_3)
# define FLIGHT_MODE_3 FLY_BY_WIRE_A
# define FLIGHT_MODE_3 Mode::Number::FLY_BY_WIRE_A
#endif
#if !defined(FLIGHT_MODE_4)
# define FLIGHT_MODE_4 FLY_BY_WIRE_A
# define FLIGHT_MODE_4 Mode::Number::FLY_BY_WIRE_A
#endif
#if !defined(FLIGHT_MODE_5)
# define FLIGHT_MODE_5 MANUAL
# define FLIGHT_MODE_5 Mode::Number::MANUAL
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 MANUAL
# define FLIGHT_MODE_6 Mode::Number::MANUAL
#endif

80
ArduPlane/control_modes.cpp
View File

@ -1,5 +1,79 @@
#include "Plane.h"
Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
{
Mode *ret = nullptr;
switch (num) {
case Mode::Number::MANUAL:
ret = &mode_manual;
break;
case Mode::Number::CIRCLE:
ret = &mode_circle;
break;
case Mode::Number::STABILIZE:
ret = &mode_stabilize;
break;
case Mode::Number::TRAINING:
ret = &mode_training;
break;
case Mode::Number::ACRO:
ret = &mode_acro;
break;
case Mode::Number::FLY_BY_WIRE_A:
ret = &mode_fbwa;
break;
case Mode::Number::FLY_BY_WIRE_B:
ret = &mode_fbwb;
break;
case Mode::Number::CRUISE:
ret = &mode_cruise;
break;
case Mode::Number::AUTOTUNE:
ret = &mode_autotune;
break;
case Mode::Number::AUTO:
ret = &mode_auto;
break;
case Mode::Number::RTL:
ret = &mode_rtl;
break;
case Mode::Number::LOITER:
ret = &mode_loiter;
break;
case Mode::Number::AVOID_ADSB:
ret = &mode_avoidADSB;
break;
case Mode::Number::GUIDED:
ret = &mode_guided;
break;
case Mode::Number::INITIALISING:
ret = &mode_initializing;
break;
case Mode::Number::QSTABILIZE:
ret = &mode_qstabilize;
break;
case Mode::Number::QHOVER:
ret = &mode_qhover;
break;
case Mode::Number::QLOITER:
ret = &mode_qloiter;
break;
case Mode::Number::QLAND:
ret = &mode_qland;
break;
case Mode::Number::QRTL:
ret = &mode_qrtl;
break;
case Mode::Number::QACRO:
ret = &mode_qacro;
break;
case Mode::Number::QAUTOTUNE:
ret = &mode_qautotune;
break;
}
return ret;
}
void Plane::read_control_switch()
{
static bool switch_debouncer;
@ -39,7 +113,7 @@ void Plane::read_control_switch()
return;
}
set_mode((enum FlightMode)(flight_modes[switchPosition].get()), MODE_REASON_TX_COMMAND);
set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), MODE_REASON_TX_COMMAND);
oldSwitchPosition = switchPosition;
}
@ -114,14 +188,14 @@ void Plane::autotune_enable(bool enable)
*/
bool Plane::fly_inverted(void)
{
if (control_mode == MANUAL) {
if (control_mode == &plane.mode_manual) {
return false;
}
if (inverted_flight) {
// controlled with aux switch
return true;
}
if (control_mode == AUTO && auto_state.inverted_flight) {
if (control_mode == &mode_auto && auto_state.inverted_flight) {
return true;
}
return false;

25
ArduPlane/defines.h
View File

@ -45,31 +45,6 @@ enum failsafe_action_long {
FS_ACTION_LONG_PARACHUTE = 3,
};
enum FlightMode {
MANUAL = 0,
CIRCLE = 1,
STABILIZE = 2,
TRAINING = 3,
ACRO = 4,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE = 7,
AUTOTUNE = 8,
AUTO = 10,
RTL = 11,
LOITER = 12,
AVOID_ADSB = 14,
GUIDED = 15,
INITIALISING = 16,
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19,
QLAND = 20,
QRTL = 21,
QAUTOTUNE = 22,
QACRO = 23,
};
enum mode_reason_t {
MODE_REASON_UNKNOWN=0,
MODE_REASON_TX_COMMAND,

134
ArduPlane/events.cpp
View File

@ -6,57 +6,58 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re
failsafe.state = fstype;
failsafe.short_timer_ms = millis();
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason);
switch(control_mode)
switch (control_mode->mode_number())
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
failsafe.saved_mode = control_mode;
case Mode::Number::MANUAL:
case Mode::Number::STABILIZE:
case Mode::Number::ACRO:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
case Mode::Number::TRAINING:
failsafe.saved_mode_number = control_mode->mode_number();
failsafe.saved_mode_set = true;
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
set_mode(FLY_BY_WIRE_A, reason);
set_mode(mode_fbwa, reason);
} else {
set_mode(CIRCLE, reason);
set_mode(mode_circle, reason);
}
break;
case QSTABILIZE:
case QLOITER:
case QHOVER:
case QAUTOTUNE:
failsafe.saved_mode = control_mode;
case Mode::Number::QSTABILIZE:
case Mode::Number::QLOITER:
case Mode::Number::QHOVER:
case Mode::Number::QAUTOTUNE:
case Mode::Number::QACRO:
failsafe.saved_mode_number = control_mode->mode_number();
failsafe.saved_mode_set = true;
set_mode(QLAND, reason);
set_mode(mode_qland, reason);
break;
case AUTO:
case AVOID_ADSB:
case GUIDED:
case LOITER:
case Mode::Number::AUTO:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
failsafe.saved_mode = control_mode;
failsafe.saved_mode_number = control_mode->mode_number();
failsafe.saved_mode_set = true;
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
set_mode(FLY_BY_WIRE_A, reason);
set_mode(mode_fbwa, reason);
} else {
set_mode(CIRCLE, reason);
set_mode(mode_circle, reason);
}
}
break;
case CIRCLE:
case RTL:
case QLAND:
case QRTL:
default:
case Mode::Number::CIRCLE:
case Mode::Number::RTL:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::INITIALISING:
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number());
}
void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason)
@ -66,57 +67,58 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea
// If the GCS is locked up we allow control to revert to RC
RC_Channels::clear_overrides();
failsafe.state = fstype;
switch(control_mode)
switch (control_mode->mode_number())
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
case CIRCLE:
case Mode::Number::MANUAL:
case Mode::Number::STABILIZE:
case Mode::Number::ACRO:
case Mode::Number::FLY_BY_WIRE_A:
case Mode::Number::AUTOTUNE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
case Mode::Number::TRAINING:
case Mode::Number::CIRCLE:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
set_mode(FLY_BY_WIRE_A, reason);
set_mode(mode_fbwa, reason);
} else {
set_mode(RTL, reason);
set_mode(mode_rtl, reason);
}
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QAUTOTUNE:
set_mode(QLAND, reason);
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QACRO:
case Mode::Number::QAUTOTUNE:
set_mode(mode_qland, reason);
break;
case AUTO:
case AVOID_ADSB:
case GUIDED:
case LOITER:
case Mode::Number::AUTO:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
set_mode(FLY_BY_WIRE_A, reason);
set_mode(mode_fbwa, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
set_mode(RTL, reason);
set_mode(mode_rtl, reason);
}
break;
case RTL:
case QLAND:
case QRTL:
default:
case Mode::Number::RTL:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
case Mode::Number::INITIALISING:
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number());
}
void Plane::failsafe_short_off_event(mode_reason_t reason)
@ -127,9 +129,9 @@ void Plane::failsafe_short_off_event(mode_reason_t reason)
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
if (control_mode == CIRCLE && failsafe.saved_mode_set) {
if (control_mode == &mode_circle && failsafe.saved_mode_set) {
failsafe.saved_mode_set = false;
set_mode(failsafe.saved_mode, reason);
set_mode_by_number(failsafe.saved_mode_number, reason);
}
}
@ -145,23 +147,23 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
switch ((Failsafe_Action)action) {
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(QLAND, MODE_REASON_BATTERY_FAILSAFE);
plane.set_mode(mode_qland, MODE_REASON_BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
case Failsafe_Action_Land:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != QLAND) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) {
// never stop a landing if we were already committed
if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(AUTO, MODE_REASON_BATTERY_FAILSAFE);
plane.set_mode(mode_auto, MODE_REASON_BATTERY_FAILSAFE);
break;
}
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != QLAND ) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) {
// never stop a landing if we were already committed
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
set_mode(mode_rtl, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
break;

12
ArduPlane/geofence.cpp
View File

@ -291,14 +291,14 @@ void Plane::geofence_check(bool altitude_check_only)
// GUIDED to the return point
if (geofence_state != nullptr &&
(g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
(control_mode == GUIDED || control_mode == AVOID_ADSB) &&
(control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
geofence_present() &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254;
set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND);
set_mode(*previous_mode, MODE_REASON_GCS_COMMAND);
}
return;
}
@ -354,7 +354,7 @@ void Plane::geofence_check(bool altitude_check_only)
// we are outside the fence
if (geofence_state->fence_triggered &&
(control_mode == GUIDED || control_mode == AVOID_ADSB || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) {
(control_mode == &mode_guided || control_mode == &mode_avoidADSB || control_mode == &mode_rtl || g.fence_action == FENCE_ACTION_REPORT)) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
@ -386,9 +386,9 @@ void Plane::geofence_check(bool altitude_check_only)
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(RTL, MODE_REASON_FENCE_BREACH);
set_mode(mode_rtl, MODE_REASON_FENCE_BREACH);
} else {
set_mode(GUIDED, MODE_REASON_FENCE_BREACH);
set_mode(mode_guided, MODE_REASON_FENCE_BREACH);
}
g.auto_trim.set(saved_auto_trim);
@ -438,7 +438,7 @@ bool Plane::geofence_stickmixing(void) {
if (geofence_enabled() &&
geofence_state != nullptr &&
geofence_state->fence_triggered &&
(control_mode == GUIDED || control_mode == AVOID_ADSB)) {
(control_mode == &mode_guided || control_mode == &mode_avoidADSB)) {
// don't mix in user input
return false;
}

8
ArduPlane/is_flying.cpp
View File

@ -53,7 +53,7 @@ void Plane::update_is_flying_5Hz(void)
gps_confirmed_movement; // locked and we're moving
}
if (control_mode == AUTO) {
if (control_mode == &mode_auto) {
/*
make is_flying() more accurate during various auto modes
*/
@ -146,7 +146,7 @@ void Plane::update_is_flying_5Hz(void)
started_flying_ms = now_ms;
}
if ((control_mode == AUTO) &&
if ((control_mode == &mode_auto) &&
((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
// We just started flying, note that time also
@ -192,7 +192,7 @@ bool Plane::is_flying(void)
*/
void Plane::crash_detection_update(void)
{
if (control_mode != AUTO || !aparm.crash_detection_enable)
if (control_mode != &mode_auto || !aparm.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
@ -317,7 +317,7 @@ void Plane::crash_detection_update(void)
* return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches
*/
bool Plane::in_preLaunch_flight_stage(void) {
return (control_mode == AUTO &&
return (control_mode == &mode_auto &&
throttle_suppressed &&
flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF &&

76
ArduPlane/mode.cpp Normal file
View File

@ -0,0 +1,76 @@
#include "Plane.h"
Mode::Mode()
{
}
void Mode::exit()
{
// call sub-classes exit
_exit();
}
bool Mode::enter()
{
// cancel inverted flight
plane.auto_state.inverted_flight = false;
// don't cross-track when starting a mission
plane.auto_state.next_wp_crosstrack = false;
// reset landing check
plane.auto_state.checked_for_autoland = false;
// zero locked course
plane.steer_state.locked_course_err = 0;
// reset crash detection
plane.crash_state.is_crashed = false;
plane.crash_state.impact_detected = false;
// reset external attitude guidance
plane.guided_state.last_forced_rpy_ms.zero();
plane.guided_state.last_forced_throttle_ms = 0;
#if CAMERA == ENABLED
plane.camera.set_is_auto_mode(this == &plane.mode_auto);
#endif
// zero initial pitch and highest airspeed on mode change
plane.auto_state.highest_airspeed = 0;
plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor;
// disable taildrag takeoff on mode change
plane.auto_state.fbwa_tdrag_takeoff_mode = false;
// start with previous WP at current location
plane.prev_WP_loc = plane.current_loc;
// new mode means new loiter
plane.loiter.start_time_ms = 0;
// record time of mode change
plane.last_mode_change_ms = AP_HAL::millis();
// assume non-VTOL mode
plane.auto_state.vtol_mode = false;
plane.auto_state.vtol_loiter = false;
// reset steering integrator on mode change
plane.steerController.reset_I();
bool enter_result = _enter();
if (enter_result) {
// -------------------
// these must be done AFTER _enter() because they use the results to set more flags
// start with throttle suppressed in auto_throttle modes
plane.throttle_suppressed = plane.auto_throttle_mode;
plane.adsb.set_is_auto_mode(plane.auto_navigation_mode);
}
return enter_result;
}

433
ArduPlane/mode.h Normal file
View File

@ -0,0 +1,433 @@
#pragma once
#include <stdint.h>
class Mode
{
public:
/* Do not allow copies */
Mode(const Mode &other) = delete;
Mode &operator=(const Mode&) = delete;
// Auto Pilot modes
// ----------------
enum Number {
MANUAL = 0,
CIRCLE = 1,
STABILIZE = 2,
TRAINING = 3,
ACRO = 4,
FLY_BY_WIRE_A = 5,
FLY_BY_WIRE_B = 6,
CRUISE = 7,
AUTOTUNE = 8,
AUTO = 10,
RTL = 11,
LOITER = 12,
AVOID_ADSB = 14,
GUIDED = 15,
INITIALISING = 16,
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19,
QLAND = 20,
QRTL = 21,
QAUTOTUNE = 22,
QACRO = 23,
};
// Constructor
Mode();
// enter this mode, always returns true/success
bool enter();
// perform any cleanups required:
void exit();
// returns a unique number specific to this mode
virtual Number mode_number() const = 0;
// returns full text name
virtual const char *name() const = 0;
// returns a string for this flightmode, exactly 4 bytes
virtual const char *name4() const = 0;
//
// methods that sub classes should override to affect movement of the vehicle in this mode
//
// convert user input to targets, implement high level control for this mode
virtual void update() = 0;
protected:
// subclasses override this to perform checks before entering the mode
virtual bool _enter() { return true; }
// subclasses override this to perform any required cleanup when exiting the mode
virtual void _exit() { return; }
};
class ModeAcro : public Mode
{
public:
Mode::Number mode_number() const override { return Mode::Number::ACRO; }
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeAuto : public Mode
{
public:
Number mode_number() const override { return Number::AUTO; }
const char *name() const override { return "AUTO"; }
const char *name4() const override { return "AUTO"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
void _exit() override;
};
class ModeAutoTune : public Mode
{
public:
Number mode_number() const override { return Number::AUTOTUNE; }
const char *name() const override { return "AUTOTUNE"; }
const char *name4() const override { return "ATUN"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
void _exit() override;
};
class ModeGuided : public Mode
{
public:
Number mode_number() const override { return Number::GUIDED; }
const char *name() const override { return "GUIDED"; }
const char *name4() const override { return "GUID"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeCircle: public Mode
{
public:
Number mode_number() const override { return Number::CIRCLE; }
const char *name() const override { return "CIRCLE"; }
const char *name4() const override { return "CIRC"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeLoiter : public Mode
{
public:
Number mode_number() const override { return Number::LOITER; }
const char *name() const override { return "LOITER"; }
const char *name4() const override { return "LOIT"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeManual : public Mode
{
public:
Number mode_number() const override { return Number::MANUAL; }
const char *name() const override { return "MANUAL"; }
const char *name4() const override { return "MANU"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
void _exit() override;
};
class ModeRTL : public Mode
{
public:
Number mode_number() const override { return Number::RTL; }
const char *name() const override { return "RTL"; }
const char *name4() const override { return "RTL "; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeStabilize : public Mode
{
public:
Number mode_number() const override { return Number::STABILIZE; }
const char *name() const override { return "STABILIZE"; }
const char *name4() const override { return "STAB"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeTraining : public Mode
{
public:
Number mode_number() const override { return Number::TRAINING; }
const char *name() const override { return "TRAINING"; }
const char *name4() const override { return "TRAN"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeInitializing : public Mode
{
public:
Number mode_number() const override { return Number::INITIALISING; }
const char *name() const override { return "INITIALISING"; }
const char *name4() const override { return "INIT"; }
// methods that affect movement of the vehicle in this mode
void update() override { }
protected:
bool _enter() override;
};
class ModeFBWA : public Mode
{
public:
Number mode_number() const override { return Number::FLY_BY_WIRE_A; }
const char *name() const override { return "FLY_BY_WIRE_A"; }
const char *name4() const override { return "FBWA"; }
// methods that affect movement of the vehicle in this mode
void update() override;
bool _enter() override;
protected:
};
class ModeFBWB : public Mode
{
public:
Number mode_number() const override { return Number::FLY_BY_WIRE_B; }
const char *name() const override { return "FLY_BY_WIRE_B"; }
const char *name4() const override { return "FBWB"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeCruise : public Mode
{
public:
Number mode_number() const override { return Number::CRUISE; }
const char *name() const override { return "CRUISE"; }
const char *name4() const override { return "CRUS"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeAvoidADSB : public Mode
{
public:
Number mode_number() const override { return Number::AVOID_ADSB; }
const char *name() const override { return "AVOID_ADSB"; }
const char *name4() const override { return "AVOI"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQStabilize : public Mode
{
public:
Number mode_number() const override { return Number::QSTABILIZE; }
const char *name() const override { return "QSTABILIZE"; }
const char *name4() const override { return "QSTB"; }
// methods that affect movement of the vehicle in this mode
void update() override;
// used as a base class for all Q modes
bool _enter() override;
protected:
};
class ModeQHover : public Mode
{
public:
Number mode_number() const override { return Number::QHOVER; }
const char *name() const override { return "QHOVER"; }
const char *name4() const override { return "QHOV"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQLoiter : public Mode
{
public:
Number mode_number() const override { return Number::QLOITER; }
const char *name() const override { return "QLOITER"; }
const char *name4() const override { return "QLOT"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQLand : public Mode
{
public:
Number mode_number() const override { return Number::QLAND; }
const char *name() const override { return "QLAND"; }
const char *name4() const override { return "QLND"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQRTL : public Mode
{
public:
Number mode_number() const override { return Number::QRTL; }
const char *name() const override { return "QRTL"; }
const char *name4() const override { return "QRTL"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQAcro : public Mode
{
public:
Number mode_number() const override { return Number::QACRO; }
const char *name() const override { return "QACO"; }
const char *name4() const override { return "QACRO"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};
class ModeQAutotune : public Mode
{
public:
Number mode_number() const override { return Number::QAUTOTUNE; }
const char *name() const override { return "QAUTOTUNE"; }
const char *name4() const override { return "QATN"; }
// methods that affect movement of the vehicle in this mode
void update() override;
protected:
bool _enter() override;
};

29
ArduPlane/mode_acro.cpp Normal file
View File

@ -0,0 +1,29 @@
#include "mode.h"
#include "Plane.h"
bool ModeAcro::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
plane.acro_state.locked_roll = false;
plane.acro_state.locked_pitch = false;
return true;
}
void ModeAcro::update()
{
// handle locked/unlocked control
if (plane.acro_state.locked_roll) {
plane.nav_roll_cd = plane.acro_state.locked_roll_err;
} else {
plane.nav_roll_cd = plane.ahrs.roll_sensor;
}
if (plane.acro_state.locked_pitch) {
plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd;
} else {
plane.nav_pitch_cd = plane.ahrs.pitch_sensor;
}
}

80
ArduPlane/mode_auto.cpp Normal file
View File

@ -0,0 +1,80 @@
#include "mode.h"
#include "Plane.h"
bool ModeAuto::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
if (plane.quadplane.available() && plane.quadplane.enable == 2) {
plane.auto_state.vtol_mode = true;
} else {
plane.auto_state.vtol_mode = false;
}
plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc;
// start or resume the mission, based on MIS_AUTORESET
plane.mission.start_or_resume();
plane.g2.soaring_controller.init_cruising();
return true;
}
void ModeAuto::_exit()
{
if (plane.mission.state() == AP_Mission::MISSION_RUNNING) {
plane.mission.stop();
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
!plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))
{
plane.landing.restart_landing_sequence();
}
}
plane.auto_state.started_flying_in_auto_ms = 0;
}
void ModeAuto::update()
{
if (plane.mission.state() != AP_Mission::MISSION_RUNNING) {
// this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if:
// restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point
plane.set_mode(plane.mode_rtl, MODE_REASON_MISSION_END);
gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
return;
}
uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id;
if (plane.quadplane.in_vtol_auto()) {
plane.quadplane.control_auto(plane.next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) {
plane.takeoff_calc_roll();
plane.takeoff_calc_pitch();
plane.calc_throttle();
} else if (nav_cmd_id == MAV_CMD_NAV_LAND) {
plane.calc_nav_roll();
plane.calc_nav_pitch();
// allow landing to restrict the roll limits
plane.nav_roll_cd = plane.landing.constrain_roll(plane.nav_roll_cd, plane.g.level_roll_limit*100UL);
if (plane.landing.is_throttle_suppressed()) {
// if landing is considered complete throttle is never allowed, regardless of landing type
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
} else {
plane.calc_throttle();
}
} else {
// we are doing normal AUTO flight, the special cases
// are for takeoff and landing
if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) {
plane.steer_state.hold_course_cd = -1;
}
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}
}

24
ArduPlane/mode_autotune.cpp Normal file
View File

@ -0,0 +1,24 @@
#include "mode.h"
#include "Plane.h"
bool ModeAutoTune::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
plane.autotune_start();
return true;
}
void ModeAutoTune::_exit()
{
// restore last gains
plane.autotune_restore();
}
void ModeAutoTune::update()
{
plane.mode_fbwa.update();
}

13
ArduPlane/mode_avoidADSB.cpp Normal file
View File

@ -0,0 +1,13 @@
#include "mode.h"
#include "Plane.h"
bool ModeAvoidADSB::_enter()
{
return plane.mode_guided.enter();
}
void ModeAvoidADSB::update()
{
plane.mode_guided.update();
}

26
ArduPlane/mode_circle.cpp Normal file
View File

@ -0,0 +1,26 @@
#include "mode.h"
#include "Plane.h"
bool ModeCircle::_enter()
{
// the altitude to circle at is taken from the current altitude
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.next_WP_loc.alt = plane.current_loc.alt;
return true;
}
void ModeCircle::update()
{
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we
// switched into the mode
plane.nav_roll_cd = plane.roll_limit_cd / 3;
plane.update_load_factor();
plane.calc_nav_pitch();
plane.calc_throttle();
}

40
ArduPlane/mode_cruise.cpp Normal file
View File

@ -0,0 +1,40 @@
#include "mode.h"
#include "Plane.h"
bool ModeCruise::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
plane.cruise_state.locked_heading = false;
plane.cruise_state.lock_timer_ms = 0;
// for ArduSoar soaring_controller
plane.g2.soaring_controller.init_cruising();
plane.set_target_altitude_current();
return true;
}
void ModeCruise::update()
{
/*
in CRUISE mode we use the navigation code to control
roll when heading is locked. Heading becomes unlocked on
any aileron or rudder input
*/
if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
plane.cruise_state.locked_heading = false;
plane.cruise_state.lock_timer_ms = 0;
}
if (!plane.cruise_state.locked_heading) {
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
plane.update_load_factor();
} else {
plane.calc_nav_roll();
}
plane.update_fbwb_speed_height();
}

45
ArduPlane/mode_fbwa.cpp Normal file
View File

@ -0,0 +1,45 @@
#include "mode.h"
#include "Plane.h"
bool ModeFBWA::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true;
}
void ModeFBWA::update()
{
// set nav_roll and nav_pitch using sticks
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
plane.update_load_factor();
float pitch_input = plane.channel_pitch->norm_input();
if (pitch_input > 0) {
plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd;
} else {
plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd);
}
plane.adjust_nav_pitch_throttle();
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
if (plane.fly_inverted()) {
plane.nav_pitch_cd = -plane.nav_pitch_cd;
}
if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// FBWA failsafe glide
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
}
if (plane.g.fbwa_tdrag_chan > 0) {
// check for the user enabling FBWA taildrag takeoff mode
bool tdrag_mode = (RC_Channels::get_radio_in(plane.g.fbwa_tdrag_chan-1) > 1700);
if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) {
if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) {
plane.auto_state.fbwa_tdrag_takeoff_mode = true;
plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode");
}
}
}
}

26
ArduPlane/mode_fbwb.cpp Normal file
View File

@ -0,0 +1,26 @@
#include "mode.h"
#include "Plane.h"
bool ModeFBWB::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
// for ArduSoar soaring_controller
plane.g2.soaring_controller.init_cruising();
plane.set_target_altitude_current();
return true;
}
void ModeFBWB::update()
{
// Thanks to Yury MonZon for the altitude limit code!
plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
plane.update_load_factor();
plane.update_fbwb_speed_height();
}

30
ArduPlane/mode_guided.cpp Normal file
View File

@ -0,0 +1,30 @@
#include "mode.h"
#include "Plane.h"
bool ModeGuided::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
plane.guided_WP_loc = plane.current_loc;
plane.set_guided_WP();
return true;
}
void ModeGuided::update()
{
if (plane.auto_state.vtol_loiter && plane.quadplane.available()) {
plane.quadplane.guided_update();
} else {
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}
}

12
ArduPlane/mode_initializing.cpp Normal file
View File

@ -0,0 +1,12 @@
#include "mode.h"
#include "Plane.h"
bool ModeInitializing::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = false;
return true;
}

25
ArduPlane/mode_loiter.cpp Normal file
View File

@ -0,0 +1,25 @@
#include "mode.h"
#include "Plane.h"
bool ModeLoiter::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.do_loiter_at_location();
if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) {
plane.g2.soaring_controller.init_thermalling();
plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
}
return true;
}
void ModeLoiter::update()
{
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}

26
ArduPlane/mode_manual.cpp Normal file
View File

@ -0,0 +1,26 @@
#include "mode.h"
#include "Plane.h"
bool ModeManual::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true;
}
void ModeManual::_exit()
{
if (plane.g.auto_trim > 0) {
plane.trim_radio();
}
}
void ModeManual::update()
{
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz());
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz());
plane.steering_control.steering = plane.steering_control.rudder = plane.channel_rudder->get_control_in_zero_dz();
}

14
ArduPlane/mode_qacro.cpp Normal file
View File

@ -0,0 +1,14 @@
#include "mode.h"
#include "Plane.h"
bool ModeQAcro::_enter()
{
//return false;
return plane.mode_qstabilize._enter();
}
void ModeQAcro::update()
{
plane.mode_qstabilize.update();
}

13
ArduPlane/mode_qautotune.cpp Normal file
View File

@ -0,0 +1,13 @@
#include "mode.h"
#include "Plane.h"
bool ModeQAutotune::_enter()
{
return plane.mode_qstabilize._enter();
}
void ModeQAutotune::update()
{
plane.mode_qstabilize.update();
}

14
ArduPlane/mode_qhover.cpp Normal file
View File

@ -0,0 +1,14 @@
#include "mode.h"
#include "Plane.h"
bool ModeQHover::_enter()
{
return plane.mode_qstabilize._enter();
}
void ModeQHover::update()
{
plane.mode_qstabilize.update();
}

13
ArduPlane/mode_qland.cpp Normal file
View File

@ -0,0 +1,13 @@
#include "mode.h"
#include "Plane.h"
bool ModeQLand::_enter()
{
return plane.mode_qstabilize._enter();
}
void ModeQLand::update()
{
plane.mode_qstabilize.update();
}

14
ArduPlane/mode_qloiter.cpp Normal file
View File

@ -0,0 +1,14 @@
#include "mode.h"
#include "Plane.h"
bool ModeQLoiter::_enter()
{
return plane.mode_qstabilize._enter();
}
void ModeQLoiter::update()
{
plane.mode_qstabilize.update();
}

13
ArduPlane/mode_qrtl.cpp Normal file
View File

@ -0,0 +1,13 @@
#include "mode.h"
#include "Plane.h"
bool ModeQRTL::_enter()
{
return plane.mode_qstabilize._enter();
}
void ModeQRTL::update()
{
plane.mode_qstabilize.update();
}

40
ArduPlane/mode_qstabilize.cpp Normal file
View File

@ -0,0 +1,40 @@
#include "mode.h"
#include "Plane.h"
bool ModeQStabilize::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_navigation_mode = false;
if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) {
plane.control_mode = plane.previous_mode;
} else {
plane.auto_throttle_mode = false;
plane.auto_state.vtol_mode = true;
}
return true;
}
void ModeQStabilize::update()
{
// set nav_roll and nav_pitch using sticks
int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max);
plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit;
plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit);
float pitch_input = plane.channel_pitch->norm_input();
// Scale from normalized input [-1,1] to centidegrees
if (plane.quadplane.tailsitter_active()) {
// For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX]
plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
} else {
// pitch is further constrained by LIM_PITCH_MIN/MAX which may impose
// tighter (possibly asymmetrical) limits than Q_ANGLE_MAX
if (pitch_input > 0) {
plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max);
} else {
plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max);
}
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get());
}
}

21
ArduPlane/mode_rtl.cpp Normal file
View File

@ -0,0 +1,21 @@
#include "mode.h"
#include "Plane.h"
bool ModeRTL::_enter()
{
plane.throttle_allows_nudging = true;
plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true;
plane.prev_WP_loc = plane.current_loc;
plane.do_RTL(plane.get_RTL_altitude());
return true;
}
void ModeRTL::update()
{
plane.calc_nav_roll();
plane.calc_nav_pitch();
plane.calc_throttle();
}

18
ArduPlane/mode_stabilize.cpp Normal file
View File

@ -0,0 +1,18 @@
#include "mode.h"
#include "Plane.h"
bool ModeStabilize::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true;
}
void ModeStabilize::update()
{
plane.nav_roll_cd = 0;
plane.nav_pitch_cd = 0;
}

44
ArduPlane/mode_training.cpp Normal file
View File

@ -0,0 +1,44 @@
#include "mode.h"
#include "Plane.h"
bool ModeTraining::_enter()
{
plane.throttle_allows_nudging = false;
plane.auto_throttle_mode = false;
plane.auto_navigation_mode = false;
return true;
}
void ModeTraining::update()
{
plane.training_manual_roll = false;
plane.training_manual_pitch = false;
plane.update_load_factor();
// if the roll is past the set roll limit, then
// we set target roll to the limit
if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) {
plane.nav_roll_cd = plane.roll_limit_cd;
} else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) {
plane.nav_roll_cd = -plane.roll_limit_cd;
} else {
plane.training_manual_roll = true;
plane.nav_roll_cd = 0;
}
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) {
plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd;
} else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) {
plane.nav_pitch_cd = plane.pitch_limit_min_cd;
} else {
plane.training_manual_pitch = true;
plane.nav_pitch_cd = 0;
}
if (plane.fly_inverted()) {
plane.nav_pitch_cd = -plane.nav_pitch_cd;
}
}

12
ArduPlane/navigation.cpp
View File

@ -108,7 +108,7 @@ void Plane::calc_airspeed_errors()
ahrs.airspeed_estimate(&airspeed_measured);
// FBW_B/cruise airspeed target
if (!failsafe.rc_failsafe && (control_mode == FLY_BY_WIRE_B || control_mode == CRUISE)) {
if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) {
if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) {
target_airspeed_cm = aparm.airspeed_cruise_cm;
} else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) {
@ -141,7 +141,7 @@ void Plane::calc_airspeed_errors()
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();
} else if ((control_mode == AUTO) &&
} else if ((control_mode == &mode_auto) &&
(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
@ -162,7 +162,7 @@ void Plane::calc_airspeed_errors()
// above.
if (auto_throttle_mode &&
aparm.min_gndspeed_cm > 0 &&
control_mode != CIRCLE) {
control_mode != &mode_circle) {
int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed_cm) {
target_airspeed_cm = min_gnd_target_airspeed;
@ -223,10 +223,10 @@ void Plane::update_loiter(uint16_t radius)
quadplane.guided_start();
}
} else if ((loiter.start_time_ms == 0 &&
(control_mode == AUTO || control_mode == GUIDED) &&
(control_mode == &mode_auto || control_mode == &mode_guided) &&
auto_state.crosstrack &&
current_loc.get_distance(next_WP_loc) > radius*3) ||
(control_mode == RTL && quadplane.available() && quadplane.rtl_mode == 1)) {
(control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) {
/*
if never reached loiter point and using crosstrack and somewhat far away from loiter point
navigate to it like in auto-mode for normal crosstrack behavior
@ -245,7 +245,7 @@ void Plane::update_loiter(uint16_t radius)
auto_state.wp_proportion > 1) {
// we've reached the target, start the timer
loiter.start_time_ms = millis();
if (control_mode == GUIDED || control_mode == AVOID_ADSB) {
if (control_mode == &mode_guided || control_mode == &mode_avoidADSB) {
// starting a loiter in GUIDED means we just reached the target point
gcs().send_mission_item_reached_message(0);
}

2
ArduPlane/qautotune.cpp
View File

@ -13,7 +13,7 @@ bool QAutoTune::init()
}
// use position hold while tuning if we were in QLOITER
bool position_hold = (plane.previous_mode == QLOITER);
bool position_hold = (plane.previous_mode == &plane.mode_qloiter);
return init_internals(position_hold,
plane.quadplane.attitude_control,

108
ArduPlane/quadplane.cpp
View File

@ -1022,7 +1022,7 @@ bool QuadPlane::is_flying(void)
if (!available()) {
return false;
}
if (plane.control_mode == GUIDED && guided_takeoff) {
if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
return true;
}
if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) {
@ -1069,13 +1069,13 @@ bool QuadPlane::is_flying_vtol(void) const
// if we are demanding more than 1% throttle then don't consider aircraft landed
return true;
}
if (plane.control_mode == QACRO) {
if (plane.control_mode == &plane.mode_qacro) {
return true;
}
if (plane.control_mode == GUIDED && guided_takeoff) {
if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
return true;
}
if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || plane.control_mode == QAUTOTUNE) {
if (plane.control_mode == &plane.mode_qstabilize || plane.control_mode == &plane.mode_qhover || plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qautotune) {
// in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
return plane.get_throttle_input() > 0;
}
@ -1160,7 +1160,7 @@ void QuadPlane::control_loiter()
plane.nav_pitch_cd,
get_desired_yaw_rate_cds());
if (plane.control_mode == QLAND) {
if (plane.control_mode == &plane.mode_qland) {
if (poscontrol.state < QPOS_LAND_FINAL && check_land_final()) {
poscontrol.state = QPOS_LAND_FINAL;
// cut IC engine if enabled
@ -1172,7 +1172,7 @@ void QuadPlane::control_loiter()
float descent_rate = (poscontrol.state == QPOS_LAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground);
pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
check_land_complete();
} else if (plane.control_mode == GUIDED && guided_takeoff) {
} else if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, false);
} else {
// update altitude target and call position controller
@ -1362,9 +1362,9 @@ bool QuadPlane::assistance_needed(float aspeed)
*/
void QuadPlane::update_transition(void)
{
if (plane.control_mode == MANUAL ||
plane.control_mode == ACRO ||
plane.control_mode == TRAINING) {
if (plane.control_mode == &plane.mode_manual ||
plane.control_mode == &plane.mode_acro ||
plane.control_mode == &plane.mode_training) {
// in manual modes quad motors are always off
if (!tilt.motors_active && !is_tailsitter()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
@ -1387,7 +1387,7 @@ void QuadPlane::update_transition(void)
(transition_failure > 0) &&
((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit");
plane.set_mode(QLAND, MODE_REASON_VTOL_FAILED_TRANSITION);
plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TRANSITION);
}
float aspeed;
@ -1724,7 +1724,7 @@ void QuadPlane::update_throttle_suppression(void)
}
// allow for takeoff
if (plane.control_mode == AUTO && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
if (plane.control_mode == &plane.mode_auto && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
return;
}
@ -1776,7 +1776,7 @@ void QuadPlane::motors_output(bool run_rate_controller)
motors->output();
return;
}
if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == QSTABILIZE) {
if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == &plane.mode_qstabilize) {
// output is direct from run_esc_calibration()
return;
}
@ -1820,27 +1820,27 @@ void QuadPlane::control_run(void)
return;
}
switch (plane.control_mode) {
case QACRO:
switch (plane.control_mode->mode_number()) {
case Mode::Number::QACRO:
control_qacro();
// QACRO uses only the multicopter controller
// so skip the Plane attitude control calls below
return;
case QSTABILIZE:
case Mode::Number::QSTABILIZE:
control_stabilize();
break;
case QHOVER:
case Mode::Number::QHOVER:
control_hover();
break;
case QLOITER:
case QLAND:
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
control_loiter();
break;
case QRTL:
case Mode::Number::QRTL:
control_qrtl();
break;
#if QAUTOTUNE_ENABLED
case QAUTOTUNE:
case Mode::Number::QAUTOTUNE:
qautotune.run();
break;
#endif
@ -1868,30 +1868,30 @@ bool QuadPlane::init_mode(void)
AP_Notify::flags.esc_calibration = false;
switch (plane.control_mode) {
case QSTABILIZE:
switch (plane.control_mode->mode_number()) {
case Mode::Number::QSTABILIZE:
init_stabilize();
break;
case QHOVER:
case Mode::Number::QHOVER:
init_hover();
break;
case QLOITER:
case Mode::Number::QLOITER:
init_loiter();
break;
case QLAND:
case Mode::Number::QLAND:
init_qland();
break;
case QRTL:
case Mode::Number::QRTL:
init_qrtl();
break;
case GUIDED:
case Mode::Number::GUIDED:
guided_takeoff = false;
break;
#if QAUTOTUNE_ENABLED
case QAUTOTUNE:
case Mode::Number::QAUTOTUNE:
return qautotune.init();
#endif
case QACRO:
case Mode::Number::QACRO:
init_qacro();
break;
default:
@ -1909,7 +1909,7 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state)
gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL not available");
return false;
}
if (plane.control_mode != AUTO) {
if (plane.control_mode != &plane.mode_auto) {
gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO");
return false;
}
@ -1945,7 +1945,7 @@ bool QuadPlane::in_vtol_auto(void) const
if (!available()) {
return false;
}
if (plane.control_mode != AUTO) {
if (plane.control_mode != &plane.mode_auto) {
return false;
}
if (plane.auto_state.vtol_mode) {
@ -1978,14 +1978,14 @@ bool QuadPlane::in_vtol_mode(void) const
if (!available()) {
return false;
}
return (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QLOITER ||
plane.control_mode == QLAND ||
plane.control_mode == QRTL ||
plane.control_mode == QACRO ||
plane.control_mode == QAUTOTUNE ||
((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) ||
return (plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qloiter ||
plane.control_mode == &plane.mode_qland ||
plane.control_mode == &plane.mode_qrtl ||
plane.control_mode == &plane.mode_qacro ||
plane.control_mode == &plane.mode_qautotune ||
((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) ||
in_vtol_auto());
}
@ -2146,7 +2146,7 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_POSITION1:
case QPOS_POSITION2: {
bool vtol_loiter_auto = false;
if (plane.control_mode == AUTO) {
if (plane.control_mode == &plane.mode_auto) {
switch (plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_LOITER_TIME:
@ -2156,7 +2156,7 @@ void QuadPlane::vtol_position_controller(void)
break;
}
}
if (plane.control_mode == QRTL || plane.control_mode == GUIDED || vtol_loiter_auto) {
if (plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_guided || vtol_loiter_auto) {
plane.ahrs.get_position(plane.current_loc);
float target_altitude = plane.next_WP_loc.alt;
if (poscontrol.slow_descent) {
@ -2531,7 +2531,7 @@ bool QuadPlane::verify_vtol_land(void)
plane.auto_state.wp_distance < 2) {
poscontrol.state = QPOS_LAND_DESCEND;
gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
if (plane.control_mode == AUTO) {
if (plane.control_mode == &plane.mode_auto) {
// set height to mission height, so we can use the mission
// WP height for triggering land final if no rangefinder
// available
@ -2562,7 +2562,7 @@ void QuadPlane::Log_Write_QControl_Tuning()
{
float des_alt_m = 0.0f;
int16_t target_climb_rate_cms = 0;
if (plane.control_mode != QSTABILIZE) {
if (plane.control_mode != &plane.mode_qstabilize) {
des_alt_m = pos_control->get_alt_target() / 100.0f;
target_climb_rate_cms = pos_control->get_vel_target_z();
}
@ -2605,9 +2605,9 @@ int8_t QuadPlane::forward_throttle_pct(void)
if (!in_vtol_mode() ||
!motors->armed() ||
vel_forward.gain <= 0 ||
plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QAUTOTUNE ||
plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qautotune ||
motors->get_desired_spool_state() < AP_Motors::DESIRED_GROUND_IDLE) {
return 0;
}
@ -2694,9 +2694,9 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
if (!in_vtol_mode() ||
!motors->armed() ||
weathervane.gain <= 0 ||
plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QAUTOTUNE) {
plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qautotune) {
weathervane.last_output = 0;
return 0;
}
@ -2750,7 +2750,7 @@ void QuadPlane::guided_start(void)
*/
void QuadPlane::guided_update(void)
{
if (plane.control_mode == GUIDED && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
if (plane.control_mode == &plane.mode_guided && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) {
throttle_wait = false;
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
takeoff_controller();
@ -2778,7 +2778,7 @@ bool QuadPlane::guided_mode_enabled(void)
return false;
}
// only use quadplane guided when in AUTO or GUIDED mode
if (plane.control_mode != GUIDED && plane.control_mode != AUTO) {
if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
return false;
}
return guided_mode != 0;
@ -2806,7 +2806,7 @@ void QuadPlane::adjust_alt_target(float altitude_cm)
// user initiated takeoff for guided mode
bool QuadPlane::do_user_takeoff(float takeoff_altitude)
{
if (plane.control_mode != GUIDED) {
if (plane.control_mode != &plane.mode_guided) {
gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode");
return false;
}
@ -2831,7 +2831,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
// return true if the wp_nav controller is being updated
bool QuadPlane::using_wp_nav(void) const
{
return plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL;
return plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland || plane.control_mode == &plane.mode_qrtl;
}
/*
@ -2917,7 +2917,7 @@ void QuadPlane::update_throttle_thr_mix(void)
return;
}
if (plane.control_mode == QSTABILIZE) {
if (plane.control_mode == &plane.mode_qstabilize) {
// manual throttle
if (plane.get_throttle_input() <= 0) {
attitude_control->set_throttle_mix_min();

11
ArduPlane/quadplane.h
View File

@ -25,6 +25,17 @@ public:
friend class QAutoTune;
friend class AP_Arming_Plane;
friend class Mode;
friend class ModeAuto;
friend class ModeAvoidADSB;
friend class ModeGuided;
friend class ModeQHover;
friend class ModeQLand;
friend class ModeQLoiter;
friend class ModeQRTL;
friend class ModeQStabilize;
friend class ModeQAutotune;
QuadPlane(AP_AHRS_NavEKF &_ahrs);
// var_info for holding Parameter information

23
ArduPlane/radio.cpp
View File

@ -128,7 +128,7 @@ void Plane::rudder_arm_disarm_check()
// if not in a manual throttle mode and not in CRUISE or FBWB
// modes then disallow rudder arming/disarming
if (auto_throttle_mode &&
(control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) {
(control_mode != &mode_cruise && control_mode != &mode_fbwb)) {
rudder_arm_timer = 0;
return;
}
@ -189,7 +189,7 @@ void Plane::read_radio()
failsafe.last_valid_rc_ms = millis();
if (control_mode == TRAINING) {
if (control_mode == &mode_training) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->recompute_pwm_no_deadzone();
@ -218,7 +218,7 @@ void Plane::read_radio()
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode);
tuning.check_input(control_mode->mode_number());
}
int16_t Plane::rudder_input(void)
@ -230,7 +230,7 @@ int16_t Plane::rudder_input(void)
}
if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) &&
!(control_mode == MANUAL || control_mode == STABILIZE || control_mode == ACRO)) {
!(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) {
// the user does not want any input except in these modes
return 0;
}
@ -258,13 +258,14 @@ void Plane::control_failsafe()
channel_pitch->set_control_in(0);
channel_rudder->set_control_in(0);
switch (control_mode) {
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND: // throttle is ignored, but reset anyways
case QRTL: // throttle is ignored, but reset anyways
case QAUTOTUNE:
switch (control_mode->mode_number()) {
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND: // throttle is ignored, but reset anyways
case Mode::Number::QRTL: // throttle is ignored, but reset anyways
case Mode::Number::QACRO:
case Mode::Number::QAUTOTUNE:
if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DESIRED_GROUND_IDLE) {
// set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
channel_throttle->set_control_in(channel_throttle->get_range() / 2);

18
ArduPlane/reverse_thrust.cpp
View File

@ -29,8 +29,8 @@ bool Plane::allow_reverse_thrust(void) const
return false;
}
switch (control_mode) {
case AUTO:
switch (control_mode->mode_number()) {
case Mode::Number::AUTO:
{
uint16_t nav_cmd = mission.get_current_nav_cmd().id;
@ -64,23 +64,23 @@ bool Plane::allow_reverse_thrust(void) const
}
break;
case LOITER:
case Mode::Number::LOITER:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER);
break;
case RTL:
case Mode::Number::RTL:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL);
break;
case CIRCLE:
case Mode::Number::CIRCLE:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE);
break;
case CRUISE:
case Mode::Number::CRUISE:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE);
break;
case FLY_BY_WIRE_B:
case Mode::Number::FLY_BY_WIRE_B:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB);
break;
case AVOID_ADSB:
case GUIDED:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED);
break;
default:

38
ArduPlane/servos.cpp
View File

@ -25,7 +25,7 @@
void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
{
uint8_t slewrate = aparm.throttle_slewrate;
if (control_mode==AUTO) {
if (control_mode == &mode_auto) {
if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
slewrate = g.takeoff_throttle_slewrate;
} else if (landing.get_throttle_slewrate() != 0 && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
@ -75,14 +75,14 @@ bool Plane::suppress_throttle(void)
return false;
}
if (control_mode==AUTO && g.auto_fbw_steer == 42) {
if (control_mode == &mode_auto && g.auto_fbw_steer == 42) {
// user has throttle control
return false;
}
bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5);
if (control_mode==AUTO &&
if (control_mode == &mode_auto &&
auto_state.takeoff_complete == false) {
uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000;
@ -391,11 +391,11 @@ void Plane::set_servos_controlled(void)
// manual pass through of throttle while throttle is suppressed
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
}
} else if (control_mode == STABILIZE ||
control_mode == TRAINING ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE) {
} else if (control_mode == &mode_stabilize ||
control_mode == &mode_training ||
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune) {
// a manual throttle mode
if (failsafe.throttle_counter) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
@ -406,7 +406,7 @@ void Plane::set_servos_controlled(void)
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
}
} else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) &&
} else if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) &&
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
@ -418,8 +418,8 @@ void Plane::set_servos_controlled(void)
#if SOARING_ENABLED == ENABLED
// suppress throttle when soaring is active
if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE ||
control_mode == AUTO || control_mode == LOITER) &&
if ((control_mode == &mode_fbwb || control_mode == &mode_cruise ||
control_mode == &mode_auto || control_mode == &mode_loiter) &&
g2.soaring_controller.is_active() &&
g2.soaring_controller.get_throttle_suppressed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
@ -464,7 +464,7 @@ void Plane::set_servos_flaps(void)
better than speed based flaps as it leads to less
possibility of oscillation
*/
if (control_mode == AUTO) {
if (control_mode == &mode_auto) {
switch (flight_stage) {
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND:
@ -521,7 +521,7 @@ void Plane::change_landing_gear(AP_LandingGear::LandingGearCommand cmd)
*/
void Plane::set_landing_gear(void)
{
if (control_mode == AUTO && hal.util->get_soft_armed() && is_flying()) {
if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying()) {
AP_LandingGear::LandingGearCommand cmd = (AP_LandingGear::LandingGearCommand)gear.last_auto_cmd;
switch (flight_stage) {
case AP_Vehicle::FixedWing::FLIGHT_LAND:
@ -634,7 +634,7 @@ void Plane::set_servos(void)
// do any transition updates for quadplane
quadplane.update();
if (control_mode == AUTO && auto_state.idle_mode) {
if (control_mode == &mode_auto && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
servos_output();
@ -659,14 +659,14 @@ void Plane::set_servos(void)
// clear ground_steering to ensure manual control if the yaw stabilizer doesn't run
steering_control.ground_steering = false;
if (control_mode == TRAINING) {
if (control_mode == &mode_training) {
steering_control.rudder = channel_rudder->get_control_in();
}
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder);
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering);
if (control_mode == MANUAL) {
if (control_mode == &mode_manual) {
set_servos_manual_passthrough();
} else {
set_servos_controlled();
@ -726,7 +726,7 @@ void Plane::set_servos(void)
#endif
if (landing.get_then_servos_neutral() > 0 &&
control_mode == AUTO &&
control_mode == &mode_auto &&
landing.get_disarm_delay() > 0 &&
landing.is_complete() &&
!arming.is_armed()) {
@ -776,7 +776,7 @@ void Plane::servos_output(void)
servo_output_mixers();
// support MANUAL_RCMASK
if (g2.manual_rc_mask.get() != 0 && control_mode == MANUAL) {
if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
SRV_Channels::copy_radio_in_out_mask(uint16_t(g2.manual_rc_mask.get()));
}
@ -804,7 +804,7 @@ void Plane::update_throttle_hover() {
void Plane::servos_auto_trim(void)
{
// only in auto modes and FBWA
if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) {
if (!auto_throttle_mode && control_mode != &mode_fbwa) {
return;
}
if (!hal.util->get_soft_armed()) {

38
ArduPlane/soaring.cpp
View File

@ -16,18 +16,18 @@ void Plane::update_soaring() {
g2.soaring_controller.update_vario();
// Check for throttle suppression change.
switch (control_mode){
case AUTO:
switch (control_mode->mode_number()) {
case Mode::Number::AUTO:
g2.soaring_controller.suppress_throttle();
break;
case FLY_BY_WIRE_B:
case CRUISE:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
if (!g2.soaring_controller.suppress_throttle()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
set_mode(RTL, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING);
set_mode(mode_rtl, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING);
}
break;
case LOITER:
case Mode::Number::LOITER:
// Do nothing. We will switch back to auto/rtl before enabling throttle.
break;
default:
@ -42,44 +42,44 @@ void Plane::update_soaring() {
return;
}
switch (control_mode){
case AUTO:
case FLY_BY_WIRE_B:
case CRUISE:
switch (control_mode->mode_number()) {
case Mode::Number::AUTO:
case Mode::Number::FLY_BY_WIRE_B:
case Mode::Number::CRUISE:
// Test for switch into thermalling mode
g2.soaring_controller.update_cruising();
if (g2.soaring_controller.check_thermal_criteria()) {
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
set_mode(LOITER, MODE_REASON_SOARING_THERMAL_DETECTED);
set_mode(mode_loiter, MODE_REASON_SOARING_THERMAL_DETECTED);
}
break;
case LOITER:
case Mode::Number::LOITER:
// Update thermal estimate and check for switch back to AUTO
g2.soaring_controller.update_thermalling(); // Update estimate
if (g2.soaring_controller.check_cruise_criteria()) {
// Exit as soon as thermal state estimate deteriorates
switch (previous_mode) {
case FLY_BY_WIRE_B:
switch (previous_mode->mode_number()) {
case Mode::Number::FLY_BY_WIRE_B:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
set_mode(RTL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
set_mode(mode_rtl, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case CRUISE: {
case Mode::Number::CRUISE: {
// return to cruise with old ground course
CruiseState cruise = cruise_state;
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
set_mode(CRUISE, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
set_mode(mode_cruise, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
cruise_state = cruise;
set_target_altitude_current();
break;
}
case AUTO:
case Mode::Number::AUTO:
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
set_mode(AUTO, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
set_mode(mode_auto, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
default:

339
ArduPlane/system.cpp
View File

@ -74,7 +74,7 @@ void Plane::init_ardupilot()
// initialise notify system
notify.init();
notify_flight_mode(control_mode);
notify_mode(*control_mode);
init_rc_out_main();
@ -161,7 +161,7 @@ void Plane::init_ardupilot()
// choose the nav controller
set_nav_controller();
set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
set_mode_by_number((enum Mode::Number)g.initial_mode.get(), MODE_REASON_UNKNOWN);
// set the correct flight mode
// ---------------------------
@ -188,7 +188,7 @@ void Plane::init_ardupilot()
//********************************************************************************
void Plane::startup_ground(void)
{
set_mode(INITIALISING, MODE_REASON_UNKNOWN);
set_mode(mode_initializing, MODE_REASON_UNKNOWN);
#if (GROUND_START_DELAY > 0)
gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay");
@ -234,62 +234,37 @@ void Plane::startup_ground(void)
gcs().send_text(MAV_SEVERITY_INFO,"Ground start complete");
}
enum FlightMode Plane::get_previous_mode() {
return previous_mode;
}
void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason)
{
#if !QAUTOTUNE_ENABLED
if (mode == QAUTOTUNE) {
if (&new_mode == plane.mode_qautotune) {
gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled");
mode = QHOVER;
new_mode = plane.mode_qhover;
}
#endif
if(control_mode == mode) {
if (control_mode == &new_mode) {
// don't switch modes if we are already in the correct mode.
return;
return true;
}
if(g.auto_trim > 0 && control_mode == MANUAL) {
trim_radio();
}
// backup current control_mode and previous_mode
Mode &old_previous_mode = *previous_mode;
Mode &old_mode = *control_mode;
// perform any cleanup required for prev flight mode
exit_mode(control_mode);
// cancel inverted flight
auto_state.inverted_flight = false;
// don't cross-track when starting a mission
auto_state.next_wp_crosstrack = false;
// reset landing check
auto_state.checked_for_autoland = false;
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
crash_state.is_crashed = false;
crash_state.impact_detected = false;
// reset external attitude guidance
guided_state.last_forced_rpy_ms.zero();
guided_state.last_forced_throttle_ms = 0;
// set mode
// update control_mode assuming success
// TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
previous_mode = control_mode;
control_mode = mode;
control_mode = &new_mode;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
#if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == AUTO);
camera.set_is_auto_mode(control_mode == &mode_auto);
#endif
if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) {
if (previous_mode == &mode_autotune && control_mode != &mode_autotune) {
// restore last gains
autotune_restore();
}
@ -304,200 +279,45 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
// start with previous WP at current location
prev_WP_loc = current_loc;
// new mode means new loiter
loiter.start_time_ms = 0;
// attempt to enter new mode
if (!new_mode.enter()) {
// Log error that we failed to enter desired flight mode
gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
// record time of mode change
last_mode_change_ms = AP_HAL::millis();
// assume non-VTOL mode
auto_state.vtol_mode = false;
auto_state.vtol_loiter = false;
switch(control_mode)
{
case INITIALISING:
throttle_allows_nudging = true;
auto_throttle_mode = true;
auto_navigation_mode = false;
break;
case MANUAL:
case STABILIZE:
case TRAINING:
case FLY_BY_WIRE_A:
throttle_allows_nudging = false;
auto_throttle_mode = false;
auto_navigation_mode = false;
break;
case AUTOTUNE:
throttle_allows_nudging = false;
auto_throttle_mode = false;
auto_navigation_mode = false;
autotune_start();
break;
case ACRO:
throttle_allows_nudging = false;
auto_throttle_mode = false;
auto_navigation_mode = false;
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
throttle_allows_nudging = false;
auto_throttle_mode = true;
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
#if SOARING_ENABLED == ENABLED
// for ArduSoar soaring_controller
g2.soaring_controller.init_cruising();
#endif
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
throttle_allows_nudging = false;
auto_throttle_mode = true;
auto_navigation_mode = false;
#if SOARING_ENABLED == ENABLED
// for ArduSoar soaring_controller
g2.soaring_controller.init_cruising();
#endif
set_target_altitude_current();
break;
case CIRCLE:
// the altitude to circle at is taken from the current altitude
throttle_allows_nudging = false;
auto_throttle_mode = true;
auto_navigation_mode = true;
next_WP_loc.alt = current_loc.alt;
break;
case AUTO:
throttle_allows_nudging = true;
auto_throttle_mode = true;
auto_navigation_mode = true;
if (quadplane.available() && quadplane.enable == 2) {
auto_state.vtol_mode = true;
} else {
auto_state.vtol_mode = false;
}
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
#if SOARING_ENABLED == ENABLED
g2.soaring_controller.init_cruising();
#endif
break;
case RTL:
throttle_allows_nudging = true;
auto_throttle_mode = true;
auto_navigation_mode = true;
prev_WP_loc = current_loc;
do_RTL(get_RTL_altitude());
break;
case LOITER:
throttle_allows_nudging = true;
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.is_active() &&
g2.soaring_controller.suppress_throttle()) {
g2.soaring_controller.init_thermalling();
g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path
}
#endif
break;
case AVOID_ADSB:
case GUIDED:
throttle_allows_nudging = true;
auto_throttle_mode = true;
auto_navigation_mode = true;
guided_throttle_passthru = false;
/*
when entering guided mode we set the target as the current
location. This matches the behaviour of the copter code
*/
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
case QLAND:
case QRTL:
case QAUTOTUNE:
case QACRO:
throttle_allows_nudging = true;
auto_navigation_mode = false;
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
// we failed entering new mode, roll back to old
previous_mode = &old_previous_mode;
control_mode = &old_mode;
return false;
}
// start with throttle suppressed in auto_throttle modes
throttle_suppressed = auto_throttle_mode;
// exit previous mode
old_mode.exit();
adsb.set_is_auto_mode(auto_navigation_mode);
// record reasons
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
logger.Write_Mode(control_mode, control_mode_reason);
// update notify with flight mode change
notify_flight_mode(control_mode);
// log and notify mode change
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
notify_mode(*control_mode);
// reset steering integrator on mode change
steerController.reset_I();
// update RC failsafe, as mode change may have necessitated changing the failsafe throttle
control_failsafe();
return true;
}
// exit_mode - perform any cleanup required when leaving a flight mode
void Plane::exit_mode(enum FlightMode mode)
bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason)
{
// stop mission when we leave auto
switch (mode) {
case AUTO:
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
!quadplane.is_vtol_land(mission.get_current_nav_cmd().id))
{
landing.restart_landing_sequence();
}
}
auto_state.started_flying_in_auto_ms = 0;
break;
#if QAUTOTUNE_ENABLED
case QAUTOTUNE:
quadplane.qautotune.stop();
break;
#endif
default:
break;
Mode *new_mode = plane.mode_from_mode_num(new_mode_number);
if (new_mode == nullptr) {
gcs().send_text(MAV_SEVERITY_INFO, "Error: invalid mode number: %d", new_mode_number);
return false;
}
return set_mode(*new_mode, reason);
}
void Plane::check_long_failsafe()
@ -514,7 +334,7 @@ void Plane::check_long_failsafe()
if (failsafe.rc_failsafe &&
(tnow - radio_timeout_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO &&
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto &&
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
@ -613,79 +433,10 @@ void Plane::update_notify()
}
// sets notify object flight mode information
void Plane::notify_flight_mode(enum FlightMode mode)
void Plane::notify_mode(const Mode& mode)
{
AP_Notify::flags.flight_mode = mode;
// set flight mode string
switch (mode) {
case MANUAL:
notify.set_flight_mode_str("MANU");
break;
case CIRCLE:
notify.set_flight_mode_str("CIRC");
break;
case STABILIZE:
notify.set_flight_mode_str("STAB");
break;
case TRAINING:
notify.set_flight_mode_str("TRAN");
break;
case ACRO:
notify.set_flight_mode_str("ACRO");
break;
case FLY_BY_WIRE_A:
notify.set_flight_mode_str("FBWA");
break;
case FLY_BY_WIRE_B:
notify.set_flight_mode_str("FBWB");
break;
case CRUISE:
notify.set_flight_mode_str("CRUS");
break;
case AUTOTUNE:
notify.set_flight_mode_str("ATUN");
break;
case AUTO:
notify.set_flight_mode_str("AUTO");
break;
case RTL:
notify.set_flight_mode_str("RTL ");
break;
case LOITER:
notify.set_flight_mode_str("LOITER");
break;
case AVOID_ADSB:
notify.set_flight_mode_str("AVOI");
break;
case GUIDED:
notify.set_flight_mode_str("GUID");
break;
case INITIALISING:
notify.set_flight_mode_str("INIT");
break;
case QSTABILIZE:
notify.set_flight_mode_str("QSTB");
break;
case QHOVER:
notify.set_flight_mode_str("QHOV");
break;
case QLOITER:
notify.set_flight_mode_str("QLOT");
break;
case QLAND:
notify.set_flight_mode_str("QLND");
break;
case QRTL:
notify.set_flight_mode_str("QRTL");
break;
case QAUTOTUNE:
notify.set_flight_mode_str("QAUTOTUNE");
break;
default:
notify.set_flight_mode_str("----");
break;
}
notify.flags.flight_mode = mode.mode_number();
notify.set_flight_mode_str(mode.name4());
}
/*
@ -746,7 +497,7 @@ bool Plane::disarm_motors(void)
if (!arming.disarm()) {
return false;
}
if (control_mode != AUTO) {
if (control_mode != &mode_auto) {
// reset the mission on disarm if we are not in auto
mission.reset();
}

4
ArduPlane/takeoff.cpp
View File

@ -227,8 +227,8 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)
*/
int8_t Plane::takeoff_tail_hold(void)
{
bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) ||
(control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode));
bool in_takeoff = ((control_mode == &mode_auto && !auto_state.takeoff_complete) ||
(control_mode == &mode_fbwa && auto_state.fbwa_tdrag_takeoff_mode));
if (!in_takeoff) {
// not in takeoff
return 0;

8
ArduPlane/tiltrotor.cpp
View File

@ -19,7 +19,7 @@ float QuadPlane::tilt_max_change(bool up)
}
if (tilt.tilt_type != TILT_TYPE_BINARY && !up) {
bool fast_tilt = false;
if (plane.control_mode == MANUAL) {
if (plane.control_mode == &plane.mode_manual) {
fast_tilt = true;
}
if (hal.util->get_soft_armed() && !in_vtol_mode() && !assisted_flight) {
@ -109,9 +109,9 @@ void QuadPlane::tiltrotor_continuous_update(void)
3) if we are in TRANSITION_TIMER mode then we are transitioning
to forward flight and should put the rotors all the way forward
*/
if (plane.control_mode == QSTABILIZE ||
plane.control_mode == QHOVER ||
plane.control_mode == QAUTOTUNE) {
if (plane.control_mode == &plane.mode_qstabilize ||
plane.control_mode == &plane.mode_qhover ||
plane.control_mode == &plane.mode_qautotune) {
tiltrotor_slew(0);
return;
}