Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Copter: refactoring about control_mode #13431

Merged
merged 5 commits into from
Mar 23, 2021
Merged

Copter: refactoring about control_mode #13431

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a30f7d8
Copter: add mode_number function to Mode class
tatsuy Jan 30, 2020
29fb6a1
Copter: change from control_mode to flightmode
tatsuy Jan 30, 2020
e802404
Copter: add allows_save_trim function to Mode class
tatsuy Feb 7, 2021
4a932de
Copter: add allows_autotune function to Mode class
tatsuy Feb 11, 2020
15fb6bf
Copter: add allows_flip function to Mode class
tatsuy Feb 11, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 3 additions & 5 deletions ArduCopter/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -652,8 +652,6 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
}
#endif

Mode::Number control_mode = copter.control_mode;

// always check if the current mode allows arming
if (!copter.flightmode->allows_arming(method == AP_Arming::Method::MAVLINK)) {
check_failed(true, "Mode not armable");
Expand Down Expand Up @@ -719,15 +717,15 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
}

// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(method == AP_Arming::Method::MAVLINK && (control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::GUIDED_NOGPS))) {
if (!(method == AP_Arming::Method::MAVLINK && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS))) {
// above top of deadband is too always high
if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
return false;
}
// in manual modes throttle must be at zero
#if FRAME_CONFIG != HELI_FRAME
if ((copter.flightmode->has_manual_throttle() || control_mode == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
return false;
}
Expand Down Expand Up @@ -854,7 +852,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_
copter.motors->armed(true);

// log flight mode in case it was changed while vehicle was disarmed
AP::logger().Write_Mode((uint8_t)copter.control_mode, copter.control_mode_reason);
AP::logger().Write_Mode((uint8_t)copter.flightmode->mode_number(), copter.control_mode_reason);

// re-enable failsafe
copter.failsafe_enable();
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/Attitude.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ void Copter::update_throttle_hover()
}

