Rover: integrate mode class

APMrover2/APMrover2.cpp

@@ -277,7 +277,7 @@ void Rover::update_logging1(void)
 void Rover::update_logging2(void)
 {
     if (should_log(MASK_LOG_STEERING)) {
-        if (control_mode == STEERING || control_mode == AUTO || control_mode == RTL || control_mode == GUIDED) {
+        if (!control_mode->manual_steering()) {
             Log_Write_Steering();
         }
     }
@@ -413,166 +413,12 @@ void Rover::update_GPS_10Hz(void)
 
 void Rover::update_current_mode(void)
 {
-    switch (control_mode) {
-    case AUTO:
-    case RTL:
-        if (!in_auto_reverse) {
-            set_reverse(false);
-        }
-        if (!do_auto_rotation) {
-            calc_lateral_acceleration();
-            calc_nav_steer();
-            calc_throttle(g.speed_cruise);
-        } else {
-            do_yaw_rotation();
-        }
-        break;
-
-    case GUIDED: {
-        if (!in_auto_reverse) {
-            set_reverse(false);
-        }
-        switch (guided_mode) {
-        case Guided_Angle:
-            nav_set_yaw_speed();
-            break;
-
-        case Guided_WP:
-            if (rtl_complete || verify_RTL()) {
-                // we have reached destination so stop where we are
-                if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) {
-                    gcs().send_mission_item_reached_message(0);
-                }
-                g2.motors.set_throttle(g.throttle_min.get());
-                g2.motors.set_steering(0.0f);
-                lateral_acceleration = 0.0f;
-            } else {
-                calc_lateral_acceleration();
-                calc_nav_steer();
-                calc_throttle(rover.guided_control.target_speed);
-                Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
-                                       Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f));
-            }
-            break;
-
-        case Guided_Velocity:
-            nav_set_speed();
-            break;
-
-        default:
-            gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
-            break;
-        }
-        break;
-    }
-
-    case STEERING: {
-        /*
-          in steering mode we control lateral acceleration
-          directly. We first calculate the maximum lateral
-          acceleration at full steering lock for this speed. That is
-          V^2/R where R is the radius of turn. We get the radius of
-          turn from half the STEER2SRV_P.
-         */
-        float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();
-
-        // constrain to user set TURN_MAX_G
-        max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
-
-        lateral_acceleration = max_g_force * (channel_steer->get_control_in()/4500.0f);
-        calc_nav_steer();
-
-        // and throttle gives speed in proportion to cruise speed, up
-        // to 50% throttle, then uses nudging above that.
-        float target_speed = channel_throttle->get_control_in() * 0.01f * 2 * g.speed_cruise;
-        set_reverse(target_speed < 0);
-        if (in_reverse) {
-            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
-        } else {
-            target_speed = constrain_float(target_speed, 0, g.speed_cruise);
-        }
-        calc_throttle(target_speed);
-        break;
-    }
-
-    case LEARNING:
-    case MANUAL:
-        // mark us as in_reverse when using a negative throttle to
-        // stop AHRS getting off
-        set_reverse(is_negative(g2.motors.get_throttle()));
-        break;
-
-    case HOLD:
-        // hold position - stop motors and center steering
-        g2.motors.set_throttle(0.0f);
-        g2.motors.set_steering(0.0f);
-        if (!in_auto_reverse) {
-            set_reverse(false);
-        }
-        break;
-
-    case INITIALISING:
-        break;
-    }
+    control_mode->update();
 }
 
 void Rover::update_navigation()
 {
-    switch (control_mode) {
-    case MANUAL:
-    case HOLD:
-    case LEARNING:
-    case STEERING:
-    case INITIALISING:
-        break;
-
-    case AUTO:
-        mission.update();
-        if (do_auto_rotation) {
-            do_yaw_rotation();
-        }
-        break;
-
-    case RTL:
-        // no loitering around the wp with the rover, goes direct to the wp position
-        if (verify_RTL()) {
-            g2.motors.set_throttle(g.throttle_min.get());
-            set_mode(HOLD);
-        } else {
-            calc_lateral_acceleration();
-            calc_nav_steer();
-        }
-        break;
-
-    case GUIDED:
-        switch (guided_mode) {
-        case Guided_Angle:
-            nav_set_yaw_speed();
-            break;
-
-        case Guided_WP:
-            // no loitering around the wp with the rover, goes direct to the wp position
-            if (rtl_complete || verify_RTL()) {
-                // we have reached destination so stop where we are
-                g2.motors.set_throttle(g.throttle_min.get());
-                g2.motors.set_steering(0.0f);
-                lateral_acceleration = 0.0f;
-            } else {
-                calc_lateral_acceleration();
-                calc_nav_steer();
-            }
-            break;
-
-        case Guided_Velocity:
-            nav_set_speed();
-            break;
-
-        default:
-            gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
-            break;
-        }
-        break;
-    }
+    control_mode->update_navigation();
 }
 
 AP_HAL_MAIN_CALLBACKS(&rover);

