From b587025ad15773bf12d52468c2216604803d740e Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Tue, 26 Aug 2014 11:53:38 -0700 Subject: [PATCH] Copter: send a camera_feedback message when there is a camera trigger --- ArduCopter/GCS_Mavlink.pde | 7 +++++++ ArduCopter/commands_logic.pde | 1 + 2 files changed, 8 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 143a232c98..d01e73750a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -593,6 +593,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) #endif break; + case MSG_CAMERA_FEEDBACK: +#if CAMERA == ENABLED + CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); + camera.send_feedback(chan, gps, ahrs, current_loc); + break; +#endif + case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); send_statustext(chan); diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 8ebd7e8481..89f0e0f17f 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -925,6 +925,7 @@ static void do_take_picture() { #if CAMERA == ENABLED camera.trigger_pic(); + gcs_send_message(MSG_CAMERA_FEEDBACK); if (should_log(MASK_LOG_CAMERA)) { DataFlash.Log_Write_Camera(ahrs, gps, current_loc); }