motor_srxl2: link an ESC powered together with the board

docs/Spektrum Smart ESC.md

@@ -306,24 +306,39 @@ them can call for reverse thrust. An automatic landing will not use it.
 
 ## If the motor does not come back after a reboot
 
-An Avian announces itself for about a third of a second after it powers up - six
-handshakes in 300 milliseconds, measured on a 70 A - and then never speaks again
-unless it is asked something it recognises. A flight controller that starts while
-the ESC is already running has missed that window, and the ESC will not answer it
-afterwards: polled by name, broadcast to, addressed on every ID from 0x40 to
-0x4F, or spoken to as though the link already existed, it stayed silent through
-every one.
-
-So the link is made at power-up or not at all. Connecting the battery powers both
-together and the announcement lands while the flight controller is listening,
-which is the normal case and needs nothing. The cases that bite are the other
-ones:
-
-* the flight controller reboots - a firmware update, a brownout, the Configurator
-  asking for a restart - while the battery stays connected. **Unplug the battery
-  and plug it in again**, or the motor will not respond.
-* bench work on USB with the ESC powered from a separate supply. Power the ESC
-  after the board has booted, not before.
+An Avian announces itself only as it powers up - five or six handshakes 50 to 60 ms apart,
+43 to about 345 ms after the board's reset when both share the battery, measured on a 70 A
+with a Lucid H7 - and one left unanswered never speaks again. Polled by name,
+broadcast to, addressed on every ID from 0x40 to 0x4F, from other source IDs, at
+400000 baud, or spoken to as though the link already existed, it stayed silent
+through every one. It also takes an answer only once those handshakes are over, and
+only if control frames are still coming then: answered and left without frames, it
+goes silent the same way.
+
+So the link is made at power-up or not at all, and the flight controller makes it
+before anything else that takes time: a board set to SRXL2 opens its ports ahead of
+the USB start-up, answers the handshake and sends control frames until the ESC
+replies, which the Avian did 349 ms after reset. A board whose ESC has not spoken 200 ms
+after reset, or 100 ms after its ports opened if that is later, stops waiting, and this
+first wait ends by 700 ms. The fixed waits later in the start-up keep answering too: on
+a Lucid H7 an ESC powered up to about 1.3 s after a restart still linked. One powered
+while the USB starts up, 0.2 to 0.3 s after reset on that board, or from about 1.5 s
+until the board is up, can still be missed.
+
+The rest of the start-up then leaves the ESC without frames for about four seconds. It
+announces itself again through them and the board links it once running, but an Avian
+still starting when that happens starts over: it obeyed the throttle about 7 seconds
+after the frames returned, and not at all if the throttle had risen right away.
+The arming block counts its 10 seconds again from when the frames return, so with one
+battery powering both, arming is allowed 10 seconds after the board has started, about
+16 seconds after the battery is connected on a Lucid H7.
+
+A linked ESC that loses its frames announces itself again, so a board that restarts
+with the battery connected gets it back; measured over a restart of about six seconds
+and over a firmware update. What still bites is an ESC powered while the board is
+starting up rather than together with it, as on the bench with the board on USB and
+the ESC on its own supply: power the ESC after the board has booted. If the motor ever
+does not respond, **unplug the battery and plug it in again**.
 
 Nothing in the firmware can work around this, so it refuses to hide it instead:
 arming is blocked while an SRXL2 link is missing, and the OSD says the hardware

src/main/fc/fc_core.c

@@ -307,16 +307,16 @@ static void updateArmingStatus(void)
         bool escLinkMissing = false;
 #ifdef USE_MOTOR_SRXL2
         /*
-         * An SRXL2 ESC announces itself in the third of a second after it gains
-         * power: if that announcement is missed the link never forms, and the
-         * throttle reaches nothing. Arming meanwhile commands a motor that is not
-         * listening - the model looks armed, the telemetry looks sane, and the
-         * propeller does not turn.
+         * An SRXL2 ESC left unanswered in the third of a second after it gains
+         * power never links, and the throttle reaches nothing. Arming meanwhile
+         * commands a motor that is not listening - the model looks armed, the
+         * telemetry looks sane, and the propeller does not turn.
          *
-         * Refuse to arm until the link is up and the ESC obeys the throttle, about
-         * 10 s after it powers up; where the link never forms, it is the
-         * difference between finding out on the bench and finding out on the
-         * takeoff roll.
+         * Refuse to arm until the link is up and the ESC obeys the throttle: 10 s
+         * after it powers up, or after the board's start-up where both share a
+         * battery (about 16 s after power on a Lucid H7). Where it never comes, it
+         * is the difference between finding out on the bench and finding out on
+         * the takeoff roll.
          */
         escLinkMissing = (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2)
                          && !srxl2MotorIsConnected();

