From 6de1c32d9db2d7b78bf02aac203b1bf3f63cb764 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 10 Jul 2018 00:49:17 +1200 Subject: [PATCH] Added shadow copies for CLI and MSP. --- src/main/blackbox/blackbox.c | 6 +- src/main/cms/cms_menu_ledstrip.c | 6 +- src/main/config/feature.c | 16 ++-- src/main/config/feature.h | 12 +-- src/main/fc/config.c | 86 +++++++++++---------- src/main/fc/config.h | 1 + src/main/fc/fc_core.c | 26 +++---- src/main/fc/fc_init.c | 48 ++++++------ src/main/fc/fc_rc.c | 6 +- src/main/fc/fc_tasks.c | 24 +++--- src/main/fc/rc_controls.c | 6 +- src/main/fc/rc_modes.c | 2 +- src/main/flight/mixer.c | 12 +-- src/main/flight/servos.c | 10 +-- src/main/interface/cli.c | 52 +++++++++---- src/main/interface/msp.c | 36 +++++++-- src/main/interface/msp_box.c | 18 ++--- src/main/io/beeper.c | 2 +- src/main/io/dashboard.c | 2 +- src/main/io/gps.c | 4 +- src/main/io/osd.c | 12 +-- src/main/io/vtx_rtc6705.c | 2 +- src/main/osd_slave/osd_slave_init.c | 6 +- src/main/rx/cc2500_frsky_d.c | 2 +- src/main/rx/cc2500_frsky_x.c | 2 +- src/main/rx/pwm.c | 4 +- src/main/rx/rx.c | 20 ++--- src/main/rx/spektrum.c | 4 +- src/main/sensors/acceleration.c | 2 +- src/main/sensors/battery.c | 6 +- src/main/sensors/esc_sensor.c | 2 +- src/main/sensors/gyro.c | 2 +- src/main/target/ALIENFLIGHTF3/config.c | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 2 +- src/main/target/ALIENFLIGHTNGF7/config.c | 2 +- src/main/target/ALIENWHOOP/config.c | 2 +- src/main/target/BLUEJAYF4/initialisation.c | 2 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 6 +- src/main/target/FF_FORTINIF4/config.c | 2 +- src/main/target/FF_PIKOBLX/config.c | 2 +- src/main/target/SITL/target.c | 2 +- src/main/telemetry/crsf.c | 2 +- src/main/telemetry/frsky_hub.c | 2 +- src/main/telemetry/ibus_shared.c | 2 +- src/main/telemetry/smartport.c | 8 +- src/test/unit/arming_prevention_unittest.cc | 2 +- src/test/unit/battery_unittest.cc.txt | 2 +- src/test/unit/blackbox_unittest.cc | 2 +- src/test/unit/flight_failsafe_unittest.cc | 2 +- src/test/unit/flight_mixer_unittest.cc.txt | 2 +- src/test/unit/ledstrip_unittest.cc | 2 +- src/test/unit/rc_controls_unittest.cc | 2 +- src/test/unit/rcdevice_unittest.cc | 2 +- src/test/unit/rx_ranges_unittest.cc | 4 +- src/test/unit/telemetry_crsf_unittest.cc | 2 +- src/test/unit/telemetry_ibus_unittest.cc | 2 +- 56 files changed, 277 insertions(+), 222 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 7395cf166..89b1b7ef6 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -448,7 +448,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) #endif case FLIGHT_LOG_FIELD_CONDITION_RSSI: - return rxConfig()->rssi_channel > 0 || featureConfigured(FEATURE_RSSI_ADC); + return rxConfig()->rssi_channel > 0 || featureIsEnabled(FEATURE_RSSI_ADC); case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME: return blackboxConfig()->p_ratio != 1; @@ -1488,7 +1488,7 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs) writeInterframe(); } #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { if (blackboxShouldLogGpsHomeFrame()) { writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); @@ -1554,7 +1554,7 @@ void blackboxUpdate(timeUs_t currentTimeUs) if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) { #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER); } else #endif diff --git a/src/main/cms/cms_menu_ledstrip.c b/src/main/cms/cms_menu_ledstrip.c index f8ef00f7f..b6e5423db 100644 --- a/src/main/cms/cms_menu_ledstrip.c +++ b/src/main/cms/cms_menu_ledstrip.c @@ -48,7 +48,7 @@ static uint8_t cmsx_FeatureLedstrip; static long cmsx_Ledstrip_FeatureRead(void) { if (!featureRead) { - cmsx_FeatureLedstrip = featureConfigured(FEATURE_LED_STRIP) ? 1 : 0; + cmsx_FeatureLedstrip = featureIsEnabled(FEATURE_LED_STRIP) ? 1 : 0; featureRead = true; } @@ -60,9 +60,9 @@ static long cmsx_Ledstrip_FeatureWriteback(const OSD_Entry *self) UNUSED(self); if (featureRead) { if (cmsx_FeatureLedstrip) - featureSet(FEATURE_LED_STRIP); + featureEnable(FEATURE_LED_STRIP); else - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); } return 0; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index f2e531497..9135d634b 100644 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -35,32 +35,32 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_DYNAMIC_FILTER | FEATURE_ANTI_GRAVITY, ); -void featureSetLocal(const uint32_t mask, uint32_t *features) +void featureSet(const uint32_t mask, uint32_t *features) { *features |= mask; } -void featureClearLocal(const uint32_t mask, uint32_t *features) +void featureClear(const uint32_t mask, uint32_t *features) { *features &= ~(mask); } -bool featureConfigured(const uint32_t mask) +bool featureIsEnabled(const uint32_t mask) { return featureConfig()->enabledFeatures & mask; } -void featureSet(const uint32_t mask) +void featureEnable(const uint32_t mask) { - featureSetLocal(mask, &featureConfigMutable()->enabledFeatures); + featureSet(mask, &featureConfigMutable()->enabledFeatures); } -void featureClear(const uint32_t mask) +void featureDisable(const uint32_t mask) { - featureClearLocal(mask, &featureConfigMutable()->enabledFeatures); + featureClear(mask, &featureConfigMutable()->enabledFeatures); } -void featureClearAll(void) +void featureDisableAll(void) { featureConfigMutable()->enabledFeatures = 0; } diff --git a/src/main/config/feature.h b/src/main/config/feature.h index 4d0a91b3e..f8f9a1676 100644 --- a/src/main/config/feature.h +++ b/src/main/config/feature.h @@ -62,11 +62,11 @@ typedef struct featureConfig_s { PG_DECLARE(featureConfig_t, featureConfig); -bool featureConfigured(const uint32_t mask); -void featureSet(const uint32_t mask); -void featureClear(const uint32_t mask); -void featureClearAll(void); +bool featureIsEnabled(const uint32_t mask); +void featureEnable(const uint32_t mask); +void featureDisable(const uint32_t mask); +void featureDisableAll(void); uint32_t featureMask(void); -void featureSetLocal(const uint32_t mask, uint32_t *features); -void featureClearLocal(const uint32_t mask, uint32_t *features); +void featureSet(const uint32_t mask, uint32_t *features); +void featureClear(const uint32_t mask, uint32_t *features); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 9259e5595..28ffc61ad 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -174,7 +174,7 @@ static void validateAndFixConfig(void) !findSerialPortConfig(FUNCTION_GPS) && #endif true) { - featureClear(FEATURE_GPS); + featureDisable(FEATURE_GPS); } #ifndef USE_OSD_SLAVE @@ -194,7 +194,7 @@ static void validateAndFixConfig(void) } if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { - featureClear(FEATURE_3D); + featureDisable(FEATURE_3D); if (motorConfig()->mincommand < 1000) { motorConfigMutable()->mincommand = 1000; @@ -207,40 +207,40 @@ static void validateAndFixConfig(void) validateAndFixGyroConfig(); - if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) { - featureSet(DEFAULT_RX_FEATURE); + if (!(featureIsEnabled(FEATURE_RX_PARALLEL_PWM) || featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_SERIAL) || featureIsEnabled(FEATURE_RX_MSP) || featureIsEnabled(FEATURE_RX_SPI))) { + featureEnable(DEFAULT_RX_FEATURE); } - if (featureConfigured(FEATURE_RX_PPM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_PPM)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_SPI); } - if (featureConfigured(FEATURE_RX_MSP)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_MSP)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_SPI); } - if (featureConfigured(FEATURE_RX_SERIAL)) { - featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_SERIAL)) { + featureDisable(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); } #ifdef USE_RX_SPI - if (featureConfigured(FEATURE_RX_SPI)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); + if (featureIsEnabled(FEATURE_RX_SPI)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM | FEATURE_RX_MSP); } #endif // USE_RX_SPI - if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { - featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { + featureDisable(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_SPI); } #ifdef USE_SOFTSPI - if (featureConfigured(FEATURE_SOFTSPI)) { - featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL); + if (featureIsEnabled(FEATURE_SOFTSPI)) { + featureDisable(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL); batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_NONE; #if defined(STM32F10X) - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); // rssi adc needs the same ports - featureClear(FEATURE_RSSI_ADC); + featureDisable(FEATURE_RSSI_ADC); // current meter needs the same ports if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { batteryConfigMutable()->currentMeterSource = CURRENT_METER_NONE; @@ -250,14 +250,14 @@ static void validateAndFixConfig(void) #endif // USE_SOFTSPI #if defined(USE_ADC) - if (featureConfigured(FEATURE_RSSI_ADC)) { + if (featureIsEnabled(FEATURE_RSSI_ADC)) { rxConfigMutable()->rssi_channel = 0; rxConfigMutable()->rssi_src_frame_errors = false; } else #endif if (rxConfigMutable()->rssi_channel #if defined(USE_PWM) || defined(USE_PPM) - || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM) + || featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) #endif ) { rxConfigMutable()->rssi_src_frame_errors = false; @@ -291,7 +291,7 @@ static void validateAndFixConfig(void) #endif if ( - featureConfigured(FEATURE_3D) || !featureConfigured(FEATURE_GPS) + featureIsEnabled(FEATURE_3D) || !featureIsEnabled(FEATURE_GPS) #if !defined(USE_GPS) || !defined(USE_GPS_RESCUE) || true #endif @@ -308,7 +308,7 @@ static void validateAndFixConfig(void) #if defined(USE_ESC_SENSOR) if (!findSerialPortConfig(FUNCTION_ESC_SENSOR)) { - featureClear(FEATURE_ESC_SENSOR); + featureDisable(FEATURE_ESC_SENSOR); } #endif @@ -316,71 +316,71 @@ static void validateAndFixConfig(void) // I have kept them all here in one place, some could be moved to sections of code above. #ifndef USE_PPM - featureClear(FEATURE_RX_PPM); + featureDisable(FEATURE_RX_PPM); #endif #ifndef USE_SERIAL_RX - featureClear(FEATURE_RX_SERIAL); + featureDisable(FEATURE_RX_SERIAL); #endif #if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2) - featureClear(FEATURE_SOFTSERIAL); + featureDisable(FEATURE_SOFTSERIAL); #endif #ifndef USE_RANGEFINDER - featureClear(FEATURE_RANGEFINDER); + featureDisable(FEATURE_RANGEFINDER); #endif #ifndef USE_TELEMETRY - featureClear(FEATURE_TELEMETRY); + featureDisable(FEATURE_TELEMETRY); #endif #ifndef USE_PWM - featureClear(FEATURE_RX_PARALLEL_PWM); + featureDisable(FEATURE_RX_PARALLEL_PWM); #endif #ifndef USE_RX_MSP - featureClear(FEATURE_RX_MSP); + featureDisable(FEATURE_RX_MSP); #endif #ifndef USE_LED_STRIP - featureClear(FEATURE_LED_STRIP); + featureDisable(FEATURE_LED_STRIP); #endif #ifndef USE_DASHBOARD - featureClear(FEATURE_DASHBOARD); + featureDisable(FEATURE_DASHBOARD); #endif #ifndef USE_OSD - featureClear(FEATURE_OSD); + featureDisable(FEATURE_OSD); #endif #ifndef USE_SERVOS - featureClear(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING); + featureDisable(FEATURE_SERVO_TILT | FEATURE_CHANNEL_FORWARDING); #endif #ifndef USE_TRANSPONDER - featureClear(FEATURE_TRANSPONDER); + featureDisable(FEATURE_TRANSPONDER); #endif #ifndef USE_RX_SPI - featureClear(FEATURE_RX_SPI); + featureDisable(FEATURE_RX_SPI); #endif #ifndef USE_SOFTSPI - featureClear(FEATURE_SOFTSPI); + featureDisable(FEATURE_SOFTSPI); #endif #ifndef USE_ESC_SENSOR - featureClear(FEATURE_ESC_SENSOR); + featureDisable(FEATURE_ESC_SENSOR); #endif #ifndef USE_GYRO_DATA_ANALYSE - featureClear(FEATURE_DYNAMIC_FILTER); + featureDisable(FEATURE_DYNAMIC_FILTER); #endif #if !defined(USE_ADC) - featureClear(FEATURE_RSSI_ADC); + featureDisable(FEATURE_RSSI_ADC); #endif #if defined(USE_BEEPER) @@ -548,6 +548,14 @@ void writeEEPROM(void) #endif } +void writeEEPROMWithFeatures(uint32_t features) +{ + featureDisableAll(); + featureEnable(features); + + writeEEPROM(); +} + void resetEEPROM(void) { resetConfigs(); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 062163d3a..5a665b691 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -53,6 +53,7 @@ void initEEPROM(void); void resetEEPROM(void); bool readEEPROM(void); void writeEEPROM(void); +void writeEEPROMWithFeatures(uint32_t features); void ensureEEPROMStructureIsValid(void); void saveConfigAndNotify(void); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 93806febb..046403705 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -268,7 +268,7 @@ void updateArmingStatus(void) && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING)); /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */ - bool ignoreThrottle = featureConfigured(FEATURE_3D) + bool ignoreThrottle = featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE)); @@ -313,7 +313,7 @@ void disarm(void) #endif BEEP_OFF; #ifdef USE_DSHOT - if (isMotorProtocolDshot() && flipOverAfterCrashMode && !featureConfigured(FEATURE_3D)) { + if (isMotorProtocolDshot() && flipOverAfterCrashMode && !featureIsEnabled(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } #endif @@ -352,7 +352,7 @@ void tryArm(void) if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashMode = false; - if (!featureConfigured(FEATURE_3D)) { + if (!featureIsEnabled(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { @@ -360,7 +360,7 @@ void tryArm(void) #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffCheckDisabled = false; #endif - if (!featureConfigured(FEATURE_3D)) { + if (!featureIsEnabled(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } @@ -387,7 +387,7 @@ void tryArm(void) //beep to indicate arming #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { + if (featureIsEnabled(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING); @@ -517,7 +517,7 @@ void runawayTakeoffTemporaryDisable(uint8_t disableFlag) uint8_t calculateThrottlePercent(void) { uint8_t ret = 0; - if (featureConfigured(FEATURE_3D) + if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) { @@ -560,7 +560,7 @@ bool processRx(timeUs_t currentTimeUs) } // in 3D mode, we need to be able to disarm by switch at any time - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) disarm(); } @@ -615,7 +615,7 @@ bool processRx(timeUs_t currentTimeUs) // - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent // - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit bool inStableFlight = false; - if (!featureConfigured(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? + if (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running? const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle; const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT); if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit)) @@ -667,9 +667,9 @@ bool processRx(timeUs_t currentTimeUs) // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) - && featureConfigured(FEATURE_MOTOR_STOP) + && featureIsEnabled(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) - && !featureConfigured(FEATURE_3D) + && !featureIsEnabled(FEATURE_3D) && !isAirmodeActive() ) { if (isUsingSticksForArming()) { @@ -711,7 +711,7 @@ bool processRx(timeUs_t currentTimeUs) processRcStickPositions(); - if (featureConfigured(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } @@ -814,7 +814,7 @@ bool processRx(timeUs_t currentTimeUs) } #ifdef USE_TELEMETRY - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY)); if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) { mspSerialReleaseSharedTelemetryPorts(); @@ -872,7 +872,7 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) && !runawayTakeoffCheckDisabled && !flipOverAfterCrashMode && !runawayTakeoffTemporarilyDisabled - && (!featureConfigured(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { + && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) { if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 185b1b0e9..7b83feaea 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -296,7 +296,7 @@ void init(void) #endif #if defined(USE_SPEKTRUM_BIND) - if (featureConfigured(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { switch (rxConfig()->serialrx_provider) { case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM2048: @@ -327,23 +327,23 @@ void init(void) #endif #if defined(AVOID_UART1_FOR_PWM_PPM) - serialInit(featureConfigured(FEATURE_SOFTSERIAL), - featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE); #elif defined(AVOID_UART2_FOR_PWM_PPM) - serialInit(featureConfigured(FEATURE_SOFTSERIAL), - featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #elif defined(AVOID_UART3_FOR_PWM_PPM) - serialInit(featureConfigured(FEATURE_SOFTSERIAL), - featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), + featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else - serialInit(featureConfigured(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); + serialInit(featureIsEnabled(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif mixerInit(mixerConfig()->mixerMode); mixerConfigureOutput(); uint16_t idlePulse = motorConfig()->mincommand; - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { idlePulse = flight3DConfig()->neutral3d; } if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) { @@ -357,12 +357,12 @@ void init(void) if (0) {} #if defined(USE_PPM) - else if (featureConfigured(FEATURE_RX_PPM)) { + else if (featureIsEnabled(FEATURE_RX_PPM)) { ppmRxInit(ppmConfig()); } #endif #if defined(USE_PWM) - else if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { + else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { pwmRxInit(pwmConfig()); } #endif @@ -449,13 +449,13 @@ void init(void) // XXX These kind of code should goto target/config.c? // XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration. #if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2) - if (featureConfigured(FEATURE_RANGEFINDER) && featureConfigured(FEATURE_SOFTSERIAL)) { + if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL2); } #endif #if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1) - if (featureConfigured(FEATURE_RANGEFINDER) && featureConfigured(FEATURE_SOFTSERIAL)) { + if (featureIsEnabled(FEATURE_RANGEFINDER) && featureIsEnabled(FEATURE_SOFTSERIAL)) { serialRemovePort(SERIAL_PORT_SOFTSERIAL1); } #endif @@ -465,9 +465,9 @@ void init(void) adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2 - adcConfigMutable()->rssi.enabled = featureConfigured(FEATURE_RSSI_ADC); + adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC); #ifdef USE_RX_SPI - adcConfigMutable()->rssi.enabled |= (featureConfigured(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D); + adcConfigMutable()->rssi.enabled |= (featureIsEnabled(FEATURE_RX_SPI) && rxSpiConfig()->rx_spi_protocol == RX_SPI_FRSKY_D); #endif adcInit(adcConfig()); #endif @@ -496,7 +496,7 @@ void init(void) servosInit(); servoConfigureOutput(); if (isMixerUsingServos()) { - //pwm_params.useChannelForwarding = featureConfigured(FEATURE_CHANNEL_FORWARDING); + //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING); servoDevInit(&servoConfig()->dev); } servosFilterInit(); @@ -558,7 +558,7 @@ void init(void) #if defined(USE_OSD) && !defined(USE_OSD_SLAVE) //The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets - if (featureConfigured(FEATURE_OSD)) { + if (featureIsEnabled(FEATURE_OSD)) { #if defined(USE_MAX7456) // If there is a max7456 chip for the OSD then use it osdDisplayPort = max7456DisplayPortInit(vcdProfile()); @@ -587,7 +587,7 @@ void init(void) #ifdef USE_DASHBOARD // Dashbord will register with CMS by itself. - if (featureConfigured(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardInit(); } #endif @@ -598,7 +598,7 @@ void init(void) #endif #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { gpsInit(); } #endif @@ -606,19 +606,19 @@ void init(void) #ifdef USE_LED_STRIP ledStripInit(); - if (featureConfigured(FEATURE_LED_STRIP)) { + if (featureIsEnabled(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif #ifdef USE_TELEMETRY - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryInit(); } #endif #ifdef USE_ESC_SENSOR - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { escSensorInit(); } #endif @@ -628,7 +628,7 @@ void init(void) #endif #ifdef USE_TRANSPONDER - if (featureConfigured(FEATURE_TRANSPONDER)) { + if (featureIsEnabled(FEATURE_TRANSPONDER)) { transponderInit(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; @@ -711,7 +711,7 @@ void init(void) batteryInit(); // always needs doing, regardless of features. #ifdef USE_DASHBOARD - if (featureConfigured(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { #ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY dashboardShowFixedPage(PAGE_GPS); #else diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index eca6e1be2..bbcf7f1d4 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -203,7 +203,7 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate) const int rxRefreshRateMs = rxRefreshRate / 1000; const int indexMax = constrain(THROTTLE_DELTA_MS / rxRefreshRateMs, 1, THROTTLE_BUFFER_MAX); - const int16_t throttleVelocityThreshold = (featureConfigured(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; + const int16_t throttleVelocityThreshold = (featureIsEnabled(FEATURE_3D)) ? currentPidProfile->itermThrottleThreshold / 2 : currentPidProfile->itermThrottleThreshold; rcCommandThrottlePrevious[index++] = rcCommand[THROTTLE]; if (index >= indexMax) { @@ -621,7 +621,7 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) } int32_t tmp; - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - PWM_RANGE_MIN); } else { @@ -635,7 +635,7 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void) rcCommand[THROTTLE] = rcLookupThrottle(tmp); - if (featureConfigured(FEATURE_3D) && !failsafeIsActive()) { + if (featureIsEnabled(FEATURE_3D) && !failsafeIsActive()) { if (!flight3DConfig()->switched_mode3d) { if (IS_RC_MODE_ACTIVE(BOX3D)) { fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4b789945e..5bfd94122 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -216,7 +216,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs) #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) { - if (!cliMode && featureConfigured(FEATURE_TELEMETRY)) { + if (!cliMode && featureIsEnabled(FEATURE_TELEMETRY)) { subTaskTelemetryPollSensors(currentTimeUs); telemetryProcess(currentTimeUs); @@ -251,12 +251,12 @@ void fcTasksInit(void) #ifdef USE_OSD_SLAVE const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts; #else - const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || featureConfigured(FEATURE_OSD); + const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || featureIsEnabled(FEATURE_OSD); #endif setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts); #ifdef USE_TRANSPONDER - setTaskEnabled(TASK_TRANSPONDER, featureConfigured(FEATURE_TRANSPONDER)); + setTaskEnabled(TASK_TRANSPONDER, featureIsEnabled(FEATURE_TRANSPONDER)); #endif #ifdef STACK_CHECK @@ -285,7 +285,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_BEEPER, true); #endif #ifdef USE_GPS - setTaskEnabled(TASK_GPS, featureConfigured(FEATURE_GPS)); + setTaskEnabled(TASK_GPS, featureIsEnabled(FEATURE_GPS)); #endif #ifdef USE_MAG setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); @@ -294,13 +294,13 @@ void fcTasksInit(void) setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif #if defined(USE_BARO) || defined(USE_GPS) - setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || featureConfigured(FEATURE_GPS)); + setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS)); #endif #ifdef USE_DASHBOARD - setTaskEnabled(TASK_DASHBOARD, featureConfigured(FEATURE_DASHBOARD)); + setTaskEnabled(TASK_DASHBOARD, featureIsEnabled(FEATURE_DASHBOARD)); #endif #ifdef USE_TELEMETRY - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { setTaskEnabled(TASK_TELEMETRY, true); if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) { // Reschedule telemetry to 500hz for Jeti Exbus @@ -312,19 +312,19 @@ void fcTasksInit(void) } #endif #ifdef USE_LED_STRIP - setTaskEnabled(TASK_LEDSTRIP, featureConfigured(FEATURE_LED_STRIP)); + setTaskEnabled(TASK_LEDSTRIP, featureIsEnabled(FEATURE_LED_STRIP)); #endif #ifdef USE_TRANSPONDER - setTaskEnabled(TASK_TRANSPONDER, featureConfigured(FEATURE_TRANSPONDER)); + setTaskEnabled(TASK_TRANSPONDER, featureIsEnabled(FEATURE_TRANSPONDER)); #endif #ifdef USE_OSD - setTaskEnabled(TASK_OSD, featureConfigured(FEATURE_OSD) && osdInitialized()); + setTaskEnabled(TASK_OSD, featureIsEnabled(FEATURE_OSD) && osdInitialized())); #endif #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif #ifdef USE_ESC_SENSOR - setTaskEnabled(TASK_ESC_SENSOR, featureConfigured(FEATURE_ESC_SENSOR)); + setTaskEnabled(TASK_ESC_SENSOR, featureIsEnabled(FEATURE_ESC_SENSOR)); #endif #ifdef USE_ADC_INTERNAL setTaskEnabled(TASK_ADC_INTERNAL, true); @@ -336,7 +336,7 @@ void fcTasksInit(void) #ifdef USE_MSP_DISPLAYPORT setTaskEnabled(TASK_CMS, true); #else - setTaskEnabled(TASK_CMS, featureConfigured(FEATURE_OSD) || featureConfigured(FEATURE_DASHBOARD)); + setTaskEnabled(TASK_CMS, featureIsEnabled(FEATURE_OSD) || featureIsEnabled(FEATURE_DASHBOARD)); #endif #endif #ifdef USE_VTX_CONTROL diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index eea48c217..d67d3baaf 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -113,7 +113,7 @@ bool areSticksInApModePosition(uint16_t ap_mode) throttleStatus_e calculateThrottleStatus(void) { - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (IS_RC_MODE_ACTIVE(BOX3D) || flight3DConfig()->switched_mode3d) { if (rcData[THROTTLE] < rxConfig()->mincheck) { return THROTTLE_LOW; @@ -240,7 +240,7 @@ void processRcStickPositions() gyroStartCalibration(false); #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { GPS_reset_home_position(); } #endif @@ -253,7 +253,7 @@ void processRcStickPositions() return; } - if (featureConfigured(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { // Inflight ACC Calibration handleInflightCalibrationStickPosition(); return; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index ce2212dbb..44a059f29 100644 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -59,7 +59,7 @@ void rcModeUpdate(boxBitmask_t *newState) } bool isAirmodeActive(void) { - return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || featureConfigured(FEATURE_AIRMODE)); + return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || featureIsEnabled(FEATURE_AIRMODE)); } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 38a72a55c..a80093aac 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -383,7 +383,7 @@ void initEscEndpoints(void) case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: disarmMotorOutput = DSHOT_DISARM_COMMAND; - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); } else { motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); @@ -395,7 +395,7 @@ void initEscEndpoints(void) break; #endif default: - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { disarmMotorOutput = flight3DConfig()->neutral3d; motorOutputLow = flight3DConfig()->limit3d_low; motorOutputHigh = flight3DConfig()->limit3d_high; @@ -536,7 +536,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode float currentThrottleInputRange = 0; - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) { rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. } @@ -708,7 +708,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } // Motor stop handling - if (featureConfigured(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !featureConfigured(FEATURE_3D) && !isAirmodeActive()) { + if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !featureIsEnabled(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { motorOutput = disarmMotorOutput; } @@ -851,7 +851,7 @@ float convertExternalToMotor(uint16_t externalValue) case true: externalValue = constrain(externalValue, PWM_RANGE_MIN, PWM_RANGE_MAX); - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (externalValue == PWM_RANGE_MID) { motorValue = DSHOT_DISARM_COMMAND; } else if (externalValue < PWM_RANGE_MID) { @@ -880,7 +880,7 @@ uint16_t convertMotorToExternal(float motorValue) switch ((int)isMotorProtocolDshot()) { #ifdef USE_DSHOT case true: - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { if (motorValue == DSHOT_DISARM_COMMAND || motorValue < DSHOT_MIN_THROTTLE) { externalValue = PWM_RANGE_MID; } else if (motorValue <= DSHOT_3D_DEADBAND_LOW) { diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 29e633282..82ac0df4f 100644 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -224,7 +224,7 @@ void servosInit(void) // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (featureConfigured(FEATURE_SERVO_TILT) || featureConfigured(FEATURE_CHANNEL_FORWARDING)) { + if (featureIsEnabled(FEATURE_SERVO_TILT) || featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) { useServo = 1; } @@ -377,13 +377,13 @@ void writeServos(void) } // Two servos for SERVO_TILT, if enabled - if (featureConfigured(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { + if (featureIsEnabled(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { updateGimbalServos(servoIndex); servoIndex += 2; } // forward AUX to remaining servo outputs (not constrained) - if (featureConfigured(FEATURE_CHANNEL_FORWARDING)) { + if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) { forwardAuxChannelsToServos(servoIndex); servoIndex += MAX_AUX_CHANNEL_COUNT; } @@ -406,7 +406,7 @@ void servoMixer(void) input[INPUT_STABILIZED_YAW] = pidData[FD_YAW].Sum * PID_SERVO_MIXER_SCALING; // Reverse yaw servo when inverted in 3D mode - if (featureConfigured(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { + if (featureIsEnabled(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } @@ -498,7 +498,7 @@ static void servoTable(void) } // camera stabilization - if (featureConfigured(FEATURE_SERVO_TILT)) { + if (featureIsEnabled(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index d54a353a5..5ebca9017 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -179,6 +179,9 @@ static bool configIsInCopy = false; static int8_t pidProfileIndexToUse = CURRENT_PROFILE_INDEX; static int8_t rateProfileIndexToUse = CURRENT_PROFILE_INDEX; +static bool featureMaskIsCopied = false; +static uint32_t featureMaskCopy; + #if defined(USE_BOARD_INFO) static bool boardInformationUpdated = false; #if defined(USE_SIGNATURE) @@ -2345,9 +2348,18 @@ static void cliMcuId(char *cmdline) cliPrintLinef("mcu_id %08x%08x%08x", U_ID_0, U_ID_1, U_ID_2); } +static uint32_t getFeatureMask(const uint32_t featureMask) +{ + if (featureMaskIsCopied) { + return featureMaskCopy; + } else { + return featureMask; + } +} + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { - const uint32_t mask = featureConfig->enabledFeatures; + const uint32_t mask = getFeatureMask(featureConfig->enabledFeatures); const uint32_t defaultMask = featureConfigDefault->enabledFeatures; for (uint32_t i = 0; featureNames[i]; i++) { // disabled features first if (strcmp(featureNames[i], emptyString) != 0) { //Skip unused @@ -2372,15 +2384,16 @@ static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, static void cliFeature(char *cmdline) { uint32_t len = strlen(cmdline); - uint32_t mask = featureMask(); - + const uint32_t mask = getFeatureMask(featureMask()); if (len == 0) { cliPrint("Enabled: "); for (uint32_t i = 0; ; i++) { - if (featureNames[i] == NULL) + if (featureNames[i] == NULL) { break; - if (mask & (1 << i)) + } + if (mask & (1 << i)) { cliPrintf("%s ", featureNames[i]); + } } cliPrintLinefeed(); } else if (strncasecmp(cmdline, "list", len) == 0) { @@ -2394,6 +2407,12 @@ static void cliFeature(char *cmdline) cliPrintLinefeed(); return; } else { + if (!featureMaskIsCopied) { + featureMaskCopy = featureMask(); + featureMaskIsCopied = true; + } + uint32_t feature; + bool remove = false; if (cmdline[0] == '-') { // remove feature @@ -2409,25 +2428,24 @@ static void cliFeature(char *cmdline) } if (strncasecmp(cmdline, featureNames[i], len) == 0) { - - mask = 1 << i; + feature = 1 << i; #ifndef USE_GPS - if (mask & FEATURE_GPS) { + if (feature & FEATURE_GPS) { cliPrintLine("unavailable"); break; } #endif #ifndef USE_RANGEFINDER - if (mask & FEATURE_RANGEFINDER) { + if (feature & FEATURE_RANGEFINDER) { cliPrintLine("unavailable"); break; } #endif if (remove) { - featureClear(mask); + featureClear(feature, &featureMaskCopy); cliPrint("Disabled"); } else { - featureSet(mask); + featureSet(feature, &featureMaskCopy); cliPrint("Enabled"); } cliPrintLinef(" %s", featureNames[i]); @@ -2946,7 +2964,7 @@ static void cliDshotProg(char *cmdline) pwmWriteDshotCommand(escIndex, getMotorCount(), command, true); } else { #if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO) - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { if (escIndex != ALL_MOTORS) { executeEscInfoCommand(escIndex); } else { @@ -3238,7 +3256,11 @@ static void cliSave(char *cmdline) #endif #endif // USE_BOARD_INFO - writeEEPROM(); + if (featureMaskIsCopied) { + writeEEPROMWithFeatures(featureMaskCopy); + } else { + writeEEPROM(); + } cliReboot(); } @@ -4642,9 +4664,9 @@ void cliEnter(serialPort_t *serialPort) #else cliPrintLine("\r\nCLI"); #endif - cliPrompt(); - setArmingDisabled(ARMING_DISABLED_CLI); + + cliPrompt(); } void cliInit(const serialConfig_t *serialConfig) diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 2168ababd..f2fc092b5 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -165,6 +165,18 @@ typedef enum { #define RTC_NOT_SUPPORTED 0xff +static bool featureMaskIsCopied = false; +static uint32_t featureMaskCopy; + +static uint32_t getFeatureMask(void) +{ + if (featureMaskIsCopied) { + return featureMaskCopy; + } else { + return featureMask(); + } +} + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #define ESC_4WAY 0xff @@ -531,7 +543,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce break; case MSP_FEATURE_CONFIG: - sbufWriteU32(dst, featureMask()); + sbufWriteU32(dst, getFeatureMask()); break; #ifdef USE_BEEPER @@ -1018,7 +1030,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) #if defined(USE_ESC_SENSOR) case MSP_ESC_SENSOR_DATA: - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { sbufWriteU8(dst, getMotorCount()); for (int i = 0; i < getMotorCount(); i++) { const escSensorData_t *escData = getEscSensorData(i); @@ -1552,7 +1564,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) readEEPROM(); break; case MSP_EEPROM_WRITE: - writeEEPROM(); + if (featureMaskIsCopied) { + writeEEPROMWithFeatures(featureMaskCopy); + } else { + writeEEPROM(); + } readEEPROM(); break; default: @@ -1999,7 +2015,12 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (ARMING_FLAG(ARMED)) { return MSP_RESULT_ERROR; } - writeEEPROM(); + + if (featureMaskIsCopied) { + writeEEPROMWithFeatures(featureMaskCopy); + } else { + writeEEPROM(); + } readEEPROM(); break; @@ -2124,8 +2145,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; #endif // USE_GPS case MSP_SET_FEATURE_CONFIG: - featureClearAll(); - featureSet(sbufReadU32(src)); // features bitmap + featureMaskCopy = sbufReadU32(src); + if (!featureMaskIsCopied) { + featureMaskIsCopied = true; + } + break; #ifdef USE_BEEPER diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index a7e03103f..ea1d5c9c5 100644 --- a/src/main/interface/msp_box.c +++ b/src/main/interface/msp_box.c @@ -166,11 +166,11 @@ void initActiveBoxIds(void) #define BME(boxId) do { bitArraySet(&ena, boxId); } while (0) BME(BOXARM); BME(BOXPREARM); - if (!featureConfigured(FEATURE_AIRMODE)) { + if (!featureIsEnabled(FEATURE_AIRMODE)) { BME(BOXAIRMODE); } - if (!featureConfigured(FEATURE_ANTI_GRAVITY)) { + if (!featureIsEnabled(FEATURE_ANTI_GRAVITY)) { BME(BOXANTIGRAVITY); } @@ -188,9 +188,9 @@ void initActiveBoxIds(void) #endif #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { #ifdef USE_GPS_RESCUE - if (!featureConfigured(FEATURE_3D)) { + if (!featureIsEnabled(FEATURE_3D)) { BME(BOXGPSRESCUE); } #endif @@ -207,7 +207,7 @@ void initActiveBoxIds(void) BME(BOXBEEPERON); #ifdef USE_LED_STRIP - if (featureConfigured(FEATURE_LED_STRIP)) { + if (featureIsEnabled(FEATURE_LED_STRIP)) { BME(BOXLEDLOW); } #endif @@ -221,7 +221,7 @@ void initActiveBoxIds(void) BME(BOXFPVANGLEMIX); - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { BME(BOX3D); } @@ -229,18 +229,18 @@ void initActiveBoxIds(void) BME(BOXFLIPOVERAFTERCRASH); } - if (featureConfigured(FEATURE_SERVO_TILT)) { + if (featureIsEnabled(FEATURE_SERVO_TILT)) { BME(BOXCAMSTAB); } - if (featureConfigured(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { BME(BOXCALIB); } BME(BOXOSD); #ifdef USE_TELEMETRY - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { BME(BOXTELEMETRY); } #endif diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index b66398573..7ceeec2dd 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -384,7 +384,7 @@ void beeperUpdate(timeUs_t currentTimeUs) if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) { beeper(BEEPER_RX_SET); #ifdef USE_GPS - } else if (featureConfigured(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) { + } else if (featureIsEnabled(FEATURE_GPS) && IS_RC_MODE_ACTIVE(BOXBEEPGPSCOUNT)) { beeperGpsStatus(); #endif } diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index eb560363a..58e49e063 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -357,7 +357,7 @@ static void showProfilePage(void) #ifdef USE_GPS static void showGpsPage(void) { - if (!featureConfigured(FEATURE_GPS)) { + if (!featureIsEnabled(FEATURE_GPS)) { pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE; return; } diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2d1aa4ffd..d42d8c550 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1214,7 +1214,7 @@ static void gpsHandlePassthrough(uint8_t data) { gpsNewData(data); #ifdef USE_DASHBOARD - if (featureConfigured(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardUpdate(micros()); } #endif @@ -1230,7 +1230,7 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) serialSetMode(gpsPort, gpsPort->mode | MODE_TX); #ifdef USE_DASHBOARD - if (featureConfigured(FEATURE_DASHBOARD)) { + if (featureIsEnabled(FEATURE_DASHBOARD)) { dashboardShowFixedPage(PAGE_GPS); } #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0a9d98d12..e86ebcf40 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -788,7 +788,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_ESC_SENSOR // Show warning if we lose motor output, the ESC is overheating or excessive current draw - if (featureConfigured(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR) && osdWarnGetState(OSD_WARNING_ESC_FAIL)) { char escWarningMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE]; unsigned pos = 0; @@ -956,13 +956,13 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_ESC_SENSOR case OSD_ESC_TMP: - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%3d%c", osdConvertTemperatureToSelectedUnit(escDataCombined->temperature * 10) / 10, osdGetTemperatureSymbolForSelectedUnit()); } break; case OSD_ESC_RPM: - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { tfp_sprintf(buff, "%5d", escDataCombined == NULL ? 0 : calcEscRpm(escDataCombined->rpm)); } break; @@ -1040,7 +1040,7 @@ static void osdDrawElements(void) #endif // GPS #ifdef USE_ESC_SENSOR - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { osdDrawSingleElement(OSD_ESC_TMP); osdDrawSingleElement(OSD_ESC_RPM); } @@ -1226,7 +1226,7 @@ void osdUpdateAlarms(void) } #ifdef USE_ESC_SENSOR - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { // This works because the combined ESC data contains the maximum temperature seen amongst all ESCs if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escDataCombined->temperature >= osdConfig()->esc_temp_alarm) { SET_BLINK(OSD_ESC_TMP); @@ -1560,7 +1560,7 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs) blinkState = (currentTimeUs / 200000) % 2; #ifdef USE_ESC_SENSOR - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED); } #endif diff --git a/src/main/io/vtx_rtc6705.c b/src/main/io/vtx_rtc6705.c index 6433aeffc..a2c4c4387 100644 --- a/src/main/io/vtx_rtc6705.c +++ b/src/main/io/vtx_rtc6705.c @@ -72,7 +72,7 @@ bool vtxRTC6705Init(void) bool vtxRTC6705CanUpdate(void) { #if defined(MAX7456_SPI_INSTANCE) && defined(RTC6705_SPI_INSTANCE) && defined(SPI_SHARED_MAX7456_AND_RTC6705) - if (featureConfigured(FEATURE_OSD)) { + if (featureIsEnabled(FEATURE_OSD)) { return !max7456DmaInProgress(); } #endif diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c index 9dede7ffd..49ff19036 100644 --- a/src/main/osd_slave/osd_slave_init.c +++ b/src/main/osd_slave/osd_slave_init.c @@ -233,7 +233,7 @@ void init(void) adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC); adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); - adcConfigMutable()->rssi.enabled = featureConfigured(FEATURE_RSSI_ADC); + adcConfigMutable()->rssi.enabled = featureIsEnabled(FEATURE_RSSI_ADC); adcInit(adcConfig()); #endif @@ -275,7 +275,7 @@ void init(void) #ifdef USE_LED_STRIP ledStripInit(); - if (featureConfigured(FEATURE_LED_STRIP)) { + if (featureIsEnabled(FEATURE_LED_STRIP)) { ledStripEnable(); } #endif @@ -285,7 +285,7 @@ void init(void) #endif #ifdef USE_TRANSPONDER - if (featureConfigured(FEATURE_TRANSPONDER)) { + if (featureIsEnabled(FEATURE_TRANSPONDER)) { transponderInit(); transponderStartRepeating(); systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED; diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index ab49cff4c..5389afc2a 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -305,7 +305,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro void frSkyDInit(void) { #if defined(USE_RX_FRSKY_SPI_TELEMETRY) && defined(USE_TELEMETRY_FRSKY_HUB) - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryEnabled = initFrSkyHubTelemetryExternal(frSkyDTelemetryWriteByte); } #endif diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index ca9432db1..3a57ccccd 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -543,7 +543,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro void frSkyXInit(void) { #if defined(USE_TELEMETRY_SMARTPORT) - if (featureConfigured(FEATURE_TELEMETRY)) { + if (featureIsEnabled(FEATURE_TELEMETRY)) { telemetryEnabled = initSmartPortTelemetryExternal(frSkyXTelemetryWriteFrame); } #endif diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index d85fc3278..1ddf46542 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -60,10 +60,10 @@ void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) rxRuntimeConfig->rxRefreshRate = 20000; // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled - if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; rxRuntimeConfig->rcReadRawFn = pwmReadRawRC; - } else if (featureConfigured(FEATURE_RX_PPM)) { + } else if (featureIsEnabled(FEATURE_RX_PPM)) { rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; rxRuntimeConfig->rcReadRawFn = ppmReadRawRC; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 8d595a9d5..d0a3736bc 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -248,7 +248,7 @@ void rxInit(void) rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; } - rcData[THROTTLE] = (featureConfigured(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; + rcData[THROTTLE] = (featureIsEnabled(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 @@ -268,7 +268,7 @@ void rxInit(void) } #ifdef USE_SERIAL_RX - if (featureConfigured(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig); if (!enabled) { rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -278,14 +278,14 @@ void rxInit(void) #endif #ifdef USE_RX_MSP - if (featureConfigured(FEATURE_RX_MSP)) { + if (featureIsEnabled(FEATURE_RX_MSP)) { rxMspInit(rxConfig(), &rxRuntimeConfig); needRxSignalMaxDelayUs = DELAY_5_HZ; } #endif #ifdef USE_RX_SPI - if (featureConfigured(FEATURE_RX_SPI)) { + if (featureIsEnabled(FEATURE_RX_SPI)) { const bool enabled = rxSpiInit(rxSpiConfig(), &rxRuntimeConfig); if (!enabled) { rxRuntimeConfig.rcReadRawFn = nullReadRawRC; @@ -295,13 +295,13 @@ void rxInit(void) #endif #if defined(USE_PWM) || defined(USE_PPM) - if (featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_PARALLEL_PWM)) { + if (featureIsEnabled(FEATURE_RX_PPM) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { rxPwmInit(rxConfig(), &rxRuntimeConfig); } #endif #if defined(USE_ADC) - if (featureConfigured(FEATURE_RSSI_ADC)) { + if (featureIsEnabled(FEATURE_RSSI_ADC)) { rssiSource = RSSI_SOURCE_ADC; } else #endif @@ -344,14 +344,14 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) bool useDataDrivenProcessing = true; #if defined(USE_PWM) || defined(USE_PPM) - if (featureConfigured(FEATURE_RX_PPM)) { + if (featureIsEnabled(FEATURE_RX_PPM)) { if (isPPMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } - } else if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { + } else if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM)) { if (isPWMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; @@ -435,7 +435,7 @@ static uint16_t getRxfailValue(uint8_t channel) case YAW: return rxConfig()->midrc; case THROTTLE: - if (featureConfigured(FEATURE_3D)) + if (featureIsEnabled(FEATURE_3D)) return rxConfig()->midrc; else return rxConfig()->rx_min_usec; @@ -510,7 +510,7 @@ static void detectAndApplySignalLossBehaviour(void) } } } - if (featureConfigured(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { + if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { // smooth output for PWM and PPM rcData[channel] = calculateChannelMovingAverage(channel, sample); } else { diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 79075ca69..1d029fc4c 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -238,7 +238,7 @@ void spektrumBind(rxConfig_t *rxConfig) switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - if (featureConfigured(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { + if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) { bindPin = txPin; } break; @@ -322,7 +322,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig switch (rxConfig->serialrx_provider) { case SERIALRX_SRXL: #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SRXL) - srxlEnabled = (featureConfigured(FEATURE_TELEMETRY) && !portShared); + srxlEnabled = (featureIsEnabled(FEATURE_TELEMETRY) && !portShared); FALLTHROUGH; #endif case SERIALRX_SPEKTRUM2048: diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index a3479d30b..e1626e535 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -528,7 +528,7 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) performAcclerationCalibration(rollAndPitchTrims); } - if (featureConfigured(FEATURE_INFLIGHT_ACC_CAL)) { + if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { performInflightAccelerationCalibration(rollAndPitchTrims); } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index d2170d80b..679d772cf 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -121,7 +121,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs) switch (batteryConfig()->voltageMeterSource) { #ifdef USE_ESC_SENSOR case VOLTAGE_METER_ESC: - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { voltageMeterESCRefresh(); voltageMeterESCReadCombined(&voltageMeter); } @@ -399,7 +399,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) case CURRENT_METER_VIRTUAL: { #ifdef USE_VIRTUAL_CURRENT_METER throttleStatus_e throttleStatus = calculateThrottleStatus(); - bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureConfigured(FEATURE_MOTOR_STOP)); + bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)); int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset); @@ -410,7 +410,7 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) case CURRENT_METER_ESC: #ifdef USE_ESC_SENSOR - if (featureConfigured(FEATURE_ESC_SENSOR)) { + if (featureIsEnabled(FEATURE_ESC_SENSOR)) { currentMeterESCRefresh(lastUpdateAt); currentMeterESCReadCombined(¤tMeter); } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 7ada2bd59..8d6226223 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -148,7 +148,7 @@ bool isEscSensorActive(void) escSensorData_t *getEscSensorData(uint8_t motorNumber) { - if (!featureConfigured(FEATURE_ESC_SENSOR)) { + if (!featureIsEnabled(FEATURE_ESC_SENSOR)) { return NULL; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bb18fa3a0..60f3e0594 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -744,7 +744,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin #ifdef USE_GYRO_DATA_ANALYSE static bool isDynamicFilterActive(void) { - return featureConfigured(FEATURE_DYNAMIC_FILTER); + return featureIsEnabled(FEATURE_DYNAMIC_FILTER); } static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 0e63f9557..9e485e97c 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -101,7 +101,7 @@ void targetConfiguration(void) rxConfigMutable()->serialrx_inverted = true; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL; telemetryConfigMutable()->telemetry_inverted = false; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); beeperDevConfigMutable()->isOpenDrain = false; beeperDevConfigMutable()->isInverted = true; parseRcChannels("AETR1234", rxConfigMutable()); diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 94ac50e09..3c67839ba 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -73,7 +73,7 @@ void targetConfiguration(void) telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); } for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index acd7a9ad9..5dc19e156 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -72,7 +72,7 @@ void targetConfiguration(void) telemetryConfigMutable()->telemetry_inverted = false; batteryConfigMutable()->voltageMeterSource = VOLTAGE_METER_ADC; batteryConfigMutable()->currentMeterSource = CURRENT_METER_ADC; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); } for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { diff --git a/src/main/target/ALIENWHOOP/config.c b/src/main/target/ALIENWHOOP/config.c index 968d9125b..191dec467 100644 --- a/src/main/target/ALIENWHOOP/config.c +++ b/src/main/target/ALIENWHOOP/config.c @@ -113,7 +113,7 @@ void targetConfiguration(void) pidConfigMutable()->runaway_takeoff_prevention = false; - featureSet((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); + featureEnable((FEATURE_DYNAMIC_FILTER | FEATURE_AIRMODE | FEATURE_ANTI_GRAVITY) ^ FEATURE_RX_PARALLEL_PWM); /* AlienWhoop PIDs tested with 6mm and 7mm motors on most frames */ for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { diff --git a/src/main/target/BLUEJAYF4/initialisation.c b/src/main/target/BLUEJAYF4/initialisation.c index fb746befc..7f3b3b3f0 100644 --- a/src/main/target/BLUEJAYF4/initialisation.c +++ b/src/main/target/BLUEJAYF4/initialisation.c @@ -56,7 +56,7 @@ void targetPreInit(void) serialPortConfig_t *portConfig = serialFindPortConfiguration(SERIAL_PORT_USART1); if (portConfig) { bool smartportEnabled = (portConfig->functionMask & FUNCTION_TELEMETRY_SMARTPORT); - if (smartportEnabled && (!telemetryConfig()->telemetry_inverted) && (featureConfigured(FEATURE_TELEMETRY))) { + if (smartportEnabled && (!telemetryConfig()->telemetry_inverted) && (featureIsEnabled(FEATURE_TELEMETRY))) { high = true; } } diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index cf2f4f605..220fa276b 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -558,10 +558,10 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) readEEPROM(); break; case BST_SET_FEATURE: - featureClearAll(); - featureSet(bstRead32()); // features bitmap + featureDisableAll(); + featureEnable(bstRead32()); // features bitmap #ifdef SERIALRX_UART - if (featureConfigured(FEATURE_RX_SERIAL)) { + if (featureIsEnabled(FEATURE_RX_SERIAL)) { serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } else { serialConfigMutable()->portConfigs[SERIALRX_UART].functionMask = FUNCTION_NONE; diff --git a/src/main/target/FF_FORTINIF4/config.c b/src/main/target/FF_FORTINIF4/config.c index 775619ba4..6252aee96 100644 --- a/src/main/target/FF_FORTINIF4/config.c +++ b/src/main/target/FF_FORTINIF4/config.c @@ -35,7 +35,7 @@ void targetConfiguration(void) { if (hardwareRevision >= FORTINIF4_REV_2) { - featureSet(FEATURE_OSD); + featureEnable(FEATURE_OSD); } telemetryConfigMutable()->halfDuplex = false; diff --git a/src/main/target/FF_PIKOBLX/config.c b/src/main/target/FF_PIKOBLX/config.c index 073fdbbda..e3c8f590f 100644 --- a/src/main/target/FF_PIKOBLX/config.c +++ b/src/main/target/FF_PIKOBLX/config.c @@ -65,7 +65,7 @@ void targetConfiguration(void) #else serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB; rxConfigMutable()->serialrx_inverted = true; - featureSet(FEATURE_TELEMETRY); + featureEnable(FEATURE_TELEMETRY); #endif parseRcChannels("TAER1234", rxConfigMutable()); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 7b3efd45f..bc13b9aef 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -436,7 +436,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) { // for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0] double outScale = 1000.0; - if (featureConfigured(FEATURE_3D)) { + if (featureIsEnabled(FEATURE_3D)) { outScale = 500.0; } diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 68fab96b7..76e4a1f07 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -445,7 +445,7 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); } crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } crsfScheduleCount = (uint8_t)index; diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 2adfec3c5..1a13b0ad2 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -193,7 +193,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && featureConfigured(FEATURE_MOTOR_STOP)) { + if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) { throttleForRPM = 0; } data = throttleForRPM; diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index a76197ea0..68d806b6e 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -228,7 +228,7 @@ static uint16_t getRPM() if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(); rpm = rcCommand[THROTTLE]; // / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && featureConfigured(FEATURE_MOTOR_STOP)) rpm = 0; + if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) rpm = 0; } else { rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b5a21b232..fb376dc5e 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -318,7 +318,7 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) } static bool reportExtendedEscSensors(void) { - return featureConfigured(FEATURE_ESC_SENSOR) && telemetryConfig()->smartport_use_extra_sensors; + return featureIsEnabled(FEATURE_ESC_SENSOR) && telemetryConfig()->smartport_use_extra_sensors; } #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId @@ -332,7 +332,7 @@ static void initSmartPortSensors(void) if (isBatteryVoltageConfigured()) { #ifdef USE_ESC_SENSOR - if (!featureConfigured(FEATURE_ESC_SENSOR)) { + if (!featureIsEnabled(FEATURE_ESC_SENSOR)) { #endif { ADD_SENSOR(FSSP_DATAID_VFAS); @@ -365,7 +365,7 @@ static void initSmartPortSensors(void) } #ifdef USE_GPS - if (featureConfigured(FEATURE_GPS)) { + if (featureIsEnabled(FEATURE_GPS)) { ADD_SENSOR(FSSP_DATAID_SPEED); ADD_SENSOR(FSSP_DATAID_LATLONG); ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long) @@ -719,7 +719,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat); *clearToSend = false; - } else if (featureConfigured(FEATURE_GPS)) { + } else if (featureIsEnabled(FEATURE_GPS)) { smartPortSendPackage(id, 0); *clearToSend = false; } else diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index cae667858..a4b164cea 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -793,7 +793,7 @@ extern "C" { uint32_t millis(void) { return micros() / 1000; } bool rxIsReceivingSignal(void) { return simulationHaveRx; } - bool featureConfigured(uint32_t f) { return simulationFeatureFlags & f; } + bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } void warningLedFlash(void) {} void warningLedDisable(void) {} void warningLedUpdate(void) {} diff --git a/src/test/unit/battery_unittest.cc.txt b/src/test/unit/battery_unittest.cc.txt index 6de02754a..d38299be4 100644 --- a/src/test/unit/battery_unittest.cc.txt +++ b/src/test/unit/battery_unittest.cc.txt @@ -273,7 +273,7 @@ uint8_t armingFlags = 0; float rcCommand[4] = {0,0,0,0}; -bool featureConfigured(uint32_t mask) +bool featureIsEnabled(uint32_t mask) { UNUSED(mask); return false; diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index 51faa78f4..59743ded7 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -388,7 +388,7 @@ bool sensors(uint32_t) {return false;} void serialWrite(serialPort_t *, uint8_t) {} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} bool isSerialTransmitBufferEmpty(const serialPort_t *) {return false;} -bool featureConfigured(uint32_t) {return false;} +bool featureIsEnabled(uint32_t) {return false;} void mspSerialReleasePortIfAllocated(serialPort_t *) {} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} serialPort_t *findSharedSerialPort(uint16_t , serialPortFunction_e ) {return NULL;} diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 33b615027..b98df40c4 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -567,7 +567,7 @@ throttleStatus_e calculateThrottleStatus() void delay(uint32_t) {} -bool featureConfigured(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { return (mask & testFeatureMask); } diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 59bf80730..868f4d72e 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -392,7 +392,7 @@ uint8_t armingFlags; void delay(uint32_t) {} -bool featureConfigured(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { return (mask & testFeatureMask); } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index f2e9e83ae..eb7594d44 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -352,7 +352,7 @@ void delay(uint32_t ms) uint32_t micros(void) { return 0; } bool shouldSoundBatteryAlarm(void) { return false; } -bool featureConfigured(uint32_t mask) { +bool featureIsEnabled(uint32_t mask) { UNUSED(mask); return false; } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 2ba60c86e..80c387f26 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -709,7 +709,7 @@ void gyroStartCalibration(bool isFirstArmingCalibration) } void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} void handleInflightCalibrationStickPosition(void) {} -bool featureConfigured(uint32_t) { return false;} +bool featureIsEnabled(uint32_t) { return false;} bool sensors(uint32_t) { return false;} void tryArm(void) {} void disarm(void) {} diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index 5e441c31c..29687cdce 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -845,7 +845,7 @@ extern "C" { sbufWriteU8(dst, (uint8_t)val); } - bool featureConfigured(uint32_t) { return false; } + bool featureIsEnabled(uint32_t) { return false; } void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 0bcdfcff2..a87b0dc2b 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -186,11 +186,11 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR return true; } -bool featureConfigured(uint32_t) { +bool featureIsEnabled(uint32_t) { return false; } -void featureClear(uint32_t) { +void featureDisable(uint32_t) { } bool rxMspFrameComplete(void) diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 4b8279404..b3670102c 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -293,7 +293,7 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} uint32_t micros(void) {return 0;} -bool featureConfigured(uint32_t) {return true;} +bool featureIsEnabled(uint32_t) {return true;} uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index add04497c..7476ca5e8 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -108,7 +108,7 @@ throttleStatus_e calculateThrottleStatus(void) return throttleStatus; } -bool featureConfigured(uint32_t mask) +bool featureIsEnabled(uint32_t mask) { return (definedFeatures & mask) != 0; }