From c599ba8c975973ff18c7b2aaa695b14ba9e164aa Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Wed, 12 Apr 2017 23:15:51 -0400
Subject: [PATCH] Sub: Disable Camera object by default and rework guards

Camera object hasn't been tested on Sub yet
---
 ArduSub/ArduSub.cpp        |  7 ++++---
 ArduSub/Sub.h              | 13 +++++++++----
 ArduSub/commands_logic.cpp |  8 ++------
 ArduSub/config.h           |  2 +-
 4 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 3acb3efffb..1d91ff71c3 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -45,7 +45,9 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(gcs_send_deferred,     50,    550),
     SCHED_TASK(gcs_data_stream_send,  50,    550),
     SCHED_TASK(update_mount,          50,     75),
+#if CAMERA == ENABLED
     SCHED_TASK(update_trigger,        50,     75),
+#endif
     SCHED_TASK(ten_hz_logging_loop,   10,    350),
     SCHED_TASK(twentyfive_hz_logging, 25,    110),
     SCHED_TASK(dataflash_periodic,    400,    300),
@@ -241,11 +243,10 @@ void Sub::update_mount()
 #endif
 }
 
-
+#if CAMERA == ENABLED
 // update camera trigger
 void Sub::update_trigger(void)
 {
-#if CAMERA == ENABLED
     camera.trigger_pic_cleanup();
     if (camera.check_trigger_pin()) {
         gcs_send_message(MSG_CAMERA_FEEDBACK);
@@ -253,8 +254,8 @@ void Sub::update_trigger(void)
             DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
         }
     }
-#endif
 }
+#endif
 
 // update_batt_compass - read battery and compass
 // should be called at 10hz
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 78dc36dfed..bcc2c4f896 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -58,7 +58,6 @@
 #include <Filter/Filter.h>             // Filter library
 #include <AP_Relay/AP_Relay.h>           // APM relay
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
-#include <AP_Camera/AP_Camera.h>          // Photo or video camera
 #include <AP_Mount/AP_Mount.h>           // Camera/Antenna mount
 #include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
 #include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
@@ -110,6 +109,10 @@
 #include <AP_Rally/AP_Rally.h>           // Rally point library
 #endif
 
+#if CAMERA == ENABLED
+#include <AP_Camera/AP_Camera.h>          // Photo or video camera
+#endif
+
 // Local modules
 #include "Parameters.h"
 
@@ -455,7 +458,6 @@ private:
     void fast_loop();
     void fifty_hz_loop();
     void update_mount();
-    void update_trigger();
     void update_batt_compass(void);
     void ten_hz_logging_loop();
     void twentyfive_hz_logging();
@@ -545,8 +547,6 @@ private:
     bool verify_wait_delay();
     bool verify_within_distance();
     bool verify_yaw();
-    void do_take_picture();
-    void log_picture();
     bool acro_init(bool ignore_checks);
     void acro_run();
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
@@ -707,10 +707,15 @@ private:
 #if CAMERA == ENABLED
     void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
     void do_digicam_control(const AP_Mission::Mission_Command& cmd);
+    void do_take_picture();
+    void log_picture();
+    void update_trigger();
 #endif
+
 #if GRIPPER_ENABLED == ENABLED
     void do_gripper(const AP_Mission::Mission_Command& cmd);
 #endif
+
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_surface(const AP_Mission::Mission_Command& cmd);
     bool verify_RTL(void);
diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp
index ed28dbd69a..749b18c659 100644
--- a/ArduSub/commands_logic.cpp
+++ b/ArduSub/commands_logic.cpp
@@ -843,10 +843,10 @@ void Sub::do_roi(const AP_Mission::Mission_Command& cmd)
     set_auto_yaw_roi(cmd.content.location);
 }
 
+#if CAMERA == ENABLED
 // do_digicam_configure Send Digicam Configure message with the camera library
 void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
 {
-#if CAMERA == ENABLED
     camera.configure(cmd.content.digicam_configure.shooting_mode,
                      cmd.content.digicam_configure.shutter_speed,
                      cmd.content.digicam_configure.aperture,
@@ -854,13 +854,11 @@ void Sub::do_digicam_configure(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);
-#endif
 }
 
 // do_digicam_control Send Digicam Control message with the camera library
 void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 {
-#if CAMERA == ENABLED
     if (camera.control(cmd.content.digicam_control.session,
                        cmd.content.digicam_control.zoom_pos,
                        cmd.content.digicam_control.zoom_step,
@@ -869,16 +867,13 @@ void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
                        cmd.content.digicam_control.cmd_id)) {
         log_picture();
     }
-#endif
 }
 
 // do_take_picture - take a picture with the camera library
 void Sub::do_take_picture()
 {
-#if CAMERA == ENABLED
     camera.trigger_pic(true);
     log_picture();
-#endif
 }
 
 // log_picture - log picture taken and send feedback to GCS
@@ -895,6 +890,7 @@ void Sub::log_picture()
         }
     }
 }
+#endif
 
 // point the camera to a specified angle
 void Sub::do_mount_control(const AP_Mission::Mission_Command& cmd)
diff --git a/ArduSub/config.h b/ArduSub/config.h
index 9a3feec41a..9c0fa14d83 100644
--- a/ArduSub/config.h
+++ b/ArduSub/config.h
@@ -191,7 +191,7 @@
 // CAMERA TRIGGER AND CONTROL
 //
 #ifndef CAMERA
-# define CAMERA        ENABLED
+# define CAMERA        DISABLED
 #endif
 
 //////////////////////////////////////////////////////////////////////////////