diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index e26915ea7e..c8afd3c8b5 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -344,7 +344,8 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) const feedback.roll_sensor*1e-2f, feedback.pitch_sensor*1e-2f, feedback.yaw_sensor*1e-2f, - 0.0f, CAMERA_FEEDBACK_PHOTO, _camera_trigger_logged); + 0.0f, CAMERA_FEEDBACK_PHOTO, + feedback.camera_trigger_logged); } @@ -508,6 +509,7 @@ void AP_Camera::prep_mavlink_msg_camera_feedback(uint64_t timestamp_us) feedback.roll_sensor = ahrs.roll_sensor; feedback.pitch_sensor = ahrs.pitch_sensor; feedback.yaw_sensor = ahrs.yaw_sensor; + feedback.camera_trigger_logged = _camera_trigger_logged; gcs().send_message(MSG_CAMERA_FEEDBACK); } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 65a84fd129..64f070477a 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -123,6 +123,7 @@ private: int32_t roll_sensor; int32_t pitch_sensor; int32_t yaw_sensor; + uint32_t camera_trigger_logged; // ID sequence number } feedback; void prep_mavlink_msg_camera_feedback(uint64_t timestamp_us);