Sub: camera is responsible for taking distance-based-images and logging

ArduSub/ArduSub.cpp

@@ -238,13 +238,7 @@ void Sub::update_mount()
 // update camera trigger
 void Sub::update_trigger(void)
 {
-    camera.trigger_pic_cleanup();
-    if (camera.check_trigger_pin()) {
-        gcs().send_message(MSG_CAMERA_FEEDBACK);
-        if (should_log(MASK_LOG_CAMERA)) {
-            DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
-        }
-    }
+    camera.update_trigger();
 }
 #endif
 
@@ -408,15 +402,9 @@ void Sub::update_GPS(void)
         // set system time if necessary
         set_system_time_from_GPS();
 
-        // checks to initialise home and take location based pictures
-        if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-
 #if CAMERA == ENABLED
-            if (camera.update_location(current_loc, sub.ahrs) == true) {
-                do_take_picture();
-            }
+        camera.update();
 #endif
-        }
     }
 }
 

ArduSub/GCS_Mavlink.cpp

@@ -578,7 +578,7 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
     case MSG_CAMERA_FEEDBACK:
 #if CAMERA == ENABLED
         CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
-        sub.camera.send_feedback(chan, sub.gps, sub.ahrs, sub.current_loc);
+        sub.camera.send_feedback(chan);
 #endif
         break;
 
@@ -1147,14 +1147,12 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_DO_DIGICAM_CONTROL:
-            if (sub.camera.control(packet.param1,
-                                   packet.param2,
-                                   packet.param3,
-                                   packet.param4,
-                                   packet.param5,
-                                   packet.param6)) {
-                sub.log_picture();
-            }
+            sub.camera.control(packet.param1,
+                               packet.param2,
+                               packet.param3,
+                               packet.param4,
+                               packet.param5,
+                               packet.param6);
             result = MAV_RESULT_ACCEPTED;
             break;
         case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
@@ -1566,7 +1564,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         //deprecated.  Use MAV_CMD_DO_DIGICAM_CONTROL
     case MAVLINK_MSG_ID_DIGICAM_CONTROL:
         sub.camera.control_msg(msg);
-        sub.log_picture();
         break;
 #endif // CAMERA == ENABLED
 

ArduSub/Sub.cpp

@@ -62,7 +62,7 @@ Sub::Sub(void) :
           mainLoop_count(0),
           ServoRelayEvents(relay),
 #if CAMERA == ENABLED
-          camera(&relay),
+          camera(&relay, MASK_LOG_CAMERA, current_loc, gps, ahrs),
 #endif
 #if MOUNT == ENABLED
           camera_mount(ahrs, current_loc),

ArduSub/Sub.h

@@ -689,8 +689,6 @@ 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
 

ArduSub/commands_logic.cpp

@@ -873,37 +873,14 @@ void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
 // do_digicam_control Send Digicam Control message with the camera library
 void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 {
-    if (camera.control(cmd.content.digicam_control.session,
-                       cmd.content.digicam_control.zoom_pos,
-                       cmd.content.digicam_control.zoom_step,
-                       cmd.content.digicam_control.focus_lock,
-                       cmd.content.digicam_control.shooting_cmd,
-                       cmd.content.digicam_control.cmd_id)) {
-        log_picture();
-    }
+    camera.control(cmd.content.digicam_control.session,
+                   cmd.content.digicam_control.zoom_pos,
+                   cmd.content.digicam_control.zoom_step,
+                   cmd.content.digicam_control.focus_lock,
+                   cmd.content.digicam_control.shooting_cmd,
+                   cmd.content.digicam_control.cmd_id);
 }
 
-// do_take_picture - take a picture with the camera library
-void Sub::do_take_picture()
-{
-    camera.trigger_pic(true);
-    log_picture();
-}
-
-// log_picture - log picture taken and send feedback to GCS
-void Sub::log_picture()
-{
-    if (!camera.using_feedback_pin()) {
-        gcs().send_message(MSG_CAMERA_FEEDBACK);
-        if (should_log(MASK_LOG_CAMERA)) {
-            DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
-        }
-    } else {
-        if (should_log(MASK_LOG_CAMERA)) {
-            DataFlash.Log_Write_Trigger(ahrs, gps, current_loc);
-        }
-    }
-}
 #endif
 
 // point the camera to a specified angle