Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

MAVLink input support (control using mobile phone)

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