From 9c75900746fee66df94b78d241e26c391d11413d Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Fri, 24 Jul 2015 01:20:49 -0700
Subject: [PATCH] Rover: bring rover mission callback inline with copter and
 plane

---
 APMrover2/Rover.cpp          |  2 +-
 APMrover2/Rover.h            |  1 +
 APMrover2/commands_logic.cpp | 19 ++++++++++---------
 3 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp
index 00cbd1b0ce..3a2250c2ff 100644
--- a/APMrover2/Rover.cpp
+++ b/APMrover2/Rover.cpp
@@ -32,7 +32,7 @@ Rover::Rover(void) :
     steerController(ahrs),
     mission(ahrs,
             FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
-            FUNCTOR_BIND_MEMBER(&Rover::verify_command, bool, const AP_Mission::Mission_Command&),
+            FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),
             FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)),
     num_gcs(MAVLINK_COMM_NUM_BUFFERS),
     ServoRelayEvents(relay),
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 093bf16e05..71eabdbc2f 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -490,6 +490,7 @@ private:
     void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
     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);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 54d4017ac5..5f910508d5 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -122,6 +122,16 @@ void Rover::exit_mission()
     }
 }
 
+// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
+//      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
+bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
+{
+    if (control_mode == AUTO) {
+        bool cmd_complete = verify_command(cmd);
+        return cmd_complete;
+    }
+    return false;
+}
 /********************************************************************************/
 // Verify command Handlers
 //      Returns true if command complete
@@ -129,12 +139,6 @@ void Rover::exit_mission()
 
 bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 {
-    // exit immediately if not in AUTO mode
-    // we return true or we will continue to be called by ap-mission
-    if (control_mode != AUTO) {
-        return true;
-    }
-
 	switch(cmd.id) {
 
 		case MAV_CMD_NAV_WAYPOINT:
@@ -145,11 +149,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 
         case MAV_CMD_CONDITION_DELAY:
             return verify_wait_delay();
-            break;
 
         case MAV_CMD_CONDITION_DISTANCE:
             return verify_within_distance();
-            break;
 
         default:
             if (cmd.id > MAV_CMD_CONDITION_LAST) {
@@ -158,7 +160,6 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
             }
             gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
             return true;
-            break;
 	}
     return false;
 }