From dd3b32c18dea611ea07e3836d78e708ee999da09 Mon Sep 17 00:00:00 2001
From: lobodol <7249581+lobodol@users.noreply.github.com>
Date: Sun, 1 Mar 2026 16:10:32 +0100
Subject: [PATCH] fix: init ESCs before anything

---
 drone-flight-controller.ino | 21 ++++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/drone-flight-controller.ino b/drone-flight-controller.ino
index e45f481..5bbcaf4 100755
--- a/drone-flight-controller.ino
+++ b/drone-flight-controller.ino
@@ -110,10 +110,28 @@ int status = STOPPED;
 int battery_voltage;
 // ---------------------------------------------------------------------------
 
+/**
+ * Configure ESC output pins and feed them a low-throttle pulse for ~5 s so
+ * they can complete their startup sequence (melody → cell detection → arming
+ * confirmation) before the rest of setup runs.
+ * Period: 1000 µs HIGH + 3000 µs LOW = 4 ms → 1250 iterations ≈ 5 s.
+ */
+void initEscs() {
+    DDRD |= B11110000; // Set pins 4-7 as outputs
+    for (int i = 0; i < 1250; i++) {
+        PORTD |= B11110000;       // Set pins 4-7 HIGH
+        delayMicroseconds(1000);  // 1000 µs pulse (minimum throttle)
+        PORTD &= B00001111;       // Set pins 4-7 LOW
+        delayMicroseconds(3000);  // remainder of 4 ms period
+    }
+}
+
 /**
  * Setup configuration
  */
 void setup() {
+    initEscs();
+
     // Start I2C communication
     Wire.begin();
     TWBR = 12; // Set the I2C clock speed to 400kHz.
@@ -122,9 +140,6 @@ void setup() {
     pinMode(13, OUTPUT);
     digitalWrite(13, HIGH);
 
-    // Set pins #4 #5 #6 #7 as outputs
-    DDRD |= B11110000;
-
     setupMpu6050Registers();
 
     calibrateMpu6050();