From 0eb03ba7d5ab31101260a044e6eade5f5fe3d865 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 15 Jun 2020 23:05:09 +1000 Subject: [PATCH] Copter: make SuperSimple type-safe --- ArduCopter/AP_State.cpp | 13 ++++++------- ArduCopter/Copter.cpp | 6 +++--- ArduCopter/Copter.h | 10 ++++++++-- ArduCopter/GCS_Copter.cpp | 4 ++-- ArduCopter/RC_Channel.cpp | 27 +++++++++++++++++---------- ArduCopter/toy_mode.cpp | 4 ++-- 6 files changed, 38 insertions(+), 26 deletions(-) diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index 269fffb08a..fa9e4629eb 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -19,27 +19,26 @@ void Copter::set_auto_armed(bool b) * * @param [in] b 0:false or disabled, 1:true or SIMPLE, 2:SUPERSIMPLE */ -void Copter::set_simple_mode(uint8_t b) +void Copter::set_simple_mode(SimpleMode b) { - if (ap.simple_mode != b) { + if (simple_mode != b) { switch (b) { - case 0: + case SimpleMode::NONE: AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off"); break; - case 1: + case SimpleMode::SIMPLE: AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on"); break; - case 2: - default: + case SimpleMode::SUPERSIMPLE: // initialise super simple heading update_super_simple_bearing(true); AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); break; } - ap.simple_mode = b; + simple_mode = b; } } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index cd5a7cf790..3de628138d 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -545,14 +545,14 @@ void Copter::update_simple_mode(void) float rollx, pitchx; // exit immediately if no new radio frame or not in simple mode - if (ap.simple_mode == 0 || !ap.new_radio_frame) { + if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) { return; } // mark radio frame as consumed ap.new_radio_frame = false; - if (ap.simple_mode == 1) { + if (simple_mode == SimpleMode::SIMPLE) { // rotate roll, pitch input by -initial simple heading (i.e. north facing) rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw; @@ -572,7 +572,7 @@ void Copter::update_simple_mode(void) void Copter::update_super_simple_bearing(bool force_update) { if (!force_update) { - if (ap.simple_mode != 2) { + if (simple_mode != SimpleMode::SUPERSIMPLE) { return; } if (home_distance() < SUPER_SIMPLE_RADIUS) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b3b9bfed10..50f5349700 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -344,7 +344,7 @@ private: typedef union { struct { uint8_t unused1 : 1; // 0 - uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE + uint8_t unused_was_simple_mode : 2; // 1,2 uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised @@ -423,6 +423,12 @@ private: // SIMPLE Mode // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming // or in SuperSimple mode when the vehicle leaves a 20m radius from home. + enum class SimpleMode { + NONE = 0, + SIMPLE = 1, + SUPERSIMPLE = 2, + } simple_mode; + float simple_cos_yaw; float simple_sin_yaw; int32_t super_simple_last_bearing; @@ -622,7 +628,7 @@ private: // AP_State.cpp void set_auto_armed(bool b); - void set_simple_mode(uint8_t b); + void set_simple_mode(SimpleMode b); void set_failsafe_radio(bool b); void set_failsafe_gcs(bool b); void update_using_interlock(); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 3e3a8ce9c0..8fb9bfcb18 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -14,12 +14,12 @@ const char* GCS_Copter::frame_string() const bool GCS_Copter::simple_input_active() const { - return copter.ap.simple_mode == 1; + return copter.simple_mode == Copter::SimpleMode::SIMPLE; } bool GCS_Copter::supersimple_input_active() const { - return copter.ap.simple_mode == 2; + return copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE; } void GCS_Copter::update_vehicle_sensor_status_flags(void) diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index f352a021c3..7c1b6c0afa 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -39,9 +39,9 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) // if none of the Aux Switches are set to Simple or Super Simple Mode then // set Simple Mode using stored parameters from EEPROM if (BIT_IS_SET(copter.g.super_simple, new_pos)) { - copter.set_simple_mode(2); + copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE); } else { - copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos)); + copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE); } } } @@ -170,17 +170,24 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi case AUX_FUNC::SIMPLE_MODE: // low = simple mode off, middle or high position turns simple mode on - copter.set_simple_mode(ch_flag == AuxSwitchPos::HIGH || - ch_flag == AuxSwitchPos::MIDDLE); + copter.set_simple_mode((ch_flag == AuxSwitchPos::LOW) ? Copter::SimpleMode::NONE : Copter::SimpleMode::SIMPLE); break; - case AUX_FUNC::SUPERSIMPLE_MODE: - // low = simple mode off, middle = simple mode, high = super simple mode - // there is an assumption here that the ch_flag - // enumeration's values match the different sorts of - // simple mode used in set_simple_mode - copter.set_simple_mode((uint8_t)ch_flag); + case AUX_FUNC::SUPERSIMPLE_MODE: { + Copter::SimpleMode newmode = Copter::SimpleMode::NONE; + switch (ch_flag) { + case AuxSwitchPos::LOW: + break; + case AuxSwitchPos::MIDDLE: + newmode = Copter::SimpleMode::SIMPLE; + break; + case AuxSwitchPos::HIGH: + newmode = Copter::SimpleMode::SUPERSIMPLE; + break; + } + copter.set_simple_mode(newmode); break; + } case AUX_FUNC::RTL: #if MODE_RTL_ENABLED == ENABLED diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index 7437f7a3b7..5a212f5a2f 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -579,11 +579,11 @@ void ToyMode::update() break; case ACTION_TOGGLE_SIMPLE: - copter.set_simple_mode(copter.ap.simple_mode?0:1); + copter.set_simple_mode(bool(copter.simple_mode)?Copter::SimpleMode::NONE:Copter::SimpleMode::SIMPLE); break; case ACTION_TOGGLE_SSIMPLE: - copter.set_simple_mode(copter.ap.simple_mode?0:2); + copter.set_simple_mode(bool(copter.simple_mode)?Copter::SimpleMode::NONE:Copter::SimpleMode::SUPERSIMPLE); break; case ACTION_ARM_LAND_RTL: