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

libraries/AP_Camera/AP_Camera.cpp

@@ -217,14 +217,12 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu
     GCS_MAVLINK::send_to_components(&msg);
 }
 
-bool AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
+void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
 {
-    bool ret = false;
-    
     // take picture
     if (is_equal(shooting_cmd,1.0f)) {
         trigger_pic(false);
-        ret = true;
+        log_picture();
     }
 
     mavlink_message_t msg;
@@ -244,13 +242,12 @@ bool AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
 
     // send to all components
     GCS_MAVLINK::send_to_components(&msg);
-    return ret;
 }
 
 /*
   Send camera feedback to the GCS
  */
-void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc)
+void AP_Camera::send_feedback(mavlink_channel_t chan)
 {
     float altitude, altitude_rel;
     if (current_loc.flags.relative_alt) {
@@ -271,42 +268,44 @@ void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS
 }
 
 
-/*  update location, for triggering by GPS distance moved
-    This function returns true if a picture should be taken
-    The caller is responsible for taking the picture based on the return value of this function.
-    The caller is also responsible for logging the details about the photo
+/*  update; triggers by distance moved
 */
-bool AP_Camera::update_location(const struct Location &loc, const AP_AHRS &ahrs)
+void AP_Camera::update()
 {
+    if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
+        return;
+    }
+
     if (is_zero(_trigg_dist)) {
-        return false;
+        return;
     }
     if (_last_location.lat == 0 && _last_location.lng == 0) {
-        _last_location = loc;
-        return false;
+        _last_location = current_loc;
+        return;
     }
-    if (_last_location.lat == loc.lat && _last_location.lng == loc.lng) {
-        // we haven't moved - this can happen as update_location() may
+    if (_last_location.lat == current_loc.lat && _last_location.lng == current_loc.lng) {
+        // we haven't moved - this can happen as update() may
         // be called without a new GPS fix
-        return false;
+        return;
     }
 
-    if (get_distance(loc, _last_location) < _trigg_dist) {
-        return false;
+    if (get_distance(current_loc, _last_location) < _trigg_dist) {
+        return;
     }
 
     if (_max_roll > 0 && labs(ahrs.roll_sensor/100) > _max_roll) {
-        return false;
+        return;
     }
 
     uint32_t tnow = AP_HAL::millis();
     if (tnow - _last_photo_time < (unsigned) _min_interval) {
-        return false;
-    }  else {
-        _last_location = loc;
-        _last_photo_time = tnow;
-        return true;
+        return;
     }
+
+    take_picture();
+
+    _last_location = current_loc;
+    _last_photo_time = tnow;
 }
 
 /*
@@ -398,3 +397,46 @@ failed:
     }
     _timer_installed = true;
 }
+
+// log_picture - log picture taken and send feedback to GCS
+void AP_Camera::log_picture()
+{
+    DataFlash_Class *df = DataFlash_Class::instance();
+    if (df == nullptr) {
+        return;
+    }
+    if (!using_feedback_pin()) {
+        gcs().send_message(MSG_CAMERA_FEEDBACK);
+        if (df->should_log(log_camera_bit)) {
+            df->Log_Write_Camera(ahrs, gps, current_loc);
+        }
+    } else {
+        if (df->should_log(log_camera_bit)) {
+            df->Log_Write_Trigger(ahrs, gps, current_loc);
+        }
+    }
+}
+
+// take_picture - take a picture
+void AP_Camera::take_picture()
+{
+    trigger_pic(true);
+    log_picture();
+}
+
+/*
+  update camera trigger - 50Hz
+ */
+void AP_Camera::update_trigger()
+{
+    trigger_pic_cleanup();
+    if (check_trigger_pin()) {
+        gcs().send_message(MSG_CAMERA_FEEDBACK);
+        DataFlash_Class *df = DataFlash_Class::instance();
+        if (df != nullptr) {
+            if (df->should_log(log_camera_bit)) {
+                df->Log_Write_Camera(ahrs, gps, current_loc);
+            }
+        }
+    }
+}

libraries/AP_Camera/AP_Camera.h

@@ -33,43 +33,38 @@ class AP_Camera {
 public:
     /// Constructor
     ///
-    AP_Camera(AP_Relay *obj_relay) :
+    AP_Camera(AP_Relay *obj_relay, uint32_t _log_camera_bit, const struct Location &_loc, const AP_GPS &_gps, const AP_AHRS &_ahrs) :
         _trigger_counter(0),    // count of number of cycles shutter has been held open
-        _image_index(0)
+        _image_index(0),
+        log_camera_bit(_log_camera_bit),
+        current_loc(_loc),
+        gps(_gps),
+        ahrs(_ahrs)
     {
 		AP_Param::setup_object_defaults(this, var_info);
         _apm_relay = obj_relay;
     }
 
-    // single entry point to take pictures
-    //  set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components
-    void            trigger_pic(bool send_mavlink_msg);
-
-    // de-activate the trigger after some delay, but without using a delay() function
-    // should be called at 50hz from main program
-    void            trigger_pic_cleanup();
-
     // MAVLink methods
     void            control_msg(mavlink_message_t* msg);
-    void            send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc);
+    void            send_feedback(mavlink_channel_t chan);
 
     // Command processing
     void            configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
-    // handle camera control. Return true if picture was triggered
-    bool            control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
+    // handle camera control
+    void            control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
 
     // set camera trigger distance in a mission
     void            set_trigger_distance(uint32_t distance_m) { _trigg_dist.set(distance_m); }
 
-    // Update location of vehicle and return true if a picture should be taken
-    bool update_location(const struct Location &loc, const AP_AHRS &ahrs);
+    void take_picture();
 
-    // check if trigger pin has fired
-    bool check_trigger_pin(void);
+    // Update - to be called periodically @at least 10Hz
+    void update();
+
+    // update camera trigger - 50Hz
+    void update_trigger();
 
-    // return true if we are using a feedback pin
-    bool using_feedback_pin(void) const { return _feedback_pin > 0; }
-    
     static const struct AP_Param::GroupInfo        var_info[];
 
 private:
@@ -105,4 +100,26 @@ private:
     static volatile bool   _camera_triggered;
     bool            _timer_installed:1;
     uint8_t         _last_pin_state;
+
+    void log_picture();
+
+    uint32_t log_camera_bit;
+    const struct Location &current_loc;
+    const AP_GPS &gps;
+    const AP_AHRS &ahrs;
+
+    // single entry point to take pictures
+    //  set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components
+    void            trigger_pic(bool send_mavlink_msg);
+
+    // de-activate the trigger after some delay, but without using a delay() function
+    // should be called at 50hz from main program
+    void trigger_pic_cleanup();
+
+    // check if trigger pin has fired
+    bool check_trigger_pin(void);
+
+    // return true if we are using a feedback pin
+    bool using_feedback_pin(void) const { return _feedback_pin > 0; }
+
 };