MAVLink input support (control using mobile phone)

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).

flix/flix.ino

@@ -60,7 +60,7 @@ void loop() {
 	sendMotors();
 	parseInput();
 #if WIFI_ENABLED == 1
-	sendMavlink();
+	processMavlink();
 #endif
 	logData();
 }

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

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

gazebo/soc/rtc_cntl_reg.h

@@ -0,0 +1 @@
+// Dummy file to make it possible to compile simulator with util.ino

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) {}