Introduce T-pro and function switches handling

radio/src/switches.cpp

@@ -70,6 +70,85 @@ uint8_t   potsPos[NUM_XPOTS];
 #define SWITCH_POSITION(sw)  (switchesPos & ((MASK_CFN_TYPE)1<<(sw)))
 #define POT_POSITION(sw)     ((potsPos[(sw)/XPOTS_MULTIPOS_COUNT] & 0x0f) == ((sw) % XPOTS_MULTIPOS_COUNT))
 
+#if defined(FUNCTION_SWITCHES)
+// Non pushed : SWSRC_Sx0 = -1024 = Sx(up) = state 0
+// Pushed : SWSRC_Sx2 = +1024 = Sx(down) = state 1
+
+uint8_t fsPreviousState = 0;
+
+void setFSStartupPosition()
+{
+  for (uint8_t i = 0; i < NUM_FUNCTIONS_SWITCHES; i++) {
+    uint8_t startPos = (g_model.functionSwitchStartConfig >> 2 * i) & 0x03;
+    switch(startPos) {
+      case FS_START_DOWN:
+        g_model.functionSwitchLogicalState &= ~(1 << i);   // clear state
+        break;
+
+      case FS_START_UP:
+        g_model.functionSwitchLogicalState |= 1 << i;
+        break;
+
+      case FS_START_PREVIOUS:
+      default:
+        // Do nothing, use existing g_model.functionSwitchLogicalState value
+        break;
+    }
+  }
+}
+
+uint8_t getFSLogicalState(uint8_t index)
+{
+  return (uint8_t )(bfSingleBitGet(g_model.functionSwitchLogicalState, index) >> (index));
+}
+
+uint8_t getFSPhysicalState(uint8_t index)
+{
+  return switchState(((index + NUM_REGULAR_SWITCHES) * 3) + 2) ? 1 : 0;
+}
+
+uint8_t getFSPreviousPhysicalState(uint8_t index)
+{
+  return (uint8_t )(bfSingleBitGet(fsPreviousState, index) >> (index));
+}
+
+void evalFunctionSwitches()
+{
+  for (uint8_t i = 0; i < NUM_FUNCTIONS_SWITCHES; i++) {
+    if (FSWITCH_CONFIG(i) == SWITCH_NONE) {
+      fsLedOff(i);
+      continue;
+    }
+
+    uint8_t physicalState = getFSPhysicalState(i);
+    if (physicalState != getFSPreviousPhysicalState(i)) {      // FS was moved
+      if (FSWITCH_CONFIG(i) == SWITCH_2POS && physicalState == 1) {
+        g_model.functionSwitchLogicalState ^= 1 << i;   // Toggle bit
+      }
+      else if (FSWITCH_CONFIG(i) == SWITCH_TOGGLE) {
+        g_model.functionSwitchLogicalState ^= (physicalState ^ g_model.functionSwitchLogicalState) & (1 << i);   // Set bit to switch value
+      }
+      if (FSWITCH_GROUP(i) && physicalState == 1) {    // switch is in a group, other in group need to be turned off
+        for (uint8_t j = 0; j < NUM_FUNCTIONS_SWITCHES; j++) {
+          if (i ==  j)
+            continue;
+          if (FSWITCH_GROUP(j) == FSWITCH_GROUP(i)) {
+            g_model.functionSwitchLogicalState &= ~(1 << j);   // clear state
+          }
+        }
+      }
+      fsPreviousState ^= 1 << i;    // Toggle state
+      storageDirty(EE_MODEL);
+    }
+
+    if (getFSLogicalState(i))
+      fsLedOn(i);
+    else
+      fsLedOff(i);
+  }
+}
+#endif
+
 div_t switchInfo(int switchPosition)
 {
   return div(switchPosition-SWSRC_FIRST_SWITCH, 3);
@@ -133,6 +212,11 @@ void getSwitchesPosition(bool startup)
   CHECK_2POS(SW_SA);
   CHECK_3POS(0, SW_SB);
   CHECK_3POS(1, SW_SC);
+#elif defined(RADIO_TPRO)
+  CHECK_3POS(0, SW_SA);
+  CHECK_3POS(1, SW_SB);
+  CHECK_2POS(SW_SC);
+  CHECK_2POS(SW_SD);
 #else
   CHECK_3POS(0, SW_SA);
   CHECK_3POS(1, SW_SB);
@@ -165,6 +249,13 @@ void getSwitchesPosition(bool startup)
   CHECK_2POS(SW_SD);
   CHECK_3POS(2, SW_SE);
   CHECK_3POS(3, SW_SF);
+#elif defined(RADIO_TPRO)
+  CHECK_2POS(SW_SE);
+  CHECK_2POS(SW_SF);
+  CHECK_2POS(SW_SG);
+  CHECK_2POS(SW_SH);
+  CHECK_2POS(SW_SI);
+  CHECK_2POS(SW_SJ);
 #elif defined(PCBX7)
   CHECK_3POS(3, SW_SD);
   #if defined(RADIO_T12)
@@ -456,8 +547,8 @@ bool getSwitch(swsrc_t swtch, uint8_t flags)
     result = latencyToggleSwitch;
   }
 #endif
-  else if (cs_idx <= SWSRC_LAST_SWITCH) {
-#if defined(PCBFRSKY)
+  else if (cs_idx <= (SWSRC_LAST_SWITCH - 3 * NUM_FUNCTIONS_SWITCHES)) {
+#if defined(PCBFRSKY) || defined(PCBFLYSKY)
     if (flags & GETSWITCH_MIDPOS_DELAY)
       result = SWITCH_POSITION(cs_idx-SWSRC_FIRST_SWITCH);
     else
@@ -465,8 +556,14 @@ bool getSwitch(swsrc_t swtch, uint8_t flags)
 #else
     result = switchState(cs_idx-SWSRC_FIRST_SWITCH);
 #endif
-
   }
+#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;
+  }
+#endif
 #if NUM_XPOTS > 0
   else if (cs_idx <= SWSRC_LAST_MULTIPOS_SWITCH) {
     result = POT_POSITION(cs_idx-SWSRC_FIRST_MULTIPOS_SWITCH);
@@ -526,13 +623,14 @@ void evalLogicalSwitches(bool isCurrentFlightmode)
 }
 
 swarnstate_t switches_states = 0;
+uint8_t fsswitches_states = 0;
 swsrc_t getMovedSwitch()
 {
   static tmr10ms_t s_move_last_time = 0;
   swsrc_t result = 0;
 
   // Switches
-  for (int i = 0; i < NUM_SWITCHES; i++) {
+  for (int i = 0; i < NUM_SWITCHES - NUM_FUNCTIONS_SWITCHES; i++) {
     if (SWITCH_EXISTS(i)) {
       swarnstate_t mask = ((swarnstate_t) 0x07 << (i * 3));
       uint8_t prev = (switches_states & mask) >> (i * 3);
@@ -544,6 +642,20 @@ swsrc_t getMovedSwitch()
       }
     }
   }
+
+#if defined(FUNCTION_SWITCHES)
+  for (int i = 0; i < NUM_FUNCTIONS_SWITCHES; i++) {
+    if (FSWITCH_CONFIG(i) != SWITCH_NONE) {
+      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;
+      }
+    }
+  }
+#endif
+
   // Multipos
   for (int i = 0; i < NUM_XPOTS; i++) {
     if (IS_POT_MULTIPOS(POT1 + i)) {