fix: switch position if disabled or transitioning

EdgeTX · Jun 25, 2023 · 9007fb4 · 9007fb4
1 parent 18da85e
commit 9007fb4
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 12 deletions.
diff --git a/radio/src/boards/generic_stm32/switches.cpp b/radio/src/boards/generic_stm32/switches.cpp
@@ -88,6 +88,7 @@ uint8_t switchGetMaxFctSwitches()
 uint32_t switchState(uint8_t pos_idx)
 {
   auto d = div(pos_idx, 3);
+  if (d.quot >= n_total_switches) return 0;
   return stm32_switch_get_state(&_switch_defs[d.quot], (SwitchHwPos)d.rem);
 }
 

diff --git a/radio/src/logs.cpp b/radio/src/logs.cpp
@@ -102,15 +102,10 @@ void initLoggingTimer() {                                       // called cyclic
 
 void writeHeader();
 
-#if defined(PCBFRSKY) || defined(PCBNV14)
-  int getSwitchState(uint8_t swtch) {
-    int value = getValue(MIXSRC_FIRST_SWITCH + swtch);
-    return (value == 0) ? 0 : (value < 0) ? -1 : +1;
-  }
-#else
-  #define GET_2POS_STATE(sw) (switchState(SW_ ## sw) ? -1 : 1)
-  #define GET_3POS_STATE(sw) (switchState(SW_ ## sw ## 0) ? -1 : (switchState(SW_ ## sw ## 2) ? 1 : 0))
-#endif
+int getSwitchState(uint8_t swtch) {
+  int value = getValue(MIXSRC_FIRST_SWITCH + swtch);
+  return (value == 0) ? 0 : (value < 0) ? -1 : +1;
+}
 
 void logsInit()
 {

diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp
@@ -280,7 +280,7 @@ static uint64_t checkSwitchPosition(uint8_t idx, bool startup)
     break;
 
   case SWITCH_HW_MID:
-    if (startup || SWITCH_POSITION(index) ||
+    if (startup || SWITCH_POSITION(index + 1) ||
         g_eeGeneral.switchesDelay == SWITCHES_DELAY_NONE ||
         (switchesMidposStart[idx] &&
          (tmr10ms_t)(get_tmr10ms() - switchesMidposStart[idx]) >
@@ -308,6 +308,7 @@ void getSwitchesPosition(bool startup)
 {
   uint64_t newPos = 0;
   for (unsigned i = 0; i < switchGetMaxSwitches(); i++) {
+    if (!SWITCH_EXISTS(i)) continue;
     newPos |= checkSwitchPosition(i, startup);
   }
 
@@ -570,8 +571,14 @@ bool getSwitch(swsrc_t swtch, uint8_t flags)
     if (cs_idx < max_reg_pos) {
       if (flags & GETSWITCH_MIDPOS_DELAY)
         result = SWITCH_POSITION(cs_idx);
-      else
-        result = switchState(cs_idx);
+      else {
+        div_t qr = div(cs_idx, 3);
+        if (SWITCH_EXISTS(qr.quot)) {
+          result = switchState(cs_idx);
+        } else {
+          result = false;
+        }
+      }
     }
 #if defined(FUNCTION_SWITCHES)
     else if (cs_idx - max_reg_pos < switchGetMaxFctSwitches() * 3) {