diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index f8d06bb68..80d0f79d0 100755 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -927,6 +927,11 @@ int32_t sum_chans512[NUM_CHNOUT] = {0}; #define MAX_ACT 0xffff uint8_t s_last_phase = 255; // TODO reinit everything here when the model changes, no??? +#if defined(CPUARM) +tmr10ms_t flightModeTransitionTime; +uint8_t flightModeTransitionLast = 255; +#endif + void evalMixes(uint8_t tick10ms) { #if defined(PCBGRUVIN9X) && defined(DEBUG) && !defined(VOICE) @@ -942,8 +947,9 @@ void evalMixes(uint8_t tick10ms) uint8_t phase = getFlightPhase(); if (s_last_phase != phase) { - if (s_last_phase != 255) PLAY_PHASE_OFF(s_last_phase); - PLAY_PHASE_ON(phase); +#if defined(CPUARM) + flightModeTransitionTime = get_tmr10ms(); +#endif if (s_last_phase == 255) { fp_act[phase] = MAX_ACT; @@ -967,6 +973,17 @@ void evalMixes(uint8_t tick10ms) s_last_phase = phase; } +#if defined(CPUARM) + if (flightModeTransitionTime && get_tmr10ms() > flightModeTransitionTime+g_eeGeneral.switchesDelay) { + flightModeTransitionTime = 0; + if (phase != flightModeTransitionLast) { + if (flightModeTransitionLast != 255) PLAY_PHASE_OFF(flightModeTransitionLast); + PLAY_PHASE_ON(phase); + flightModeTransitionLast = phase; + } + } +#endif + int32_t weight = 0; if (s_fade_flight_phases) { memclear(sum_chans512, sizeof(sum_chans512)); diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 88ef3bb7c..7dc31506d 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -675,6 +675,7 @@ enum StartupWarningStates { extern uint8_t s_current_mixer_flight_mode; extern uint8_t s_last_phase; +extern uint8_t flightModeTransitionLast; #if defined(CPUARM) #define bitfield_channels_t uint32_t diff --git a/radio/src/switches.cpp b/radio/src/switches.cpp index d3dd3a2af..26d1f08c5 100644 --- a/radio/src/switches.cpp +++ b/radio/src/switches.cpp @@ -435,7 +435,10 @@ bool getSwitch(int8_t swtch) #if defined(CPUARM) && defined(FLIGHT_MODES) else if (cs_idx >= SWSRC_FIRST_FLIGHT_MODE) { uint8_t idx = cs_idx - SWSRC_FIRST_FLIGHT_MODE; - result = (idx == s_current_mixer_flight_mode); + if (flags & GETSWITCH_MIDPOS_DELAY) + result = (idx == flightModeTransitionLast); + else + result = (idx == s_current_mixer_flight_mode); } #endif else {