From 172f6b173ad7232e6df91eb23592b35be64a5c13 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Wed, 17 Jan 2024 15:39:40 +0300
Subject: [PATCH] MAVLink input support (control using mobile phone)

---
 README.md                 |  3 ++-
 flix/flix.ino             |  2 +-
 flix/mavlink.ino          | 34 ++++++++++++++++++++++++++++++++++
 flix/wifi.ino             |  6 ++++++
 gazebo/soc/rtc_cntl_reg.h |  1 +
 gazebo/soc/soc.h          |  3 +++
 6 files changed, 47 insertions(+), 2 deletions(-)
 create mode 100644 gazebo/soc/rtc_cntl_reg.h
 create mode 100644 gazebo/soc/soc.h

diff --git a/README.md b/README.md
index 9c5e887..1b49a0e 100644
--- a/README.md
+++ b/README.md
@@ -12,9 +12,10 @@
 * In-RAM logging.
 * Command line interface through USB port.
 * Wi-Fi support.
+* MAVLink support.
+* Control using mobile phone (with QGroundControl app).
 * ESCs with reverse mode support.
 * *Textbook and videos for students on writing a flight controller\*.*
-* *MAVLink support\*.*
 * *Completely 3D-printed frame*.*
 * *Position control and autonomous flights using external camera\**.
 * [Building and running instructions](docs/build.md).
diff --git a/flix/flix.ino b/flix/flix.ino
index fd692b6..d4e2531 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -60,7 +60,7 @@ void loop() {
 	sendMotors();
 	parseInput();
 #if WIFI_ENABLED == 1
-	sendMavlink();
+	processMavlink();
 #endif
 	logData();
 }
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 3a61471..ffb9797 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -10,6 +10,12 @@
 #define SYSTEM_ID 1
 #define PERIOD_SLOW 1.0
 #define PERIOD_FAST 0.1
+#define MAVLINK_CONTROL_SCALE 0.7f
+
+void processMavlink() {
+	sendMavlink();
+	receiveMavlink();
+}
 
 void sendMavlink() {
 	static float lastSlow = 0;
@@ -60,4 +66,32 @@ void sendMessage(const void *msg) {
 	sendWiFi(buf, len);
 }
 
+void receiveMavlink() {
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+	int len = receiveWiFi(buf, MAVLINK_MAX_PACKET_LEN);
+
+	// New packet, parse it
+	mavlink_message_t msg;
+	mavlink_status_t status;
+	for (int i = 0; i < len; i++) {
+		if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
+			handleMavlink(&msg);
+		}
+	}
+}
+
+void handleMavlink(const void *_msg) {
+	mavlink_message_t *msg = (mavlink_message_t *)_msg;
+	if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
+		mavlink_manual_control_t manualControl;
+		mavlink_msg_manual_control_decode(msg, &manualControl);
+		controls[RC_CHANNEL_THROTTLE] = manualControl.z / 1000.0f;
+		controls[RC_CHANNEL_PITCH] = manualControl.x / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_ROLL] = manualControl.y / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE;
+		controls[RC_CHANNEL_MODE] = 1; // STAB mode
+		controls[RC_CHANNEL_AUX] = 1; // armed
+	}
+}
+
 #endif
diff --git a/flix/wifi.ino b/flix/wifi.ino
index 05c03df..562610e 100644
--- a/flix/wifi.ino
+++ b/flix/wifi.ino
@@ -20,6 +20,7 @@ void setupWiFi() {
 	Serial.println("Setup Wi-Fi");
 	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
 	IPAddress myIP = WiFi.softAPIP();
+	udp.begin(WIFI_UDP_PORT);
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
@@ -29,4 +30,9 @@ void sendWiFi(const uint8_t *buf, int len) {
 	udp.endPacket();
 }
 
+int receiveWiFi(uint8_t *buf, int len) {
+	udp.parsePacket();
+	return udp.read(buf, len);
+}
+
 #endif
diff --git a/gazebo/soc/rtc_cntl_reg.h b/gazebo/soc/rtc_cntl_reg.h
new file mode 100644
index 0000000..f595162
--- /dev/null
+++ b/gazebo/soc/rtc_cntl_reg.h
@@ -0,0 +1 @@
+// Dummy file to make it possible to compile simulator with util.ino
diff --git a/gazebo/soc/soc.h b/gazebo/soc/soc.h
new file mode 100644
index 0000000..dcb15c2
--- /dev/null
+++ b/gazebo/soc/soc.h
@@ -0,0 +1,3 @@
+// Dummy file to make it possible to compile simulator with util.ino
+
+#define WRITE_PERI_REG(addr, val) {}