From 5a75737df9fdc190282175c6c77b1d010a34737c Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Mon, 5 Feb 2018 12:42:58 +0100 Subject: [PATCH] Implement getBoxIdState packFlightModeFlags is simplified by using it. Base for #5111 --- src/main/common/utils.h | 1 + src/main/fc/rc_modes.h | 19 ++++++++----- src/main/fc/runtime_config.h | 22 ++++++++++----- src/main/interface/msp_box.c | 55 +++++++++++++----------------------- 4 files changed, 47 insertions(+), 50 deletions(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 568ae0f24..c8d9219b7 100644 --- a/src/main/common/utils.h +++ b/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 diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 44071ce13..534a4454f 100644 --- a/src/main/fc/rc_modes.h +++ b/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, diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index ce764e667..5025e953b 100644 --- a/src/main/fc/runtime_config.h +++ b/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), diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 516f02969..9ed2f7d5b 100644 --- a/src/main/interface/msp_box.c +++ b/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 } }