// do not update in manual throttle modes or Drift
if (flightmode->has_manual_throttle() || (control_mode == Mode::Number::DRIFT)) {
if (flightmode->has_manual_throttle() || (copter.flightmode->mode_number() == Mode::Number::DRIFT)) {
return;
}

Expand Down
1 change: 0 additions & 1 deletion ArduCopter/Copter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -650,7 +650,6 @@ bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
Copter::Copter(void)
: logger(g.log_bitmask),
flight_modes(&g.flight_mode1),
control_mode(Mode::Number::STABILIZE),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I not sure that this removal don't have side effect. I will check that.

simple_cos_yaw(1.0f),
super_simple_cos_yaw(1.0),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
Expand Down
5 changes: 2 additions & 3 deletions ArduCopter/Copter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -378,7 +378,7 @@ class Copter : public AP_Vehicle {

// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
Mode::Number control_mode;
Mode *flightmode;
Mode::Number prev_control_mode;

RCMapper rcmap;
Expand Down Expand Up @@ -806,7 +806,7 @@ class Copter : public AP_Vehicle {
// mode.cpp
bool set_mode(Mode::Number mode, ModeReason reason);
bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
uint8_t get_mode() const override { return (uint8_t)control_mode; }
peterbarker marked this conversation as resolved.
Show resolved Hide resolved
uint8_t get_mode() const override { return (uint8_t)flightmode->mode_number(); }
rmackay9 marked this conversation as resolved.
Show resolved Hide resolved
void update_flight_mode();
void notify_flight_mode();

Expand Down Expand Up @@ -909,7 +909,6 @@ class Copter : public AP_Vehicle {
bool get_wp_bearing_deg(float &bearing) const override;
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;

Mode *flightmode;
#if MODE_ACRO_ENABLED == ENABLED
#if FRAME_CONFIG == HELI_FRAME
ModeAcro_Heli mode_acro;
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/GCS_Copter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;

switch (copter.control_mode) {
switch (copter.flightmode->mode_number()) {
case Mode::Number::AUTO:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/GCS_Mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ MAV_MODE GCS_MAVLINK_Copter::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 (copter.control_mode) {
switch (copter.flightmode->mode_number()) {
case Mode::Number::AUTO:
case Mode::Number::RTL:
case Mode::Number::LOITER:
Expand Down Expand Up @@ -72,7 +72,7 @@ MAV_MODE GCS_MAVLINK_Copter::base_mode() const

uint32_t GCS_Copter::custom_mode() const
{
return (uint32_t)copter.control_mode;
return (uint32_t)copter.flightmode->mode_number();
}

MAV_STATE GCS_MAVLINK_Copter::vehicle_system_status() const
Expand Down Expand Up @@ -928,7 +928,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
bool shot_mode = (!is_zero(packet.param1) && (copter.control_mode == Mode::Number::GUIDED || copter.control_mode == Mode::Number::GUIDED_NOGPS));
bool shot_mode = (!is_zero(packet.param1) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS));

if (!shot_mode) {
#if MODE_BRAKE_ENABLED == ENABLED
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/Log.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -618,7 +618,7 @@ void Copter::Log_Write_Vehicle_Startup_Messages()
{
// only 200(?) bytes are guaranteed by AP_Logger
logger.Write_MessageF("Frame: %s/%s", motors->get_frame_string(), motors->get_type_string());
logger.Write_Mode((uint8_t)control_mode, control_mode_reason);
logger.Write_Mode((uint8_t)flightmode->mode_number(), control_mode_reason);
ahrs.Log_Write_Home_And_Origin();
gps.Write_AP_Logger_Log_Startup_messages();
}
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/RC_Channel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
default:
// return to flight mode switch's flight mode if we are currently
// in this mode
if (copter.control_mode == mode) {
if (copter.flightmode->mode_number() == mode) {
rc().reset_mode_switch();
}
}
Expand Down Expand Up @@ -201,7 +201,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi

case AUX_FUNC::SAVE_TRIM:
if ((ch_flag == AuxSwitchPos::HIGH) &&
(copter.control_mode <= Mode::Number::ACRO) &&
(copter.flightmode->allows_save_trim()) &&
(copter.channel_throttle->get_control_in() == 0)) {
copter.save_trim();
}
Expand All @@ -213,7 +213,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi
if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {

// do not allow saving new waypoints while we're in auto or disarmed
if (copter.control_mode == Mode::Number::AUTO || !copter.motors->armed()) {
if (copter.flightmode->mode_number() == Mode::Number::AUTO || !copter.motors->armed()) {
return;
}

Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/afs_copter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
*/
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
{
switch (copter.control_mode) {
switch (copter.flightmode->mode_number()) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This isn't your bug. But Mode::Number::GUIDED_NOGPS is missing here. Some future PR might investigate using is_autopilot here.

case Mode::Number::AUTO:
case Mode::Number::GUIDED:
case Mode::Number::RTL:
Expand Down
12 changes: 6 additions & 6 deletions ArduCopter/avoidance_adsb.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,15 +20,15 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
copter.failsafe.adsb = true;
failsafe_state_change = true;
// record flight mode in case it's required for the recovery
prev_control_mode = copter.control_mode;
prev_control_mode = copter.flightmode->mode_number();
}

// take no action in some flight modes
if (copter.control_mode == Mode::Number::LAND ||
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is also not your bug, but this should be asking if the vehicle is landing for any reason (is_landing) - we could be doing an auto-mode landing and we ought not stop doing that.

Perhaps a future PR...

if (copter.flightmode->mode_number() == Mode::Number::LAND ||
#if MODE_THROW_ENABLED == ENABLED
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should retaining the #if here - the change is unrelated to the PR and I'm not sure its a good one.

copter.control_mode == Mode::Number::THROW ||
copter.flightmode->mode_number() == Mode::Number::THROW ||
#endif
copter.control_mode == Mode::Number::FLIP) {
copter.flightmode->mode_number() == Mode::Number::FLIP) {
actual_action = MAV_COLLISION_ACTION_NONE;
}

Expand Down Expand Up @@ -158,15 +158,15 @@ int16_t AP_Avoidance_Copter::get_altitude_minimum() const
bool AP_Avoidance_Copter::check_flightmode(bool allow_mode_change)
{
// ensure copter is in avoid_adsb mode
if (allow_mode_change && copter.control_mode != Mode::Number::AVOID_ADSB) {
if (allow_mode_change && copter.flightmode->mode_number() != Mode::Number::AVOID_ADSB) {
if (!copter.set_mode(Mode::Number::AVOID_ADSB, ModeReason::AVOIDANCE)) {
// failed to set mode so exit immediately
return false;
}
}

// check flight mode
return (copter.control_mode == Mode::Number::AVOID_ADSB);
return (copter.flightmode->mode_number() == Mode::Number::AVOID_ADSB);
}

bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/baro_ground_effect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void Copter::update_ground_effect_detector(void)
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
bool xy_speed_low = (position_ok() || ekf_has_relative_position()) && xy_speed_cms <= 125.0f;
bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == Mode::Number::ALT_HOLD && small_angle_request);
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (flightmode->mode_number() == Mode::Number::ALT_HOLD && small_angle_request);

bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f;
bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/crash_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,14 @@ void Copter::crash_check()
}

// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) {
if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
crash_counter = 0;
return;
}

#if MODE_AUTOROTATE_ENABLED == ENABLED
//return immediately if in autorotation mode
if (control_mode == Mode::Number::AUTOROTATE) {
if (flightmode->mode_number() == Mode::Number::AUTOROTATE) {
crash_counter = 0;
return;
}
Expand Down Expand Up @@ -266,7 +266,7 @@ void Copter::parachute_check()
}

// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) {
if (flightmode->mode_number() == Mode::Number::ACRO || flightmode->mode_number() == Mode::Number::FLIP) {
control_loss_count = 0;
return;
}
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/ekf_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,7 @@ void Copter::failsafe_ekf_event()
}

// sometimes LAND *does* require GPS so ensure we are in non-GPS land
if (control_mode == Mode::Number::LAND && landing_with_GPS()) {
if (flightmode->mode_number() == Mode::Number::LAND && landing_with_GPS()) {
mode_land.do_not_use_GPS();
return;
}
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/events.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void Copter::failsafe_radio_on_event()
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land;

} else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
} else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode");
desired_action = Failsafe_Action_None;
Expand Down Expand Up @@ -190,7 +190,7 @@ void Copter::failsafe_gcs_on_event(void)
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land;

} else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
} else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
desired_action = Failsafe_Action_None;
Expand Down Expand Up @@ -262,7 +262,7 @@ void Copter::failsafe_terrain_on_event()
if (should_disarm_on_failsafe()) {
arming.disarm(AP_Arming::Method::TERRAINFAILSAFE);
#if MODE_RTL_ENABLED == ENABLED
} else if (control_mode == Mode::Number::RTL) {
} else if (flightmode->mode_number() == Mode::Number::RTL) {
mode_rtl.restart_without_terrain();
#endif
} else {
Expand Down Expand Up @@ -336,7 +336,7 @@ bool Copter::should_disarm_on_failsafe() {
return true;
}

switch (control_mode) {
switch (flightmode->mode_number()) {
case Mode::Number::STABILIZE:
case Mode::Number::ACRO:
// if throttle is zero OR vehicle is landed disarm motors
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/heli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ void Copter::update_heli_control_dynamics(void)
bool Copter::should_use_landing_swash() const
{
if (flightmode->has_manual_throttle() ||
control_mode == Mode::Number::DRIFT) {
flightmode->mode_number() == Mode::Number::DRIFT) {
// manual modes always uses full swash range
return false;
}
Expand Down
11 changes: 5 additions & 6 deletions ArduCopter/mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +185,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
{

// return immediately if we are already in the desired mode
if (mode == control_mode) {
if (mode == flightmode->mode_number()) {
control_mode_reason = reason;
return true;
}
Expand Down Expand Up @@ -271,13 +271,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
exit_mode(flightmode, new_flightmode);

// store previous flight mode (only used by tradeheli's autorotation)
prev_control_mode = control_mode;
prev_control_mode = flightmode->mode_number();

// update flight mode
flightmode = new_flightmode;
control_mode = mode;
control_mode_reason = reason;
logger.Write_Mode((uint8_t)control_mode, reason);
logger.Write_Mode((uint8_t)flightmode->mode_number(), reason);
gcs().send_message(MSG_HEARTBEAT);

#if HAL_ADSB_ENABLED
Expand All @@ -292,7 +291,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#endif

#if CAMERA == ENABLED
camera.set_is_auto_mode(control_mode == Mode::Number::AUTO);
camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO);
#endif

// update notify object
Expand Down Expand Up @@ -402,7 +401,7 @@ void Copter::exit_mode(Mode *&old_flightmode,
// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
void Copter::notify_flight_mode() {
AP_Notify::flags.autopilot_mode = flightmode->is_autopilot();
AP_Notify::flags.flight_mode = (uint8_t)control_mode;
AP_Notify::flags.flight_mode = (uint8_t)flightmode->mode_number();
notify.set_flight_mode_str(flightmode->name4());
}

Expand Down
Loading