Copter: make SuperSimple type-safe

Maxilos · Jun 16, 2020 · 0eb03ba · 0eb03ba
1 parent d35643a
commit 0eb03ba
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 26 deletions.
diff --git 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
@@ -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
@@ -344,7 +344,7 @@ class Copter : public AP_Vehicle {
     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 @@ class Copter : public AP_Vehicle {
     // 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 @@ class Copter : public AP_Vehicle {
 
     // 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
@@ -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
@@ -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
@@ -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: