From 7bb4a396085aeea94307141a5f2e734e9355103f Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 26 Aug 2014 14:08:54 -0700 Subject: [PATCH] Camera: record number of pictures taken since boot The field is useful for detecting in a GCS if a mavlink_msg_camera_feedback message was dropped. --- libraries/AP_Camera/AP_Camera.cpp | 3 ++- libraries/AP_Camera/AP_Camera.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 3cd5b5e698..708fb1674e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -77,6 +77,7 @@ AP_Camera::relay_pic() void AP_Camera::trigger_pic() { + _image_index++; switch (_trigger_type) { case AP_CAMERA_TRIGGER_TYPE_SERVO: @@ -177,7 +178,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS mavlink_msg_camera_feedback_send(chan, gps.time_epoch_usec(), - 0, 0, 0, + 0, 0, _image_index, current_loc.lat, current_loc.lng, altitude/100.0, altitude_rel/100.0, ahrs.roll_sensor/100.0, ahrs.pitch_sensor/100.0, ahrs.yaw_sensor/100.0, diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index f2574b26fe..e40340ff35 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -68,8 +68,9 @@ private: void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation - AP_Float _trigg_dist; // distance between trigger points (meters) + AP_Float _trigg_dist; // distance between trigger points (meters) struct Location _last_location; + uint16_t _image_index; // number of pictures taken since boot };