Copter: add nav_guided suport to Auto mode

ArduCopter/APM_Config.h

@@ -38,6 +38,7 @@
 //#define OPTFLOW               ENABLED             // enable optical flow sensor and OF_LOITER flight mode at a cost of 5K of flash space
 //#define SPRAYER               ENABLED             // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
 //#define EPM_ENABLED           ENABLED             // enable epm cargo gripper costs 500bytes of flash
+//#define NAV_GUIDED            ENABLED             // enable external navigation computer to control vehicle through MAV_CMD_NAV_GUIDED mission commands
 
 // other settings
 //#define THROTTLE_IN_DEADBAND   100                // redefine size of throttle deadband in pwm (0 ~ 1000)

ArduCopter/ArduCopter.pde

@@ -744,6 +744,16 @@ static AP_EPM epm;
 static AP_Parachute parachute(relay);
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// Nav Guided - allows external computer to control the vehicle during missions
+////////////////////////////////////////////////////////////////////////////////
+#if NAV_GUIDED == ENABLED
+static struct {
+    uint32_t start_time;        // system time in milliseconds that control was handed to the external computer
+    Vector3f start_position;    // vehicle position when control was ahnded to the external computer
+} nav_guided;
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 // function definitions to keep compiler from complaining about undeclared functions
 ////////////////////////////////////////////////////////////////////////////////

ArduCopter/commands_logic.pde

@@ -8,6 +8,9 @@ static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
 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);
+#endif
 static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
 static void do_within_distance(const AP_Mission::Mission_Command& cmd);
 static void do_change_alt(const AP_Mission::Mission_Command& cmd);
@@ -21,6 +24,9 @@ static void do_parachute(const AP_Mission::Mission_Command& cmd);
 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);
+#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);
 
 // start_command - this function will be called when the ap_mission lib wishes to start a new command
@@ -68,6 +74,12 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
         do_spline_wp(cmd);
         break;
 
+#if NAV_GUIDED == ENABLED
+    case MAV_CMD_NAV_GUIDED:                    // 90  accept navigation commands from external nav computer
+        do_nav_guided(cmd);
+        break;
+#endif
+
     //
     // conditional commands
     //
@@ -208,6 +220,12 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
         return verify_spline_wp(cmd);
         break;
 
+#if NAV_GUIDED == ENABLED
+    case MAV_CMD_NAV_GUIDED:
+        return verify_nav_guided(cmd);
+        break;
+#endif
+
     ///
     /// conditional commands
     ///
@@ -483,6 +501,22 @@ static void do_spline_wp(const AP_Mission::Mission_Command& cmd)
     auto_spline_start(local_pos, stopped_at_start, seg_end_type, next_destination);
 }
 
+#if NAV_GUIDED == ENABLED
+// do_nav_guided - initiate accepting commands from exernal nav computer
+static void do_nav_guided(const AP_Mission::Mission_Command& cmd)
+{
+    // record start time so it can be compared vs timeout
+    nav_guided.start_time = millis();
+
+    // record start position so it can be compared vs horizontal limit
+    nav_guided.start_position = inertial_nav.get_position();
+
+    // set spline navigation target
+    auto_nav_guided_start();
+}
+#endif  // NAV_GUIDED
+
+
 #if PARACHUTE == ENABLED
 // do_parachute - configure or release parachute
 static void do_parachute(const AP_Mission::Mission_Command& cmd)
@@ -660,6 +694,42 @@ 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)
+{
+    // check if we have passed the timeout
+    if ((cmd.p1 > 0) && ((millis() - nav_guided.start_time) / 1000 >= cmd.p1)) {
+        return true;
+    }
+
+    // get current location
+    const Vector3f& curr_pos = inertial_nav.get_position();
+
+    // 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
+
+
 /********************************************************************************/
 //	Condition (May) commands
 /********************************************************************************/
@@ -690,6 +760,9 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
         case Auto_Circle:
             // move circle altitude up to target (we will need to store this target in circle class)
             break;
+        case Auto_NavGuided:
+            // ignore altitude
+            break;
         }
     }
     // To-Do: store desired altitude in a variable so that it can be verified later

ArduCopter/config.h

@@ -406,6 +406,12 @@
  # define PARACHUTE ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Nav-Guided - allows external nav computer to control vehicle
+#ifndef NAV_GUIDED
+ # define NAV_GUIDED DISABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // RADIO CONFIGURATION
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/control_auto.pde

@@ -70,6 +70,12 @@ static void auto_run()
     case Auto_Spline:
         auto_spline_run();
         break;
+
+#if NAV_GUIDED == ENABLED
+    case Auto_NavGuided:
+        auto_nav_guided_run();
+        break;
+#endif
     }
 }
 
@@ -364,6 +370,25 @@ void auto_circle_run()
     attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
 }
 
+#if NAV_GUIDED == ENABLED
+// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
+void auto_nav_guided_start()
+{
+    auto_mode = Auto_NavGuided;
+
+    // call regular guided flight mode initialisation
+    guided_init(true);
+}
+
+// auto_nav_guided_run - allows control by external navigation controller
+//      called by auto_run at 100hz or more
+void auto_nav_guided_run()
+{
+    // call regular guided flight mode run function
+    guided_run();
+}
+#endif  // NAV_GUIDED
+
 // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
 // set rtl parameter to true if this is during an RTL
 uint8_t get_default_auto_yaw_mode(bool rtl)

ArduCopter/defines.h

@@ -181,7 +181,8 @@ enum AutoMode {
     Auto_RTL,
     Auto_CircleMoveToEdge,
     Auto_Circle,
-    Auto_Spline
+    Auto_Spline,
+    Auto_NavGuided
 };
 
 // RTL states