Position control draft

Virtual rc is disabled if rxRxPin < 0

.github/workflows/build.yml

@@ -27,6 +27,8 @@ jobs:
       run: make BOARD=esp32:esp32:esp32c3
     - name: Build firmware for ESP32-S3
       run: make BOARD=esp32:esp32:esp32s3
+    - name: Build espnow-proxy
+      run: arduino-cli compile --fqbn esp32:esp32:esp32 tools/espnow-proxy
     - name: Check c_cpp_properties.json
       run: tools/check_c_cpp_properties.py
 

README.md

@@ -84,7 +84,7 @@ Additional articles:
 |*Boost converter (optional, for more stable power supply)*|*5V output*|<img src="docs/img/buck-boost.jpg" width=100>|1|
 |Motor|8520 3.7V brushed motor.<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).<br>Make sure the motor shaft diameter and propeller hole diameter match!|<img src="docs/img/motor.jpeg" width=100>|4|
 |Propeller|55 mm or 65 mm|<img src="docs/img/prop.jpg" width=100>|4|
-|MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
+|MOSFET (transistor)|UMW 100N03A or [analog](https://t.me/opensourcequadcopter/33).<br>Warning: don't use KIA 100N03A or other manufacturers, they might not work!|<img src="docs/img/100n03a.jpg" width=100>|4|
 |Pull-down resistor<br>Voltage measurement resistor|10 kΩ|<img src="docs/img/resistor10k.jpg" width=100>|6|
 |3.7V Li-Po battery|LW 952540 (or any compatible by the size).<br>Make sure the battery has enough discharge rate — 25C or more!|<img src="docs/img/battery.jpg" width=100>|1|
 |Battery connector cable|MX2.0 2P female|<img src="docs/img/mx.png" width=100>|1|

docs/usage.md

@@ -25,7 +25,7 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
    * `FlixPeriph`, the latest version.
    * `MAVLink`, version 2.0.25.
 5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
-6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
+6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini, *ESP32S3 Dev Module* for ESP32-S3 Super Mini) and the port.
 7. Set *Tools* ⇒ *Core Debug Level* to *Error* to see the errors in the serial console. Set *Tools* ⇒ *USB CDC on Boot* to *Enabled* for ESP32-S3/ESP32-C3 boards.
 8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
 
@@ -76,8 +76,25 @@ See other available Make commands in [Makefile](../Makefile).
 In case if using different IMU model than MPU9250, change `imu` variable declaration in the `imu.ino`:
 
 ```cpp
-ICM20948 imu(SPI);  // For ICM-20948
-MPU6050 imu(Wire);  // For MPU-6050
+ICM20948 imu(SPI);  // For ICM-20948 via SPI
+// or
+MPU6050 imu(Wire);  // For MPU-6050 via I2C
+```
+
+If using non-default SPI pins, pass SCK, MISO, and MOSI pin numbers to `SPI.begin` call and SS pin to `imu` constructor like that:
+
+```cpp
+ICM20948(SPI, <SS>);
+// ...
+SPI.begin(<SCK>, <MISO>, <MOSI>);
+imu.begin();
+```
+
+If using non-default I2C pins, pass SDA and SCL pin numbers to `Wire.setPins` call like that:
+
+```cpp
+Wire.begin(<SDA>, <SCL>);
+imu.begin();
 ```
 
 ### Connect using QGroundControl
@@ -105,7 +122,7 @@ To access the console using serial port:
 To access the console using QGroundControl:
 
 1. Connect to the drone using QGroundControl app.
-2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
+2. Go to the QGroundControl menu ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
 
 <img src="img/cli.png" width="400">
 
@@ -333,7 +350,7 @@ To setup ESP-NOW communication:
 
 1. Flash the second ESP32 board with ESP-NOW proxy sketch: [`tools/espnow-proxy/espnow-proxy.ino`](../tools/espnow-proxy/espnow-proxy.ino). Use Arduino IDE or command line: `make upload_proxy`.
 
