A couple of changes on top of projectkkglider branch

radio/src/mixer.cpp

@@ -524,7 +524,10 @@ uint8_t s_current_mixer_flight_mode;
 void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
 {
   evalInputs(mode);
-  evalLogicalSwitches();
+
+#if defined(CPUARM)
+  evalLogicalSwitches(mode==e_perout_mode_normal);
+#endif
 
 #if defined(MODULE_ALWAYS_SEND_PULSES)
   checkStartupWarnings();
@@ -930,6 +933,8 @@ void evalMixes(uint8_t tick10ms)
   static uint16_t delta = 0;
   static ACTIVE_PHASES_TYPE s_fade_flight_phases = 0;
 
+  LS_RECURSIVE_EVALUATION_RESET();
+  
   uint8_t phase = getFlightPhase();
 
   if (s_last_phase != phase) {
@@ -951,7 +956,9 @@ void evalMixes(uint8_t tick10ms)
         fp_act[s_last_phase] = 0;
         fp_act[phase] = MAX_ACT;
       }
-      logicalSwitchesCopyState(); //push last logical switches state from old to new phase
+#if defined(CPUARM)
+      logicalSwitchesCopyState(s_last_phase, phase); // push last logical switches state from old to new phase
+#endif
     }
     s_last_phase = phase;
   }
@@ -960,6 +967,7 @@ void evalMixes(uint8_t tick10ms)
   if (s_fade_flight_phases) {
     memclear(sum_chans512, sizeof(sum_chans512));
     for (uint8_t p=0; p<MAX_FLIGHT_MODES; p++) {
+      LS_RECURSIVE_EVALUATION_RESET();
       if (s_fade_flight_phases & ((ACTIVE_PHASES_TYPE)1 << p)) {
         s_current_mixer_flight_mode = p;
         evalFlightModeMixes(p==phase ? e_perout_mode_normal : e_perout_mode_inactive_phase, p==phase ? tick10ms : 0);
@@ -967,6 +975,7 @@ void evalMixes(uint8_t tick10ms)
           sum_chans512[i] += (chans[i] >> 4) * fp_act[p];
         weight += fp_act[p];
       }
+      LS_RECURSIVE_EVALUATION_RESET();
     }
     assert(weight);
     s_current_mixer_flight_mode = phase;

radio/src/opentx.h

@@ -671,7 +671,6 @@ enum StartupWarningStates {
 #endif
 
 extern uint8_t s_current_mixer_flight_mode;
-extern uint8_t s_last_phase;
 
 #if defined(CPUARM)
   #define bitfield_channels_t uint32_t
@@ -696,8 +695,14 @@ getvalue_t getValue(uint8_t i);
 bool getSwitch(int8_t swtch);
 void logicalSwitchesTimerTick();
 void logicalSwitchesReset();
-void evalLogicalSwitches();
+
+#if defined(CPUARM)
+void evalLogicalSwitches(bool isCurrentPhase=true);
 void logicalSwitchesCopyState();
+#define LS_RECURSIVE_EVALUATION_RESET()
+#else
+#define LS_RECURSIVE_EVALUATION_RESET() s_last_switch_used = 0
+#endif
 
 #if defined(PCBTARANIS)
   void getSwitchesPosition(bool startup);

radio/src/switches.cpp

@@ -38,23 +38,39 @@
 
 #define CS_LAST_VALUE_INIT -32768
 
+#if defined(CPUARM)
+
+enum LogicalSwitchContextState {
+  SWITCH_START,
+  SWITCH_DELAY,
+  SWITCH_ENABLE
+};
+
 PACK(typedef struct {
   uint8_t state:1;
-#if defined(CPUARM)
+  uint8_t timerState:2;
-  uint8_t ddState:2;
   uint8_t spare:5;
-  uint8_t ddTimer;
+  uint8_t timer;
-#else
-  uint8_t spare:7;
-#endif
   int16_t lastValue;
-}) lsw_struct;
+}) LogicalSwitchContext;
 
 PACK(typedef struct {
-  lsw_struct ls[NUM_LOGICAL_SWITCH];
+  LogicalSwitchContext lsw[NUM_LOGICAL_SWITCH];
-}) lsw_array;
+}) LogicalSwitchesFlightModeContext;
+LogicalSwitchesFlightModeContext lswFm[MAX_FLIGHT_MODES];
 
