Copter: move guided submode enumeration into ModeGuided

ArduCopter/Copter.h

@@ -791,7 +791,7 @@ private:
 #if FRAME_CONFIG == HELI_FRAME
     void Log_Write_Heli(void);
 #endif
-    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void Log_Write_GuidedTarget(ModeGuided::SubMode submode, const Vector3f& pos_target, const Vector3f& vel_target);
     void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
     void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
     void Log_Write_Vehicle_Startup_Messages();

ArduCopter/GCS_Mavlink.cpp

@@ -126,16 +126,16 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
     if (!copter.flightmode->in_guided_mode()) {
         return;
     }
-    
+
-    const GuidedMode guided_mode = copter.mode_guided.mode();
+    const ModeGuided::SubMode guided_mode = copter.mode_guided.submode();
     Vector3f target_pos;
     Vector3f target_vel;
     uint16_t type_mask;
 
-    if (guided_mode == Guided_WP) {
+    if (guided_mode == ModeGuided::SubMode::WP) {
         type_mask = 0x0FF8; // ignore everything except position
         target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
-    } else if (guided_mode == Guided_Velocity) {
+    } else if (guided_mode == ModeGuided::SubMode::Velocity) {
         type_mask = 0x0FC7; // ignore everything except velocity
         target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s
     } else {

ArduCopter/Log.cpp

@@ -382,12 +382,12 @@ struct PACKED log_GuidedTarget {
 // Write a Guided mode target
 // pos_target is lat, lon, alt OR offset from ekf origin in cm OR roll, pitch, yaw target in centi-degrees
 // vel_target is cm/s
-void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
+void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode target_type, const Vector3f& pos_target, const Vector3f& vel_target)
 {
     struct log_GuidedTarget pkt = {
         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
         time_us         : AP_HAL::micros64(),
-        type            : target_type,
+        type            : (uint8_t)target_type,
         pos_target_x    : pos_target.x,
         pos_target_y    : pos_target.y,
         pos_target_z    : pos_target.z,
@@ -571,7 +571,7 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {}
 void Copter::Log_Write_Data(LogDataID id, float value) {}
 void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {}
 void Copter::Log_Sensor_Health() {}
-void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
+void Copter::Log_Write_GuidedTarget(ModeGuided::SubMode&, const Vector3f& pos_target, const Vector3f& vel_target) {}
 void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {}
 void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {}
 void Copter::Log_Write_Vehicle_Startup_Messages() {}

ArduCopter/defines.h

@@ -97,15 +97,6 @@ enum AutoMode {
     Auto_NavPayloadPlace,
 };
 
-// Guided modes
-enum GuidedMode {
-    Guided_TakeOff,
-    Guided_WP,
-    Guided_Velocity,
-    Guided_PosVel,
-    Guided_Angle,
-};
-
 // Airmode
 enum class AirMode {
     AIRMODE_NONE,

ArduCopter/mode.h

@@ -846,7 +846,15 @@ public:
 
     bool do_user_takeoff_start(float takeoff_alt_cm) override;
 
-    GuidedMode mode() const { return guided_mode; }
+    enum class SubMode {
+        TakeOff,
+        WP,
+        Velocity,
+        PosVel,
+        Angle,
+    };
+
+    SubMode submode() const { return guided_mode; }
 
     void angle_control_start();
     void angle_control_run();
@@ -881,7 +889,7 @@ private:
     bool use_pilot_yaw(void) const;
 
     // controls which controller is run (pos or vel):
-    GuidedMode guided_mode = Guided_TakeOff;
+    SubMode guided_mode = SubMode::TakeOff;
     bool send_notification;     // used to send one time notification to ground station
 
 };

ArduCopter/mode_guided.cpp

@@ -55,12 +55,12 @@ void ModeGuided::run()
     // call the correct auto controller
     switch (guided_mode) {
 
-    case Guided_TakeOff:
+    case SubMode::TakeOff:
         // run takeoff controller
         takeoff_run();
         break;
 
-    case Guided_WP:
+    case SubMode::WP:
         // run position controller
         pos_control_run();
         if (send_notification && wp_nav->reached_wp_destination()) {
@@ -69,17 +69,17 @@ void ModeGuided::run()
         }
         break;
 
-    case Guided_Velocity:
+    case SubMode::Velocity:
         // run velocity controller
         vel_control_run();
         break;
 
-    case Guided_PosVel:
+    case SubMode::PosVel:
         // run position-velocity controller
         posvel_control_run();
         break;
 
-    case Guided_Angle:
+    case SubMode::Angle:
         // run angle controller
         angle_control_run();
         break;
@@ -100,7 +100,7 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
 // do_user_takeoff_start - initialises waypoint controller to implement take-off
 bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 {
-    guided_mode = Guided_TakeOff;
+    guided_mode = SubMode::TakeOff;
 
     // initialise wpnav destination
     Location target_loc = copter.current_loc;
@@ -139,7 +139,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 void ModeGuided::pos_control_start()
 {
     // set to position control mode
-    guided_mode = Guided_WP;
+    guided_mode = SubMode::WP;
 
     // initialise waypoint and spline controller
     wp_nav->wp_and_spline_init();
@@ -159,7 +159,7 @@ void ModeGuided::pos_control_start()
 void ModeGuided::vel_control_start()
 {
     // set guided_mode to velocity controller
-    guided_mode = Guided_Velocity;
+    guided_mode = SubMode::Velocity;
 
     // initialise horizontal speed, acceleration
     pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
@@ -177,7 +177,7 @@ void ModeGuided::vel_control_start()
 void ModeGuided::posvel_control_start()
 {
     // set guided_mode to velocity controller
-    guided_mode = Guided_PosVel;
+    guided_mode = SubMode::PosVel;
 
     pos_control->init_xy_controller();
 
@@ -202,14 +202,14 @@ void ModeGuided::posvel_control_start()
 
 bool ModeGuided::is_taking_off() const
 {
-    return guided_mode == Guided_TakeOff;
+    return guided_mode == SubMode::TakeOff;
 }
 
 // initialise guided mode's angle controller
 void ModeGuided::angle_control_start()
 {
     // set guided_mode to velocity controller
-    guided_mode = Guided_Angle;
+    guided_mode = SubMode::Angle;
 
     // set vertical speed and acceleration
     pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up());
@@ -250,7 +250,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
 #endif
 
     // ensure we are in position control mode
-    if (guided_mode != Guided_WP) {
+    if (guided_mode != SubMode::WP) {
         pos_control_start();
     }
 
@@ -270,7 +270,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
 
 bool ModeGuided::get_wp(Location& destination)
 {
-    if (guided_mode != Guided_WP) {
+    if (guided_mode != SubMode::WP) {
         return false;
     }
     return wp_nav->get_oa_wp_destination(destination);
@@ -292,7 +292,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
 #endif
 
     // ensure we are in position control mode
-    if (guided_mode != Guided_WP) {
+    if (guided_mode != SubMode::WP) {
         pos_control_start();
     }
 
@@ -318,7 +318,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
 void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request)
 {
     // check we are in velocity control mode
-    if (guided_mode != Guided_Velocity) {
+    if (guided_mode != SubMode::Velocity) {
         vel_control_start();
     }
 
@@ -349,7 +349,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
 #endif
 
     // check we are in velocity control mode
-    if (guided_mode != Guided_PosVel) {
+    if (guided_mode != SubMode::PosVel) {
         posvel_control_start();
     }
 
@@ -371,7 +371,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
 void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
 {
     // check we are in velocity control mode
-    if (guided_mode != Guided_Angle) {
+    if (guided_mode != SubMode::Angle) {
         angle_control_start();
     }
 
@@ -784,11 +784,11 @@ bool ModeGuided::limit_check()
 
 uint32_t ModeGuided::wp_distance() const
 {
-    switch(mode()) {
+    switch(guided_mode) {
-    case Guided_WP:
+    case SubMode::WP:
         return wp_nav->get_wp_distance_to_destination();
         break;
-    case Guided_PosVel:
+    case SubMode::PosVel:
         return pos_control->get_pos_error_xy();
         break;
     default:
@@ -798,25 +798,37 @@ uint32_t ModeGuided::wp_distance() const
 
 int32_t ModeGuided::wp_bearing() const
 {
-    switch(mode()) {
+    switch(guided_mode) {
-    case Guided_WP:
+    case SubMode::WP:
         return wp_nav->get_wp_bearing_to_destination();
         break;
-    case Guided_PosVel:
+    case SubMode::PosVel:
         return pos_control->get_bearing_to_target();
         break;
-    default:
+    case SubMode::TakeOff:
+    case SubMode::Velocity:
+    case SubMode::Angle:
+        // these do not have bearings
         return 0;
     }
+    // compiler guarantees we don't get here
+    return 0.0;
 }
 
 float ModeGuided::crosstrack_error() const
 {
-    if (mode() == Guided_WP) {
+    switch (guided_mode) {
+    case SubMode::WP:
         return wp_nav->crosstrack_error();
-    } else {
+    case SubMode::TakeOff:
+    case SubMode::Velocity:
+    case SubMode::PosVel:
+    case SubMode::Angle:
+        // no track to have a crosstrack to
         return 0;
     }
+    // compiler guarantees we don't get here
+    return 0;
 }
 
 #endif