From ceadcd74d03a620a74536d621ab50ce1315755ff Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 27 Dec 2020 19:18:12 -0500
Subject: [PATCH] mavlink: CAMERA_TRIGGER stream check free tx buf before send

---
 src/modules/mavlink/mavlink_messages.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 9942c48d5e..1b715462d6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -2131,7 +2131,12 @@ public:
 
 	unsigned get_size() override
 	{
-		return _trigger_sub.advertised() ? MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+		if (_trigger_sub.advertised()) {
+			return MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES
+			       + MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; // TODO: MAV_CMD_DO_DIGICAM_CONTROL
+		}
+
+		return 0;
 	}
 
 private:
@@ -2149,7 +2154,7 @@ protected:
 	{
 		camera_trigger_s trigger;
 
-		if (_trigger_sub.update(&trigger)) {
+		if ((_mavlink->get_free_tx_buf() >= get_size()) && _trigger_sub.update(&trigger)) {
 			mavlink_camera_trigger_t msg{};
 
 			msg.time_usec = trigger.timestamp;