Instead of trying to latch the desired features...

...and apply them after a soft reset (which also required an additional
write to flash), it is now such that features and settings are modified
and stored in flash as before.

After initialisation completes, the active features are latched and are
not to be modified until the next startup. This guarantees that all
saved modifications are persistent even when power is switched of
(without a reset in between).

When a soft reset is required, the active features and the currently
configured features are used to detect if the oneshot feature has
changed state, in which case motor PWM outputs are stopped and soft
reset is done after a 1.5 second delay.

During normal operation the active features will not change and all
changes to features ordered via MSP commands or the CLI are applied to
the configuration that gets saved to flash.

The required effect of modifying features without changing the actions
in the running mainloop is achieved. The user needs to be aware that
changes to features are not applied immidiatly.

src/main/config/config.c

@@ -134,6 +134,7 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
 
 master_t masterConfig;                 // master config struct with data independent from profiles
 profile_t *currentProfile;
+static uint32_t activeFeaturesLatch = 0;
 
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
@@ -724,26 +725,26 @@ void activateConfig(void)
 
 void validateAndFixConfig(void)
 {
-    if (!(feature(FEATURE_RX_PARALLEL_PWM) || feature(FEATURE_RX_PPM) || feature(FEATURE_RX_SERIAL) || feature(FEATURE_RX_MSP))) {
+    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
         featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
     }
 
-    if (feature(FEATURE_RX_PPM)) {
+    if (featureConfigured(FEATURE_RX_PPM)) {
         featureClear(FEATURE_RX_PARALLEL_PWM);
     }
 
-    if (feature(FEATURE_RX_MSP)) {
+    if (featureConfigured(FEATURE_RX_MSP)) {
         featureClear(FEATURE_RX_SERIAL);
         featureClear(FEATURE_RX_PARALLEL_PWM);
         featureClear(FEATURE_RX_PPM);
     }
 
-    if (feature(FEATURE_RX_SERIAL)) {
+    if (featureConfigured(FEATURE_RX_SERIAL)) {
         featureClear(FEATURE_RX_PARALLEL_PWM);
         featureClear(FEATURE_RX_PPM);
     }
 
-    if (feature(FEATURE_RX_PARALLEL_PWM)) {
+    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
 #if defined(STM32F10X)
         // rssi adc needs the same ports
         featureClear(FEATURE_RSSI_ADC);
@@ -764,7 +765,7 @@ void validateAndFixConfig(void)
 
 
 #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
-    if (feature(FEATURE_SOFTSERIAL) && (
+    if (featureConfigured(FEATURE_SOFTSERIAL) && (
             0
 #ifdef USE_SOFTSERIAL1
             || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
@@ -779,7 +780,7 @@ void validateAndFixConfig(void)
 #endif
 
 #if defined(NAZE) && defined(SONAR)
-    if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
@@ -937,11 +938,30 @@ void changeControlRateProfile(uint8_t profileIndex)
     activateControlRateConfig();
 }
 
-bool feature(uint32_t mask)
+void handleOneshotFeatureChangeOnRestart(void)
+{
+    // When OneShot125 feature changed state, shutdown PWM on all motors and apply a delay before
+    if ((masterConfig.enabledFeatures ^ activeFeaturesLatch) & FEATURE_ONESHOT125) {
+        StopPwmAllMotors();
+        delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS);
+    }
+}
+
+void latchActiveFeatures()
+{
+    activeFeaturesLatch = masterConfig.enabledFeatures;
+}
+
+bool featureConfigured(uint32_t mask)
 {
     return masterConfig.enabledFeatures & mask;
 }
 
+bool feature(uint32_t mask)
+{
+    return activeFeaturesLatch & mask;
+}
+
 void featureSet(uint32_t mask)
 {
     masterConfig.enabledFeatures |= mask;

src/main/config/config.h

@@ -19,6 +19,7 @@
 
 #define MAX_PROFILE_COUNT 3
 #define MAX_CONTROL_RATE_PROFILE_COUNT 3
+#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500
 
 
 typedef enum {
@@ -44,6 +45,9 @@ typedef enum {
     FEATURE_BLACKBOX = 1 << 19
 } features_e;
 
+void handleOneshotFeatureChangeOnRestart(void);
+void latchActiveFeatures(void);
+bool featureConfigured(uint32_t mask);
 bool feature(uint32_t mask);
 void featureSet(uint32_t mask);
 void featureClear(uint32_t mask);

src/main/drivers/pwm_output.c

@@ -142,6 +142,16 @@ void pwmWriteMotor(uint8_t index, uint16_t value)
         motors[index]->pwmWritePtr(index, value);
 }
 
+void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
+{
+    uint8_t index;
+
+    for(index = 0; index < motorCount; index++){
+        // Set the compare register to 0, which stops the output pulsing if the timer overflows
+        *motors[index]->ccr = 0;
+    }
+}
+
 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 {
     uint8_t index;

src/main/drivers/pwm_output.h

@@ -18,6 +18,7 @@
 #pragma once
 
 void pwmWriteMotor(uint8_t index, uint16_t value);
+void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
 
 void pwmWriteServo(uint8_t index, uint16_t value);

src/main/flight/mixer.c

@@ -513,6 +513,11 @@ void stopMotors(void)
     delay(50); // give the timers and ESCs a chance to react.
 }
 
+void StopPwmAllMotors()
+{
+    pwmShutdownPulsesForAllMotors(motorCount);
+}
+
 #ifndef USE_QUAD_MIXER_ONLY
 static void airplaneMixer(void)
 {

src/main/flight/mixer.h

@@ -115,3 +115,4 @@ void mixerResetMotors(void);
 void mixTable(void);
 void writeMotors(void);
 void stopMotors(void);
+void StopPwmAllMotors(void);

src/main/io/serial_cli.c

@@ -1410,6 +1410,7 @@ static void cliReboot(void) {
     cliPrint("\r\nRebooting");
     waitForSerialPortToFinishTransmitting(cliPort);
     stopMotors();
+    handleOneshotFeatureChangeOnRestart();
     systemReset();
 }
 

src/main/io/serial_msp.c

@@ -1750,6 +1750,7 @@ void mspProcess(void)
                 delay(50);
             }
             stopMotors();
+            handleOneshotFeatureChangeOnRestart();
             systemReset();
         }
     }

src/main/main.c

@@ -168,6 +168,9 @@ void init(void)
 
     systemInit();
 
+    // Set active features to be used for feature() in the remainder of init().
+    latchActiveFeatures();
+
     ledInit();
 
 #ifdef SPEKTRUM_BIND
@@ -451,6 +454,9 @@ void init(void)
     LED2_ON;
 #endif
 
+    // Set active features AGAIN since some may be modified by init().
+    latchActiveFeatures();
+
     systemState |= SYSTEM_STATE_READY;
 }