Rover: support do-guided-limits mission command

APMrover2/commands_logic.cpp

@@ -96,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;
@@ -182,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:
@@ -226,12 +231,10 @@ 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) {
-        // initialise guided limits
-        //rover.mode_guided.limit_init_time_and_pos();
-
         start_guided(cmd.content.location);
     }
 }
@@ -315,7 +318,21 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 // check if guided has completed
 bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
-    return false;
+    // 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) <= 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
@@ -391,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

@@ -323,6 +323,7 @@ private:
     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);
@@ -388,6 +389,12 @@ public:
     // 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();
+
 protected:
 
     enum GuidedMode {
@@ -405,6 +412,14 @@ protected:
     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 Guided_Limit {
+        uint32_t timeout_ms;// timeout (in seconds) 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;
 };
 
 

APMrover2/mode_auto.cpp

@@ -20,6 +20,9 @@ bool ModeAuto::_enter()
     // other initialisation
     auto_triggered = false;
 
+    // clear guided limits
+    rover.mode_guided.limit_clear();
+
     // restart mission processing
     mission.start_or_resume();
     return true;
@@ -181,7 +184,7 @@ void ModeAuto::start_guided(const Location& loc)
         _submode = Auto_Guided;
 
         // initialise guided start time and position as reference for limit checking
-        //rover.mode_guided.limit_init_time_and_pos();
+        rover.mode_guided.limit_init_time_and_location();
 
         // sanity check target location
         Location loc_sanitised = loc;

APMrover2/mode_guided.cpp

@@ -201,3 +201,42 @@ bool ModeGuided::start_loiter()
     }
     return false;
 }
+
+// set guided timeout and movement limits
+void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max)
+{
+    limit.timeout_ms = timeout_ms;
+    limit.horiz_max = horiz_max;
+}
+
+// clear/turn off guided limits
+void ModeGuided::limit_clear()
+{
+    limit.timeout_ms = 0;
+    limit.horiz_max = 0.0f;
+}
+
+// initialise guided start time and location as reference for limit checking
+//  only called from AUTO mode's start_guided method
+void ModeGuided::limit_init_time_and_location()
+{
+    limit.start_time = AP_HAL::millis();
+    limit.start_loc = rover.current_loc;
+}
+
+// returns true if guided mode has breached a limit
+bool ModeGuided::limit_breached()
+{
+    // check if we have passed the timeout
+    if ((limit.timeout_ms > 0) && (millis() - limit.start_time >= limit.timeout_ms)) {
+        return true;
+    }
+
+    // check if we have gone beyond horizontal limit
+    if (limit.horiz_max > 0.0f) {
+        return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max);
+    }
+
+    // if we got this far we must be within limits
+    return false;
+}