From 21dc47c472b7116caf86e7c39c221547d4da6e82 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Mon, 5 May 2025 00:44:06 +0300
Subject: [PATCH] Make mavlink print buffered Combine all output of each step
 into one SERIAL_CONTROL message

---
 flix/mavlink.ino | 11 ++++++++++-
 gazebo/flix.h    |  1 +
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 8537f8b..a38661b 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -13,6 +13,7 @@
 #define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
 
 float mavlinkControlScale = 0.7;
+String mavlinkPrintBuffer;
 
 extern double controlsTime;
 extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
@@ -23,6 +24,8 @@ void processMavlink() {
 }
 
 void sendMavlink() {
+	sendMavlinkPrint();
+
 	static double lastSlow = 0;
 	static double lastFast = 0;
 
@@ -197,7 +200,12 @@ void handleMavlink(const void *_msg) {
 
 // Send shell output to GCS
 void mavlinkPrint(const char* str) {
-	// Send data in chunks
+	mavlinkPrintBuffer += str;
+}
+
+void sendMavlinkPrint() {
+	// Send mavlink print data in chunks
+	const char *str = mavlinkPrintBuffer.c_str();
 	for (int i = 0; i < strlen(str); i += MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN) {
 		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
 		strlcpy(data, str + i, sizeof(data));
@@ -206,6 +214,7 @@ void mavlinkPrint(const char* str) {
 			SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, strlen(data), (uint8_t *)data, 0, 0);
 		sendMessage(&msg);
 	}
+	mavlinkPrintBuffer.clear();
 }
 
 // Convert Forward-Left-Up to Forward-Right-Down quaternion
diff --git a/gazebo/flix.h b/gazebo/flix.h
index a4866a0..0b80c46 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -51,6 +51,7 @@ void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
 void mavlinkPrint(const char* str);
+void sendMavlinkPrint();
 inline Quaternion fluToFrd(const Quaternion &q);
 void failsafe();
 void armingFailsafe();