Copter: Refactor verify_command

- abstracted verify_command so that command-wide actions can happen more cleanly, namely mission_item_reached
- This also brings the structure in line with plane and rover

ArduCopter/Copter.cpp

@@ -27,7 +27,7 @@ Copter::Copter(void) :
     sonar_enabled(true),
     mission(ahrs, 
             FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
-            FUNCTOR_BIND_MEMBER(&Copter::verify_command, bool, const AP_Mission::Mission_Command &),
+            FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
             FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
     control_mode(STABILIZE),
 #if FRAME_CONFIG == HELI_FRAME  // helicopter constructor requires more arguments

ArduCopter/Copter.h

@@ -901,6 +901,7 @@ private:
     void gcs_send_text_fmt(const prog_char_t *fmt, ...);
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
+    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
 
     bool do_guided(const AP_Mission::Mission_Command& cmd);
     void do_takeoff(const AP_Mission::Mission_Command& cmd);

ArduCopter/commands_logic.cpp

@@ -159,85 +159,12 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
 // Verify command Handlers
 /********************************************************************************/
 
-// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
+// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
-//  should return true once the active navigation command completes successfully
+//      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
-//  called at 10hz or higher
+bool Copter::verify_command_callback(const AP_Mission::Mission_Command& cmd)
-bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
 {
-    bool cmd_complete;
+    if (control_mode == AUTO) {
-
+        bool cmd_complete = verify_command(cmd);
-    switch(cmd.id) {
-
-    //
-    // navigation commands
-    //
-    case MAV_CMD_NAV_TAKEOFF:
-        cmd_complete = verify_takeoff();
-        break;
-
-    case MAV_CMD_NAV_WAYPOINT:
-        cmd_complete = verify_nav_wp(cmd);
-        break;
-
-    case MAV_CMD_NAV_LAND:
-        cmd_complete = verify_land();
-        break;
-
-    case MAV_CMD_NAV_LOITER_UNLIM:
-        cmd_complete = verify_loiter_unlimited();
-        break;
-
-    case MAV_CMD_NAV_LOITER_TURNS:
-        cmd_complete = verify_circle(cmd);
-        break;
-
-    case MAV_CMD_NAV_LOITER_TIME:
-        cmd_complete = verify_loiter_time();
-        break;
-
-    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-        cmd_complete = verify_RTL();
-        break;
-
-    case MAV_CMD_NAV_SPLINE_WAYPOINT:
-        cmd_complete = verify_spline_wp(cmd);
-        break;
-
-#if NAV_GUIDED == ENABLED
-    case MAV_CMD_NAV_GUIDED_ENABLE:
-        cmd_complete = verify_nav_guided_enable(cmd);
-        break;
-#endif
-
-    ///
-    /// conditional commands
-    ///
-    case MAV_CMD_CONDITION_DELAY:
-        cmd_complete = verify_wait_delay();
-        break;
-
-    case MAV_CMD_CONDITION_DISTANCE:
-        cmd_complete = verify_within_distance();
-        break;
-
-    case MAV_CMD_CONDITION_CHANGE_ALT:
-        cmd_complete = verify_change_alt();
-        break;
-
-    case MAV_CMD_CONDITION_YAW:
-        cmd_complete = verify_yaw();
-        break;
-
-    case MAV_CMD_DO_PARACHUTE:
-        // assume parachute was released successfully
-        cmd_complete = true;
-        break;
-
-    default:
-        // return true if we do not recognise the command so that we move on to the next command
-        cmd_complete = true;
-        break;
-    }
 
         // send message to GCS
         if (cmd_complete) {
@@ -246,6 +173,88 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
 
         return cmd_complete;
     }
+    return false;
+}
+
+
+// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
+//  should return true once the active navigation command completes successfully
+//  called at 10hz or higher
+bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
+{
+    switch(cmd.id) {
+
+    //
+    // navigation commands
+    //
+    case MAV_CMD_NAV_TAKEOFF:
+        return verify_takeoff();
+        break;
+
+    case MAV_CMD_NAV_WAYPOINT:
+        return verify_nav_wp(cmd);
+        break;
+
+    case MAV_CMD_NAV_LAND:
+        return verify_land();
+        break;
+
+    case MAV_CMD_NAV_LOITER_UNLIM:
+        return verify_loiter_unlimited();
+        break;
+
+    case MAV_CMD_NAV_LOITER_TURNS:
+        return verify_circle(cmd);
+        break;
+
+    case MAV_CMD_NAV_LOITER_TIME:
+        return verify_loiter_time();
+        break;
+
+    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+        return verify_RTL();
+        break;
+
+    case MAV_CMD_NAV_SPLINE_WAYPOINT:
+        return verify_spline_wp(cmd);
+        break;
+
+#if NAV_GUIDED == ENABLED
+    case MAV_CMD_NAV_GUIDED_ENABLE:
+        return verify_nav_guided_enable(cmd);
+        break;
+#endif
+
+    ///
+    /// conditional commands
+    ///
+    case MAV_CMD_CONDITION_DELAY:
+        return verify_wait_delay();
+        break;
+
+    case MAV_CMD_CONDITION_DISTANCE:
+        return verify_within_distance();
+        break;
+
+    case MAV_CMD_CONDITION_CHANGE_ALT:
+        return verify_change_alt();
+        break;
+
+    case MAV_CMD_CONDITION_YAW:
+        return verify_yaw();
+        break;
+
+    case MAV_CMD_DO_PARACHUTE:
+        // assume parachute was released successfully
+        return true;
+        break;
+
+    default:
+        // return true if we do not recognise the command so that we move on to the next command
+        return true;
+        break;
+    }
+}
 
 // exit_mission - function that is called once the mission completes
 void Copter::exit_mission()