src/main/fc/fc_init.c

@@ -199,6 +199,20 @@ void flashLedsAndBeep(void)
     LED1_OFF;
 }
 
+#if defined(USE_GPS) || defined(USE_MAG)
+// A Smart ESC powered during these waits would otherwise stay deaf until its power is cycled
+static void initDelay(timeMs_t ms)
+{
+#if defined(USE_MOTOR_SRXL2) && !defined(SITL_BUILD) && !defined(BRUSHED_MOTORS)
+    if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) {
+        srxl2MotorServiceFor(ms);
+        return;
+    }
+#endif
+    delay(ms);
+}
+#endif
+
 void init(void)
 {
 #if defined(USE_FLASHFS)
@@ -281,6 +295,19 @@ void init(void)
     }
 #endif
 
+    timerInit();  // timer must be initialized before any channel is allocated
+
+    serialInit(feature(FEATURE_SOFTSERIAL));
+
+#if defined(USE_MOTOR_SRXL2) && !defined(SITL_BUILD) && !defined(BRUSHED_MOTORS)
+    // Ahead of the USB wait, which outlasts the announcement of a Smart ESC powered with the board
+    if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) {
+        srxl2MotorSetReverseChannel(motorConfig()->srxl2ReverseChannel);
+        srxl2MotorSetTelemetryRate(motorConfig()->srxl2TelemetryRate);
+        srxl2MotorAwaitLink();
+    }
+#endif
+
 #ifdef USE_VCP
     // Early initialize USB hardware.
 #if defined(USE_USB_MSC)
@@ -299,11 +326,6 @@ void init(void)
 #endif
 #endif
 
-    timerInit();  // timer must be initialized before any channel is allocated
-
-    serialInit(feature(FEATURE_SOFTSERIAL));
-
-
     // Initialize MSP serial ports here so LOG can share a port with MSP.
     // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs
     // to run after the sensors have been detected.
@@ -532,7 +554,7 @@ void init(void)
 #endif
 
 #if defined(USE_GPS) || defined(USE_MAG)
