Copter: support GUIDED_ENABLE and GUIDED_LIMITS

Split of NAV_GUIDED into these two command necessitated guided mode
store the limits

ArduCopter/commands_logic.pde

@@ -9,7 +9,8 @@ static void do_circle(const AP_Mission::Mission_Command& cmd);
 static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
 static void do_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED
-static void do_nav_guided(const AP_Mission::Mission_Command& cmd);
+static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
+static void do_guided_limits(const AP_Mission::Mission_Command& cmd);
 #endif
 static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
 static void do_within_distance(const AP_Mission::Mission_Command& cmd);
@@ -28,7 +29,7 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
 static bool verify_circle(const AP_Mission::Mission_Command& cmd);
 static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED
-static bool verify_nav_guided(const AP_Mission::Mission_Command& cmd);
+static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
 static void auto_spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination);
 
@@ -78,11 +79,9 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
 #if NAV_GUIDED == ENABLED
-#ifdef MAV_CMD_NAV_GUIDED
+    case MAV_CMD_NAV_GUIDED_ENABLE:             // 92  accept navigation commands from external nav computer
-    case MAV_CMD_NAV_GUIDED:             // 90  accept navigation commands from external nav computer
+        do_nav_guided_enable(cmd);
-        do_nav_guided(cmd);
         break;
-#endif
 #endif
 
     //
@@ -176,6 +175,12 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
+#if NAV_GUIDED == ENABLED
+    case MAV_CMD_DO_GUIDED_LIMITS:                      // 220  accept guided mode limits
+        do_guided_limits(cmd);
+        break;
+#endif
+
     default:
         // do nothing with unrecognized MAVLink messages
         break;
@@ -232,11 +237,9 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
         break;
 
 #if NAV_GUIDED == ENABLED
-#ifdef MAV_CMD_NAV_GUIDED
+    case MAV_CMD_NAV_GUIDED_ENABLE:
-    case MAV_CMD_NAV_GUIDED:
+        return verify_nav_guided_enable(cmd);
-        return verify_nav_guided(cmd);
         break;
-#endif
 #endif
 
     ///
@@ -515,17 +518,16 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd)
 }
 
 #if NAV_GUIDED == ENABLED
-// do_nav_guided - initiate accepting commands from exernal nav computer
+// do_nav_guided_enable - initiate accepting commands from external nav computer
-static void do_nav_guided(const AP_Mission::Mission_Command& cmd)
+static void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
-    // record start time so it can be compared vs timeout
+    if (cmd.p1 > 0) {
-    nav_guided.start_time = millis();
+        // initialise guided limits
+        guided_limit_init_time_and_pos();
 
-    // record start position so it can be compared vs horizontal limit
+        // set spline navigation target
-    nav_guided.start_position = inertial_nav.get_position();
+        auto_nav_guided_start();
-
+    }
-    // set spline navigation target
-    auto_nav_guided_start();
 }
 #endif  // NAV_GUIDED
 
@@ -574,6 +576,17 @@ static void do_gripper(const AP_Mission::Mission_Command& cmd)
 }
 #endif
 
+#if NAV_GUIDED == ENABLED
+// do_guided_limits - pass guided limits to guided controller
+static void do_guided_limits(const AP_Mission::Mission_Command& cmd)
+{
+    guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
+                     cmd.content.guided_limits.alt_min * 100.0f,    // convert meters to cm
+                     cmd.content.guided_limits.alt_max * 100.0f,    // convert meters to cm
+                     cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
+}
+#endif
+
 /********************************************************************************/
 //	Verify Nav (Must) commands
 /********************************************************************************/
@@ -730,36 +743,15 @@ static bool verify_spline_wp(const AP_Mission::Mission_Command& cmd)
 
 #if NAV_GUIDED == ENABLED
 // verify_nav_guided - check if we have breached any limits
-static bool verify_nav_guided(const AP_Mission::Mission_Command& cmd)
+static bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
-    // check if we have passed the timeout
+    // if disabling guided mode then immediately return true so we move to next command
-    if ((cmd.p1 > 0) && ((millis() - nav_guided.start_time) / 1000 >= cmd.p1)) {
+    if (cmd.p1 == 0) {
         return true;
     }
 
-    // get current location
+    // check time and position limits
-    const Vector3f& curr_pos = inertial_nav.get_position();
+    return guided_limit_check();
-
-    // check if we have gone below min alt
-    if (cmd.content.nav_guided.alt_min != 0 && (curr_pos.z / 100) < cmd.content.nav_guided.alt_min) {
-        return true;
-    }
-
-    // check if we have gone above max alt
-    if (cmd.content.nav_guided.alt_max != 0 && (curr_pos.z / 100) > cmd.content.nav_guided.alt_max) {
-        return true;
-    }
-
-    // check if we have gone beyond horizontal limit
-    if (cmd.content.nav_guided.horiz_max != 0) {
-        float horiz_move = pv_get_horizontal_distance_cm(nav_guided.start_position, curr_pos) / 100;
-        if (horiz_move > cmd.content.nav_guided.horiz_max) {
-            return true;
-        }
-    }
-
-    // if we got here we should continue with the external nav controls
-    return false;
 }
 #endif  // NAV_GUIDED
 