-lsw_array lswFm[2];
+#define LS_LAST_VALUE(fm, idx) lswFm[fm].lsw[idx].lastValue
+        
+#else
+
+int16_t lsLastValue[NUM_LOGICAL_SWITCH];
+#define LS_LAST_VALUE(fm, idx) lsLastValue[idx]
+
+#define GETSWITCH_RECURSIVE_TYPE uint16_t
+volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_used = 0;
+volatile GETSWITCH_RECURSIVE_TYPE s_last_switch_value = 0;
+
+#endif
 
 #if defined(PCBTARANIS)
 tmr10ms_t switchesMidposStart[6] = { 0 };
@@ -161,6 +177,182 @@ void getSwitchesPosition(bool startup)
 #define SWITCH_POSITION(idx) switchState((EnumKeys)(SW_BASE+(idx)))
 #endif
 
+bool getLogicalSwitch(uint8_t idx)
+{
+  LogicalSwitchData * ls = lswAddress(idx);
+  bool result;
+
+#if defined(CPUARM)
+  int8_t s = ls->andsw;
+#else
+  uint8_t s = ls->andsw;
+  if (s > SWSRC_LAST_SWITCH) {
+    s += SWSRC_SW1-SWSRC_LAST_SWITCH-1;
+  }
+#endif
+
+  if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) {
+    LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = CS_LAST_VALUE_INIT;
+    result = false;
+  }
+  else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) {
+    bool res1 = getSwitch(ls->v1);
+    bool res2 = getSwitch(ls->v2);
+    switch (ls->func) {
+      case LS_FUNC_AND:
+        result = (res1 && res2);
+        break;
+      case LS_FUNC_OR:
+        result = (res1 || res2);
+        break;
+      // case LS_FUNC_XOR:
+      default:
+        result = (res1 ^ res2);
+        break;
+    }
+  }
+  else if (s == LS_FAMILY_TIMER) {
+    result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) <= 0);
+  }
+  else if (s == LS_FAMILY_STICKY) {
+    result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) & (1<<0));
+  }
+#if defined(CPUARM)
+  else if (s == LS_FAMILY_STAY) {
+    result = (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) & (1<<0));
+  }
+#endif
+  else {
+    getvalue_t x = getValue(ls->v1);
+    getvalue_t y;
+    if (s == LS_FAMILY_COMP) {
+      y = getValue(ls->v2);
+
+      switch (ls->func) {
+        case LS_FUNC_EQUAL:
+          result = (x==y);
+          break;
+        case LS_FUNC_GREATER:
+          result = (x>y);
+          break;
+        default:
+          result = (x<y);
+          break;
+      }
+    }
+    else {
+      uint8_t v1 = ls->v1;
+#if defined(FRSKY)
+      // Telemetry
+      if (v1 >= MIXSRC_FIRST_TELEM) {
+        if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1))
+          return false;
+
+        y = convertLswTelemValue(ls);
+
+#if defined(FRSKY_HUB) && defined(GAUGES)
+        if (s == LS_FAMILY_OFS) {
+          uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT;
+          if (idx < THLD_MAX) {
+            // Fill the threshold array
+            barsThresholds[idx] = 128 + ls->v2;
+          }
+        }
+#endif
+      }
+      else if (v1 >= MIXSRC_GVAR1) {
+        y = ls->v2;
+      }
+      else {
+        y = calc100toRESX(ls->v2);
+      }
+#else
+      if (v1 >= MIXSRC_FIRST_TELEM) {
+        y = (int16_t)3 * (128+ls->v2); // it's a Timer
+      }
+      else if (v1 >= MIXSRC_GVAR1) {
+        y = ls->v2; // it's a GVAR
+      }
+      else {
+        y = calc100toRESX(ls->v2);
+      }
+#endif
+
+      switch (ls->func) {
+#if defined(CPUARM)
+        case LS_FUNC_VEQUAL:
+          result = (x==y);
+          break;
+#endif
+        case LS_FUNC_VALMOSTEQUAL:
+#if defined(GVARS)
+          if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR)
+            result = (x==y);
+          else
+#endif
+          result = (abs(x-y) < (1024 / STICK_TOLERANCE));
+          break;
+        case LS_FUNC_VPOS:
+          result = (x>y);
+          break;
+        case LS_FUNC_VNEG:
+          result = (x<y);
+          break;
+        case LS_FUNC_APOS:
+          result = (abs(x)>y);
+          break;
+        case LS_FUNC_ANEG:
+          result = (abs(x)<y);
+          break;
+        default:
+        {
+          if (LS_LAST_VALUE(s_current_mixer_flight_mode, idx) == CS_LAST_VALUE_INIT)
+            LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = x;
+          int16_t diff = x - LS_LAST_VALUE(s_current_mixer_flight_mode, idx);
+          if (ls->func == LS_FUNC_DIFFEGREATER)
+            result = (y >= 0 ? (diff >= y) : (diff <= y));
+          else
+            result = (abs(diff) >= y);
+          if (result)
+            LS_LAST_VALUE(s_current_mixer_flight_mode, idx) = x;
+          break;
+        }
+      }
+    }
+  }
+
+#if defined(CPUARM)
+    if (ls->delay || ls->duration) {
+      LogicalSwitchContext &context = lswFm[s_current_mixer_flight_mode].lsw[idx];
+      if (result) {
+        if (context.timerState == SWITCH_START) {
+          // set delay timer
+          context.timerState = SWITCH_DELAY;
+          context.timer = ls->delay;
+        }
+        if (context.timerState == SWITCH_DELAY) {
+          if (context.timer) {
+            result = false;   //return false while delay timer running
+          }
+          else if (ls->duration) {
+            // set duration timer
+            context.timerState = SWITCH_ENABLE;
+            context.timer = ls->duration;
+          }
+        }
+        else if (context.timerState == SWITCH_ENABLE) {
+          result = (context.timer > 0);  //return false after duration timer runs out
+        }
+      }
+      else {
+        context.timerState = SWITCH_START;
+      }
+    }
+#endif
+
+  return result;
+}
+
 bool getSwitch(int8_t swtch)
 {
   bool result;
@@ -221,202 +413,50 @@ bool getSwitch(int8_t swtch)
 #endif
   else {
     cs_idx -= SWSRC_FIRST_LOGICAL_SWITCH;
-    result = lswFm[(s_last_phase == s_current_mixer_flight_mode)?0:1].ls[cs_idx].state;
+#if defined(CPUARM)
+    result = lswFm[s_current_mixer_flight_mode].lsw[cs_idx].state;
+#else
+    GETSWITCH_RECURSIVE_TYPE mask = ((GETSWITCH_RECURSIVE_TYPE)1 << cs_idx);
+    if (s_last_switch_used & mask) {
+      result = (s_last_switch_value & mask);
+    }
+    else {
+      s_last_switch_used |= mask;
+      result = getLogicalSwitch(cs_idx);
+      if (result) {
+        s_last_switch_value |= mask;
+      }
+      else {
+        s_last_switch_value &= ~mask;
+      }
+    }      
+#endif    
   }
 
   return swtch > 0 ? result : !result;
 }
 
