diff --git a/radio/src/storage/storage_common.cpp b/radio/src/storage/storage_common.cpp index 396ede0df..da86cebe8 100644 --- a/radio/src/storage/storage_common.cpp +++ b/radio/src/storage/storage_common.cpp @@ -51,8 +51,9 @@ void preModelLoad() if (pulsesStarted()) { pausePulses(); } - pauseMixerCalculations(); + + stopTrainer(); } void postRadioSettingsLoad() diff --git a/radio/src/trainer.cpp b/radio/src/trainer.cpp index a5e99adde..6c6bc7b79 100644 --- a/radio/src/trainer.cpp +++ b/radio/src/trainer.cpp @@ -48,6 +48,11 @@ void checkTrainerSignalWarning() } #if defined(PCBSKY9X) +void stopTrainer() +{ + stop_trainer_capture(); +} + void checkTrainerSettings() { uint8_t requiredTrainerMode = SLAVE_MODE(); @@ -55,50 +60,59 @@ void checkTrainerSettings() if (requiredTrainerMode != currentTrainerMode) { currentTrainerMode = requiredTrainerMode; if (requiredTrainerMode) - stop_trainer_capture(); + stopTrainer(); else init_trainer_capture(); } } #else +void stopTrainer() +{ + switch (currentTrainerMode) { + case TRAINER_MODE_MASTER_TRAINER_JACK: + stop_trainer_capture(); + break; + + case TRAINER_MODE_SLAVE: + stop_trainer_ppm(); + break; + +#if defined(TRAINER_MODULE_CPPM) + case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: + stop_trainer_module_cppm(); + break; +#endif + +#if defined(TRAINER_MODULE_SBUS) + case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: + stop_trainer_module_sbus(); + break; +#endif + +#if defined(TRAINER_BATTERY_COMPARTMENT) + case TRAINER_MODE_MASTER_BATTERY_COMPARTMENT: +#if defined(AUX_SERIAL) + if (g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) + auxSerialStop(); +#endif +#if defined(AUX2_SERIAL) + if (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) + aux2SerialStop(); +#endif + break; +#endif + } + + currentTrainerMode = 0xFF; +} + void checkTrainerSettings() { uint8_t requiredTrainerMode = g_model.trainerData.mode; if (requiredTrainerMode != currentTrainerMode) { - switch (currentTrainerMode) { - case TRAINER_MODE_MASTER_TRAINER_JACK: - stop_trainer_capture(); - break; - - case TRAINER_MODE_SLAVE: - stop_trainer_ppm(); - break; - -#if defined(TRAINER_MODULE_CPPM) - case TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE: - stop_trainer_module_cppm(); - break; -#endif - -#if defined(TRAINER_MODULE_SBUS) - case TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE: - stop_trainer_module_sbus(); - break; -#endif - -#if defined(TRAINER_BATTERY_COMPARTMENT) - case TRAINER_MODE_MASTER_BATTERY_COMPARTMENT: -#if defined(AUX_SERIAL) - if (g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) - auxSerialStop(); -#endif -#if defined(AUX2_SERIAL) - if (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER) - aux2SerialStop(); -#endif - break; -#endif + if (currentTrainerMode != 0xFF) { + stopTrainer(); } currentTrainerMode = requiredTrainerMode; diff --git a/radio/src/trainer.h b/radio/src/trainer.h index e3c67acac..1222d4f55 100644 --- a/radio/src/trainer.h +++ b/radio/src/trainer.h @@ -35,6 +35,7 @@ extern uint8_t currentTrainerMode; void checkTrainerSignalWarning(); void checkTrainerSettings(); +void stopTrainer(); void forceResetTrainerSettings(); // Needs to be inlined to avoid slow function calls in ISR routines