APMrover2/GCS_Mavlink.cpp

@@ -7,7 +7,6 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
 {
     uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
     uint8_t system_status = MAV_STATE_ACTIVE;
-    const uint32_t custom_mode = control_mode;
 
     if (failsafe.triggered != 0) {
         system_status = MAV_STATE_CRITICAL;
@@ -21,30 +20,16 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
     // 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 (control_mode) {
-    case MANUAL:
-    case LEARNING:
-    case STEERING:
-        base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-        break;
-    case AUTO:
-    case RTL:
-    case GUIDED:
-        base_mode = MAV_MODE_FLAG_GUIDED_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:
-        system_status = MAV_STATE_CALIBRATING;
-        break;
-    case HOLD:
-        system_status = 0;
-        break;
+    if (control_mode->has_manual_input()) {
+        base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
     }
 
-#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED)
-    if (control_mode != INITIALISING) {
+    if (control_mode->is_autopilot_mode()) {
+        base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+    }
+
+#if defined(ENABLE_STICK_MIXING) && (ENABLE_STICK_MIXING == ENABLED) // TODO ???? Remove !
+    if (control_mode->stick_mixing_enabled()) {
         // all modes except INITIALISING have some form of manual
         // override if stick mixing is enabled
         base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
@@ -54,9 +39,14 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
 #if HIL_MODE != HIL_MODE_DISABLED
     base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 #endif
-
+    if (control_mode == &mode_initializing) {
+        system_status = MAV_STATE_CALIBRATING;
+    }
+    if (control_mode == &mode_hold) {
+        system_status = MAV_STATE_STANDBY;
+    }
     // we are armed if we are not initialising
-    if (control_mode != INITIALISING && arming.is_armed()) {
+    if (control_mode != &mode_initializing && arming.is_armed()) {
         base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
@@ -65,7 +55,7 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
 
     gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_GROUND_ROVER,
                                             base_mode,
-                                            custom_mode,
+                                            control_mode->mode_number(),
                                             system_status);
 }
 
@@ -140,7 +130,7 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
 {
     mavlink_msg_nav_controller_output_send(
         chan,
-        lateral_acceleration,  // use nav_roll to hold demanded Y accel
+        control_mode->lateral_acceleration,  // use nav_roll to hold demanded Y accel
         ahrs.groundspeed() * ins.get_gyro().z,  // use nav_pitch to hold actual Y accel
         nav_controller->nav_bearing_cd() * 0.01f,
         nav_controller->target_bearing_cd() * 0.01f,
@@ -343,7 +333,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
         break;
 
     case MSG_NAV_CONTROLLER_OUTPUT:
-        if (rover.control_mode != MANUAL) {
+        if (rover.control_mode->is_autopilot_mode()) {
             CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
             rover.send_nav_controller_output(chan);
         }
@@ -672,7 +662,7 @@ GCS_MAVLINK_Rover::data_stream_send(void)
     if (stream_trigger(STREAM_EXTRA1)) {
         send_message(MSG_ATTITUDE);
         send_message(MSG_SIMSTATE);
-        if (rover.control_mode != MANUAL) {
+        if (rover.control_mode->is_autopilot_mode()) {
             send_message(MSG_PID_TUNING);
         }
     }
@@ -708,14 +698,11 @@ GCS_MAVLINK_Rover::data_stream_send(void)
 
 bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
-    if (rover.control_mode != GUIDED) {
+    if (rover.control_mode != &rover.mode_guided) {
         // only accept position updates when in GUIDED mode
         return false;
     }
 
-    // This method is only called when we are in Guided WP GUIDED mode
-    rover.guided_mode = Guided_WP;
-
     // make any new wp uploaded instant (in case we are already in Guided mode)
     rover.set_guided_WP(cmd.content.location);
     return true;
@@ -796,7 +783,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             switch (packet.command) {
 
             case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-                rover.set_mode(RTL);
+                rover.set_mode(rover.mode_rtl);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
@@ -863,7 +850,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 break;
 
             case MAV_CMD_MISSION_START:
-                rover.set_mode(AUTO);
+                rover.set_mode(rover.mode_auto);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
@@ -920,19 +907,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             switch (static_cast<uint16_t>(packet.param1)) {
             case MAV_MODE_MANUAL_ARMED:
             case MAV_MODE_MANUAL_DISARMED:
-                rover.set_mode(MANUAL);
+                rover.set_mode(rover.mode_manual);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_AUTO_ARMED:
             case MAV_MODE_AUTO_DISARMED:
-                rover.set_mode(AUTO);
+                rover.set_mode(rover.mode_auto);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
             case MAV_MODE_STABILIZE_DISARMED:
             case MAV_MODE_STABILIZE_ARMED:
-                rover.set_mode(LEARNING);
+                rover.set_mode(rover.mode_learning);
                 result = MAV_RESULT_ACCEPTED;
                 break;
 
@@ -1014,11 +1001,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             // param2 : Speed - normalized to 0 .. 1
 
             // exit if vehicle is not in Guided mode
-            if (rover.control_mode != GUIDED) {
+            if (rover.control_mode != &rover.mode_guided) {
                 break;
             }
 
-            rover.guided_mode = Guided_Angle;
+            rover.mode_guided.guided_mode = ModeGuided::Guided_Angle;
             rover.guided_control.msg_time_ms = AP_HAL::millis();
             rover.guided_control.turn_angle = packet.param1;
             rover.guided_control.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
@@ -1132,7 +1119,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             mavlink_msg_set_attitude_target_decode(msg, &packet);
 
             // exit if vehicle is not in Guided mode
-            if (rover.control_mode != GUIDED) {
+            if (rover.control_mode != &rover.mode_guided) {
                 break;
             }
             // record the time when the last message arrived
@@ -1173,7 +1160,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
 
             // exit if vehicle is not in Guided mode
-            if (rover.control_mode != GUIDED) {
+            if (rover.control_mode != &rover.mode_guided) {
                 break;
             }
 
@@ -1258,7 +1245,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             mavlink_msg_set_position_target_global_int_decode(msg, &packet);
 
             // exit if vehicle is not in Guided mode
-            if (rover.control_mode != GUIDED) {
+            if (rover.control_mode != &rover.mode_guided) {
                 break;
             }
             // check for supported coordinate frames

APMrover2/Log.cpp

@@ -197,7 +197,7 @@ void Rover::Log_Write_Steering()
     struct log_Steering pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
         time_us        : AP_HAL::micros64(),
-        demanded_accel : lateral_acceleration,
+        demanded_accel : control_mode->lateral_acceleration,
         achieved_accel : ahrs.groundspeed() * ins.get_gyro().z,
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
@@ -329,7 +329,7 @@ void Rover::Log_Write_Rangefinder()
     struct log_Rangefinder pkt = {
         LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
         time_us               : AP_HAL::micros64(),
-        lateral_accel         : lateral_acceleration,
+        lateral_accel   : control_mode->lateral_acceleration,
         rangefinder1_distance : rangefinder.distance_cm(0),
         rangefinder2_distance : rangefinder.distance_cm(1),
         detected_count        : obstacle.detected_count,
@@ -526,7 +526,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages()
 {
     // only 200(?) bytes are guaranteed by DataFlash
     Log_Write_Startup(TYPE_GROUNDSTART_MSG);
-    DataFlash.Log_Write_Mode(control_mode);
+    DataFlash.Log_Write_Mode(control_mode->mode_number());
     Log_Write_Home_And_Origin();
     gps.Write_DataFlash_Log_Startup_messages();
 }

APMrover2/Rover.cpp

@@ -40,7 +40,7 @@ Rover::Rover(void) :
 #if MOUNT == ENABLED
     camera_mount(ahrs, current_loc),
 #endif
-    control_mode(INITIALISING),
+    control_mode(&mode_initializing),
     throttle(500),
 #if FRSKY_TELEM_ENABLED == ENABLED
     frsky_telemetry(ahrs, battery, rangefinder),

APMrover2/Rover.h

@@ -65,6 +65,8 @@
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 #include "AP_MotorsUGV.h"
 
+#include "mode.h"
+
 #include "AP_Arming.h"
 #include "compat.h"
 
@@ -106,6 +108,13 @@ public:
     friend class AP_AdvancedFailsafe_Rover;
 #endif
     friend class GCS_Rover;
+    friend class Mode;
+    friend class ModeAuto;
+    friend class ModeGuided;
+    friend class ModeHold;
+    friend class ModeSteering;
+    friend class ModeManual;
+    friend class ModeRTL;
 
     Rover(void);
 
@@ -221,10 +230,9 @@ private:
     // if USB is connected
     bool usb_connected;
 
-    // Radio
     // This is the state of the flight control system
-    // There are multiple states defined such as MANUAL, FBW-A, AUTO
-    enum mode control_mode;
+    // There are multiple states defined such as MANUAL, AUTO, ...
+    Mode *control_mode;
 
     // Used to maintain the state of the previous control switch position
     // This is set to -1 when we need to re-read the switch
@@ -284,9 +292,6 @@ private:
         uint32_t detected_time_ms;
     } obstacle;
 
-    // this is set to true when auto has been triggered to start
-    bool auto_triggered;
-
     // Ground speed
     // The amount current ground speed is below min ground speed.  meters per second
     float ground_speed;
@@ -310,10 +315,6 @@ private:
     uint32_t control_sensors_enabled;
     uint32_t control_sensors_health;
 
-    // Navigation control variables
-    // The instantaneous desired lateral acceleration in m/s/s
-    float lateral_acceleration;
-
     // Waypoint distances
     // Distance between rover and next waypoint.  Meters
     float wp_distance;
@@ -399,12 +400,6 @@ private:
     // time that rudder/steering arming has been running
     uint32_t rudder_arm_timer;
 
-    // true if we are in an auto-throttle mode, which means
-    // we need to run the speed controller
-    bool auto_throttle_mode;
-
-    // Guided control
-    GuidedMode guided_mode;             // controls which controller is run (waypoint or velocity)
     // Store parameters from Guided msg
     struct {
       float turn_angle;          // target heading in centi-degrees
@@ -422,6 +417,15 @@ private:
     // True when we are doing motor test
     bool motor_test;
 
+    ModeInitializing mode_initializing;
+    ModeHold mode_hold;
+    ModeManual mode_manual;
+    ModeGuided mode_guided;
+    ModeAuto mode_auto;
+    ModeLearning mode_learning;
+    ModeSteering mode_steering;
+    ModeRTL mode_rtl;
+
 private:
     // private member functions
     void ahrs_update();
@@ -458,6 +462,10 @@ private:
     void gcs_update(void);
     void gcs_retry_deferred(void);
 
+    Mode *control_mode_from_num(enum mode num);
+    bool set_mode(Mode &mode);
+    bool mavlink_set_mode(uint8_t mode);
+
     void do_erase_logs(void);
     void Log_Write_Performance();
     void Log_Write_Steering();
@@ -480,11 +488,7 @@ private:
     void Log_Arm_Disarm();
 
     void load_parameters(void);
-    bool auto_check_trigger(void);
     bool use_pivot_steering(void);
-    void calc_throttle(float target_speed);
-    void calc_lateral_acceleration();
-    void calc_nav_steer();
     void set_servos(void);
     void set_auto_WP(const struct Location& loc);
     void set_guided_WP(const struct Location& loc);
@@ -538,8 +542,7 @@ private:
     void init_ardupilot();
     void startup_ground(void);
     void set_reverse(bool reverse);
-    void set_mode(enum mode mode);
-    bool mavlink_set_mode(uint8_t mode);
+
     void failsafe_trigger(uint8_t failsafe_type, bool on);
     void startup_INS_ground(void);
     void update_notify();

APMrover2/Steering.cpp

@@ -1,58 +1,12 @@
 #include "Rover.h"
 
-/*
-    check for triggering of start of auto mode
-*/
-bool Rover::auto_check_trigger(void) {
-    // only applies to AUTO mode
-    if (control_mode != AUTO) {
-        return true;
-    }
-
-    // check for user pressing the auto trigger to off
-    if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "AUTO triggered off");
-        auto_triggered = false;
-        return false;
-    }
-
-    // if already triggered, then return true, so you don't
-    // need to hold the switch down
-    if (auto_triggered) {
-        return true;
-    }
-
-    if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) {
-        // no trigger configured - let's go!
-        auto_triggered = true;
-        return true;
-    }
-
-    if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
-        auto_triggered = true;
-        return true;
-    }
-
-    if (!is_zero(g.auto_kickstart)) {
-        const float xaccel = ins.get_accel().x;
-        if (xaccel >= g.auto_kickstart) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
-            auto_triggered = true;
-            return true;
-        }
-    }
-
-    return false;
-}
-
 /*
     work out if we are going to use pivot steering
 */
 bool Rover::use_pivot_steering(void)
 {
     // check cases where we clearly cannot use pivot steering
-    if (control_mode < AUTO || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
+    if (control_mode->is_autopilot_mode() || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
         pivot_steering_active = false;
         return false;
     }
@@ -82,7 +36,7 @@ bool Rover::use_pivot_steering(void)
 bool Rover::in_stationary_loiter()
 {
     // Confirm we are in AUTO mode and need to loiter for a time period
-    if ((loiter_start_time > 0) && (control_mode == AUTO)) {
+    if ((loiter_start_time > 0) && (control_mode == &mode_auto)) {
         // Check if active loiter is enabled AND we are outside the waypoint loiter radius
         // then the vehicle still needs to move so return false
         if (active_loiter && (wp_distance > g.waypoint_radius)) {
@@ -94,156 +48,12 @@ bool Rover::in_stationary_loiter()
     return false;
 }
 
-/*
-    calculate the throtte for auto-throttle modes
-*/
-void Rover::calc_throttle(float target_speed) {
-    // If not autostarting OR we are loitering at a waypoint
-    // then set the throttle to minimum
-    if (!auto_check_trigger() || in_stationary_loiter()) {
-        g2.motors.set_throttle(g.throttle_min.get());
-        // Stop rotation in case of loitering and skid steering
-        if (g2.motors.have_skid_steering()) {
-            g2.motors.set_steering(0.0f);
-        }
-        return;
-    }
-
-    const float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
-    const int throttle_target = throttle_base + throttle_nudge;
-
-    /*
-        reduce target speed in proportion to turning rate, up to the
-        SPEED_TURN_GAIN percentage.
-    */
-    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
-    steer_rate = constrain_float(steer_rate, 0.0f, 1.0f);
-
-    // use g.speed_turn_gain for a 90 degree turn, and in proportion
-    // for other turn angles
-    const int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
-    const float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0.0f, 1.0f);
-    const float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
-
-    float reduction = 1.0f - steer_rate * speed_turn_reduction;
-
-    if (control_mode >= AUTO && guided_mode != Guided_Velocity && wp_distance <= g.speed_turn_dist) {
-        // in auto-modes we reduce speed when approaching waypoints
-        const float reduction2 = 1.0f - speed_turn_reduction;
-        if (reduction2 < reduction) {
-            reduction = reduction2;
-        }
-    }
-
-    // reduce the target speed by the reduction factor
-    target_speed *= reduction;
-
-    groundspeed_error = fabsf(target_speed) - ground_speed;
-
-    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100.0f) / 100.0f);
-
-    // also reduce the throttle by the reduction factor. This gives a
-    // much faster response in turns
-    throttle *= reduction;
-
-    if (in_reverse) {
-        g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
-    } else {
-        g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
-    }
-
-    if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
-        // the user has asked to use reverse throttle to brake. Apply
-        // it in proportion to the ground speed error, but only when
-        // our ground speed error is more than BRAKING_SPEEDERR.
-        //
-        // We use a linear gain, with 0 gain at a ground speed error
-        // of braking_speederr, and 100% gain when groundspeed_error
-        // is 2*braking_speederr
-        const float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
-        const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
-        g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
-
-        // temporarily set us in reverse to allow the PWM setting to
-        // go negative
-        set_reverse(true);
-    }
-
-    if (guided_mode != Guided_Velocity) {
-        if (use_pivot_steering()) {
-            // In Guided Velocity, only the steering input is used to calculate the pivot turn.
-            g2.motors.set_throttle(0.0f);
-        }
-    }
-}
-
-/*****************************************
-    Calculate desired turn angles (in medium freq loop)
- *****************************************/
-
-void Rover::calc_lateral_acceleration() {
-    switch (control_mode) {
-    case AUTO:
-        // If we have reached the waypoint previously navigate
-        // back to it from our current position
-        if (previously_reached_wp && (loiter_duration > 0)) {
-            nav_controller->update_waypoint(current_loc, next_WP);
-        } else {
-            nav_controller->update_waypoint(prev_WP, next_WP);
-        }
-        break;
-
-    case RTL:
-    case GUIDED:
-    case STEERING:
-        nav_controller->update_waypoint(current_loc, next_WP);
-        break;
-    default:
-        return;
-    }
-
-    // Calculate the required turn of the wheels
-
-    // negative error = left turn
-    // positive error = right turn
-    lateral_acceleration = nav_controller->lateral_acceleration();
-    if (use_pivot_steering()) {
-        const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
-        if (bearing_error > 0) {
-            lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
-        } else {
-            lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;
-        }
-    }
-}
-
-/*
-    calculate steering angle given lateral_acceleration
-*/
-void Rover::calc_nav_steer() {
-    // check to see if the rover is loitering
-    if (in_stationary_loiter()) {
-        g2.motors.set_steering(0.0f);
-        return;
-    }
-
-    // add in obstacle avoidance
-    if (!in_reverse) {
-        lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
-    }
-
-    // constrain to max G force
-    lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
-
-    g2.motors.set_steering(steerController.get_steering_out_lat_accel(lateral_acceleration));
-}
-
 /*****************************************
     Set the flight control servos based on the current calculated values
 *****************************************/
 void Rover::set_servos(void) {
     // Apply slew rate limit on non Manual modes
-    if (control_mode == MANUAL || control_mode == LEARNING) {
+    if (control_mode == &mode_manual || control_mode == &mode_learning) {
         g2.motors.slew_limit_throttle(false);
         if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
             g2.motors.set_throttle(0.0f);

APMrover2/commands.cpp

@@ -29,7 +29,7 @@ void Rover::set_auto_WP(const struct Location& loc)
 
 void Rover::set_guided_WP(const struct Location& loc)
 {
-    guided_mode = Guided_WP;
+    rover.mode_guided.guided_mode = ModeGuided::Guided_WP;
     // copy the current location into the OldWP slot
     // ---------------------------------------
     prev_WP = current_loc;
@@ -47,12 +47,11 @@ void Rover::set_guided_WP(const struct Location& loc)
 
 void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
 {
-    guided_mode = Guided_Velocity;
+    rover.mode_guided.guided_mode = ModeGuided::Guided_Velocity;
     rover.guided_control.target_steer_speed = target_steer_speed;
     rover.guided_control.target_speed = target_speed;
 
     next_WP = current_loc;
-    lateral_acceleration = 0.0f;
     // this is handy for the groundstation
     wp_totalDistance = 0;
     wp_distance      = 0.0f;

APMrover2/commands_logic.cpp

@@ -11,7 +11,7 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
     }
 
     // exit immediately if not in AUTO mode
-    if (control_mode != AUTO) {
+    if (control_mode != &mode_auto) {
         return false;
     }
 
@@ -130,9 +130,9 @@ bool Rover::start_command(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 Rover::exit_mission()
 {
-    if (control_mode == AUTO) {
+    if (control_mode == &mode_auto) {
         gcs().send_text(MAV_SEVERITY_NOTICE, "No commands. Can't set AUTO. Setting HOLD");
-        set_mode(HOLD);
+        set_mode(mode_hold);
     }
 }
 
@@ -140,7 +140,7 @@ void Rover::exit_mission()
 //      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 Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
-    if (control_mode == AUTO) {
+    if (control_mode == &mode_auto) {
         const bool cmd_complete = verify_command(cmd);
 
         // send message to GCS
@@ -216,7 +216,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 void Rover::do_RTL(void)
 {
     prev_WP = current_loc;
-    control_mode = RTL;
+    control_mode = &mode_rtl;
     next_WP = home;
 }
 
@@ -379,7 +379,6 @@ void Rover::nav_set_yaw_speed()
         gcs().send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
         g2.motors.set_throttle(g.throttle_min.get());
         g2.motors.set_steering(0.0f);
-        lateral_acceleration = 0.0f;
         return;
     }
 
@@ -390,9 +389,9 @@ void Rover::nav_set_yaw_speed()
     // 0.5 would set speed to the cruise speed
     // 1 is double the cruise speed.
     const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
-    calc_throttle(target_speed);
+    rover.control_mode->calc_throttle(target_speed);
 
-    Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
+    Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
 }
 
 void Rover::nav_set_speed()
@@ -402,7 +401,6 @@ void Rover::nav_set_speed()
         gcs().send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
         g2.motors.set_throttle(g.throttle_min.get());
         g2.motors.set_steering(0.0f);
-        lateral_acceleration = 0.0f;
         prev_WP = current_loc;
         next_WP = current_loc;
         set_guided_WP(current_loc);  // exit Guided_Velocity to prevent spam
@@ -416,9 +414,9 @@ void Rover::nav_set_speed()
     nav_controller->update_waypoint(current_loc, next_WP);
 
     g2.motors.set_steering(steer_value);
-    calc_throttle(guided_control.target_speed);
+    rover.control_mode->calc_throttle(guided_control.target_speed);
 
-    Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
+    Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
 }
 
 /********************************************************************************/
@@ -461,7 +459,7 @@ void Rover::do_yaw(const AP_Mission::Mission_Command& cmd)
     const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
     g2.motors.set_steering(steering);
     next_navigation_leg_cd = condition_value;
-    calc_throttle(g.speed_cruise);
+    control_mode->calc_throttle(g.speed_cruise);
 
     do_auto_rotation = true;
 }
@@ -483,7 +481,7 @@ bool Rover::do_yaw_rotation()
         // Calculate the steering to apply base on error calculated
         const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
         g2.motors.set_steering(steering);
-        calc_throttle(g.speed_cruise);
+        control_mode->calc_throttle(g.speed_cruise);
         do_auto_rotation = true;
         return false;
     }

APMrover2/commands_process.cpp

@@ -4,7 +4,7 @@
 // --------------------
 void Rover::update_commands(void)
 {
-    if (control_mode == AUTO) {
+    if (control_mode == &mode_auto) {
         if (home_is_set != HOME_UNSET && mission.num_commands() > 1) {
             mission.update();
         }

APMrover2/control_modes.cpp

@@ -2,6 +2,40 @@
 
 static const int16_t CH_7_PWM_TRIGGER = 1800;
 
+Mode *Rover::control_mode_from_num(const enum mode num)
+{
+    Mode *ret = nullptr;
+    switch (num) {
+    case MANUAL:
+        ret = &mode_manual;
+        break;
+    case LEARNING:
+        ret = &mode_learning;
+        break;
+    case STEERING:
+        ret = &mode_steering;
+        break;
+    case HOLD:
+        ret = &mode_hold;
+        break;
+    case AUTO:
+        ret = &mode_auto;
+        break;
+    case RTL:
+        ret = &mode_rtl;
+        break;
+    case GUIDED:
+       ret = &mode_guided;
+        break;
+    case INITIALISING:
+        ret = &mode_initializing;
+        break;
+    default:
+        break;
+    }
+    return ret;
+}
+
 void Rover::read_control_switch()
 {
     static bool switch_debouncer;
@@ -36,7 +70,10 @@ void Rover::read_control_switch()
             return;
         }
 
-        set_mode((enum mode)modes[switchPosition].get());
+        Mode *new_mode = control_mode_from_num((enum mode)modes[switchPosition].get());
+        if (new_mode != nullptr) {
+            set_mode(*new_mode);
+        }
 
         oldSwitchPosition = switchPosition;
         prev_WP = current_loc;
@@ -92,8 +129,7 @@ void Rover::read_trim_switch()
             if (ch7_flag) {
                 ch7_flag = false;
 
-                switch (control_mode) {
-                case MANUAL:
+                if (control_mode == &mode_manual) {
                     hal.console->printf("Erasing waypoints\n");
                     // if SW7 is ON in MANUAL = Erase the Flight Plan
                     mission.clear();
@@ -101,10 +137,7 @@ void Rover::read_trim_switch()
                         // if roll is full right store the current location as home
                         set_home_to_current_location(false);
                     }
-                    break;
-
-                case LEARNING:
-                case STEERING: {
+                } else if (control_mode == &mode_learning || control_mode == &mode_steering) {
                     // if SW7 is ON in LEARNING = record the Wp
 
                     // create new mission command
@@ -120,14 +153,9 @@ void Rover::read_trim_switch()
                     if (mission.add_cmd(cmd)) {
                         hal.console->printf("Learning waypoint %u", static_cast<uint32_t>(mission.num_commands()));
                     }
-                    break;
-                }
-                case AUTO:
+                } else if (control_mode == &mode_auto) {
                     // if SW7 is ON in AUTO = set to RTL
-                    set_mode(RTL);
-                    break;
-
-                default:
+                    set_mode(mode_rtl);
                     break;
                 }
             }

APMrover2/crash_check.cpp

@@ -13,7 +13,7 @@ void Rover::crash_check()
   static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
 
   // return immediately if disarmed, or crash checking disabled or in HOLD mode
-  if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) {
+  if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) {
     crash_counter = 0;
     return;
   }
@@ -38,7 +38,7 @@ void Rover::crash_check()
     // send message to gcs
     gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD");
     // change mode to hold and disarm
-    set_mode(HOLD);
+    set_mode(mode_hold);
     if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
       disarm_motors();
     }

APMrover2/defines.h

@@ -40,12 +40,6 @@ enum mode {
     INITIALISING = 16
 };
 
-enum GuidedMode {
-    Guided_WP,
-    Guided_Angle,
-    Guided_Velocity
-};
-
 // types of failsafe events
 #define FAILSAFE_EVENT_THROTTLE (1<<0)
 #define FAILSAFE_EVENT_GCS      (1<<1)

APMrover2/failsafe.cpp

@@ -74,18 +74,18 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
     if (failsafe.triggered == 0 &&
         failsafe.bits != 0 &&
         millis() - failsafe.start_time > g.fs_timeout * 1000 &&
-        control_mode != RTL &&
-        control_mode != HOLD) {
+        control_mode != &mode_rtl &&
+        control_mode != &mode_hold) {
         failsafe.triggered = failsafe.bits;
         gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
         switch (g.fs_action) {
             case 0:
                 break;
             case 1:
-                set_mode(RTL);
+                set_mode(mode_rtl);
                 break;
             case 2:
-                set_mode(HOLD);
+                set_mode(mode_hold);
                 break;
         }
     }

APMrover2/radio.cpp

@@ -53,7 +53,7 @@ void Rover::rudder_arm_disarm_check()
     }
 
     // if not in a manual throttle mode then disallow rudder arming/disarming
-    if (auto_throttle_mode) {
+    if (control_mode->auto_throttle()) {
         rudder_arm_timer = 0;
         return;
     }

APMrover2/sensors.cpp

@@ -239,29 +239,13 @@ void Rover::update_sensor_status_flags(void)
                                                          ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
                                                          ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
                                                          ~MAV_SYS_STATUS_LOGGING);
-
-    switch (control_mode) {
-    case MANUAL:
-    case HOLD:
-        break;
-
-    case LEARNING:
-    case STEERING:
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL;    // 3D angular rate control
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;  // attitude stabilisation
-        break;
-
-    case AUTO:
-    case RTL:
-    case GUIDED:
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL;    // 3D angular rate control
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;  // attitude stabilisation
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;            // yaw position
-        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;     // X/Y position control
-        break;
-
-    case INITIALISING:
-        break;
+    if (control_mode->attitude_stabilized()) {
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
+    }
+    if (control_mode->is_autopilot_mode()) {
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
+        control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
     }
 
     if (rover.DataFlash.logging_enabled()) {

APMrover2/system.cpp

@@ -121,7 +121,7 @@ void Rover::init_ardupilot()
     // initialise notify system
     notify.init(false);
     AP_Notify::flags.failsafe_battery = false;
-    notify_mode(control_mode);
+    notify_mode((enum mode)control_mode->mode_number());
 
     ServoRelayEvents.set_channel_mask(0xFFF0);
 
@@ -209,7 +209,12 @@ void Rover::init_ardupilot()
 
     startup_ground();
 
-    set_mode((enum mode)g.initial_mode.get());
+    Mode *initial_mode = control_mode_from_num((enum mode)g.initial_mode.get());
+    if (initial_mode == nullptr) {
+        initial_mode = &mode_initializing;
+    }
+    set_mode(*initial_mode);
+
 
     // set the correct flight mode
     // ---------------------------
@@ -227,7 +232,7 @@ void Rover::init_ardupilot()
 //*********************************************************************************
 void Rover::startup_ground(void)
 {
-    set_mode(INITIALISING);
+    set_mode(mode_initializing);
 
     gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
 
@@ -278,75 +283,32 @@ void Rover::set_reverse(bool reverse)
     in_reverse = reverse;
 }
 
-void Rover::set_mode(enum mode mode)
+bool Rover::set_mode(Mode &new_mode)
 {
-    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 we are changing out of AUTO mode reset the loiter timer and stop current mission
-    if (control_mode == AUTO) {
-        loiter_start_time = 0;
-        if (mission.state() == AP_Mission::MISSION_RUNNING) {
-            mission.stop();
-        }
+    Mode &old_mode = *control_mode;
+    if (!new_mode.enter()) {
+        return false;
     }
 
-    control_mode = mode;
-    throttle = 500;
-    if (!in_auto_reverse) {
-        set_reverse(false);
-    }
-    g.pidSpeedThrottle.reset_I();
+    control_mode = &new_mode;
 
 #if FRSKY_TELEM_ENABLED == ENABLED
-    frsky_telemetry.update_control_mode(control_mode);
+    frsky_telemetry.update_control_mode(control_mode->mode_number());
 #endif
 
-    if (control_mode != AUTO) {
-        auto_triggered = false;
-    }
-
-    switch (control_mode) {
-    case MANUAL:
-    case HOLD:
-    case LEARNING:
-    case STEERING:
-        auto_throttle_mode = false;
-        break;
-
-    case AUTO:
-        auto_throttle_mode = true;
-        rtl_complete = false;
-        restart_nav();
-        break;
-
-    case RTL:
-        auto_throttle_mode = true;
-        do_RTL();
-        break;
-
-    case GUIDED:
-        auto_throttle_mode = true;
-        /*
-           when entering guided mode we set the target as the current
-           location. This matches the behaviour of the copter code.
-           */
-        set_guided_WP(current_loc);
-        break;
-
-    default:
-        auto_throttle_mode = true;
-        do_RTL();
-        break;
-    }
+    old_mode.exit();
 
     if (should_log(MASK_LOG_MODE)) {
-        DataFlash.Log_Write_Mode(control_mode);
+        DataFlash.Log_Write_Mode(control_mode->mode_number());
     }
 
-    notify_mode(control_mode);
+    notify_mode((enum mode)control_mode->mode_number());
+    return true;
 }
 
 /*
@@ -354,18 +316,11 @@ void Rover::set_mode(enum mode mode)
  */
 bool Rover::mavlink_set_mode(uint8_t mode)
 {
-    switch (mode) {
-    case MANUAL:
-    case HOLD:
-    case LEARNING:
-    case STEERING:
-    case GUIDED:
-    case AUTO:
-    case RTL:
-        set_mode((enum mode)mode);
-        return true;
+    Mode *new_mode = control_mode_from_num((enum mode)mode);
+    if (new_mode == nullptr) {
+        return false;
     }
-    return false;
+    return set_mode(*new_mode);
 }
 
 void Rover::startup_INS_ground(void)
@@ -532,7 +487,7 @@ bool Rover::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();
     }