Change to use extended RC channels message instead of a separate comm…

…and arming command message. This ensures the arming status is sent periodically and at the same rate as channel 5.
mha1 · Oct 10, 2024 · cbe082c · cbe082c
1 parent 5e8d614
commit cbe082c
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 40 deletions.
diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp
@@ -92,31 +92,15 @@ uint8_t createCrossfireModelIDFrame(uint8_t moduleIdx, uint8_t * frame)
   return buf - frame;
 }
 
-uint8_t createCrossfireArmFrame(uint8_t moduleIdx, uint8_t * frame, uint8_t crsfArmingMode, uint8_t armStatus)
-{
-  uint8_t * buf = frame;
-  *buf++ = UART_SYNC;                                 /* device address */
-  *buf++ = 9;                                         /* frame length */
-  *buf++ = COMMAND_ID;                                /* cmd type */
-  *buf++ = MODULE_ADDRESS;                            /* Destination Address */
-  *buf++ = RADIO_ADDRESS;                             /* Origin Address */
-  *buf++ = SUBCOMMAND_CRSF;                           /* sub command */
-  *buf++ = COMMAND_SF_ARM;                            /* command to notify ELRS module about SF Arm status */
-  *buf++ = crsfArmingMode;                            /* ch5 or CRSF message based arming */
-  *buf++ = armStatus;                                 /* Arm status */
-  *buf++ = crc8_BA(frame + 2, 7);
-  *buf++ = crc8(frame + 2, 8);
-  return buf - frame;
-}
-
 // Range for pulses (channels output) is [-1024:+1024]
-uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
+uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses, bool RCext, uint8_t armStatus)
 {
+  uint8_t extAdjust = RCext ? 1: 0;
   uint8_t * buf = frame;
   *buf++ = MODULE_ADDRESS;
-  *buf++ = 24; // 1(ID) + 22 + 1(CRC)
+  *buf++ = 24 + extAdjust ; // 1(ID) + 22 + (+1 if RCext) + 1(CRC)
   uint8_t * crc_start = buf;
-  *buf++ = CHANNELS_ID;
+  *buf++ = RCext ? CHANNELS_ID_EXT : CHANNELS_ID;
   uint32_t bits = 0;
   uint8_t bitsavailable = 0;
   for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
@@ -129,7 +113,15 @@ uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
       bitsavailable -= 8;
     }
   }
-  *buf++ = crc8(crc_start, 23);
+
+  if (RCext) {
+    TRACE("[XF] RCext arm %d", armStatus);
+    *buf++ = armStatus;  // Arm status
+  } else {
+    TRACE("[XF] RC");
+  }
+
+  *buf++ = crc8(crc_start, 23 + extAdjust);
   return buf - frame;
 }
 
@@ -173,33 +165,21 @@ static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
       }
     }
 
-    static bool sendCrsfArmMsg = false;
-
-    static bool lastArmStatus = false;
-    static bool lastcrsfArmingMode = false;
-
-    bool crsfArmingMode = g_model.crsfArmingMode;
-    bool armStatus = crsfArmingMode && isFunctionActive(FUNCTION_ARM);
-
     if (moduleState[module].counter == CRSF_FRAME_MODELID) {
-      TRACE("[XF] sending ModelID %d", g_model.header.modelId[module]);
+      TRACE("[XF] ModelID %d", g_model.header.modelId[module]);
       p_buf += createCrossfireModelIDFrame(module, p_buf);
       moduleState[module].counter = CRSF_FRAME_MODELID_SENT;
-      sendCrsfArmMsg = true;
-    } else if((crsfArmingMode != lastcrsfArmingMode) || (armStatus != lastArmStatus) || sendCrsfArmMsg) {
-      lastcrsfArmingMode = crsfArmingMode;
-      lastArmStatus = armStatus;
-      sendCrsfArmMsg = false;  
-      TRACE("[XF] sending CRSF arming = %d, Arm status %d", crsfArmingMode, armStatus);
-      p_buf += createCrossfireArmFrame(module, p_buf, crsfArmingMode, armStatus);
     } else if (moduleState[module].counter == CRSF_FRAME_MODELID_SENT && crossfireModuleStatus[module].queryCompleted == false) {
+      TRACE("[XF] Ping");
       p_buf += createCrossfirePingFrame(module, p_buf);
     } else if (moduleState[module].mode == MODULE_MODE_BIND) {
+      TRACE("[XF] Bind");
       p_buf += createCrossfireBindFrame(module, p_buf);
       moduleState[module].mode = MODULE_MODE_NORMAL;
     } else {
       /* TODO: nChannels */
-      p_buf += createCrossfireChannelsFrame(p_buf, channels);
+      bool crsfArmingMode = g_model.crsfArmingMode;
+      p_buf += createCrossfireChannelsFrame(p_buf, channels, g_model.crsfArmingMode, crsfArmingMode && isFunctionActive(FUNCTION_ARM));
     }
   }
 }

diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h
@@ -37,6 +37,7 @@
 #define BARO_ALT_ID                    0x09
 #define LINK_ID                        0x14
 #define CHANNELS_ID                    0x16
+#define CHANNELS_ID_EXT                0x17
 #define LINK_RX_ID                     0x1C
 #define LINK_TX_ID                     0x1D
 #define ATTITUDE_ID                    0x1E

diff --git a/radio/src/tests/crossfire.cpp b/radio/src/tests/crossfire.cpp
@@ -25,7 +25,7 @@
 
 #if defined(CROSSFIRE)
 
-uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses);
+uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses, bool RCext, uint8_t armStatus);
 TEST(Crossfire, createCrossfireChannelsFrame)
 {
   int16_t pulsesStart[MAX_TRAINER_CHANNELS];
@@ -36,7 +36,7 @@ TEST(Crossfire, createCrossfireChannelsFrame)
     pulsesStart[i] = -1024 + (2048 / MAX_TRAINER_CHANNELS) * i;
   }
 
-  createCrossfireChannelsFrame(crossfire, pulsesStart);
+  createCrossfireChannelsFrame(crossfire, pulsesStart, false, false);
 
   // TODO check
 }