fix(tpro): function switches auto-switch & position matching (#2505)

* fix(tpro): auto-switch for function switches

Fixes #1488

* fix(tpro): matching function switch UP position

radio/src/switches.cpp

@@ -578,9 +578,9 @@ bool getSwitch(swsrc_t swtch, uint8_t flags)
   }
 #if defined(FUNCTION_SWITCHES)
   else if (cs_idx <= SWSRC_LAST_SWITCH) {
-    div_t qr = div(cs_idx - 3 * NUM_FUNCTIONS_SWITCHES, 3);
-    auto value = getFSLogicalState(qr.quot + 1);
-    result = qr.rem == -2 ? 1 - value : value;
+    div_t qr = div(cs_idx - SWSRC_FIRST_FUNCTION_SWITCH, 3);
+    auto value = getFSLogicalState(qr.quot);
+    result = qr.rem == 0 ? !value : (qr.rem == 2 ? value : false);
   }
 #endif
 #if NUM_XPOTS > 0
@@ -668,8 +668,8 @@ swsrc_t getMovedSwitch()
       auto prev = (uint8_t )(bfSingleBitGet(fsswitches_states, i) >> (i));
       uint8_t next = getFSLogicalState(i);
       if (prev != next) {
-        switches_states ^= (-next ^ switches_states) & (1 << i);
-        result = 2 + (3 * (i + NUM_REGULAR_SWITCHES)) + next;
+        fsswitches_states = (fsswitches_states & ~(1 << i)) | (next << i);
+        result = SWSRC_FIRST_FUNCTION_SWITCH + i*3 + (next ? 2 : 0);
       }
     }
   }