Rover: move AP_Mission object into ModeAuto class

This mirrors what we did for Copter

APMrover2/Log.cpp

@@ -215,7 +215,7 @@ void Rover::Log_Write_Startup(uint8_t type)
         LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
         time_us         : AP_HAL::micros64(),
         startup_type    : type,
-        command_total   : mission.num_commands()
+        command_total   : mode_auto.mission.num_commands()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }

APMrover2/Parameters.cpp

@@ -397,7 +397,7 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Group: MIS_
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
-    GOBJECT(mission, "MIS_",       AP_Mission),
+    GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
 
     // @Group: RSSI_
     // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp

APMrover2/RC_Channel.cpp

@@ -138,7 +138,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
 
             // if disarmed clear mission and set home to current location
             if (!rover.arming.is_armed()) {
-                rover.mission.clear();
+                rover.mode_auto.mission.clear();
                 rover.set_home_to_current_location(false);
                 return;
             }
@@ -155,8 +155,8 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi
                 cmd.id = MAV_CMD_NAV_WAYPOINT;
 
                 // save command
-                if (rover.mission.add_cmd(cmd)) {
+                if (rover.mode_auto.mission.add_cmd(cmd)) {
-                    hal.console->printf("Added waypoint %u", (unsigned)rover.mission.num_commands());
+                    hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands());
                 }
             }
         }

APMrover2/Rover.h

@@ -195,12 +195,6 @@ private:
     // selected navigation controller
     AP_Navigation *nav_controller;
 
-    // Mission library
-    AP_Mission mission{
-            FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
-            FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),
-            FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)};
-
 #if AP_AHRS_NAVEKF_AVAILABLE
     OpticalFlow optflow;
 #endif
@@ -307,14 +301,6 @@ private:
     uint32_t control_sensors_enabled;
     uint32_t control_sensors_health;
 
-    // Conditional command
-    // A value used in condition commands (eg delay, change alt, etc.)
-    // For example in a change altitude command, it is the altitude to change to.
-    int32_t condition_value;
-    // A starting value used to check the status of a conditional command.
-    // For example in a delay command the condition_start records that start time for the delay
-    int32_t condition_start;
-
     // 3D Location vectors
     // Location structure defined in AP_Common
     // The home location used for RTL.  The location is set when we first get stable GPS lock
@@ -339,11 +325,6 @@ private:
     static const AP_Param::Info var_info[];
     static const LogStructure log_structure[];
 
-    // Loiter control
-    uint16_t loiter_duration;       // How long we should loiter at the nav_waypoint (time in seconds)
-    uint32_t loiter_start_time;     // How long have we been loitering - The start time in millis
-    bool previously_reached_wp;     // set to true if we have EVER reached the waypoint
-
     // time that rudder/steering arming has been running
     uint32_t rudder_arm_timer;
 
@@ -417,30 +398,6 @@ private:
 
     // commands_logic.cpp
     void update_mission(void);
-    bool start_command(const AP_Mission::Mission_Command& cmd);
-    void exit_mission();
-    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
-    bool verify_command(const AP_Mission::Mission_Command& cmd);
-    void do_RTL(void);
-    void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
-    void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
-    bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
-    bool verify_RTL();
-    bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
-    bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
-    bool verify_nav_set_yaw_speed();
-    void do_wait_delay(const AP_Mission::Mission_Command& cmd);
-    void do_within_distance(const AP_Mission::Mission_Command& cmd);
-    bool verify_wait_delay();
-    bool verify_within_distance();
-    void do_change_speed(const AP_Mission::Mission_Command& cmd);
-    void do_set_home(const AP_Mission::Mission_Command& cmd);
-    void do_set_reverse(const AP_Mission::Mission_Command& cmd);
-
-    enum Mis_Done_Behave {
-        MIS_DONE_BEHAVE_HOLD      = 0,
-        MIS_DONE_BEHAVE_LOITER    = 1
-    };
 
     // commands.cpp
     void update_home_from_EKF();

APMrover2/commands.cpp

@@ -55,8 +55,8 @@ bool Rover::set_home(const Location& loc, bool lock)
         // log new home position which mission library will pull from ahrs
         if (should_log(MASK_LOG_CMD)) {
             AP_Mission::Mission_Command temp_cmd;
-            if (mission.read_cmd_from_storage(0, temp_cmd)) {
+            if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) {
-                DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
+                DataFlash.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd);
             }
         }
     }
@@ -67,7 +67,7 @@ bool Rover::set_home(const Location& loc, bool lock)
     }
 
     // Save Home to EEPROM
-    mission.write_home_to_storage();
+    mode_auto.mission.write_home_to_storage();
 
     // send text of home position to ground stations
     gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",

APMrover2/commands_logic.cpp