-    delay(500);
+    initDelay(500);
 
     /* Extra 500ms delay prior to initialising hardware if board is cold-booting */
     if (!isMPUSoftReset()) {
@@ -542,7 +564,7 @@ void init(void)
         for (int i = 0; i < 5; i++) {
             LED1_TOGGLE;
             LED0_TOGGLE;
-            delay(100);
+            initDelay(100);
         }
 
         LED0_OFF;

src/main/io/motor_srxl2.c

@@ -103,6 +103,15 @@
 #define SRXL2_LISTEN_WINDOW_MS      250
 #define SRXL2_HANDSHAKE_INTERVAL_MS 50
 
+// Powered with the board, an Avian sends them 43 to about 345 ms after the board's reset, long before
+// init reaches the motors. It links only once they are over, and only if control frames are
+// still coming then: it replied at 349 ms. Unanswered, it never links
+#define SRXL2_BOOT_LINK_MS          700
+// By then a powered ESC has spoken, so one that has not is not waited for
+#define SRXL2_BOOT_SILENT_MS        200
+// The task's 200 Hz. The baud change needs it: the port reads empty with the last byte still going out
+#define SRXL2_BOOT_TICK_MS          5
+
 // Rate at which Control Data goes out once the link is up, which is not the motor update
 // rate: the specification has the master emit one packet per RF frame, tens of hertz. 115200
 // baud would not carry more anyway, an eighteen-byte frame being about 1.6 ms on the wire
@@ -119,10 +128,10 @@
 // cycle. Measured on an Avian 70A against a fixed 1250 us command
 #define SRXL2_TELEM_REQUEST_MIN     2
 
-// Declare the link dead if the ESC stops answering for this long. A running Avian never
-// speaks unprompted, so only a telemetry reply refreshes this timer and a request rate
-// slower than the timeout would disconnect a healthy ESC on schedule. It answers about two
-// requests in three, so the margin also has to cover consecutive misses. This is why the
+// Declare the link dead if the ESC stops answering for this long. For the arming check only a
+// telemetry reply counts, since an Avian that has lost its frames still announces itself, so a
+// request rate slower than the timeout would block a healthy ESC on schedule. It answers about
+// two requests in three, so the margin also has to cover consecutive misses. This is why the
 // telemetry rate table stops at one request every five frames
 #define SRXL2_LINK_TIMEOUT_MS       500
 
@@ -133,7 +142,7 @@
 #define SRXL2_READY_DELAY_MS        10000
 
 // Unfed this long, an Avian drops the link (it announces itself again about 185 ms after its last
-// frame), and one still starting starts over
+// frame), and one still starting starts over: its readiness counts again from when frames return
 #define SRXL2_STARVED_MS            150
 
 // Telemetry older than this reads as stale. Generous next to the link timeout on purpose:
@@ -205,6 +214,7 @@ typedef struct {
 
     timeMs_t  stateEnteredMs;
     timeMs_t  lastRxMs;
+    timeMs_t  lastReplyMs;              /* last telemetry reply, 0 before the first */
     timeMs_t  lastTxMs;
     timeMs_t  lastControlMs;
 
@@ -461,6 +471,7 @@ static void srxl2HandleTelemetry(srxl2Esc_t *e, const uint8_t *buf, uint8_t len)
     if (len < 3 + 1 + 16 + 2) {
         return;
     }
+    e->lastReplyMs = millis();
     const uint8_t *payload = &buf[4];
     if (payload[0] == SRXL2_TELEM_SENSOR_ESC) {
         srxl2DecodeEscTelemetry(e, payload);
@@ -614,6 +625,13 @@ static void srxl2SendControlData(srxl2Esc_t *e)
 
 bool srxl2MotorInitialize(void)
 {
+    // Opened already by srxl2MotorAwaitLink(), and starting over would drop the link it made
+    static bool initialized;
+    if (initialized) {
+        return escCount > 0;
+    }
+    initialized = true;
+
     memset(esc, 0, sizeof(esc));
     escCount = 0;
 
@@ -890,10 +908,8 @@ static void srxl2ProcessEsc(srxl2Esc_t *e, timeMs_t now)
             // Walk the whole ESC range rather than the default ID alone: an ESC with a
             // non-zero unit ID never announces itself, and polling is the only way to find
             // it. Polling the same ID on every bus is not a collision, each ESC hearing only
-            // its own master. What finds an Avian, though, is its own announcement at
-            // power-up: a running one answered none of 128 handshakes, 128 broadcasts and
-            // 319 telemetry requests, so a board that reboots under a powered ESC never
-            // links, and no amount of asking changes that
+            // its own master. An Avian left unanswered at power-up answers no poll, though:
+            // only its announcement finds it, see srxl2MotorAwaitLink()
             srxl2SendHandshake(e, SRXL2_ESC_ID_FIRST + e->pollId, SRXL2_BAUD_BIT_400K);
             e->pollId++;
             if (SRXL2_ESC_ID_FIRST + e->pollId > SRXL2_ESC_ID_LAST) {
@@ -909,13 +925,19 @@ static void srxl2ProcessEsc(srxl2Esc_t *e, timeMs_t now)
             e->lastRxMs = now;          /* do not time out on the handshake gap */
             e->lastControlMs = now;
             e->runningSinceMs = now;
+            // An Avian answered again during its announcements did not link when they ended
+            e->lastKeepaliveMs = now;
             srxl2SetState(e, SRXL2_RUNNING);
         }
         break;
 
     case SRXL2_RUNNING:
         if (now - e->lastControlMs >= SRXL2_CONTROL_INTERVAL_MS) {
             if (now - e->lastControlMs >= SRXL2_STARVED_MS) {
+                // Judged when the frames stopped: still starting then, it starts over however long the gap
+                if ((timeDelta_t)(e->lastControlMs - e->runningSinceMs) < SRXL2_READY_DELAY_MS) {
+                    e->runningSinceMs = now;
+                }
                 e->gapStartMs = e->lastControlMs;
                 e->gapEndMs = now;
             }
@@ -934,14 +956,14 @@ static void srxl2ProcessEsc(srxl2Esc_t *e, timeMs_t now)
             // throttle indefinitely with no telemetry request sent at all; what stops it is
             // the absence of control frames, and it picks the throttle back up by itself
             // when they return. Tearing the link down here would cause the outage it means
-            // to detect, and permanently, since a running Avian answers no discovery. The
-            // one case that does need it is a slave that reset, which comes back at 115200
-            // and cannot be heard from 400000
+            // to detect. The one case that does need it is a slave that reset, which comes
+            // back at 115200 and cannot be heard from 400000
             if (e->agreedBaudBits != 0) {
                 serialSetBaudRate(e->port, SRXL2_BAUD_LOW);
                 e->baudSwitchPending = false;
                 e->agreedBaudBits = 0;
                 e->deviceId = 0;
+                e->lastReplyMs = 0;
                 srxl2SetState(e, SRXL2_POLLING);
             }
         }
@@ -986,6 +1008,54 @@ void srxl2MotorProcess(void)
     DEBUG_SET(DEBUG_ALWAYS, 3, crcErrors);
 }
 
+void srxl2MotorAwaitLink(void)
+{
+    if (!srxl2MotorInitialize()) {
+        return;
+    }
+
+    // Once it has replied the rest of init can go without frames: a linked ESC that loses them
+    // announces itself again, and the task answers it
+    const timeMs_t silentMs = MAX((timeMs_t)SRXL2_BOOT_SILENT_MS, millis() + 2 * SRXL2_HANDSHAKE_INTERVAL_MS);
+    timeMs_t now;
+    bool waiting;
+    do {
+        srxl2MotorProcess();
+        now = millis();
+        waiting = false;
+        for (uint8_t i = 0; i < escCount; i++) {
+            const bool absent = esc[i].deviceId == 0 && now >= silentMs;
+            waiting = waiting || (esc[i].lastReplyMs == 0 && !absent);
+        }
+        if (waiting) {
+            delay(SRXL2_BOOT_TICK_MS);
+        }
+    } while (waiting && now < SRXL2_BOOT_LINK_MS);
+}
+
+void srxl2MotorServiceFor(uint32_t ms)
+{
+    // Only a link begun in this wait extends it, so a stuck ESC cannot stretch every wait of init
+    uint8_t heardBefore = 0;
+    for (uint8_t i = 0; i < escCount; i++) {
+        heardBefore |= (esc[i].deviceId != 0) << i;
+    }
+
+    const timeMs_t start = millis();
+    timeMs_t now;
+    bool linking;
+    do {
+        srxl2MotorProcess();
+        delay(SRXL2_BOOT_TICK_MS);
+        now = millis();
+        // Heard but not yet replied: it links only if frames still come when its announcements end
+        linking = false;
+        for (uint8_t i = 0; i < escCount; i++) {
+            linking = linking || (!(heardBefore & (1 << i)) && esc[i].deviceId != 0 && esc[i].lastReplyMs == 0);
+        }
+    } while (now - start < ms || (linking && now - start < ms + SRXL2_BOOT_LINK_MS));
+}
+
 uint8_t srxl2MotorCount(void)
 {
     return escCount;
@@ -1005,7 +1075,7 @@ bool srxl2MotorIsConnected(void)
         if (esc[i].state != SRXL2_RUNNING || esc[i].deviceId == 0) {
             return false;
         }
-        if (now - esc[i].lastRxMs >= SRXL2_LINK_TIMEOUT_MS) {
+        if (now - esc[i].lastReplyMs >= SRXL2_LINK_TIMEOUT_MS) {
             return false;
         }
         if (now - esc[i].runningSinceMs < SRXL2_READY_DELAY_MS) {

src/main/io/motor_srxl2.h

@@ -145,6 +145,19 @@ srxl2CalResult_e srxl2MotorCalibrationLastResult(void);
  */
 bool srxl2MotorInitialize(void);
 
+/*
+ * Open the ports and link the ESCs powered with the board, which only listen in their
+ * first moments. Returns once each has replied or has stayed silent long enough to be
+ * absent, and 700 ms after reset at the latest. For init, ahead of its long waits.
+ */
+void srxl2MotorAwaitLink(void);
+
+/*
+ * Keep the ESCs answered for ms milliseconds, in place of one of init's fixed waits: an ESC
+ * powered during it would otherwise stay deaf. A link under way is let finish, up to 700 ms more.
+ */
+void srxl2MotorServiceFor(uint32_t ms);
+
 /*
  * Stage one motor value. Takes microseconds on INAV's usual 1000..2000 scale
  * (or the reversible-motor scale, where the neutral sits in the middle), so it