Implement getBoxIdState

packFlightModeFlags is simplified by using it. Base for #5111

src/main/common/utils.h

@@ -80,6 +80,7 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
     (32*((v)/2L>>31 > 0) \
      + LOG2_32BIT((v)*1L >>16*((v)/2L>>31 > 0) \
                          >>16*((v)/2L>>31 > 0)))
+#define LOG2(v) LOG2_64BIT(v)
 
 #if 0
 // ISO C version, but no type checking

src/main/fc/rc_modes.h

@@ -22,19 +22,26 @@
 #include "pg/pg.h"
 
 typedef enum {
+    // ARM flag
     BOXARM = 0,
+    // FLIGHT_MODE
     BOXANGLE,
     BOXHORIZON,
-    BOXBARO,
-    BOXANTIGRAVITY,
     BOXMAG,
+    BOXBARO,
+    BOXGPSHOME,
+    BOXGPSHOLD,
     BOXHEADFREE,
+    BOXPASSTHRU,
+    BOXRANGEFINDER,
+    BOXFAILSAFE,
+    BOXID_FLIGHTMODE_LAST = BOXFAILSAFE,
+
+    // RCMODE flags
+    BOXANTIGRAVITY,
     BOXHEADADJ,
     BOXCAMSTAB,
     BOXCAMTRIG,
-    BOXGPSHOME,
-    BOXGPSHOLD,
-    BOXPASSTHRU,
     BOXBEEPERON,
     BOXLEDMAX,
     BOXLEDLOW,
@@ -44,12 +51,10 @@ typedef enum {
     BOXOSD,
     BOXTELEMETRY,
     BOXGTUNE,
-    BOXRANGEFINDER,
     BOXSERVO1,
     BOXSERVO2,
     BOXSERVO3,
     BOXBLACKBOX,
-    BOXFAILSAFE,
     BOXAIRMODE,
     BOX3DDISABLE,
     BOXFPVANGLEMIX,

src/main/fc/runtime_config.h

@@ -84,13 +84,21 @@ extern uint16_t flightModeFlags;
 #define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask)
 #define FLIGHT_MODE(mask) (flightModeFlags & (mask))
 
-// macro to initialize map from flightModeFlags to boxId_e. Keep it in sync with flightModeFlags_e enum.
-// Each boxId_e is at index of flightModeFlags_e bit, value is -1 if boxId_e does not exist.
-// It is much more memory efficient than full map (uint32_t -> uint8_t)
-#define FLIGHT_MODE_BOXID_MAP_INITIALIZER {                             \
-        BOXANGLE, BOXHORIZON, BOXMAG, BOXBARO, BOXGPSHOME, BOXGPSHOLD,  \
-        BOXHEADFREE, -1, BOXPASSTHRU, BOXRANGEFINDER, BOXFAILSAFE}  \
-        /**/
+// macro to initialize map from boxId_e to log2(flightModeFlags). Keep it in sync with flightModeFlags_e enum.
+// [BOXARM] is left unpopulated
+#define BOXID_TO_FLIGHT_MODE_MAP_INITIALIZER {           \
+   [BOXANGLE]       = LOG2(ANGLE_MODE),                  \
+   [BOXHORIZON]     = LOG2(HORIZON_MODE),                \
+   [BOXMAG]         = LOG2(MAG_MODE),                    \
+   [BOXBARO]        = LOG2(BARO_MODE),                   \
+   [BOXGPSHOME]     = LOG2(GPS_HOME_MODE),               \
+   [BOXGPSHOLD]     = LOG2( GPS_HOLD_MODE),              \
+   [BOXHEADFREE]    = LOG2(HEADFREE_MODE),               \
+   [BOXPASSTHRU]    = LOG2(PASSTHRU_MODE),               \
+   [BOXRANGEFINDER] = LOG2(RANGEFINDER_MODE),            \
+   [BOXFAILSAFE]    = LOG2(FAILSAFE_MODE),               \
+}                                                        \
+/**/
 
 typedef enum {
     GPS_FIX_HOME   = (1 << 0),

src/main/interface/msp_box.c

@@ -270,53 +270,36 @@ void initActiveBoxIds(void)
     activeBoxIds = ena;                               // set global variable
 }
 
