fix(el18): Inability to set custom failsafe (#3298)

EdgeTX · Mar 22, 2023 · 99ab8f2 · 99ab8f2
1 parent 2ae0ae0
commit 99ab8f2
Show file tree

Hide file tree

Showing 2 changed files with 53 additions and 24 deletions.
diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp
@@ -45,8 +45,8 @@
 #define FAILSAFE_HOLD 1
 #define FAILSAFE_CUSTOM 2
 
-#define FAILSAFE_KEEP_LAST 0x8000
-
+#define FAILSAFE_HOLD_VALUE         0x8000
+#define FAILSAFE_NOPULSES_VALUE     0x8001
 //get channel value outside of afhds3 namespace
 int32_t getChannelValue(uint8_t channel);
 
@@ -249,7 +249,7 @@ class ProtoState
 
     void requestInfoAndRun(bool send = false);
 
-    uint8_t setFailSafe(int16_t* target);
+    uint8_t setFailSafe(int16_t* target, uint8_t rfchannelcount=AFHDS3_MAX_CHANNELS);
 
     inline int16_t convert(int channelValue);
 
@@ -464,16 +464,26 @@ void ProtoState::setupFrame()
     COMMAND cmd = periodicRequestCommands[cmdIndex++];
 
     if (cmd == COMMAND::VIRTUAL_FAILSAFE) {
-      if (isConnected() && !hasTelemetry()) {
-          TRACE("AFHDS ONE WAY FAILSAFE");
+      Config_u* cfg = this->getConfig();
+      uint8_t len =_phyMode_channels[cfg->v0.PhyMode];
+      if (!hasTelemetry()) {
           uint16_t failSafe[AFHDS3_MAX_CHANNELS + 1] = {
-              ((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
-          setFailSafe((int16_t*)(&failSafe[1]));
+          ((AFHDS3_MAX_CHANNELS << 8) | CHANNELS_DATA_MODE::FAIL_SAFE), 0};
+          setFailSafe((int16_t*)(&failSafe[1]), len);
+          TRACE("AFHDS ONE WAY FAILSAFE");
           trsp.sendFrame(COMMAND::CHANNELS_FAILSAFE_DATA,
                    FRAME_TYPE::REQUEST_SET_NO_RESP, (uint8_t*)failSafe,
                    AFHDS3_MAX_CHANNELS * 2 + 2);
           return;
       }
+      else if( isConnected() ){
+          uint8_t data[AFHDS3_MAX_CHANNELS*2 + 3] = { (uint8_t)(RX_CMD_FAILSAFE_VALUE&0xFF), (uint8_t)((RX_CMD_FAILSAFE_VALUE>>8)&0xFF), (uint8_t)(2*len)};
+          int16_t failSafe[18];
+          setFailSafe(&failSafe[0], len);
+          std::memcpy( &data[3], failSafe, len );
+          trsp.sendFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, 2*len+3);
+          return;
+      }
     } else {
       trsp.sendFrame(cmd, FRAME_TYPE::REQUEST_GET_DATA);
       return;
@@ -823,34 +833,37 @@ void ProtoState::applyConfigFromModel()
 
 inline int16_t ProtoState::convert(int channelValue)
 {
-  //pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
-  //988 - 750 = 238
-  //238 * 20 = 4760
-  //2250 - 2012 = 238
-  //238 * 20 = 4760
-  // 988   ---- 2012
-  //-10240 ---- 10240
-  //-1024  ---- 1024
-  return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 10, AFHDS3_FAILSAFE_MAX);
+  return ::limit<int16_t>(AFHDS3_FAILSAFE_MIN, channelValue * 14.65, AFHDS3_FAILSAFE_MAX);
 }
 
-uint8_t ProtoState::setFailSafe(int16_t* target)
+uint8_t ProtoState::setFailSafe(int16_t* target, uint8_t rfchannelsCount )
 {
   int16_t pulseValue = 0;
   uint8_t channels_start = moduleData->channelsStart;
-  uint8_t channels_last = channels_start + 8 + moduleData->channelsCount;
-
-  for (uint8_t channel = channels_start; channel < channels_last; channel++) {
+  uint8_t channelsCount = 8 + moduleData->channelsCount;
+  uint8_t channels_last = channels_start + channelsCount;
+  std::memset(target, 0, 2*rfchannelsCount );
+  for (uint8_t channel = channels_start, i=0; i<rfchannelsCount && channel < channels_last; channel++, i++) {
     if (moduleData->failsafeMode == FAILSAFE_CUSTOM) {
-      pulseValue = convert(g_model.failsafeChannels[channel]);
+      if(FAILSAFE_CHANNEL_HOLD==g_model.failsafeChannels[channel]){
+        pulseValue = FAILSAFE_HOLD_VALUE;
+      }else if(FAILSAFE_CHANNEL_NOPULSE==g_model.failsafeChannels[channel]){
+        pulseValue = FAILSAFE_NOPULSES_VALUE;
+      }
+      else{
+        pulseValue = convert(g_model.failsafeChannels[channel]);
+      }
     }
     else if (moduleData->failsafeMode == FAILSAFE_HOLD) {
-      pulseValue = FAILSAFE_KEEP_LAST;
+      pulseValue = FAILSAFE_HOLD_VALUE;
+    }
+    else if (moduleData->failsafeMode == FAILSAFE_NOPULSES) {
+      pulseValue = FAILSAFE_NOPULSES_VALUE;
     }
     else {
-      pulseValue = convert(::getChannelValue(channel));
+      pulseValue = FAILSAFE_NOPULSES_VALUE;
     }
-    target[channel - channels_start] = pulseValue;
+    target[i] = pulseValue;
   }
   //return max channels because channel count can not be change after bind
   return (uint8_t) (AFHDS3_MAX_CHANNELS);

diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h
@@ -68,6 +68,22 @@ enum COMMAND: uint8_t
   UNDEFINED = 0xFF
 };
 
+#define RX_CMD_RANGE                    ( 0x7013 )
+#define RX_CMD_FAILSAFE_VALUE           ( 0x6011 )
+#define RX_CMD_FAILSAFE_TIME            ( 0x6012 )
+#define RX_CMD_RSSI_CHANNEL_SETUP       ( 0x602B )
+#define RX_CMD_GET_CAPABILITIES         ( 0x7015 )
+#define RX_CMD_OUT_PWM_PPM_MODE         ( 0x7016 )   //PWM or PPM
+#define RX_CMD_FREQUENCY_V0             ( 0x7017 )
+#define RX_CMD_PORT_TYPE_V1             ( 0x7027 )
+#define RX_CMD_FREQUENCY_V1             ( 0x7028 )
+#define RX_CMD_FREQUENCY_V1_2           ( 0x7028 )
+#define RX_CMD_BUS_TYPE_V0              ( 0x7018 ) //I-BUS/S-BUS
+#define RX_CMD_IBUS_SETUP               ( 0x7019 )
+#define RX_CMD_IBUS_DIRECTION           ( 0x7020 ) //IBUS INPUT or OUTPUT
+#define RX_CMD_BUS_FAILSAFE             ( 0x702A )
+#define RX_CMD_GET_VERSION              ( 0x701F )
+
 // one byte frames for request queue
 struct Frame
 {