From 1e39c179084cd93c9fe2adbcdda9ba2abc03d35a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 30 Oct 2018 12:56:06 +1100
Subject: [PATCH] AP_Mission: return true if cmd-do was handled, false
 otherwise

---
 libraries/AP_Mission/AP_Mission.cpp          |  1 -
 libraries/AP_Mission/AP_Mission_Commands.cpp | 48 ++++++++++----------
 2 files changed, 24 insertions(+), 25 deletions(-)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 8697fb897e..bf000d90f7 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -268,7 +268,6 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_SET_RELAY:
     case MAV_CMD_DO_REPEAT_SERVO:
     case MAV_CMD_DO_REPEAT_RELAY:
-    case MAV_CMD_DO_CONTROL_VIDEO:
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
     case MAV_CMD_DO_DIGICAM_CONTROL:
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp
index ea680dbf82..dd99be3405 100644
--- a/libraries/AP_Mission/AP_Mission_Commands.cpp
+++ b/libraries/AP_Mission/AP_Mission_Commands.cpp
@@ -8,7 +8,7 @@ bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd
 {
     AP_Gripper *gripper = AP::gripper();
     if (gripper == nullptr) {
-        return true;
+        return false;
     }
 
     // Note: we ignore the gripper num parameter because we only
@@ -18,65 +18,64 @@ bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd
         gripper->release();
         // Log_Write_Event(DATA_GRIPPER_RELEASE);
         gcs().send_text(MAV_SEVERITY_INFO, "Gripper Released");
-        break;
+        return true;
     case GRIPPER_ACTION_GRAB:
         gripper->grab();
         // Log_Write_Event(DATA_GRIPPER_GRAB);
         gcs().send_text(MAV_SEVERITY_INFO, "Gripper Grabbed");
-        break;
+        return true;
     default:
-        // do nothing
-        break;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        AP_HAL::panic("Unhandled gripper case");
+#endif
+        return false;
     }
-
-    return true;
 }
 
 bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd)
 {
     AP_ServoRelayEvents *sre = AP::servorelayevents();
     if (sre == nullptr) {
-        return true;
+        return false;
     }
 
     switch (cmd.id) {
     case MAV_CMD_DO_SET_SERVO:
         sre->do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
-        break;
+        return true;
 
     case MAV_CMD_DO_SET_RELAY:
         sre->do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
-        break;
+        return true;
 
     case MAV_CMD_DO_REPEAT_SERVO:
         sre->do_repeat_servo(cmd.content.repeat_servo.channel,
                              cmd.content.repeat_servo.pwm,
                              cmd.content.repeat_servo.repeat_count,
                              cmd.content.repeat_servo.cycle_time * 1000.0f);
-        break;
+        return true;
 
     case MAV_CMD_DO_REPEAT_RELAY:
         sre->do_repeat_relay(cmd.content.repeat_relay.num,
                              cmd.content.repeat_relay.repeat_count,
                              cmd.content.repeat_relay.cycle_time * 1000.0f);
-        break;
+        return true;
     default:
-        break;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        AP_HAL::panic("Unhandled servo/relay case");
+#endif
+        return false;
     }
-
-    return true;
 }
 
 bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
 {
     AP_Camera *camera = AP::camera();
     if (camera == nullptr) {
-        return true;
+        return false;
     }
 
     switch (cmd.id) {
-    case MAV_CMD_DO_CONTROL_VIDEO:                      // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
-        break;
 
     case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
         camera->configure(
@@ -87,7 +86,7 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
             cmd.content.digicam_configure.exposure_type,
             cmd.content.digicam_configure.cmd_id,
             cmd.content.digicam_configure.engine_cutoff_time);
-        break;
+        return true;
 
     case MAV_CMD_DO_DIGICAM_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
         camera->control(
@@ -97,16 +96,17 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
             cmd.content.digicam_control.focus_lock,
             cmd.content.digicam_control.shooting_cmd,
             cmd.content.digicam_control.cmd_id);
-        break;
+        return true;
 
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         camera->set_trigger_distance(cmd.content.cam_trigg_dist.meters);
-        break;
+        return true;
 
     default:
-        return true;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        AP_HAL::panic("Unhandled camera case");
+#endif
+        return false;
     }
-
-    return true;
 }