Rover: add NAV_GUIDED_ENABLED support

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;
@@ -92,6 +96,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         }
         break;
 
+    case MAV_CMD_DO_GUIDED_LIMITS:
+        do_guided_limits(cmd);
+        break;
+
     default:
         // return false for unhandled commands
         return false;
@@ -158,6 +166,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();
 
@@ -175,6 +186,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_SET_ROI:
     case MAV_CMD_DO_SET_REVERSE:
     case MAV_CMD_DO_FENCE_ENABLE:
+    case MAV_CMD_DO_GUIDED_LIMITS:
         return true;
 
     default:
@@ -219,6 +231,14 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
     set_desired_location(cmdloc, next_leg_bearing_cd);
 }
 
+// start guided within auto to allow external navigation system to control vehicle
+void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
+{
+    if (cmd.p1 > 0) {
+        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 +315,26 @@ 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)
+{
+    // if we failed to enter guided or this command disables guided
+    // return true so we move to next command
+    if (_submode != Auto_Guided || cmd.p1 == 0) {
+        return true;
+    }
+
+    // if a location target was set, return true once vehicle is close
+    if (guided_target.valid) {
+        if (rover.current_loc.get_distance(guided_target.loc) <= rover.g.waypoint_radius) {
+            return true;
+        }
+    }
+
+    // guided command complete once a limit is breached
+    return rover.mode_guided.limit_breached();
+}
+
 // verify_yaw - return true if we have reached the desired heading
 bool ModeAuto::verify_nav_set_yaw_speed()
 {
@@ -368,3 +408,12 @@ void ModeAuto::do_set_reverse(const AP_Mission::Mission_Command& cmd)
 {
     set_reversed(cmd.p1 == 1);
 }
+
+// set timeout and position limits for guided within auto
+void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
+{
+    rover.mode_guided.limit_set(
+        cmd.p1 * 1000, // convert seconds to ms
+        cmd.content.guided_limits.horiz_max);
+}
+

APMrover2/mode.h

@@ -67,6 +67,9 @@ class Mode
     // 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(); }
 
@@ -104,7 +107,7 @@ class Mode
     bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
 
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
-    virtual bool reached_destination() { return true; }
+    virtual bool reached_destination() const { return true; }
 
     // set desired heading and speed - supported in Auto and Guided modes
     virtual void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
@@ -254,12 +257,15 @@ class ModeAuto : public Mode
     // 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;
 
     // set desired location, heading and speed
     void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN) override;
-    bool reached_destination() override;
+    bool reached_destination() const override;
 
     // heading and speed control
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
@@ -282,7 +288,8 @@ class ModeAuto : public Mode
         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 @@ class ModeAuto : public Mode
     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);
@@ -314,8 +323,11 @@ class ModeAuto : public Mode
     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);
+    void do_guided_limits(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 +340,13 @@ class ModeAuto : public Mode
     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-within-Auto variables
+    struct {
+        Location loc;           // location target sent to external navigation
+        bool valid;             // true if loc is valid
+        uint32_t last_sent_ms;  // system time that target was last sent to offboard navigation
+    } guided_target;
+
     // 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 +371,15 @@ class ModeGuided : public Mode
     // 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() const 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;
@@ -366,6 +391,12 @@ class ModeGuided : public Mode
     // vehicle start loiter
     bool start_loiter();
 
+    // guided limits
+    void limit_set(uint32_t timeout_ms, float horiz_max);
+    void limit_clear();
+    void limit_init_time_and_location();
+    bool limit_breached() const;
+
 protected:
 
     enum GuidedMode {
@@ -383,6 +414,14 @@ class ModeGuided : public Mode
     bool have_attitude_target;  // true if we have a valid attitude target
     uint32_t _des_att_time_ms;  // system time last call to set_desired_attitude was made (used for timeout)
     float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
+
+    // limits
+    struct {
+        uint32_t timeout_ms;// timeout from the time that guided is invoked
+        float horiz_max;    // horizontal position limit in meters from where guided mode was initiated (0 = no limit)
+        uint32_t start_time;// system time in milliseconds that control was handed to the external computer
+        Location start_loc; // starting location for checking horiz_max limit
+    } limit;
 };
 
 
@@ -468,7 +507,7 @@ class ModeRTL : public Mode
     bool is_autopilot_mode() const override { return true; }
 
     float get_distance_to_destination() const override { return _distance_to_destination; }
-    bool reached_destination() override { return _reached_destination; }
+    bool reached_destination() const override { return _reached_destination; }
 
 protected:
 
@@ -489,7 +528,7 @@ class ModeSmartRTL : public Mode
     bool is_autopilot_mode() const override { return true; }
 
     float get_distance_to_destination() const override { return _distance_to_destination; }
-    bool reached_destination() override { return smart_rtl_state == SmartRTL_StopAtHome; }
+    bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; }
 
     // save current position for use by the smart_rtl flight mode
     void save_position();