@@ -4,8 +4,8 @@
 void Rover::update_mission(void)
 {
     if (control_mode == &mode_auto) {
-        if (ahrs.home_is_set() && mission.num_commands() > 1) {
+        if (ahrs.home_is_set() && mode_auto.mission.num_commands() > 1) {
-            mission.update();
+            mode_auto.mission.update();
         }
     }
 }
@@ -13,11 +13,11 @@ void Rover::update_mission(void)
 /********************************************************************************/
 // Command Event Handlers
 /********************************************************************************/
-bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
+bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
 {
     // log when new commands start
-    if (should_log(MASK_LOG_CMD)) {
+    if (rover.should_log(MASK_LOG_CMD)) {
-        DataFlash.Log_Write_Mission_Cmd(mission, cmd);
+        rover.DataFlash.Log_Write_Mission_Cmd(mission, cmd);
     }
 
     gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)",
@@ -68,12 +68,12 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_SET_ROI:
         if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
             // switch off the camera tracking if enabled
-            if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+            if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
-                camera_mount.set_mode_to_default();
+                rover.camera_mount.set_mode_to_default();
             }
         } else {
             // send the command to the camera mount
-            camera_mount.set_roi_target(cmd.content.location);
+            rover.camera_mount.set_roi_target(cmd.content.location);
         }
         break;
 #endif
@@ -102,22 +102,22 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 }
 
 // exit_mission - callback function called from ap-mission when the mission has completed
-void Rover::exit_mission()
+void ModeAuto::exit_mission()
 {
     // play a tone
     AP_Notify::events.mission_complete = 1;
     // send message
     gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete");
 
-    if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && set_mode(mode_loiter, MODE_REASON_MISSION_END)) {
+    if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && rover.set_mode(rover.mode_loiter, MODE_REASON_MISSION_END)) {
         return;
     }
-    set_mode(mode_hold, MODE_REASON_MISSION_END);
+    rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END);
 }
 
 // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
 //      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)
+bool ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
     const bool cmd_complete = verify_command(cmd);
 
@@ -138,7 +138,7 @@ should move onto the next mission element.
 Return true if we do not recognize the command so that we move on to the next command
 *******************************************************************************/
 
-bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
+bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
 {
     switch (cmd.id) {
     case MAV_CMD_NAV_WAYPOINT:
@@ -184,13 +184,13 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 //  Nav (Must) commands
 /********************************************************************************/
 
-void Rover::do_RTL(void)
+void ModeAuto::do_RTL(void)
 {
     // start rtl in auto mode
-    mode_auto.start_RTL();
+    start_RTL();
 }
 
-void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
+void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
 {
     // just starting so we haven't previously reached the waypoint
     previously_reached_wp = false;
@@ -210,12 +210,12 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_a
 
     // retrieve and sanitize target location
     Location cmdloc = cmd.content.location;
-    location_sanitize(current_loc, cmdloc);
+    location_sanitize(rover.current_loc, cmdloc);
-    mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd);
+    set_desired_location(cmdloc, next_leg_bearing_cd);
 }
 
 // do_set_yaw_speed - turn to a specified heading and achieve and given speed
-void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
 {
     float desired_heading_cd;
 
@@ -229,17 +229,17 @@ void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
     }
 
     // set auto target
-    const float speed_max = control_mode->get_speed_default();
+    const float speed_max = get_speed_default();
-    mode_auto.set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
+    set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
 }
 
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
-bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
+bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     // exit immediately if we haven't reached the destination
-    if (!mode_auto.reached_destination()) {
+    if (!reached_destination()) {
         return false;
     }
 
@@ -269,19 +269,19 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
-bool Rover::verify_RTL()
+bool ModeAuto::verify_RTL()
 {
-    return mode_auto.reached_destination();
+    return reached_destination();
 }
 
-bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
+bool ModeAuto::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 {
     verify_nav_wp(cmd);
     return false;
 }
 
 // verify_loiter_time - check if we have loitered long enough
-bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
+bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
     const bool result = verify_nav_wp(cmd);
     if (result) {
@@ -291,22 +291,22 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 }
 
 // verify_yaw - return true if we have reached the desired heading
-bool Rover::verify_nav_set_yaw_speed()
+bool ModeAuto::verify_nav_set_yaw_speed()
 {
-    return mode_auto.reached_heading();
+    return reached_heading();
 }
 
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/
 
-void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
 {
     condition_start = millis();
     condition_value = static_cast<int32_t>(cmd.content.delay.seconds * 1000);  // convert seconds to milliseconds
 }
 
-void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
 {
     condition_value = cmd.content.distance.meters;
 }
@@ -315,7 +315,7 @@ void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
 // Verify Condition (May) commands
 /********************************************************************************/
 
-bool Rover::verify_wait_delay()
+bool ModeAuto::verify_wait_delay()
 {
     if (static_cast<uint32_t>(millis() - condition_start) > static_cast<uint32_t>(condition_value)) {
         condition_value = 0;
@@ -324,9 +324,9 @@ bool Rover::verify_wait_delay()
     return false;
 }
 
-bool Rover::verify_within_distance()
+bool ModeAuto::verify_within_distance()
 {
-    if (mode_auto.get_distance_to_destination() < condition_value) {
+    if (get_distance_to_destination() < condition_value) {
         condition_value = 0;
         return true;
     }
@@ -338,24 +338,24 @@ bool Rover::verify_within_distance()
 //  Do (Now) commands
 /********************************************************************************/
 
-void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     // set speed for active mode
-    if (control_mode->set_desired_speed(cmd.content.speed.target_ms)) {
+    if (set_desired_speed(cmd.content.speed.target_ms)) {
         gcs().send_text(MAV_SEVERITY_INFO, "speed: %.1f m/s", static_cast<double>(cmd.content.speed.target_ms));
     }
 }
 
-void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
-    if (cmd.p1 == 1 && have_position) {
+    if (cmd.p1 == 1 && rover.have_position) {
-        set_home_to_current_location(false);
+        rover.set_home_to_current_location(false);
     } else {
-        set_home(cmd.content.location, false);
+        rover.set_home(cmd.content.location, false);
     }
 }
 
-void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
+void ModeAuto::do_set_reverse(const AP_Mission::Mission_Command& cmd)
 {
-    control_mode->set_reversed(cmd.p1 == 1);
+    set_reversed(cmd.p1 == 1);
 }

APMrover2/mode.cpp

@@ -8,7 +8,6 @@ Mode::Mode() :
     channel_steer(rover.channel_steer),
     channel_throttle(rover.channel_throttle),
     channel_lateral(rover.channel_lateral),
-    mission(rover.mission),
     attitude_control(rover.g2.attitude_control)
 { }
 

APMrover2/mode.h

@@ -4,6 +4,7 @@
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Mission/AP_Mission.h>
 
 #include "defines.h"
 
@@ -195,7 +196,6 @@ protected:
     class RC_Channel *&channel_steer;
     class RC_Channel *&channel_throttle;
     class RC_Channel *&channel_lateral;
-    class AP_Mission &mission;
     class AR_AttitudeControl &attitude_control;
 
 
@@ -267,6 +267,11 @@ public:
     // start RTL (within auto)
     void start_RTL();
 
+    AP_Mission mission{
+        FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command&),
+        FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&),
+        FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
+
 protected:
 
     bool _enter() override;
@@ -289,6 +294,46 @@ private:
     bool auto_triggered;
 
     bool _reached_heading;      // true when vehicle has reached desired heading in TurnToHeading sub mode
+
+    bool start_command(const AP_Mission::Mission_Command& cmd);
+    void exit_mission();
+    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
+
+    bool verify_command(const AP_Mission::Mission_Command& cmd);
+    void do_RTL(void);
+    void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
+    void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+    bool verify_RTL();
+    bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
+    bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_set_yaw_speed();
+    void do_wait_delay(const AP_Mission::Mission_Command& cmd);
+    void do_within_distance(const AP_Mission::Mission_Command& cmd);
+    bool verify_wait_delay();
+    bool verify_within_distance();
+    void do_change_speed(const AP_Mission::Mission_Command& cmd);
+    void do_set_home(const AP_Mission::Mission_Command& cmd);
+    void do_set_reverse(const AP_Mission::Mission_Command& cmd);
+
+    enum Mis_Done_Behave {
+        MIS_DONE_BEHAVE_HOLD      = 0,
+        MIS_DONE_BEHAVE_LOITER    = 1
+    };
+
+    // Loiter control
+    uint16_t loiter_duration;       // How long we should loiter at the nav_waypoint (time in seconds)
+    uint32_t loiter_start_time;     // How long have we been loitering - The start time in millis
+    bool previously_reached_wp;     // set to true if we have EVER reached the waypoint
+
+    // Conditional command
+    // A value used in condition commands (eg delay, change alt, etc.)
+    // For example in a change altitude command, it is the altitude to change to.
+    int32_t condition_value;
+    // A starting value used to check the status of a conditional command.
+    // For example in a delay command the condition_start records that start time for the delay
+    int32_t condition_start;
+
 };
 
 

APMrover2/system.cpp

@@ -181,11 +181,11 @@ void Rover::startup_ground(void)
     startup_INS_ground();
 
     // initialise mission library
-    mission.init();
+    mode_auto.mission.init();
 
     // initialise DataFlash library
 #if LOGGING_ENABLED == ENABLED
-    DataFlash.set_mission(&mission);
+    DataFlash.set_mission(&mode_auto.mission);
     DataFlash.setVehicle_Startup_Log_Writer(
         FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
         );
@@ -354,7 +354,7 @@ bool Rover::disarm_motors(void)
     }
     if (control_mode != &mode_auto) {
         // reset the mission on disarm if we are not in auto
-        mission.reset();
+        mode_auto.mission.reset();
     }
 
     // only log if disarming was successful