@@ -887,17 +879,6 @@ static bool do_guided(const AP_Mission::Mission_Command& cmd)
             return true;
             break;
 
-#ifdef MAV_CMD_NAV_VELOCITY
-        case MAV_CMD_NAV_VELOCITY:
-            // set target velocity
-            pos_or_vel.x = cmd.content.nav_velocity.x * 100.0f;
-            pos_or_vel.y = cmd.content.nav_velocity.y * 100.0f;
-            pos_or_vel.z = cmd.content.nav_velocity.z * 100.0f;
-            guided_set_velocity(pos_or_vel);
-            return true;
-            break;
-#endif
-
         case MAV_CMD_CONDITION_YAW:
             do_yaw(cmd);
             return true;

ArduCopter/control_auto.pde

@@ -30,6 +30,9 @@ static bool auto_init(bool ignore_checks)
         // initialise waypoint and spline controller
         wp_nav.wp_and_spline_init();
 
+        // clear guided limits
+        guided_limit_clear();
+
         // start/resume the mission (based on MIS_RESTART parameter)
         mission.start_or_resume();
         return true;
@@ -398,6 +401,9 @@ void auto_nav_guided_start()
 
     // call regular guided flight mode initialisation
     guided_init(true);
+
+    // initialise guided start time and position as reference for limit checking
+    guided_limit_init_time_and_pos();
 }
 
 // auto_nav_guided_run - allows control by external navigation controller

ArduCopter/control_guided.pde

@@ -8,6 +8,15 @@
  # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM     500     // point nose at target if it is more than 5m away
 #endif
 
+struct Guided_Limit {
+    uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
+    float alt_min_cm;   // lower altitude limit in cm above home (0 = no limit)
+    float alt_max_cm;   // upper altitude limit in cm above home (0 = no limit)
+    float horiz_max_cm; // horizontal position limit in cm 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
+    Vector3f start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
+} guided_limit;
+
 // guided_init - initialise guided controller
 static bool guided_init(bool ignore_checks)
 {
@@ -221,3 +230,68 @@ static void guided_vel_control_run()
         attitude_control.angle_ef_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true);
     }
 }
+
+// Guided Limit code
+
+// guided_limit_clear - clear/turn off guided limits
+static void guided_limit_clear()
+{
+    guided_limit.timeout_ms = 0;
+    guided_limit.alt_min_cm = 0.0f;
+    guided_limit.alt_max_cm = 0.0f;
+    guided_limit.horiz_max_cm = 0.0f;
+}
+
+// guided_limit_set - set guided timeout and movement limits
+static void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
+{
+    guided_limit.timeout_ms = timeout_ms;
+    guided_limit.alt_min_cm = alt_min_cm;
+    guided_limit.alt_max_cm = alt_max_cm;
+    guided_limit.horiz_max_cm = horiz_max_cm;
+}
+
+// guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
+//  only called from AUTO mode's auto_nav_guided_start function
+static void guided_limit_init_time_and_pos()
+{
+    // initialise start time
+    guided_limit.start_time = hal.scheduler->millis();
+
+    // initialise start position from current position
+    guided_limit.start_pos = inertial_nav.get_position();
+}
+
+// guided_limit_check - returns true if guided mode has breached a limit
+//  used when guided is invoked from the NAV_GUIDED_ENABLE mission command
+static bool guided_limit_check()
+{
+    // check if we have passed the timeout
+    if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) {
+        return true;
+    }
+
+    // get current location
+    const Vector3f& curr_pos = inertial_nav.get_position();
+
+    // check if we have gone below min alt
+    if ((guided_limit.alt_min_cm != 0.0f) && (curr_pos.z < guided_limit.alt_min_cm)) {
+        return true;
+    }
+
+    // check if we have gone above max alt
+    if ((guided_limit.alt_max_cm != 0.0f) && (curr_pos.z > guided_limit.alt_max_cm)) {
+        return true;
+    }
+
+    // check if we have gone beyond horizontal limit
+    if (guided_limit.horiz_max_cm > 0.0f) {
+        float horiz_move = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
+        if (horiz_move > guided_limit.horiz_max_cm) {
+            return true;
+        }
+    }
+
+    // if we got this far we must be within limits
+    return false;
+}