+// return state of given boxId box, handling ARM and FLIGHT_MODE
+bool getBoxIdState(boxId_e boxid)
+{
+    const uint8_t boxIdToFlightModeMap[] = BOXID_TO_FLIGHT_MODE_MAP_INITIALIZER;
+
+    // we assume that all boxId below BOXID_FLIGHTMODE_LAST except BOXARM are mapped to flightmode
+    STATIC_ASSERT(ARRAYLEN(boxIdToFlightModeMap) == BOXID_FLIGHTMODE_LAST + 1, FLIGHT_MODE_BOXID_MAP_INITIALIZER_does_not_match_boxId_e);
+
+    if (boxid == BOXARM) {
+        return ARMING_FLAG(ARMED);
+    } else if (boxid <= BOXID_FLIGHTMODE_LAST) {
+        return FLIGHT_MODE(1 << boxIdToFlightModeMap[boxid]);
+    } else {
+        return IS_RC_MODE_ACTIVE(boxid);
+    }
+}
+
 // pack used flightModeFlags into supplied array
 // returns number of bits used
 int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags)
 {
     // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
     memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t));
-
-    // enabled BOXes, bits indexed by boxId_e
-    boxBitmask_t boxEnabledMask;
-    boxBitmask_t boxFlightModeMask;
-    memset(&boxEnabledMask, 0, sizeof(boxEnabledMask));
-    memset(&boxFlightModeMask, 0, sizeof(boxFlightModeMask));
-
-    // copy ARM state
-    if (ARMING_FLAG(ARMED)) {
-        bitArraySet(&boxEnabledMask, BOXARM);
-        bitArraySet(&boxFlightModeMask, BOXARM);
-    }
-
-    // enable BOXes dependent on FLIGHT_MODE, use mapping table (from runtime_config.h)
-    // flightMode_boxId_map[HORIZON_MODE] == BOXHORIZON
-    static const int8_t flightMode_boxId_map[] = FLIGHT_MODE_BOXID_MAP_INITIALIZER;
-    for (unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) {
-        const int8_t boxid = flightMode_boxId_map[i];
-        if (boxid != -1) {    // boxId_e does exist for this FLIGHT_MODE
-            bitArraySet(&boxFlightModeMask, boxid);
-            if (FLIGHT_MODE(1 << i))  // this flightmode is active
-                bitArraySet(&boxEnabledMask, boxid);
-        }
-    }
-
-    // enable BOXes dependent on rcMode bits, indexes are the same.
-    // only subset of BOXes depend on rcMode (non-ARM or FLIGHT_MODE), use mask to select them
-    // NOTE: ARM and FLIGHT modes are potentially contingent on other conditions.
-    //       Therefore, they must be masked/enabled separately from simple "range conditions" (RC)
-    for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) {
-        if (!bitArrayGet(&boxFlightModeMask, i) && IS_RC_MODE_ACTIVE(i))
-            bitArraySet(&boxEnabledMask, i);
-    }
-
     // map boxId_e enabled bits to MSP status indexes
     // only active boxIds are sent in status over MSP, other bits are not counted
     unsigned mspBoxIdx = 0;           // index of active boxId (matches sent permanentId and boxNames)
     for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) {
         if (activeBoxIdGet(boxId)) {
-            if (bitArrayGet(&boxEnabledMask,  boxId))
-                bitArraySet(mspFlightModeFlags,  mspBoxIdx);      // box is enabled
+            if (getBoxIdState(boxId))
+                bitArraySet(mspFlightModeFlags, mspBoxIdx);       // box is enabled
             mspBoxIdx++;                                          // box is active, count it
         }
     }