Rover: support guided-within-auto

also adds support for sending position targets to offboard controller which is assumed to return velocity commands

APMrover2/GCS_Mavlink.cpp

@@ -525,7 +525,7 @@ bool GCS_MAVLINK_Rover::in_hil_mode() const
 
 bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
 {
-    if (rover.control_mode != &rover.mode_guided) {
+    if (!rover.control_mode->in_guided_mode()) {
         // only accept position updates when in GUIDED mode
         return false;
     }
@@ -666,7 +666,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
         // param2 : Speed - normalized to 0 .. 1
 
         // exit if vehicle is not in Guided mode
-        if (rover.control_mode != &rover.mode_guided) {
+        if (!rover.control_mode->in_guided_mode()) {
             return MAV_RESULT_FAILED;
         }
 
@@ -750,7 +750,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 != &rover.mode_guided) {
+            if (!rover.control_mode->in_guided_mode()) {
                 break;
             }
 
@@ -782,7 +782,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 != &rover.mode_guided) {
+            if (!rover.control_mode->in_guided_mode()) {
                 break;
             }
 
@@ -899,7 +899,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 != &rover.mode_guided) {
+            if (!rover.control_mode->in_guided_mode()) {
                 break;
             }
             // check for supported coordinate frames

APMrover2/commands_logic.cpp

@@ -37,6 +37,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_nav_wp(cmd, true);
         break;
 
+    case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
+        do_nav_guided_enable(cmd);
+        break;
+
     case MAV_CMD_NAV_SET_YAW_SPEED:
         do_nav_set_yaw_speed(cmd);
         break;
@@ -158,6 +162,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time(cmd);
 
+    case MAV_CMD_NAV_GUIDED_ENABLE:
+        return verify_nav_guided_enable(cmd);
+
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
 
@@ -219,6 +226,16 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
     set_desired_location(cmdloc, next_leg_bearing_cd);
 }
 
+void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
+{
+    if (cmd.p1 > 0) {
+        // initialise guided limits
+        //rover.mode_guided.limit_init_time_and_pos();
+
+        start_guided(cmd.content.location);
+    }
+}
+
 // do_set_yaw_speed - turn to a specified heading and achieve and given speed
 void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
 {
@@ -295,6 +312,12 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
     return result;
 }
 
+// check if guided has completed
+bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
+{
+    return false;
+}
+
 // verify_yaw - return true if we have reached the desired heading
 bool ModeAuto::verify_nav_set_yaw_speed()
 {

APMrover2/mode.h

@@ -67,6 +67,9 @@ public:
     // return if in non-manual mode : Auto, Guided, RTL, SmartRTL
     virtual bool is_autopilot_mode() const { return false; }
 
+    // return if external control is allowed in this mode (Guided or Guided-within-Auto)
+    virtual bool in_guided_mode() const { return false; }
+
     // returns true if vehicle can be armed or disarmed from the transmitter in this mode
     virtual bool allows_arming_from_transmitter() { return !is_autopilot_mode(); }
 
@@ -254,6 +257,9 @@ public:
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
 
+    // return if external control is allowed in this mode (Guided or Guided-within-Auto)
+    bool in_guided_mode() const override { return _submode == Auto_Guided; }
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
@@ -282,7 +288,8 @@ protected:
         Auto_WP,                // drive to a given location
         Auto_HeadingAndSpeed,   // turn to a given heading
         Auto_RTL,               // perform RTL within auto mode
-        Auto_Loiter             // perform Loiter within auto mode
+        Auto_Loiter,            // perform Loiter within auto mode
+        Auto_Guided             // handover control to external navigation system from within auto mode
     } _submode;
 
 private:
@@ -301,11 +308,13 @@ private:
     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_guided_enable(const AP_Mission::Mission_Command& cmd);
     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_guided_enable(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);
@@ -316,6 +325,8 @@ private:
     void do_set_reverse(const AP_Mission::Mission_Command& cmd);
 
     bool start_loiter();
+    void start_guided(const Location& target_loc);
+    void send_guided_position_target();
 
     enum Mis_Done_Behave {
         MIS_DONE_BEHAVE_HOLD      = 0,
@@ -328,6 +339,11 @@ private:
     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
 
+    // Guided variables
+    Location guided_target;         // location target sent to external navigation
+    bool guided_target_valid;       // true if guided_target is valid
+    uint32_t guided_target_sent_ms; // system time that target was last sent to offboard navigation
+
     // 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.
@@ -352,9 +368,15 @@ public:
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
 
+    // return if external control is allowed in this mode (Guided or Guided-within-Auto)
+    bool in_guided_mode() const override { return true; }
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
+    // return true if vehicle has reached destination
+    bool reached_destination() override;
+
     // set desired location, heading and speed
     void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN) override;
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;

APMrover2/mode_auto.cpp

@@ -1,6 +1,8 @@
 #include "mode.h"
 #include "Rover.h"
 
+#define AUTO_GUIDED_SEND_TARGET_MS 1000
+
 bool ModeAuto::_enter()
 {
     // fail to enter auto if no mission commands
@@ -89,6 +91,14 @@ void ModeAuto::update()
         case Auto_Loiter:
             rover.mode_loiter.update();
             break;
+
+        case Auto_Guided:
+        {
+            // send location target to offboard navigation system
+            send_guided_position_target();
+            rover.mode_guided.update();
+            break;
+        }
     }
 }
 
@@ -113,12 +123,25 @@ void ModeAuto::set_desired_location(const struct Location& destination, float ne
 // return true if vehicle has reached or even passed destination
 bool ModeAuto::reached_destination()
 {
-    if (_submode == Auto_WP) {
+    switch (_submode) {
+    case Auto_WP:
         return _reached_destination;
-    }
+        break;
-    if (_submode == Auto_RTL) {
+    case Auto_HeadingAndSpeed:
+        // always return true because this is the safer option to allow missions to continue
+        return true;
+        break;
+    case Auto_RTL:
         return rover.mode_rtl.reached_destination();
+        break;
+    case Auto_Loiter:
+        return rover.mode_loiter.reached_destination();
+        break;
+    case Auto_Guided:
+        return rover.mode_guided.reached_destination();
+        break;
     }
+
     // we should never reach here but just in case, return true to allow missions to continue
     return true;
 }
@@ -151,6 +174,50 @@ void ModeAuto::start_RTL()
     }
 }
 
+// hand over control to external navigation controller in AUTO mode
+void ModeAuto::start_guided(const Location& loc)
+{
+    if (rover.mode_guided.enter()) {
+        _submode = Auto_Guided;
+
+        // initialise guided start time and position as reference for limit checking
+        //rover.mode_guided.limit_init_time_and_pos();
+
+        // sanity check target location
+        Location loc_sanitised = loc;
+        if ((loc_sanitised.lat != 0) || (loc_sanitised.lng != 0)) {
+            location_sanitize(rover.current_loc, loc_sanitised);
+            guided_target = loc_sanitised;
+            guided_target_valid = true;
+        } else {
+            guided_target_valid = false;
+        }
+    }
+}
+
+// send latest position target to offboard navigation system
+void ModeAuto::send_guided_position_target()
+{
+    if (!guided_target_valid) {
+        return;
+    }
+
+    // send at maximum of 1hz
+    uint32_t now_ms = AP_HAL::millis();
+    if ((guided_target_sent_ms == 0) || (now_ms - guided_target_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) {
+        guided_target_sent_ms = now_ms;
+
+        // get system id and component id of offboard navigation system
+        uint8_t sysid = 0;
+        uint8_t compid = 0;
+        mavlink_channel_t chan;
+        if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) {
+            gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target);
+        }
+    }
+
+}
+
 /*
     check for triggering of start of auto mode
 */

APMrover2/mode_guided.cpp

@@ -116,6 +116,24 @@ float ModeGuided::get_distance_to_destination() const
     return _distance_to_destination;
 }
 
+// return true if vehicle has reached or even passed destination
+bool ModeGuided::reached_destination()
+{
+    switch (_guided_mode) {
+    case Guided_WP:
+        return _reached_destination;
+        break;
+    case Guided_HeadingAndSpeed:
+    case Guided_TurnRateAndSpeed:
+    case Guided_Loiter:
+        return true;
+        break;
+    }
+
+    // we should never reach here but just in case, return true is the safer option
+    return true;
+}
+
 // set desired location
 void ModeGuided::set_desired_location(const struct Location& destination,
                                       float next_leg_bearing_cd)