+#if defined(CPUARM)
 /**
   @brief Calculates new state of logical switches for s_current_mixer_flight_mode
 */
-void evalLogicalSwitches()
+void evalLogicalSwitches(bool isCurrentPhase)
 {
-  for(unsigned int cs_idx=0; cs_idx<NUM_LOGICAL_SWITCH; cs_idx++) {
+  for (unsigned int idx=0; idx<NUM_LOGICAL_SWITCH; idx++) {
-
+    LogicalSwitchContext &context = lswFm[s_current_mixer_flight_mode].lsw[idx];
-    bool result = false;
+    bool result = getLogicalSwitch(idx);
-
+    if (isCurrentPhase) {
-    LogicalSwitchData * ls = lswAddress(cs_idx);
-    lsw_struct * lsd = &lswFm[(s_last_phase == s_current_mixer_flight_mode)?0:1].ls[cs_idx];
-
-#if defined(CPUARM)
-    int8_t s = ls->andsw;
-#else
-    uint8_t s = ls->andsw;
-    if (s > SWSRC_LAST_SWITCH) {
-      s += SWSRC_SW1-SWSRC_LAST_SWITCH-1;
-    }
-#endif
-    if (ls->func == LS_FUNC_NONE || (s && !getSwitch(s))) {
-      lsd->lastValue = CS_LAST_VALUE_INIT;
-      result = false;
-    }
-    else if ((s=lswFamily(ls->func)) == LS_FAMILY_BOOL) {
-      bool res1 = getSwitch(ls->v1);
-      bool res2 = getSwitch(ls->v2);
-      switch (ls->func) {
-        case LS_FUNC_AND:
-          result = (res1 && res2);
-          break;
-        case LS_FUNC_OR:
-          result = (res1 || res2);
-          break;
-        // case LS_FUNC_XOR:
-        default:
-          result = (res1 ^ res2);
-          break;
-      }
-    }
-    else if (s == LS_FAMILY_TIMER) {
-      result = (lsd->lastValue <= 0);
-    }
-    else if (s == LS_FAMILY_STICKY) {
-      result = (lsd->lastValue & (1<<0));
-    }
-#if defined(CPUARM)
-    else if (s == LS_FAMILY_STAY) {
-      result = (lsd->lastValue & (1<<0));
-    }
-#endif
-    else {
-      getvalue_t x = getValue(ls->v1);
-      getvalue_t y;
-      if (s == LS_FAMILY_COMP) {
-        y = getValue(ls->v2);
-
-        switch (ls->func) {
-          case LS_FUNC_EQUAL:
-            result = (x==y);
-            break;
-          case LS_FUNC_GREATER:
-            result = (x>y);
-            break;
-          default:
-            result = (x<y);
-            break;
-        }
-      }
-      else {
-        uint8_t v1 = ls->v1;
-#if defined(FRSKY)
-        // Telemetry
-        if (v1 >= MIXSRC_FIRST_TELEM) {
-          if ((!TELEMETRY_STREAMING() && v1 >= MIXSRC_FIRST_TELEM+TELEM_FIRST_STREAMED_VALUE-1) || IS_FAI_FORBIDDEN(v1-1)) {
-            lsd->state = false;
-            continue;
-          }
-
-
-          y = convertLswTelemValue(ls);
-
-#if defined(FRSKY_HUB) && defined(GAUGES)
-          if (s == LS_FAMILY_OFS) {
-            uint8_t idx = v1-MIXSRC_FIRST_TELEM+1-TELEM_ALT;
-            if (idx < THLD_MAX) {
-              // Fill the threshold array
-              barsThresholds[idx] = 128 + ls->v2;
-            }
-          }
-#endif
-        }
-        else if (v1 >= MIXSRC_GVAR1) {
-          y = ls->v2;
-        }
-        else {
-          y = calc100toRESX(ls->v2);
-        }
-#else
-        if (v1 >= MIXSRC_FIRST_TELEM) {
-          y = (int16_t)3 * (128+ls->v2); // it's a Timer
-        }
-        else if (v1 >= MIXSRC_GVAR1) {
-          y = ls->v2; // it's a GVAR
-        }
-        else {
-          y = calc100toRESX(ls->v2);
-        }
-#endif
-
-        switch (ls->func) {
-#if defined(CPUARM)
-          case LS_FUNC_VEQUAL:
-            result = (x==y);
-            break;
-#endif
-          case LS_FUNC_VALMOSTEQUAL:
-#if defined(GVARS)
-            if (v1 >= MIXSRC_GVAR1 && v1 <= MIXSRC_LAST_GVAR)
-              result = (x==y);
-            else
-#endif
-            result = (abs(x-y) < (1024 / STICK_TOLERANCE));
-            break;
-          case LS_FUNC_VPOS:
-            result = (x>y);
-            break;
-          case LS_FUNC_VNEG:
-            result = (x<y);
-            break;
-          case LS_FUNC_APOS:
-            result = (abs(x)>y);
-            break;
-          case LS_FUNC_ANEG:
-            result = (abs(x)<y);
-            break;
-          default:
-          {
-            if (lsd->lastValue == CS_LAST_VALUE_INIT)
-              lsd->lastValue = x;
-            int16_t diff = x - lsd->lastValue;
-            if (ls->func == LS_FUNC_DIFFEGREATER)
-              result = (y >= 0 ? (diff >= y) : (diff <= y));
-            else
-              result = (abs(diff) >= y);
-            if (result)
-              lsd->lastValue = x;
-            break;
-          }
-        }
-      }
-    }
-
-#if defined(CPUARM)
-    if (ls->delay || ls->duration) {
       if (result) {
-        if (lsd->ddState == 0) {
+        if (!context.state) PLAY_LOGICAL_SWITCH_ON(idx);
-          //set delay timer
-          lsd->ddTimer = ls->delay;
-          lsd->ddState++;
-        }
-        if (lsd->ddState == 1) {
-          if (lsd->ddTimer) {
-            result = false;   //return false while delay timer running
-          }
-          else if (ls->duration) {
-            //set duration timer 
-            lsd->ddTimer = ls->duration;
-            lsd->ddState++;
-          }
-        }
-        else if (lsd->ddState == 2) {
-          result = (lsd->ddTimer > 0);  //return false after duration timer runs out
-        }
       }
       else {
-        lsd->ddState = 0;
+        if (context.state) PLAY_LOGICAL_SWITCH_OFF(idx);
       }
     }
-#endif
+    context.state = result;
-
-    if (result) {
-      if (!lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_ON(cs_idx);
-    }
-    else {
-      if (lsd->state && s_last_phase == s_current_mixer_flight_mode) PLAY_LOGICAL_SWITCH_OFF(cs_idx);
-    }
-    lsd->state = result;
   }
 }
+#endif
 
 swstate_t switches_states = 0;
 int8_t getMovedSwitch()
@@ -608,13 +648,15 @@ void checkSwitches()
 #endif    
 }
 
-void logicalSwitchesTimerTick() {
+void logicalSwitchesTimerTick()
-  for (uint8_t fm=0; fm<2; fm++) {
+{
+#if defined(CPUARM)
+  for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
+#endif
     for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
       LogicalSwitchData * ls = lswAddress(i);
-      lsw_struct * lsd = &lswFm[fm].ls[i];
       if (ls->func == LS_FUNC_TIMER) {
-        int16_t *lastValue = &lsd->lastValue;
+        int16_t *lastValue = &LS_LAST_VALUE(fm, i);
         if (*lastValue == 0 || *lastValue == CS_LAST_VALUE_INIT) {
           *lastValue = -lswTimerValue(ls->v1);
         }
@@ -631,7 +673,7 @@ void logicalSwitchesTimerTick() {
           uint8_t state;
           uint8_t last;
         }) ls_sticky_struct;
-        ls_sticky_struct & lastValue = (ls_sticky_struct &)lsd->lastValue;;
+        ls_sticky_struct & lastValue = (ls_sticky_struct &)LS_LAST_VALUE(fm, i);
         bool before = lastValue.last & 0x01;
         if (lastValue.state) {
           bool now = getSwitch(ls->v2);
@@ -659,7 +701,7 @@ void logicalSwitchesTimerTick() {
           uint16_t duration:15;
         }) ls_stay_struct;
 
-        ls_stay_struct & lastValue = (ls_stay_struct &)lsd->lastValue;;
+        ls_stay_struct & lastValue = (ls_stay_struct &)LS_LAST_VALUE(fm, i);
         lastValue.state = false;
         bool state = getSwitch(ls->v1);
         if (state) {
@@ -674,12 +716,17 @@ void logicalSwitchesTimerTick() {
           lastValue.duration = 0;
         }
       }
-      //decrement delay/duration timer
+      
-      if (lsd->ddTimer)
+      // decrement delay/duration timer
-        lsd->ddTimer--;
+      LogicalSwitchContext &context = lswFm[fm].lsw[i];
+      if (context.timer) {
+        context.timer--;
+      }
 #endif
     }
+#if defined(CPUARM)
   }
+#endif
 }
 
 LogicalSwitchData *lswAddress(uint8_t idx)
@@ -710,13 +757,17 @@ int16_t lswTimerValue(delayval_t val)
   return (val < -109 ? 129+val : (val < 7 ? (113+val)*5 : (53+val)*10));
 }
 
-void logicalSwitchesReset() {
+void logicalSwitchesReset()
-  for (uint8_t fm=0; fm<2; fm++) {
+{
-    lsw_array * lsa = &lswFm[fm];
+#if defined(CPUARM)
+  for (uint8_t fm=0; fm<MAX_FLIGHT_MODES; fm++) {
+#endif
     for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
-      lsa->ls[i].lastValue = CS_LAST_VALUE_INIT;
+      LS_LAST_VALUE(fm, i) = CS_LAST_VALUE_INIT;
     }
+#if defined(CPUARM)
   }
+#endif
 }
 
 getvalue_t convertLswTelemValue(LogicalSwitchData * ls)
@@ -733,6 +784,9 @@ getvalue_t convertLswTelemValue(LogicalSwitchData * ls)
   return val;
 }
 
-void logicalSwitchesCopyState() {
+#if defined(CPUARM)
-  lswFm[1] = lswFm[0];
+void logicalSwitchesCopyState(uint8_t src, uint8_t dst)
+{
+  lswFm[dst] = lswFm[src];
 }
+#endif