diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 6e41a7b8c1..12edc609bb 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -279,7 +279,9 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) // 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)) { @@ -289,12 +291,12 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) // 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; - flightModeFlags_e flightModeCopyMask = ~0; // only modes with bit set will be copied for (unsigned i = 0; i < ARRAYLEN(flightMode_boxId_map); i++) { - if (flightMode_boxId_map[i] != -1 // boxId_e does exist for this FLIGHT_MODE - && (flightModeCopyMask & (1 << i)) // this flightmode is copy is enabled - && FLIGHT_MODE(1 << i)) { // this flightmode is active - bitArraySet(&boxEnabledMask, 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); } } @@ -302,45 +304,10 @@ int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) // 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) -#define BM(x) (1ULL << (x)) - // limited to 64 BOXes now to keep code simple - const uint64_t rcModeCopyMask = BM(BOXANTIGRAVITY) - | BM(BOXHEADADJ) - | BM(BOXCAMSTAB) - | BM(BOXCAMTRIG) - | BM(BOXBEEPERON) - | BM(BOXLEDMAX) - | BM(BOXLEDLOW) - | BM(BOXLLIGHTS) - | BM(BOXCALIB) - | BM(BOXGOV) - | BM(BOXOSD) - | BM(BOXTELEMETRY) - | BM(BOXGTUNE) - | BM(BOXSERVO1) - | BM(BOXSERVO2) - | BM(BOXSERVO3) - | BM(BOXBLACKBOX) - | BM(BOXAIRMODE) - | BM(BOX3DDISABLE) - | BM(BOXFPVANGLEMIX) - | BM(BOXBLACKBOXERASE) - | BM(BOXCAMERA1) - | BM(BOXCAMERA2) - | BM(BOXCAMERA3) - | BM(BOXFLIPOVERAFTERCRASH) - | BM(BOXPREARM) - | BM(BOXBEEPGPSCOUNT) - | BM(BOX3DONASWITCH) - | BM(BOXVTXPITMODE); - STATIC_ASSERT(sizeof(rcModeCopyMask) * 8 >= CHECKBOX_ITEM_COUNT, copy_mask_too_small_for_boxes); - for (unsigned i = 0; i < CHECKBOX_ITEM_COUNT; i++) { - if ((rcModeCopyMask & BM(i)) // mode copy is enabled - && IS_RC_MODE_ACTIVE(i)) { // mode is active + for (unsigned i = 1; i < CHECKBOX_ITEM_COUNT; i++) { // skip BOXARM + if (!bitArrayGet(&boxFlightModeMask, i) && IS_RC_MODE_ACTIVE(i)) bitArraySet(&boxEnabledMask, i); - } } -#undef BM // map boxId_e enabled bits to MSP status indexes // only active boxIds are sent in status over MSP, other bits are not counted