-2. Open Serial Monitor or use `make monitor` command. The ESP32 will print its MAC address and generated encryption key, for example:
+2. Open Serial Monitor in Arduino IDE or use `make monitor` command. The ESP32 will print its MAC address and generated encryption key, for example:
 
    ```
    espnow 7a:c8:e3:eb:bf:e9 &PiuSysxP9+$L&5E
@@ -352,7 +369,10 @@ To setup ESP-NOW communication:
    * Type: Serial.
    * Serial Port: choose the port of the proxy ESP32 board, e. g. `/dev/cu.usbserial-0001`.
    * Baud Rate: 115200.
-5. Click *Save*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
+5. Click *Save*, click *Connect*. QGroundControl should connect to the drone using ESP-NOW and begin showing the telemetry.
+
+> [!TIP]
+> Make sure Arduino IDE is not running when using ESP-NOW proxy board, as it may block it.
 
 ## Flight log
 

docs/user.md

@@ -4,6 +4,15 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
+Author: Alican Erüst.<br>
+Description: QX95 mm frame, 55 mm propellers, 3.7 V 25C 1050 mAh LiPo battery, MPU6050 IMU, Logitech F310 gamepad controller, with a total quadcopter weight of 66 g.
+
+<img src="img/user/alicanerus/1.jpg" height=200> <img src="img/user/alicanerus/2.jpg" height=200> <img src="img/user/alicanerus/3.jpg" height=200>
+
+[Flight video](https://drive.google.com/file/d/1k0WeWTKnCAfaugkX7LcmNxsUuq79RL8Z/view?usp=sharing).
+
+---
+
 Author: [Неруш Михаил](https://t.me/NerushMV).<br>
 Description: custom frame made of 4 mm plywood, 8520 brushed motors, 75 mm propellers, MPU-6500. FlySky FS-i6X with ESP32-based adapter for ESP-NOW communication (using PPM output).<br>
 Sources and materials: [link](https://drive.google.com/drive/folders/1uWiDcuorLrtVs_IIR7Y13omij-7Q1nx8).

flix/cli.ino

@@ -9,7 +9,7 @@
 #include "filter.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern const int RAW, ACRO, STAB, AUTO;
+extern const int RAW, ACRO, STAB, AUTO, POS;
 extern const int W_AP, W_STA, W_ESPNOW;
 extern float t, dt, loopRate;
 extern uint16_t channels[16];
@@ -50,8 +50,8 @@ const char* motd =
 "sta <ssid> <password> - configure Wi-Fi client mode\n"
 "espnow <mac> [<key>] - configure ESP-NOW peer\n"
 "mot - show motor output\n"
+"mfr/mfl/mrr/mrl [<thrust>] - test motor (remove props)\n"
 "log [dump] - print log header [and data]\n"
-"mfr, mfl, mrr, mrl - test motor (remove props)\n"
 "log - show log info\n"
 "log header - show log header\n"
 "log reset - reset log\n"
@@ -134,6 +134,8 @@ void doCommand(String str, bool echo = false) {
 		mode = ACRO;
 	} else if (command == "auto") {
 		mode = AUTO;
+	} else if (command == "pos") {
+		mode = POS;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {
@@ -176,13 +178,13 @@ void doCommand(String str, bool echo = false) {
 	} else if (command == "ca") {
 		calibrateAccel();
 	} else if (command == "mfr") {
-		testMotor(MOTOR_FRONT_RIGHT);
+		testMotor(MOTOR_FRONT_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
 	} else if (command == "mfl") {
-		testMotor(MOTOR_FRONT_LEFT);
+		testMotor(MOTOR_FRONT_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
 	} else if (command == "mrr") {
-		testMotor(MOTOR_REAR_RIGHT);
+		testMotor(MOTOR_REAR_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
 	} else if (command == "mrl") {
-		testMotor(MOTOR_REAR_LEFT);
+		testMotor(MOTOR_REAR_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());
 	} else if (command == "sys") {
 #ifdef ESP32
 		print("Chip: %s\n", ESP.getChipModel());

flix/control.ino

@@ -34,7 +34,7 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3, POS = 4;
 int mode = STAB;
 bool armed = false;
 
@@ -56,6 +56,7 @@ int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
+extern Vector position, positionTarget;
 
 void control() {
 	interpretControls();
@@ -79,7 +80,18 @@ void interpretControls() {
 
 	thrustTarget = controlThrottle;
 
-	if (mode == STAB) {
+	if (mode == POS) {
+		if (controlRoll != 0 || controlPitch != 0) {
+			positionTarget.x = NAN;
+			positionTarget.y = NAN;
+		} else if (invalid(positionTarget.x) || invalid(positionTarget.y)) {
+			// reset position target
+			positionTarget.x = position.x;
+			positionTarget.y = position.y;
+		}
+	}
+
+	if (mode == STAB || (mode == POS && invalid(positionTarget.x))) {
 		float yawTarget = attitudeTarget.getYaw();
 		if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target
 		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));

flix/estimate.ino

@@ -15,7 +15,7 @@ bool landed;
 float accWeight = 0.003;
 float levelWeight = 0.0002;
 LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
-NotchFilter<Vector> ratesNotch(382, 40);
+NotchFilter<Vector> ratesNotch(382, 0);
 
 void setupEstimate() {
 	print("Setup estimation\n");
@@ -26,6 +26,7 @@ void estimate() {
 	applyGyro();
 	applyAcc();
 	applyLevel();
+	estimatePosition();
 }
 
 void applyGyro() {

flix/log.ino

@@ -6,7 +6,7 @@
 #include "vector.h"
 #include "util.h"
 
-int logMemory = 0; // 0 - RAM, 1 - PSRAM, -1 - disabled
+int logMemory = -1; // 0 - RAM, 1 - PSRAM, -1 - disabled
 float logUsage = 0.5; // fraction of free memory to use for log
 
 struct LogValue {
@@ -68,6 +68,11 @@ LogTopic logTopics[] = {
 		{"attitude.pitch", []() { return attitude.getPitch(); }},
 		{"attitude.yaw", []() { return attitude.getYaw(); }}),
 
+	LogTopic(50,
+		{"position.x", &position.x},
+		{"position.y", &position.y},
+		{"position.z", &position.z}),
+
 	// rc
 	LogTopic(10,
 		{"controlTime", &controlTime, false}, // trigger value

flix/mavlink.ino

@@ -196,6 +196,13 @@ void handleMavlink(const void *_msg) {
 		sendMessage(&msg);
 	}
 
+	if (msg.msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
+		mavlink_vision_position_estimate_t m;
+		mavlink_msg_vision_position_estimate_decode(&msg, &m);
+		Vector pos(m.x, m.y, m.z);
+		correctPosition(pos);
+	}
+
 	if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
 		mavlink_mission_request_list_t m;
 		mavlink_msg_mission_request_list_decode(&msg, &m);

flix/motors.ino

@@ -7,7 +7,7 @@
 
 float motors[4]; // normalized motor thrusts in range [0..1]
 
-int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
+int motorPins[4] = {-1, -1, -1, -1}; // default pin numbers
 int pwmFrequency = 78000;
 int pwmResolution = 10;
 int pwmStop = 0;
@@ -51,9 +51,9 @@ bool motorsActive() {
 	return motors[0] != 0 || motors[1] != 0 || motors[2] != 0 || motors[3] != 0;
 }
 
-void testMotor(int n) {
+void testMotor(int n, float thrust) {
 	print("Testing motor %d\n", n);
-	motors[n] = 0.2;
+	motors[n] = thrust;
 	delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
 	sendMotors();
 	pause(3);

flix/parameters.ino

@@ -5,14 +5,16 @@
 
 #include <Preferences.h>
 #include "util.h"
+#include "pid.h"
 
 extern int channelZero[16], channelMax[16];
 extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
 extern int rcRxPin, voltagePin;
 extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel;
-extern float rcLossTimeout, descendTime;
+extern float rcLossTimeout, descendTime, disarmTilt;
 extern float voltageScale;
 extern LowPassFilter<float> voltageFilter;
+extern PID posX_PID, posY_PID;
 
 Preferences storage;
 
@@ -59,6 +61,12 @@ Parameter parameters[] = {
 	{"CTL_FLT_MODE_0", &flightModes[0]},
 	{"CTL_FLT_MODE_1", &flightModes[1]},
 	{"CTL_FLT_MODE_2", &flightModes[2]},
+	// position
+	{"POS_XY_P", &posX_PID.p, setupPosition},
+	{"POS_XY_I", &posX_PID.i, setupPosition},
+	{"POS_XY_D", &posX_PID.d, setupPosition},
+	{"POS_XY_WU", &posX_PID.windup, setupPosition},
+	{"POS_XY_D_A", &posX_PID.lpf.alpha, setupPosition},
 	// imu
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},
@@ -146,6 +154,7 @@ Parameter parameters[] = {
 	// safety
 	{"SF_RC_LOSS_TIME", &rcLossTimeout},
 	{"SF_DESCEND_TIME", &descendTime},
+	{"SF_DISARM_TILT", &disarmTilt}
 };
 
 void setupParameters() {

flix/position.ino

@@ -0,0 +1,95 @@
+#include "vector.h"
+#include "quaternion.h"
+#include "pid.h"
+#include "util.h"
+
+float mass = 0.065;
+float motorThrust = 0.031 * ONE_G;
+float positionWeight = 0.9;
+float velocityWeight = 0.9;
+
+PID velX_PID(1.0, 0.0, 0.02);
+PID velY_PID(1.0, 0.0, 0.02);
+PID velZ_PID(1.0, 0.0, 0.02);
+
+// PID posX_PID(1.0, 0.0, 0.0);
+// PID posY_PID(1.0, 0.0, 0.0);
+// PID posZ_PID(1.0, 0.0, 0.0);
+
+PID posX_PID(0.1, 0.0, 0.0);
+PID posY_PID(0.1, 0.0, 0.0);
+PID posZ_PID(0.1, 0.0, 0.0);
+
+Vector acceleration;
+Vector velocity;
+Vector position;
+Vector velocityTarget(NAN, NAN, NAN);
+Vector positionTarget(NAN, NAN, NAN);
+
+extern Quaternion attitudeTarget;
+extern float thrustTarget;
+
+void setupPosition() {
+	// TODO: posY_PID = posX_PID;
+	// posX_PID.copy(posY_PID);
+	posY_PID.p = posX_PID.p;
+	posY_PID.i = posX_PID.i;
+	posY_PID.d = posX_PID.d;
+}
+
+void estimatePosition() {
+	acceleration = Quaternion::rotateVector(acceleration, attitude) + Vector(0, 0, -ONE_G);
+	velocity += acceleration * dt;
+	position += velocity * dt;
+}
+
+void correctPosition(Vector& pos) {
+	// position += (pos - position) * positionWeight;
+	position = pos;
+	// positionUpdated = true;
+
+	if (invalid(positionTarget.x) || invalid(positionTarget.y)) return;
+
+	attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
+	attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
+}
+
+void correctVelocity(Vector& vel) {
+	velocity += (vel - velocity) * velocityWeight;
+}
+
+void controlVelocity() {
+	Vector accelerationTarget(
+		velX_PID.update(velocityTarget.x - velocity.x),
+		velY_PID.update(velocityTarget.y - velocity.y),
+		velZ_PID.update(velocityTarget.z - velocity.z)
+	);
+
+	Vector thrustVector = (accelerationTarget + Vector(0, 0, ONE_G)) * mass;
+
+	const Vector up(0, 0, 1);
+
+	attitudeTarget = Quaternion::fromBetweenVectors(up, thrustVector);
+	float maxThrust = motorThrust * 4; // 4 motors
+	thrustTarget = thrustVector.norm() / maxThrust;
+}
+
+void controlPosition() {
+	if (positionTarget.invalid()) return;
+
+	velocityTarget = Vector(
+		posX_PID.update(positionTarget.x - position.x),
+		posY_PID.update(positionTarget.y - position.y),
+		posZ_PID.update(positionTarget.z - position.z)
+	);
+
+	controlVelocity();
+}
+
+void controlPositionSimple() {
+	if (positionTarget.invalid()) return;
+
+	// straight attitude target control
+	attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x));
+	attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y));
+}

flix/safety.ino

@@ -8,10 +8,12 @@ extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 float rcLossTimeout = 1;
 float descendTime = 10;
+float disarmTilt = radians(120);
 
 void failsafe() {
 	rcLossFailsafe();
 	autoFailsafe();
+	tiltFailsafe();
 }
 
 // RC loss failsafe
@@ -36,7 +38,7 @@ void descend() {
 // Allow pilot to interrupt automatic flight
 void autoFailsafe() {
 	static float roll, pitch, yaw, throttle;
-	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
+	if (abs(roll - controlRoll) > 0.05 || abs(pitch - controlPitch) > 0.05 || abs(yaw - controlYaw) > 0.05 || abs(throttle - controlThrottle) > 0.05) {
 		// controls changed and mode switch is not configured
 		if (mode == AUTO && invalid(controlMode)) mode = STAB; // regain control by the pilot
 	}
@@ -45,3 +47,15 @@ void autoFailsafe() {
 	yaw = controlYaw;
 	throttle = controlThrottle;
 }
+
+// Disarm if tilted too much
+void tiltFailsafe() {
+	if (!armed) return;
+	if (mode != STAB) return;
+
+	Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+	float tilt = acos(up.z);
+	if (disarmTilt && tilt > disarmTilt) {
+		armed = false;
+	}
+}

flix/wifi.ino

@@ -58,7 +58,7 @@ void sendWiFi(const uint8_t *buf, int len) {
 		espnow.write(buf, len);
 
 		static Rate discovery(2);
-		if (discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
+		if (espnow.isEncrypted() && discovery) espnowBroadcast.write((const uint8_t *)"flix", 4); // broadcast message to help finding this device
 		return;
 	}
 
@@ -89,7 +89,7 @@ void printWiFiInfo() {
 		print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
 		print("Peer MAC: %s\n", MacAddress(espnow.addr()).toString().c_str());
 		print("Encrypted: %d\n", espnow.isEncrypted());
-		print("Channel: %d\n", espnow.getChannel());
+		print("Channel: %d\n", WiFi.channel());
 		print("Lost packets: %d\n", espnow.lost);
 	} else if (WiFi.getMode() == WIFI_MODE_AP) {
 		print("Mode: Access Point (AP)\n");

gazebo/flix.h

@@ -38,7 +38,7 @@ const char* getModeName();
 void sendMotors();
 int getDutyCycle(float value);
 bool motorsActive();
-void testMotor(int n);
+void testMotor(int, float);
 void print(const char* format, ...);
 void pause(float duration);
 void doCommand(String str, bool echo);
@@ -72,6 +72,7 @@ void failsafe();
 void rcLossFailsafe();
 void descend();
 void autoFailsafe();
+void tiltFailsafe();
 int parametersCount();
 const char *getParameterName(int index);
 float getParameter(int index);

tools/espnow-proxy/espnow-proxy.ino

@@ -11,18 +11,22 @@
 #include <Preferences.h>
 #include "../../flix/util.h"
 
-const int CHANNEL = 6;
+const bool DISABLE_SWARM = true;
+const int CHANNEL = -1; // -1 means auto search
 char key[ESP_NOW_KEY_LEN + 1] = {0}; // with trailing null
 
 Preferences storage;
 
 std::vector<ESPNOWSerial *> peers;
+bool stop = false;
 
 void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, void *arg) {
 	if (len != 4 || memcmp(data, "flix", 4) != 0) return; // check if discovery message
 
+	if (stop) return;
+
 	Serial.printf("New peer: " MACSTR "\n", MAC2STR(info->src_addr));
-	ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, CHANNEL, WIFI_IF_AP);
+	ESPNOWSerial *link = new ESPNOWSerial(info->src_addr, WiFi.channel(), WIFI_IF_STA);
 	link->begin();
 	link->setKey((const uint8_t *)key);
 	peers.push_back(link);
@@ -30,9 +34,8 @@ void onNewPeer(const esp_now_recv_info_t *info, const uint8_t *data, int len, vo
 
 void setup() {
 	Serial.begin(115200);
-	WiFi.mode(WIFI_AP);
+	WiFi.mode(WIFI_STA);
 	WiFi.setSleep(false);
-	WiFi.setChannel(CHANNEL);
 
 	ESP_NOW.onNewPeer(onNewPeer, NULL);
 	ESP_NOW.begin();
@@ -43,12 +46,6 @@ void setup() {
 		storage.putString("key", key);
 	}
 	strcpy(key, storage.getString("key").c_str());
-
-	// Discover the first peer
-	while (peers.empty()) {
-		Serial.printf("espnow %s %s\n", WiFi.softAPmacAddress().c_str(), key);
-		delay(500);
-	}
 }
 
 void generateRandomKey() {
@@ -61,6 +58,20 @@ void generateRandomKey() {
 void loop() {
 	uint8_t buf[5000];
 
+	static int channelIndex = 0;
+	static const int channels[] = {6, 11, 1, 2, 6, 11, 3, 4, 5, 6, 1, 11, 8, 6, 9, 10, 11, 6, 1, 12, 13}; // 6, 1 and 11 are most common
+
+	static unsigned long last = 0;
+	if (!stop && millis() - last > 500) {
+		// Change search channel
+		last = millis();
+		channelIndex = (channelIndex + 1) % (sizeof(channels) / sizeof(channels[0]));
+		int channel = CHANNEL < 0 ? channels[channelIndex] : CHANNEL;
+		Serial.printf("Run on Flix: espnow %s %s\n", WiFi.STA.macAddress().c_str(), key);
+		Serial.printf("Searching channel %d\n", channel);
+		WiFi.setChannel(channel);
+	}
+
 	// Send from Serial to ESP-NOW
 	while (Serial.available() > 0) {
 		int b = Serial.read();
@@ -81,6 +92,15 @@ void loop() {
 	// Send from ESP-NOW to Serial
 	for (ESPNOWSerial *link : peers) {
 		int len = link->read(buf, sizeof(buf));
+		if (!stop) {
+			for (int i = 0; i < len; i++) {
+				if (buf[i] == MAVLINK_STX) {
+					// Got MAVLink message, stop discovery
+					Serial.printf("Received MAVLink from " MACSTR "\n", MAC2STR(link->addr()));
+					if (DISABLE_SWARM) stop = true;
+				}
+			}
+		}
 		if (len > 0) {
 			Serial.write(buf, len);
 		}