diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index cdd0b45803..32b14e73a6 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -74,8 +74,9 @@ AP_Camera::relay_pic() } /// single entry point to take pictures +/// set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components void -AP_Camera::trigger_pic() +AP_Camera::trigger_pic(bool send_mavlink_msg) { _image_index++; switch (_trigger_type) @@ -87,6 +88,20 @@ AP_Camera::trigger_pic() relay_pic(); // basic relay activation break; } + + if (send_mavlink_msg) { + // create command long mavlink message + mavlink_command_long_t cmd_msg; + memset(&cmd_msg, 0, sizeof(cmd_msg)); + cmd_msg.command = MAV_CMD_DO_DIGICAM_CONTROL; + + // create message + mavlink_message_t msg; + mavlink_msg_command_long_encode(0, 0, &msg, &cmd_msg); + + // forward to all components + GCS_MAVLINK::send_to_components(&msg); + } } /// de-activate the trigger after some delay, but without using a delay() function @@ -150,7 +165,7 @@ AP_Camera::control_msg(mavlink_message_t* msg) */ if (packet.shot) { - trigger_pic(); + trigger_pic(false); } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 438a7cd6bd..932be5748a 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +40,8 @@ public: } // single entry point to take pictures - void trigger_pic(); + // 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