From db006b1585e8f0273911be8bafe101c632db3a43 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 17 Jun 2017 11:37:14 +0100 Subject: [PATCH 1/6] Merge pull request #2856 from ledvinap/improvement-64bit-boxid array based box masks --- Makefile | 2 + src/main/blackbox/blackbox.c | 15 +- src/main/common/bitarray.c | 39 +++ src/main/common/bitarray.h | 20 ++ src/main/fc/config.c | 2 +- src/main/fc/fc_msp.c | 227 ++++++++------ src/main/fc/fc_msp.h | 4 +- src/main/fc/fc_rc.c | 1 + src/main/fc/rc_adjustments.c | 11 +- src/main/fc/rc_adjustments.h | 2 +- src/main/fc/rc_controls.c | 60 +--- src/main/fc/rc_controls.h | 85 +---- src/main/fc/rc_modes.c | 101 ++++++ src/main/fc/rc_modes.h | 105 +++++++ src/main/fc/settings.c | 1 + src/main/flight/altitude.c | 1 + src/main/flight/failsafe.c | 1 + src/main/flight/mixer.c | 11 +- src/main/flight/navigation.c | 2 +- src/main/flight/servos.c | 1 + src/main/io/ledstrip.c | 1 + src/main/io/vtx_control.h | 2 +- src/main/msp/msp.h | 3 +- src/main/rx/rx.c | 2 + src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/target/MULTIFLITEPICO/config.c | 1 + src/main/telemetry/crsf.c | 2 +- src/main/telemetry/telemetry.c | 2 +- src/test/Makefile | 8 +- ...nittest.cc.txt => rc_controls_unittest.cc} | 291 +++++++++--------- src/test/unit/rx_rx_unittest.cc | 6 +- src/test/unit/target.h | 1 + 32 files changed, 593 insertions(+), 419 deletions(-) create mode 100644 src/main/common/bitarray.c create mode 100644 src/main/common/bitarray.h create mode 100644 src/main/fc/rc_modes.c create mode 100644 src/main/fc/rc_modes.h rename src/test/unit/{rc_controls_unittest.cc.txt => rc_controls_unittest.cc} (65%) diff --git a/Makefile b/Makefile index e2b76957ed..4757c25f84 100644 --- a/Makefile +++ b/Makefile @@ -665,6 +665,7 @@ COMMON_SRC = \ build/version.c \ $(TARGET_DIR_SRC) \ main.c \ + common/bitarray.c \ common/encoding.c \ common/filter.c \ common/maths.c \ @@ -736,6 +737,7 @@ FC_SRC = \ fc/fc_rc.c \ fc/rc_adjustments.c \ fc/rc_controls.c \ + fc/rc_modes.c \ fc/cli.c \ fc/settings.c \ flight/altitude.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c665bddb48..e0fcfee6b7 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -47,6 +47,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" @@ -325,7 +326,7 @@ typedef struct blackboxSlowState_s { extern uint16_t motorOutputHigh, motorOutputLow; //From rc_controls.c -extern uint32_t rcModeActivationMask; +extern boxBitmask_t rcModeActivationMask; static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; @@ -753,7 +754,7 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { - slow->flightModeFlags = rcModeActivationMask; //was flightModeFlags; + memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); @@ -861,7 +862,7 @@ static void blackboxStart(void) */ blackboxBuildConditionCache(); - blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX); + blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX); blackboxResetIterationTimers(); @@ -870,7 +871,7 @@ static void blackboxStart(void) * it finally plays the beep for this arming event. */ blackboxLastArmingBeep = getArmingBeepTimeMicros(); - blackboxLastFlightModeFlags = rcModeActivationMask; // record startup status + memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } @@ -1383,10 +1384,10 @@ static void blackboxCheckAndLogFlightMode(void) flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags // Use != so that we can still detect a change if the counter wraps - if (rcModeActivationMask != blackboxLastFlightModeFlags) { + if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) { eventData.lastFlags = blackboxLastFlightModeFlags; - blackboxLastFlightModeFlags = rcModeActivationMask; - eventData.flags = rcModeActivationMask; + memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); + memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags)); blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData); } diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c new file mode 100644 index 0000000000..53ca99127c --- /dev/null +++ b/src/main/common/bitarray.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "bitarray.h" + +#define BITARRAY_BIT_OP(array, bit, op) ((array)[(bit) / (sizeof((array)[0]) * 8)] op (1 << ((bit) % (sizeof((array)[0]) * 8)))) + +bool bitArrayGet(const void *array, unsigned bit) +{ + return BITARRAY_BIT_OP((uint32_t*)array, bit, &); +} + +void bitArraySet(void *array, unsigned bit) +{ + BITARRAY_BIT_OP((uint32_t*)array, bit, |=); +} + +void bitArrayClr(void *array, unsigned bit) +{ + BITARRAY_BIT_OP((uint32_t*)array, bit, &=~); +} + diff --git a/src/main/common/bitarray.h b/src/main/common/bitarray.h new file mode 100644 index 0000000000..48032bb297 --- /dev/null +++ b/src/main/common/bitarray.h @@ -0,0 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +bool bitArrayGet(const void *array, unsigned bit); +void bitArraySet(void *array, unsigned bit); +void bitArrayClr(void *array, unsigned bit); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8bc950e525..931e5a502c 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -335,7 +335,7 @@ void activateConfig(void) resetAdjustmentStates(); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); useAdjustmentConfig(currentPidProfile); #ifdef GPS diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index dbcbfbd089..fd7c245257 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -29,6 +29,7 @@ #include "build/version.h" #include "common/axis.h" +#include "common/bitarray.h" #include "common/color.h" #include "common/maths.h" #include "common/streambuf.h" @@ -59,6 +60,8 @@ #include "fc/fc_msp.h" #include "fc/fc_rc.h" #include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/altitude.h" @@ -110,12 +113,13 @@ #endif #define STATIC_ASSERT(condition, name) \ - typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] + typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] __attribute__((unused)) static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #ifndef USE_OSD_SLAVE +// permanent IDs must uniquely identify BOX meaning, DO NOT REUSE THEM! static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOXARM, "ARM", 0 }, { BOXANGLE, "ANGLE", 1 }, @@ -152,9 +156,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index -static uint32_t activeBoxIds; -// check that all boxId_e values fit -STATIC_ASSERT(sizeof(activeBoxIds) * 8 >= CHECKBOX_ITEM_COUNT, CHECKBOX_ITEMS_wont_fit_in_activeBoxIds); + +static boxBitmask_t activeBoxIds; static const char pidnames[] = "ROLL;" @@ -276,23 +279,41 @@ const box_t *findBoxByPermanentId(uint8_t permanentId) return NULL; } -static void serializeBoxNamesReply(sbuf_t *dst) +static bool activeBoxIdGet(boxId_e boxId) { - for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) { - if(activeBoxIds & (1 << id)) { - const box_t *box = findBoxByBoxId(id); - sbufWriteString(dst, box->boxName); - sbufWriteU8(dst, ';'); - } - } + if(boxId > sizeof(activeBoxIds) * 8) + return false; + return bitArrayGet(&activeBoxIds, boxId); } -static void serializeBoxIdsReply(sbuf_t *dst) + +// callcack for box serialization +typedef void serializeBoxFn(sbuf_t *dst, const box_t *box); + +static void serializeBoxNameFn(sbuf_t *dst, const box_t *box) { + sbufWriteString(dst, box->boxName); + sbufWriteU8(dst, ';'); +} + +static void serializeBoxPermanentIdFn(sbuf_t *dst, const box_t *box) +{ + sbufWriteU8(dst, box->permanentId); +} + +// serialize 'page' of boxNames. +// Each page contains at most 32 boxes +static void serializeBoxReply(sbuf_t *dst, int page, serializeBoxFn *serializeBox) +{ + unsigned boxIdx = 0; + unsigned pageStart = page * 32; + unsigned pageEnd = pageStart + 32; for (boxId_e id = 0; id < CHECKBOX_ITEM_COUNT; id++) { - if(activeBoxIds & (1 << id)) { - const box_t *box = findBoxByBoxId(id); - sbufWriteU8(dst, box->permanentId); + if (activeBoxIdGet(id)) { + if (boxIdx >= pageStart && boxIdx < pageEnd) { + (*serializeBox)(dst, findBoxByBoxId(id)); + } + boxIdx++; // count active boxes } } } @@ -300,9 +321,11 @@ static void serializeBoxIdsReply(sbuf_t *dst) void initActiveBoxIds(void) { // calculate used boxes based on features and set corresponding activeBoxIds bits - uint32_t ena = 0; // temporary variable to collect result + boxBitmask_t ena; // temporary variable to collect result + memset(&ena, 0, sizeof(ena)); + // macro to enable boxId (BoxidMaskEnable). Reference to ena is hidden, local use only -#define BME(boxId) do { ena |= (1 << (boxId)); } while(0) +#define BME(boxId) do { bitArraySet(&ena, boxId); } while(0) BME(BOXARM); if (!feature(FEATURE_AIRMODE)) { @@ -399,63 +422,68 @@ void initActiveBoxIds(void) #undef BME // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions) - for(boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) - if((ena & (1 << boxId)) - && findBoxByBoxId(boxId) == NULL) - ena &= ~ (1 << boxId); // this should not happen, but handle it gracefully + for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) + if (bitArrayGet(&ena, boxId) + && findBoxByBoxId(boxId) == NULL) + bitArrayClr(&ena, boxId); // this should not happen, but handle it gracefully - activeBoxIds = ena; // set global variable + activeBoxIds = ena; // set global variable } -static uint32_t packFlightModeFlags(void) +// pack used flightModeFlags into supplied array +// returns number of bits used +static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) { // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES - uint32_t boxEnabledMask = 0; // enabled BOXes, bits indexed by boxId_e + // enabled BOXes, bits indexed by boxId_e + boxBitmask_t boxEnabledMask; + memset(&boxEnabledMask, 0, sizeof(boxEnabledMask)); // 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 + 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 - boxEnabledMask |= (1 << flightMode_boxId_map[i]); + bitArraySet(&boxEnabledMask, flightMode_boxId_map[i]); } } // enable BOXes dependent on rcMode bits, indexes are the same. // only subset of BOXes depend on rcMode, use mask to select them -#define BM(x) (1 << (x)) - const uint32_t rcModeCopyMask = BM(BOXHEADADJ) | BM(BOXCAMSTAB) | BM(BOXCAMTRIG) | BM(BOXBEEPERON) +#define BM(x) (1ULL << (x)) + // limited to 64 BOXes now to keep code simple + const uint64_t rcModeCopyMask = 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(BOXBLACKBOX) | BM(BOXBLACKBOXERASE) | BM(BOXAIRMODE) | BM(BOXANTIGRAVITY) | BM(BOXFPVANGLEMIX); - for(unsigned i = 0; i < sizeof(rcModeCopyMask) * 8; i++) { - if((rcModeCopyMask & BM(i)) // mode copy is enabled + 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 - boxEnabledMask |= (1 << i); + bitArraySet(&boxEnabledMask, i); } } #undef BM // copy ARM state - if(ARMING_FLAG(ARMED)) - boxEnabledMask |= (1 << BOXARM); + if (ARMING_FLAG(ARMED)) + bitArraySet(&boxEnabledMask, BOXARM); // map boxId_e enabled bits to MSP status indexes // only active boxIds are sent in status over MSP, other bits are not counted - uint32_t mspBoxEnabledMask = 0; unsigned mspBoxIdx = 0; // index of active boxId (matches sent permanentId and boxNames) for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++) { - if(activeBoxIds & (1 << boxId)) { - if (boxEnabledMask & (1 << boxId)) - mspBoxEnabledMask |= 1 << mspBoxIdx; // box is enabled - mspBoxIdx++; // box is active, count it + if (activeBoxIdGet(boxId)) { + if (bitArrayGet(&boxEnabledMask, boxId)) + bitArraySet(mspFlightModeFlags, mspBoxIdx); // box is enabled + mspBoxIdx++; // box is active, count it } } - - return mspBoxEnabledMask; + // return count of used bits + return mspBoxIdx; } static void serializeSDCardSummaryReply(sbuf_t *dst) @@ -830,20 +858,6 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro switch (cmdMSP) { case MSP_STATUS_EX: - sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); -#ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); -#else - sbufWriteU16(dst, 0); -#endif - sbufWriteU16(dst, 0); // sensors - sbufWriteU32(dst, 0); // flight modes - sbufWriteU8(dst, 0); // profile - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU8(dst, 1); // max profiles - sbufWriteU8(dst, 0); // control rate profile - break; - case MSP_STATUS: sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL)); #ifdef USE_I2C @@ -855,7 +869,12 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro sbufWriteU32(dst, 0); // flight modes sbufWriteU8(dst, 0); // profile sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU16(dst, 0); // gyro cycle time + if (cmdMSP == MSP_STATUS_EX) { + sbufWriteU8(dst, 1); // max profiles + sbufWriteU8(dst, 0); // control rate profile + } else { + sbufWriteU16(dst, 0); // gyro cycle time + } break; default: @@ -872,32 +891,35 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn switch (cmdMSP) { case MSP_STATUS_EX: - sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); -#ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); -#else - sbufWriteU16(dst, 0); -#endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentPidProfileIndex()); - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU8(dst, MAX_PROFILE_COUNT); - sbufWriteU8(dst, getCurrentControlRateProfileIndex()); - break; - case MSP_STATUS: - sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); + { + boxBitmask_t flightModeFlags; + const int flagBits = packFlightModeFlags(&flightModeFlags); + + sbufWriteU16(dst, getTaskDeltaTime(TASK_GYROPID)); #ifdef USE_I2C - sbufWriteU16(dst, i2cGetErrorCounter()); + sbufWriteU16(dst, i2cGetErrorCounter()); #else - sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); #endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - sbufWriteU32(dst, packFlightModeFlags()); - sbufWriteU8(dst, getCurrentPidProfileIndex()); - sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); - sbufWriteU16(dst, 0); // gyro cycle time + sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits + sbufWriteU8(dst, getCurrentPidProfileIndex()); + sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); + if (cmdMSP == MSP_STATUS_EX) { + sbufWriteU8(dst, MAX_PROFILE_COUNT); + sbufWriteU8(dst, getCurrentControlRateProfileIndex()); + } else { // MSP_STATUS + sbufWriteU16(dst, 0); // gyro cycle time + } + + // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow + // header is emited even when all bits fit into 32 bits to allow future extension + int byteCount = (flagBits - 32 + 7) / 8; // 32 already stored, round up + byteCount = constrain(byteCount, 0, 15); // limit to 16 bytes (128 bits) + sbufWriteU8(dst, byteCount); + sbufWriteData(dst, ((uint8_t*)&flightModeFlags) + 4, byteCount); + } break; case MSP_RAW_IMU: @@ -1059,14 +1081,6 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } break; - case MSP_BOXNAMES: - serializeBoxNamesReply(dst); - break; - - case MSP_BOXIDS: - serializeBoxIdsReply(dst); - break; - case MSP_MOTOR_CONFIG: sbufWriteU16(dst, motorConfig()->minthrottle); sbufWriteU16(dst, motorConfig()->maxthrottle); @@ -1333,13 +1347,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn uint8_t band=0, channel=0; vtxCommonGetBandAndChannel(&band,&channel); - + uint8_t powerIdx=0; // debug vtxCommonGetPowerIndex(&powerIdx); - + uint8_t pitmode=0; vtxCommonGetPitMode(&pitmode); - + sbufWriteU8(dst, deviceType); sbufWriteU8(dst, band); sbufWriteU8(dst, channel); @@ -1359,6 +1373,29 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn } return true; } + +static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +{ + UNUSED(mspPostProcessFn); + + switch (cmdMSP) { + case MSP_BOXNAMES: + { + const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + serializeBoxReply(dst, page, &serializeBoxNameFn); + } + break; + case MSP_BOXIDS: + { + const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0; + serializeBoxReply(dst, page, &serializeBoxPermanentIdFn); + } + break; + default: + return MSP_RESULT_CMD_UNKNOWN; + } + return MSP_RESULT_ACK; +} #endif #ifdef GPS @@ -1496,7 +1533,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); } else { return MSP_RESULT_ERROR; } @@ -1658,7 +1695,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) }*/ validateAndFixGyroConfig(); - if (sbufBytesRemaining(src)) { + if (sbufBytesRemaining(src)) { motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src); } break; @@ -1761,13 +1798,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) < 2) break; - + uint8_t power = sbufReadU8(src); uint8_t current_power = 0; vtxCommonGetPowerIndex(¤t_power); if (current_power != power) vtxCommonSetPowerByIndex(power); - + uint8_t pitmode = sbufReadU8(src); uint8_t current_pitmode = 0; vtxCommonGetPitMode(¤t_pitmode); @@ -2177,6 +2214,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro #ifndef USE_OSD_SLAVE } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { ret = MSP_RESULT_ACK; + } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) { + /* ret */; #endif #ifdef USE_OSD_SLAVE } else if (mspOsdSlaveProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index ac3bc51958..92e0cc2647 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -18,12 +18,12 @@ #pragma once #include "msp/msp.h" -#include "rc_controls.h" +#include "fc/rc_modes.h" typedef struct box_e { const uint8_t boxId; // see boxId_e const char *boxName; // GUI-readable box name - const uint8_t permanentId; // + const uint8_t permanentId; // permanent ID used to identify BOX. This ID is unique for one function, DO NOT REUSE IT } box_t; const box_t *findBoxByBoxId(boxId_e boxId); diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index f472787727..afc1f45b7d 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -37,6 +37,7 @@ #include "fc/fc_core.h" #include "fc/fc_rc.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "rx/rx.h" diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 2fbba702b0..3ad6b04867 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -88,7 +88,7 @@ static void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustm } #endif -static uint8_t adjustmentStateMask = 0; +STATIC_UNIT_TESTED uint8_t adjustmentStateMask = 0; #define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex) adjustmentStateMask |= (1 << adjustmentIndex) #define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex) adjustmentStateMask &= ~(1 << adjustmentIndex) @@ -199,9 +199,9 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 -static adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; +STATIC_UNIT_TESTED adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; -static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) +STATIC_UNIT_TESTED void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { adjustmentState_t *adjustmentState = &adjustmentStates[index]; @@ -386,10 +386,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig) continue; } - const int32_t signedDiff = now - adjustmentState->timeoutAt; - const bool canResetReadyStates = signedDiff >= 0L; - - if (canResetReadyStates) { + if (cmp32(now, adjustmentState->timeoutAt) >= 0) { adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ; MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex); } diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index 3d44a4629a..60900403a6 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -19,7 +19,7 @@ #include #include "config/parameter_group.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" typedef enum { ADJUSTMENT_NONE = 0, diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0bcdfe7cb5..259cab256f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -65,8 +65,6 @@ static bool isUsingSticksToArm = true; float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e - PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, @@ -85,15 +83,13 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .auto_disarm_delay = 5 ); -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); - -bool isAirmodeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); -} - -bool isAntiGravityModeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); -} +PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); +PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, + .deadband3d_low = 1406, + .deadband3d_high = 1514, + .neutral3d = 1460, + .deadband3d_throttle = 50 +); bool isUsingSticksForArming(void) { @@ -308,51 +304,13 @@ void processRcStickPositions(throttleStatus_e throttleStatus) } -bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId) -{ - uint8_t index; - - for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; - - if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { - return true; - } - } - - return false; -} - -bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { - if (!IS_RANGE_USABLE(range)) { - return false; - } - - const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1); - return (channelValue >= 900 + (range->startStep * 25) && - channelValue < 900 + (range->endStep * 25)); -} - -void updateActivatedModes(void) -{ - rcModeActivationMask = 0; - - for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); - - if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { - ACTIVATE_RC_MODE(modeActivationCondition->modeId); - } - } -} - int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(pidProfile_t *pidProfileToUse) { pidProfile = pidProfileToUse; - isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); + isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM); } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 9be4198d2a..a7eb5c10a0 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -21,47 +21,6 @@ #include "config/parameter_group.h" -typedef enum { - BOXARM = 0, - BOXANGLE, - BOXHORIZON, - BOXBARO, - BOXANTIGRAVITY, - BOXMAG, - BOXHEADFREE, - BOXHEADADJ, - BOXCAMSTAB, - BOXCAMTRIG, - BOXGPSHOME, - BOXGPSHOLD, - BOXPASSTHRU, - BOXBEEPERON, - BOXLEDMAX, - BOXLEDLOW, - BOXLLIGHTS, - BOXCALIB, - BOXGOV, - BOXOSD, - BOXTELEMETRY, - BOXGTUNE, - BOXSONAR, - BOXSERVO1, - BOXSERVO2, - BOXSERVO3, - BOXBLACKBOX, - BOXFAILSAFE, - BOXAIRMODE, - BOX3DDISABLESWITCH, - BOXFPVANGLEMIX, - BOXBLACKBOXERASE, - CHECKBOX_ITEM_COUNT -} boxId_e; - -extern uint32_t rcModeActivationMask; - -#define IS_RC_MODE_ACTIVE(modeId) ((1 << (modeId)) & rcModeActivationMask) -#define ACTIVATE_RC_MODE(modeId) (rcModeActivationMask |= (1 << modeId)) - typedef enum rc_alias { ROLL = 0, PITCH, @@ -109,17 +68,6 @@ typedef enum { #define THR_CE (3 << (2 * THROTTLE)) #define THR_HI (2 << (2 * THROTTLE)) -#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 - -#define CHANNEL_RANGE_MIN 900 -#define CHANNEL_RANGE_MAX 2100 - -#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step) -#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25) - -#define MIN_MODE_RANGE_STEP 0 -#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) - // Roll/pitch rates are a proportion used for mixing, so it tops out at 1.0: #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 100 @@ -128,29 +76,6 @@ typedef enum { #define CONTROL_RATE_CONFIG_TPA_MAX 100 -// steps are 25 apart -// a value of 0 corresponds to a channel value of 900 or less -// a value of 48 corresponds to a channel value of 2100 or more -// 48 steps between 900 and 1200 -typedef struct channelRange_s { - uint8_t startStep; - uint8_t endStep; -} channelRange_t; - -typedef struct modeActivationCondition_s { - boxId_e modeId; - uint8_t auxChannelIndex; - channelRange_t range; -} modeActivationCondition_t; - -PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); - -typedef struct modeActivationProfile_s { - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; -} modeActivationProfile_t; - -#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) - extern float rcCommand[4]; typedef struct rcControlsConfig_s { @@ -186,15 +111,9 @@ bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(void); void processRcStickPositions(throttleStatus_e throttleStatus); -bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); -void updateActivatedModes(void); - -bool isAirmodeActive(void); -bool isAntiGravityModeActive(void); - bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); -bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); +struct modeActivationCondition_s; +void useRcControlsConfig(struct pidProfile_s *pidProfileToUse); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c new file mode 100644 index 0000000000..16d5369c25 --- /dev/null +++ b/src/main/fc/rc_modes.c @@ -0,0 +1,101 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "rc_modes.h" + +#include "common/bitarray.h" +#include "common/maths.h" + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" + +#include "rx/rx.h" + +boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e + +PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, + PG_MODE_ACTIVATION_PROFILE, 0); + + +bool IS_RC_MODE_ACTIVE(boxId_e boxId) +{ + return bitArrayGet(&rcModeActivationMask, boxId); +} + +void rcModeUpdate(boxBitmask_t *newState) +{ + rcModeActivationMask = *newState; +} + +bool isAirmodeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); +} + +bool isAntiGravityModeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); +} + +bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { + if (!IS_RANGE_USABLE(range)) { + return false; + } + + const uint16_t channelValue = constrain(rcData[auxChannelIndex + NON_AUX_CHANNEL_COUNT], CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX - 1); + return (channelValue >= 900 + (range->startStep * 25) && + channelValue < 900 + (range->endStep * 25)); +} + + +void updateActivatedModes(void) +{ + boxBitmask_t newMask; + memset(&newMask, 0, sizeof(newMask)); + + for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); + + if (isRangeActive(modeActivationCondition->auxChannelIndex, &modeActivationCondition->range)) { + boxId_e mode = modeActivationCondition->modeId; + if (mode < CHECKBOX_ITEM_COUNT) + bitArraySet(&newMask, mode); + } + } + rcModeUpdate(&newMask); +} + +bool isModeActivationConditionPresent(boxId_e modeId) +{ + uint8_t index; + + for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(index); + + if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) { + return true; + } + } + + return false; +} diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h new file mode 100644 index 0000000000..2b0d223786 --- /dev/null +++ b/src/main/fc/rc_modes.h @@ -0,0 +1,105 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +#include "config/parameter_group.h" + +typedef enum { + BOXARM = 0, + BOXANGLE, + BOXHORIZON, + BOXBARO, + BOXANTIGRAVITY, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, + BOXCAMSTAB, + BOXCAMTRIG, + BOXGPSHOME, + BOXGPSHOLD, + BOXPASSTHRU, + BOXBEEPERON, + BOXLEDMAX, + BOXLEDLOW, + BOXLLIGHTS, + BOXCALIB, + BOXGOV, + BOXOSD, + BOXTELEMETRY, + BOXGTUNE, + BOXSONAR, + BOXSERVO1, + BOXSERVO2, + BOXSERVO3, + BOXBLACKBOX, + BOXFAILSAFE, + BOXAIRMODE, + BOX3DDISABLESWITCH, + BOXFPVANGLEMIX, + BOXBLACKBOXERASE, + CHECKBOX_ITEM_COUNT +} boxId_e; + +// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior +typedef struct { uint32_t bits[(CHECKBOX_ITEM_COUNT + 31) / 32]; } boxBitmask_t; + +#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 + +#define CHANNEL_RANGE_MIN 900 +#define CHANNEL_RANGE_MAX 2100 + +#define MODE_STEP_TO_CHANNEL_VALUE(step) (CHANNEL_RANGE_MIN + 25 * step) +#define CHANNEL_VALUE_TO_STEP(channelValue) ((constrain(channelValue, CHANNEL_RANGE_MIN, CHANNEL_RANGE_MAX) - CHANNEL_RANGE_MIN) / 25) + +#define MIN_MODE_RANGE_STEP 0 +#define MAX_MODE_RANGE_STEP ((CHANNEL_RANGE_MAX - CHANNEL_RANGE_MIN) / 25) + +// steps are 25 apart +// a value of 0 corresponds to a channel value of 900 or less +// a value of 48 corresponds to a channel value of 2100 or more +// 48 steps between 900 and 1200 +typedef struct channelRange_s { + uint8_t startStep; + uint8_t endStep; +} channelRange_t; + +typedef struct modeActivationCondition_s { + boxId_e modeId; + uint8_t auxChannelIndex; + channelRange_t range; +} modeActivationCondition_t; + +PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions); + +typedef struct modeActivationProfile_s { + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; +} modeActivationProfile_t; + +#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) + +bool IS_RC_MODE_ACTIVE(boxId_e boxId); +void rcModeUpdate(boxBitmask_t *newState); + +bool isAirmodeActive(void); +bool isAntiGravityModeActive(void); + +bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); +void updateActivatedModes(void); +bool isModeActivationConditionPresent(boxId_e modeId); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 6907ab8058..a8bfe6772d 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -40,6 +40,7 @@ #include "fc/controlrate_profile.h" #include "fc/fc_core.h" #include "fc/rc_adjustments.h" +#include "fc/rc_controls.h" #include "fc/settings.h" #include "flight/altitude.h" diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c index ada46f6d4c..7ae6f62496 100644 --- a/src/main/flight/altitude.c +++ b/src/main/flight/altitude.c @@ -33,6 +33,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/altitude.h" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index bf21c74d87..a9c9e9c877 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -31,6 +31,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index de7c331f70..f5fcd647e5 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -40,6 +40,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/fc_core.h" @@ -52,16 +53,6 @@ #include "sensors/battery.h" - -PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); - -PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, - .deadband3d_low = 1406, - .deadband3d_high = 1514, - .neutral3d = 1460, - .deadband3d_throttle = 50 -); - PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #ifndef TARGET_DEFAULT_MIXER diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index d5ce24a515..1ee364b87b 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -37,7 +37,7 @@ #include "fc/config.h" #include "fc/fc_core.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index d137127416..72b08821e6 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -38,6 +38,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/imu.h" diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 9b0154d1a0..0be72d3a94 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -44,6 +44,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "flight/failsafe.h" diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index 76007d6172..3715b60e00 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -17,7 +17,7 @@ #pragma once -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #define MAX_CHANNEL_ACTIVATION_CONDITION_COUNT 10 diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 9a9ddb0f4b..702898bb9b 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -23,7 +23,8 @@ typedef enum { MSP_RESULT_ACK = 1, MSP_RESULT_ERROR = -1, - MSP_RESULT_NO_REPLY = 0 + MSP_RESULT_NO_REPLY = 0, + MSP_RESULT_CMD_UNKNOWN = -2, // don't know how to process command, try next handler } mspResult_e; typedef enum { diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8deb8010c..46c5f95208 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,6 +41,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "flight/failsafe.h" @@ -287,6 +288,7 @@ void rxInit(void) rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; // Initialize ARM switch to OFF position when arming via switch is defined + // TODO - move to rc_mode.c for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { const modeActivationCondition_t *modeActivationCondition = modeActivationConditions(i); if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 970fab00f8..34a62f3849 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -483,7 +483,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationConditions(0), currentPidProfile); + useRcControlsConfig(currentPidProfile); } else { ret = BST_FAILED; } diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 4ba3a85876..aff23afdea 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -27,6 +27,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" +#include "fc/rc_modes.h" #include "fc/rc_controls.h" #include "flight/failsafe.h" diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 34e8878ad7..5f32767a66 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -38,7 +38,7 @@ #include "io/gps.h" #include "io/serial.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "io/gps.h" diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 39b8470c03..304271d8c9 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -35,7 +35,7 @@ #include "io/serial.h" #include "fc/config.h" -#include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "msp/msp_serial.h" diff --git a/src/test/Makefile b/src/test/Makefile index 7528b6f90d..4d72128342 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -106,7 +106,11 @@ parameter_groups_unittest_SRC := \ rc_controls_unittest_SRC := \ $(USER_DIR)/fc/rc_controls.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/config/parameter_group.c \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/fc/rc_adjustments.c \ + $(USER_DIR)/fc/rc_modes.c \ rx_crsf_unittest_SRC := \ @@ -125,6 +129,8 @@ rx_ranges_unittest_SRC := \ rx_rx_unittest_SRC := \ $(USER_DIR)/rx/rx.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/config/feature.c diff --git a/src/test/unit/rc_controls_unittest.cc.txt b/src/test/unit/rc_controls_unittest.cc similarity index 65% rename from src/test/unit/rc_controls_unittest.cc.txt rename to src/test/unit/rc_controls_unittest.cc index d21defb795..06e107a209 100644 --- a/src/test/unit/rc_controls_unittest.cc.txt +++ b/src/test/unit/rc_controls_unittest.cc @@ -26,121 +26,123 @@ extern "C" { #include "common/maths.h" #include "common/axis.h" + #include "config/parameter_group_ids.h" + + #include "blackbox/blackbox.h" + #include "drivers/sensor.h" - #include "drivers/accgyro.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "io/beeper.h" - #include "io/escservo.h" - #include "io/rc_controls.h" #include "rx/rx.h" #include "flight/pid.h" + + #include "fc/controlrate_profile.h" + #include "fc/rc_modes.h" + #include "fc/rc_adjustments.h" + + #include "fc/rc_controls.h" } #include "unittest_macros.h" #include "gtest/gtest.h" -extern "C" { -extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); -} - class RcControlsModesTest : public ::testing::Test { protected: - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - virtual void SetUp() { - memset(&modeActivationConditions, 0, sizeof(modeActivationConditions)); } }; TEST_F(RcControlsModesTest, updateActivatedModesWithAllInputsAtMidde) { // given - rcModeActivationMask = 0; + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); // and memset(&rxRuntimeConfig, 0, sizeof(rxRuntimeConfig_t)); - rxRuntimeConfig.auxChannelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; + rxRuntimeConfig.channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } // when - updateActivatedModes(modeActivationConditions); + updateActivatedModes(); // then - for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { + for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) { #ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); #endif - EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index)); + EXPECT_EQ(false, IS_RC_MODE_ACTIVE((boxId_e)index)); } } TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues) { // given - modeActivationConditions[0].modeId = (boxId_e)0; - modeActivationConditions[0].auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1700); - modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(0)->modeId = (boxId_e)0; + modeActivationConditionsMutable(0)->auxChannelIndex = AUX1 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1700); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - modeActivationConditions[1].modeId = (boxId_e)1; - modeActivationConditions[1].auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[1].range.startStep = CHANNEL_VALUE_TO_STEP(1300); - modeActivationConditions[1].range.endStep = CHANNEL_VALUE_TO_STEP(1700); + modeActivationConditionsMutable(1)->modeId = (boxId_e)1; + modeActivationConditionsMutable(1)->auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1300); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(1700); - modeActivationConditions[2].modeId = (boxId_e)2; - modeActivationConditions[2].auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[2].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[2].range.endStep = CHANNEL_VALUE_TO_STEP(1200); + modeActivationConditionsMutable(2)->modeId = (boxId_e)2; + modeActivationConditionsMutable(2)->auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1200); - modeActivationConditions[3].modeId = (boxId_e)3; - modeActivationConditions[3].auxChannelIndex = AUX4 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[3].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[3].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(3)->modeId = (boxId_e)3; + modeActivationConditionsMutable(3)->auxChannelIndex = AUX4 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(3)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(3)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - modeActivationConditions[4].modeId = (boxId_e)4; - modeActivationConditions[4].auxChannelIndex = AUX5 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[4].range.startStep = CHANNEL_VALUE_TO_STEP(900); - modeActivationConditions[4].range.endStep = CHANNEL_VALUE_TO_STEP(925); + modeActivationConditionsMutable(4)->modeId = (boxId_e)4; + modeActivationConditionsMutable(4)->auxChannelIndex = AUX5 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(4)->range.startStep = CHANNEL_VALUE_TO_STEP(900); + modeActivationConditionsMutable(4)->range.endStep = CHANNEL_VALUE_TO_STEP(925); - EXPECT_EQ(0, modeActivationConditions[4].range.startStep); - EXPECT_EQ(1, modeActivationConditions[4].range.endStep); + EXPECT_EQ(0, modeActivationConditions(4)->range.startStep); + EXPECT_EQ(1, modeActivationConditions(4)->range.endStep); - modeActivationConditions[5].modeId = (boxId_e)5; - modeActivationConditions[5].auxChannelIndex = AUX6 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[5].range.startStep = CHANNEL_VALUE_TO_STEP(2075); - modeActivationConditions[5].range.endStep = CHANNEL_VALUE_TO_STEP(2100); + modeActivationConditionsMutable(5)->modeId = (boxId_e)5; + modeActivationConditionsMutable(5)->auxChannelIndex = AUX6 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(5)->range.startStep = CHANNEL_VALUE_TO_STEP(2075); + modeActivationConditionsMutable(5)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); - EXPECT_EQ(47, modeActivationConditions[5].range.startStep); - EXPECT_EQ(48, modeActivationConditions[5].range.endStep); + EXPECT_EQ(47, modeActivationConditions(5)->range.startStep); + EXPECT_EQ(48, modeActivationConditions(5)->range.endStep); - modeActivationConditions[6].modeId = (boxId_e)6; - modeActivationConditions[6].auxChannelIndex = AUX7 - NON_AUX_CHANNEL_COUNT; - modeActivationConditions[6].range.startStep = CHANNEL_VALUE_TO_STEP(925); - modeActivationConditions[6].range.endStep = CHANNEL_VALUE_TO_STEP(950); + modeActivationConditionsMutable(6)->modeId = (boxId_e)6; + modeActivationConditionsMutable(6)->auxChannelIndex = AUX7 - NON_AUX_CHANNEL_COUNT; + modeActivationConditionsMutable(6)->range.startStep = CHANNEL_VALUE_TO_STEP(925); + modeActivationConditionsMutable(6)->range.endStep = CHANNEL_VALUE_TO_STEP(950); - EXPECT_EQ(1, modeActivationConditions[6].range.startStep); - EXPECT_EQ(2, modeActivationConditions[6].range.endStep); + EXPECT_EQ(1, modeActivationConditions(6)->range.startStep); + EXPECT_EQ(2, modeActivationConditions(6)->range.endStep); // and - rcModeActivationMask = 0; + boxBitmask_t mask; + memset(&mask, 0, sizeof(mask)); + rcModeUpdate(&mask); // and memset(&rxRuntimeConfig, 0, sizeof(rxRuntimeConfig_t)); - rxRuntimeConfig.auxChannelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; + rxRuntimeConfig.channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT; // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -163,33 +165,28 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV expectedMask |= (0 << 6); // when - updateActivatedModes(modeActivationConditions); + updateActivatedModes(); // then - for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { + for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) { #ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); #endif - EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index)); + EXPECT_EQ((bool)(expectedMask & (1 << index)), IS_RC_MODE_ACTIVE((boxId_e)index)); } } enum { - COUNTER_GENERATE_PITCH_ROLL_CURVE = 0, COUNTER_QUEUE_CONFIRMATION_BEEP, COUNTER_CHANGE_CONTROL_RATE_PROFILE }; -#define CALL_COUNT_ITEM_COUNT 3 +#define CALL_COUNT_ITEM_COUNT 2 static int callCounts[CALL_COUNT_ITEM_COUNT]; #define CALL_COUNTER(item) (callCounts[item]) extern "C" { -void generatePitchRollCurve(controlRateConfig_t *) { - callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++; -} - void beeperConfirmationBeeps(uint8_t) { callCounts[COUNTER_QUEUE_CONFIRMATION_BEEP]++; } @@ -223,17 +220,19 @@ void resetMillis(void) { #define DEFAULT_MIN_CHECK 1100 #define DEFAULT_MAX_CHECK 1900 -rxConfig_t rxConfig; +extern "C" { + PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); + void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig); -extern uint8_t adjustmentStateMask; -extern adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; - -static const adjustmentConfig_t rateAdjustmentConfig = { - .adjustmentFunction = ADJUSTMENT_RC_RATE, - .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } -}; + extern uint8_t adjustmentStateMask; + extern adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; + static const adjustmentConfig_t rateAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_RC_RATE, + .mode = ADJUSTMENT_MODE_STEP, + .data = { 1 } + }; +} class RcControlsAdjustmentsTest : public ::testing::Test { protected: controlRateConfig_t controlRateConfig = { @@ -251,10 +250,10 @@ protected: adjustmentStateMask = 0; memset(&adjustmentStates, 0, sizeof(adjustmentStates)); - memset(&rxConfig, 0, sizeof (rxConfig)); - rxConfig.mincheck = DEFAULT_MIN_CHECK; - rxConfig.maxcheck = DEFAULT_MAX_CHECK; - rxConfig.midrc = 1500; + PG_RESET_CURRENT(rxConfig); + rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; + rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; + rxConfigMutable()->midrc = 1500; controlRateConfig.rcRate8 = 90; controlRateConfig.rcExpo8 = 0; @@ -276,8 +275,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -286,11 +284,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsSticksInMiddle) resetMillis(); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 90); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 0); EXPECT_EQ(adjustmentStateMask, 0); } @@ -310,10 +307,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp }; // and - memset(&rxConfig, 0, sizeof (rxConfig)); - rxConfig.mincheck = DEFAULT_MIN_CHECK; - rxConfig.maxcheck = DEFAULT_MAX_CHECK; - rxConfig.midrc = 1500; + PG_RESET_CURRENT(rxConfig); + rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; + rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; + rxConfigMutable()->midrc = 1500; // and adjustmentStateMask = 0; @@ -321,8 +318,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp configureAdjustment(0, AUX3 - NON_AUX_CHANNEL_COUNT, &rateAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -341,15 +337,13 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 496; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 91); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); - // // now pretend a short amount of time has passed, but not enough time to allow the value to have been increased // @@ -358,7 +352,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 497; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -380,7 +374,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp ~(1 << 0); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); EXPECT_EQ(controlRateConfig.rcRate8, 91); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -400,11 +394,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 499; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 2); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); @@ -417,7 +410,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 500; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); @@ -431,7 +424,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 997; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 92); @@ -447,11 +440,10 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp fixedMillis = 998; // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(controlRateConfig.rcRate8, 93); - EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3); EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 3); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } @@ -459,7 +451,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp static const adjustmentConfig_t rateProfileAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_RATE_PROFILE, .mode = ADJUSTMENT_MODE_SELECT, - .data = { { 3 } } + .data = { 3 } }; TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) @@ -469,8 +461,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) configureAdjustment(adjustmentIndex, AUX4 - NON_AUX_CHANNEL_COUNT, &rateProfileAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -486,7 +477,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) (1 << adjustmentIndex); // when - processRcAdjustments(&controlRateConfig, &rxConfig); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); @@ -497,61 +488,54 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawPAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_P, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawIAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_I, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; static const adjustmentConfig_t pidYawDAdjustmentConfig = { .adjustmentFunction = ADJUSTMENT_YAW_D, .mode = ADJUSTMENT_MODE_STEP, - .data = { { 1 } } + .data = { 1 } }; TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) { // given - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); - - escAndServoConfig_t escAndServoConfig; - memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); - pidProfile_t pidProfile; memset(&pidProfile, 0, sizeof (pidProfile)); - pidProfile.pidController = 0; - pidProfile.P8[PIDPITCH] = 0; - pidProfile.P8[PIDROLL] = 5; - pidProfile.P8[YAW] = 7; - pidProfile.I8[PIDPITCH] = 10; - pidProfile.I8[PIDROLL] = 15; - pidProfile.I8[YAW] = 17; - pidProfile.D8[PIDPITCH] = 20; - pidProfile.D8[PIDROLL] = 25; - pidProfile.D8[YAW] = 27; - + pidProfile.pid[PID_PITCH].P = 0; + pidProfile.pid[PID_PITCH].I = 10; + pidProfile.pid[PID_PITCH].D = 20; + pidProfile.pid[PID_ROLL].P = 5; + pidProfile.pid[PID_ROLL].I = 15; + pidProfile.pid[PID_ROLL].D = 25; + pidProfile.pid[PID_YAW].P = 7; + pidProfile.pid[PID_YAW].I = 17; + pidProfile.pid[PID_YAW].D = 27; + useAdjustmentConfig(&pidProfile); // and controlRateConfig_t controlRateConfig; memset(&controlRateConfig, 0, sizeof (controlRateConfig)); @@ -564,8 +548,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -588,34 +571,30 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController0) (1 << 5); // when - useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); - processRcAdjustments(&controlRateConfig, &rxConfig); + useRcControlsConfig(&pidProfile); + processRcAdjustments(&controlRateConfig); // then EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(1, pidProfile.P8[PIDPITCH]); - EXPECT_EQ(6, pidProfile.P8[PIDROLL]); - EXPECT_EQ(8, pidProfile.P8[YAW]); - EXPECT_EQ(11, pidProfile.I8[PIDPITCH]); - EXPECT_EQ(16, pidProfile.I8[PIDROLL]); - EXPECT_EQ(18, pidProfile.I8[YAW]); - EXPECT_EQ(21, pidProfile.D8[PIDPITCH]); - EXPECT_EQ(26, pidProfile.D8[PIDROLL]); - EXPECT_EQ(28, pidProfile.D8[YAW]); + EXPECT_EQ(1, pidProfile.pid[PID_PITCH].P); + EXPECT_EQ(11, pidProfile.pid[PID_PITCH].I); + EXPECT_EQ(21, pidProfile.pid[PID_PITCH].D); + EXPECT_EQ(6, pidProfile.pid[PID_ROLL].P); + EXPECT_EQ(16, pidProfile.pid[PID_ROLL].I); + EXPECT_EQ(26, pidProfile.pid[PID_ROLL].D); + EXPECT_EQ(8, pidProfile.pid[PID_YAW].P); + EXPECT_EQ(18, pidProfile.pid[PID_YAW].I); + EXPECT_EQ(28, pidProfile.pid[PID_YAW].D); } +#if 0 // only one PID controller + TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) { // given - modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; - memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); - - escAndServoConfig_t escAndServoConfig; - memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); - pidProfile_t pidProfile; memset(&pidProfile, 0, sizeof (pidProfile)); pidProfile.pidController = 2; @@ -628,6 +607,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) pidProfile.D_f[PIDPITCH] = 20.0f; pidProfile.D_f[PIDROLL] = 25.0f; pidProfile.D_f[PIDYAW] = 27.0f; + useAdjustmentConfig(&pidProfile); // and controlRateConfig_t controlRateConfig; @@ -641,8 +621,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); // and - uint8_t index; - for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + for (int index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { rcData[index] = PWM_RANGE_MIDDLE; } @@ -665,7 +644,7 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) (1 << 5); // when - useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + useRcControlsConfig(&escAndServoConfig, &pidProfile); processRcAdjustments(&controlRateConfig, &rxConfig); // then @@ -685,33 +664,39 @@ TEST_F(RcControlsAdjustmentsTest, processPIDIncreasePidController2) } +#endif + extern "C" { void saveConfigAndNotify(void) {} -void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {} -void changeProfile(uint8_t) {} +void generateThrottleCurve(void) {} +void changePidProfile(uint8_t) {} +void pidInitConfig(const pidProfile_t *) {} void accSetCalibrationCycles(uint16_t) {} -void gyroSetCalibrationCycles(uint16_t) {} +void gyroStartCalibration(void) {} void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} bool feature(uint32_t) { return false;} bool sensors(uint32_t) { return false;} void mwArm(void) {} void mwDisarm(void) {} -void displayDisablePageCycling() {} -void displayEnablePageCycling() {} +void dashboardDisablePageCycling() {} +void dashboardEnablePageCycling() {} bool failsafeIsActive() { return false; } bool rxIsReceivingSignal() { return true; } -uint8_t getCurrentControlRateProfile(void) { +uint8_t getCurrentControlRateProfileIndex(void) { return 0; } void GPS_reset_home_position(void) {} void baroSetCalibrationCycles(uint16_t) {} +void blackboxLogEvent(FlightLogEvent, flightLogEventData_t *) {} + uint8_t armingFlags = 0; int16_t heading; uint8_t stateFlags = 0; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; rxRuntimeConfig_t rxRuntimeConfig; +PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); } diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index cbaa95b2d9..db4b9c6515 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -24,11 +24,13 @@ extern "C" { #include "platform.h" #include "rx/rx.h" - #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "common/maths.h" + #include "common/utils.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" + #include "io/beeper.h" uint32_t rcModeActivationMask; @@ -42,8 +44,6 @@ extern "C" { PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = 0 ); - PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); - } #include "unittest_macros.h" diff --git a/src/test/unit/target.h b/src/test/unit/target.h index 21a6a2fce2..b9cc9faf7d 100644 --- a/src/test/unit/target.h +++ b/src/test/unit/target.h @@ -63,6 +63,7 @@ #define SERIAL_PORT_COUNT 8 #define DEFAULT_AUX_CHANNEL_COUNT MAX_AUX_CHANNEL_COUNT +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 // needed for unittest #define TARGET_BOARD_IDENTIFIER "TEST" From a10e1d05ed33202a2cf7d0e26b2d125919282cae Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 18 Jun 2017 12:52:58 +1200 Subject: [PATCH 2/6] Fixed tests. --- src/test/Makefile | 17 +++++++++++++---- src/test/unit/flight_failsafe_unittest.cc | 19 +++++++++++++------ src/test/unit/flight_imu_unittest.cc | 7 +++++++ src/test/unit/rx_ranges_unittest.cc | 3 +-- 4 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 4d72128342..6a02179fdd 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -68,13 +68,18 @@ encoding_unittest_SRC := \ flight_failsafe_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/flight/failsafe.c flight_imu_unittest_SRC := \ - $(USER_DIR)/flight/imu.c \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/config/feature.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/flight/altitude.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/flight/imu.c flight_mixer_unittest := \ @@ -93,6 +98,8 @@ io_serial_unittest_SRC := \ ledstrip_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/io/ledstrip.c @@ -123,8 +130,10 @@ rx_ibus_unittest_SRC := \ rx_ranges_unittest_SRC := \ - $(USER_DIR)/rx/rx.c \ - $(USER_DIR)/common/maths.c + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/common/maths.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/rx/rx.c rx_rx_unittest_SRC := \ diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 5976c96b61..e7b88145e0 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -29,14 +29,19 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" + #include "common/bitarray.h" #include "fc/runtime_config.h" - - #include "io/beeper.h" + #include "fc/rc_modes.h" #include "fc/rc_controls.h" - #include "rx/rx.h" #include "flight/failsafe.h" + + #include "io/beeper.h" + + #include "rx/rx.h" + + extern boxBitmask_t rcModeActivationMask; } #include "unittest_macros.h" @@ -304,7 +309,10 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // and throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure failsafeConfigMutable()->failsafe_kill_switch = true; // configure AUX switch as kill switch - ACTIVATE_RC_MODE(BOXFAILSAFE); // and activate it + boxBitmask_t newMask; + memset(&newMask, 0, sizeof(newMask)); + bitArraySet(&newMask, BOXFAILSAFE); + rcModeUpdate(&newMask); // activate BOXFAILSAFE mode sysTickUptime = 0; // restart time from 0 failsafeOnValidDataReceived(); // set last valid sample at current time sysTickUptime = PERIOD_RXDATA_FAILURE + 1; // adjust time to point just past the failure time to @@ -324,7 +332,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to failsafeOnValidDataReceived(); // cause a recovered link - rcModeActivationMask = DE_ACTIVATE_ALL_BOXES; // BOXFAILSAFE must be off (kill switch) + memset(&rcModeActivationMask, 0, sizeof(rcModeActivationMask)); // BOXFAILSAFE must be off (kill switch) // when failsafeUpdateState(); @@ -404,7 +412,6 @@ extern "C" { int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t armingFlags; float rcCommand[4]; -uint32_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; bool isUsingSticksToArm = true; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index fc626b5e0e..df30642aea 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -28,6 +28,7 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" + #include "config/feature.h" #include "config/parameter_group_ids.h" #include "drivers/accgyro/accgyro.h" @@ -57,6 +58,12 @@ extern "C" { PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); + + PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0); + + PG_RESET_TEMPLATE(featureConfig_t, featureConfig, + .enabledFeatures = 0 + ); } #include "unittest_macros.h" diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 59a4ecf8d4..c75d26ec84 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "common/maths.h" #include "config/parameter_group_ids.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "rx/rx.h" } @@ -38,8 +39,6 @@ extern "C" { uint32_t rcModeActivationMask; extern uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range); - -PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); } #define RANGE_CONFIGURATION(min, max) new (rxChannelRangeConfig_t) {min, max} From 75511249d8e330c37332f4f6fe41982c4c4cf173 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 18 Jun 2017 16:33:55 +0100 Subject: [PATCH 3/6] Merge pull request #2867 from cleanflight/fix-flight-mode-flags CF/BF - Fix missing initialiser in flightModeFlags --- src/main/fc/fc_msp.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index fd7c245257..749492918e 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -435,6 +435,7 @@ void initActiveBoxIds(void) static 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; From 0227b7fb28e30cc706702f6a33d549b508be816c Mon Sep 17 00:00:00 2001 From: Hydra Date: Sat, 17 Jun 2017 21:02:59 +0100 Subject: [PATCH 4/6] CF/BF - Update re-instated unit tests due to rc_modes changes. --- src/test/unit/flight_failsafe_unittest.cc | 6 ++++-- src/test/unit/flight_imu_unittest.cc | 6 ++++++ src/test/unit/ledstrip_unittest.cc | 1 + src/test/unit/rx_ibus_unittest.cc | 1 + src/test/unit/rx_ranges_unittest.cc | 5 +++++ 5 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index e7b88145e0..afc75d113a 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -309,10 +309,11 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) // and throttleStatus = THROTTLE_HIGH; // throttle HIGH to go for a failsafe landing procedure failsafeConfigMutable()->failsafe_kill_switch = true; // configure AUX switch as kill switch + boxBitmask_t newMask; - memset(&newMask, 0, sizeof(newMask)); bitArraySet(&newMask, BOXFAILSAFE); rcModeUpdate(&newMask); // activate BOXFAILSAFE mode + sysTickUptime = 0; // restart time from 0 failsafeOnValidDataReceived(); // set last valid sample at current time sysTickUptime = PERIOD_RXDATA_FAILURE + 1; // adjust time to point just past the failure time to @@ -332,7 +333,8 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent) sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to failsafeOnValidDataReceived(); // cause a recovered link - memset(&rcModeActivationMask, 0, sizeof(rcModeActivationMask)); // BOXFAILSAFE must be off (kill switch) + memset(&newMask, 0, sizeof(newMask)); + rcModeUpdate(&newMask); // BOXFAILSAFE must be off (kill switch) // when failsafeUpdateState(); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index df30642aea..f4cbe02c1e 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -43,6 +43,7 @@ extern "C" { #include "fc/runtime_config.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "rx/rx.h" @@ -234,6 +235,11 @@ bool sensors(uint32_t mask) return false; }; +bool feature(uint32_t mask) { + UNUSED(mask); + return false; +} + uint32_t millis(void) { return 0; } uint32_t micros(void) { return 0; } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 62ffa90f43..d4d47cb1d9 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -35,6 +35,7 @@ extern "C" { #include "fc/config.h" #include "fc/rc_controls.h" + #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "drivers/light_ws2811strip.h" diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index 7f675ff95f..eb2453dc52 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -28,6 +28,7 @@ extern "C" { #include "telemetry/ibus_shared.h" #include "telemetry/telemetry.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "sensors/barometer.h" #include "sensors/battery.h" } diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index c75d26ec84..4edffc3757 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -217,5 +217,10 @@ void failsafeOnValidDataFailed(void) { } +bool IS_RC_MODE_ACTIVE(boxId_e) +{ + return false; +} + } From d93218f21c9be923c9d6ccc5e583aa4ee9c4b317 Mon Sep 17 00:00:00 2001 From: Hydra Date: Sun, 18 Jun 2017 17:13:54 +0100 Subject: [PATCH 5/6] CF/BF - Fix incorrect use of pointer. --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 749492918e..28f6ea4258 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -435,7 +435,7 @@ void initActiveBoxIds(void) static int packFlightModeFlags(boxBitmask_t *mspFlightModeFlags) { // Serialize the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES - memset(&mspFlightModeFlags, 0, sizeof(boxBitmask_t)); + memset(mspFlightModeFlags, 0, sizeof(boxBitmask_t)); // enabled BOXes, bits indexed by boxId_e boxBitmask_t boxEnabledMask; From f380c3d774da8eed2098854fb9e022120b10c4fd Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 19 Jun 2017 12:36:40 +1200 Subject: [PATCH 6/6] Fixed unit tests. --- src/test/unit/flight_imu_unittest.cc | 5 ----- src/test/unit/rc_controls_unittest.cc | 5 +++-- src/test/unit/rx_ranges_unittest.cc | 5 ----- 3 files changed, 3 insertions(+), 12 deletions(-) diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index f4cbe02c1e..b9fc11a320 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -235,11 +235,6 @@ bool sensors(uint32_t mask) return false; }; -bool feature(uint32_t mask) { - UNUSED(mask); - return false; -} - uint32_t millis(void) { return 0; } uint32_t micros(void) { return 0; } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 06e107a209..415f9deb1c 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -26,6 +26,7 @@ extern "C" { #include "common/maths.h" #include "common/axis.h" + #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "blackbox/blackbox.h" @@ -250,7 +251,7 @@ protected: adjustmentStateMask = 0; memset(&adjustmentStates, 0, sizeof(adjustmentStates)); - PG_RESET_CURRENT(rxConfig); + PG_RESET(rxConfig); rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; rxConfigMutable()->midrc = 1500; @@ -307,7 +308,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp }; // and - PG_RESET_CURRENT(rxConfig); + PG_RESET(rxConfig); rxConfigMutable()->mincheck = DEFAULT_MIN_CHECK; rxConfigMutable()->maxcheck = DEFAULT_MAX_CHECK; rxConfigMutable()->midrc = 1500; diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 4edffc3757..c75d26ec84 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -217,10 +217,5 @@ void failsafeOnValidDataFailed(void) { } -bool IS_RC_MODE_ACTIVE(boxId_e) -{ - return false; -} - }