From f9590d4141ed295055d87720052e3d8dbc11431b Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Tue, 18 Feb 2014 20:44:10 +0100 Subject: [PATCH] Vario bugfixes --- radio/src/audio_arm.cpp | 87 ++++++++++++++++++++------------------ radio/src/audio_arm.h | 49 +++++++++++++--------- radio/src/maths.cpp | 93 ++++++++++++++++++++++++++--------------- 3 files changed, 135 insertions(+), 94 deletions(-) diff --git a/radio/src/audio_arm.cpp b/radio/src/audio_arm.cpp index f9a9bc38d..4c374dc73 100644 --- a/radio/src/audio_arm.cpp +++ b/radio/src/audio_arm.cpp @@ -452,35 +452,35 @@ int AudioQueue::mixWav(AudioContext &context, AudioBuffer *buffer, int volume, u AudioFragment & fragment = context.fragment; if (fragment.file[1]) { - result = f_open(&context.state.wav.file, fragment.file, FA_OPEN_EXISTING | FA_READ); + result = f_open(&context.wav.file, fragment.file, FA_OPEN_EXISTING | FA_READ); fragment.file[1] = 0; if (result == FR_OK) { - result = f_read(&context.state.wav.file, wavBuffer, RIFF_CHUNK_SIZE+8, &read); + result = f_read(&context.wav.file, wavBuffer, RIFF_CHUNK_SIZE+8, &read); if (result == FR_OK && read == RIFF_CHUNK_SIZE+8 && !memcmp(wavBuffer, "RIFF", 4) && !memcmp(wavBuffer+8, "WAVEfmt ", 8)) { uint32_t size = *((uint32_t *)(wavBuffer+16)); - result = (size < 256 ? f_read(&context.state.wav.file, wavBuffer, size+8, &read) : FR_DENIED); + result = (size < 256 ? f_read(&context.wav.file, wavBuffer, size+8, &read) : FR_DENIED); if (result == FR_OK && read == size+8) { - context.state.wav.codec = ((uint16_t *)wavBuffer)[0]; - context.state.wav.freq = ((uint16_t *)wavBuffer)[2]; + context.wav.codec = ((uint16_t *)wavBuffer)[0]; + context.wav.freq = ((uint16_t *)wavBuffer)[2]; uint32_t *wavSamplesPtr = (uint32_t *)(wavBuffer + size); uint32_t size = wavSamplesPtr[1]; - if (context.state.wav.freq != 0 && context.state.wav.freq * (AUDIO_SAMPLE_RATE / context.state.wav.freq) == AUDIO_SAMPLE_RATE) { - context.state.wav.resampleRatio = (AUDIO_SAMPLE_RATE / context.state.wav.freq); - context.state.wav.readSize = (context.state.wav.codec == CODEC_ID_PCM_S16LE ? 2*AUDIO_BUFFER_SIZE : AUDIO_BUFFER_SIZE) / context.state.wav.resampleRatio; + if (context.wav.freq != 0 && context.wav.freq * (AUDIO_SAMPLE_RATE / context.wav.freq) == AUDIO_SAMPLE_RATE) { + context.wav.resampleRatio = (AUDIO_SAMPLE_RATE / context.wav.freq); + context.wav.readSize = (context.wav.codec == CODEC_ID_PCM_S16LE ? 2*AUDIO_BUFFER_SIZE : AUDIO_BUFFER_SIZE) / context.wav.resampleRatio; } else { result = FR_DENIED; } while (result == FR_OK && memcmp(wavSamplesPtr, "data", 4) != 0) { - result = f_lseek(&context.state.wav.file, f_tell(&context.state.wav.file)+size); + result = f_lseek(&context.wav.file, f_tell(&context.wav.file)+size); if (result == FR_OK) { - result = f_read(&context.state.wav.file, wavBuffer, 8, &read); + result = f_read(&context.wav.file, wavBuffer, 8, &read); if (read != 8) result = FR_DENIED; wavSamplesPtr = (uint32_t *)wavBuffer; size = wavSamplesPtr[1]; } } - context.state.wav.size = size; + context.wav.size = size; } else { result = FR_DENIED; @@ -494,34 +494,34 @@ int AudioQueue::mixWav(AudioContext &context, AudioBuffer *buffer, int volume, u read = 0; if (result == FR_OK) { - result = f_read(&context.state.wav.file, wavBuffer, context.state.wav.readSize, &read); + result = f_read(&context.wav.file, wavBuffer, context.wav.readSize, &read); if (result == FR_OK) { - if (read > context.state.wav.size) { - read = context.state.wav.size; + if (read > context.wav.size) { + read = context.wav.size; } - context.state.wav.size -= read; + context.wav.size -= read; - if (read != context.state.wav.readSize) { - f_close(&context.state.wav.file); + if (read != context.wav.readSize) { + f_close(&context.wav.file); fragment.clear(); } uint16_t * samples = buffer->data; - if (context.state.wav.codec == CODEC_ID_PCM_S16LE) { + if (context.wav.codec == CODEC_ID_PCM_S16LE) { read /= 2; for (uint32_t i=0; i 0) { int points; - double toneIdx = context.state.tone.idx; + double toneIdx = context.tone.idx; - if (fragment.tone.freq != context.state.tone.freq) { - context.state.tone.freq = fragment.tone.freq; - context.state.tone.step = double(DIM(sineValues)*fragment.tone.freq) / AUDIO_SAMPLE_RATE; - context.state.tone.volume = evalVolumeRatio(fragment.tone.freq, volume); + if (fragment.tone.reset) { + context.tone.duration = 0; + context.tone.pause = 0; + } + + if (fragment.tone.freq != context.tone.freq) { + context.tone.freq = fragment.tone.freq; + context.tone.step = double(DIM(sineValues)*fragment.tone.freq) / AUDIO_SAMPLE_RATE; + context.tone.volume = evalVolumeRatio(fragment.tone.freq, volume); } if (fragment.tone.freqIncr) { @@ -577,38 +582,37 @@ int AudioQueue::mixTone(AudioContext &context, AudioBuffer *buffer, int volume, else { duration = remainingDuration; points = (duration * AUDIO_BUFFER_SIZE) / AUDIO_BUFFER_DURATION; - unsigned int end = toneIdx + (context.state.tone.step * points); + unsigned int end = toneIdx + (context.tone.step * points); if (end > DIM(sineValues)) end -= (end % DIM(sineValues)); else end = DIM(sineValues); - points = (double(end) - toneIdx) / context.state.tone.step; + points = (double(end) - toneIdx) / context.tone.step; } for (int i=0; idata[i], sample, fade); - toneIdx += context.state.tone.step; + toneIdx += context.tone.step; if ((unsigned int)toneIdx >= DIM(sineValues)) toneIdx -= DIM(sineValues); } if (remainingDuration > AUDIO_BUFFER_DURATION) { - context.state.tone.duration += AUDIO_BUFFER_DURATION; - context.state.tone.idx = toneIdx; + context.tone.duration += AUDIO_BUFFER_DURATION; + context.tone.idx = toneIdx; return AUDIO_BUFFER_SIZE; } else { - context.state.tone.duration = 32000; // once the tone is finished, it's not possible to update its frequency and duration - context.state.tone.idx = 0; + context.tone.duration = 32000; // once the tone is finished, it's not possible to update its frequency and duration } } - remainingDuration = fragment.tone.pause - context.state.tone.pause; + remainingDuration = fragment.tone.pause - context.tone.pause; if (remainingDuration > 0) { result = AUDIO_BUFFER_SIZE; - context.state.tone.pause += min(AUDIO_BUFFER_DURATION-duration, fragment.tone.pause); - if (fragment.tone.pause > context.state.tone.pause) + context.tone.pause += min(AUDIO_BUFFER_DURATION-duration, fragment.tone.pause); + if (fragment.tone.pause > context.tone.pause) return result; } @@ -729,10 +733,13 @@ void AudioQueue::playTone(uint16_t freq, uint16_t len, uint16_t pause, uint8_t f if (flags & PLAY_BACKGROUND) { AudioFragment & fragment = backgroundContext.fragment; + if (fragment.type != FRAGMENT_TONE) + backgroundContext.clear(); fragment.type = FRAGMENT_TONE; fragment.tone.freq = freq; fragment.tone.duration = len; fragment.tone.pause = pause; + fragment.tone.reset = (flags & PLAY_NOW); } else { freq += g_eeGeneral.speakerPitch * 15; diff --git a/radio/src/audio_arm.h b/radio/src/audio_arm.h index b6be64126..40be7179a 100644 --- a/radio/src/audio_arm.h +++ b/radio/src/audio_arm.h @@ -78,6 +78,7 @@ struct AudioFragment { uint16_t duration; uint16_t pause; int8_t freqIncr; + uint8_t reset; } tone; char file[AUDIO_FILENAME_MAXLEN+1]; @@ -93,31 +94,39 @@ class AudioContext { public: AudioFragment fragment; - union { #if defined(SDCARD) - struct { - FIL file; - uint8_t codec; - uint32_t freq; - uint32_t size; - uint8_t resampleRatio; - uint16_t readSize; - } wav; + struct { + FIL file; + uint8_t codec; + uint32_t freq; + uint32_t size; + uint8_t resampleRatio; + uint16_t readSize; + } wav; #endif - struct { - double step; - double idx; - float volume; - uint16_t freq; - uint16_t duration; - uint16_t pause; - } tone; - } state; + struct { + double step; + double idx; + float volume; + uint16_t freq; + uint16_t duration; + uint16_t pause; + inline void clear() + { + step = 0; + idx = 0; + volume = 0; + freq = 0; + duration = 0; + pause = 0; + } + } tone; inline void clear() { - memset(this, 0, sizeof(AudioContext)); + fragment.clear(); + tone.clear(); } }; @@ -261,7 +270,7 @@ void audioStart(); #define AUDIO_TRIM_END(f) AUDIO_BUZZER(audioEvent(AU_TRIM_END, f), beep(2)) #define AUDIO_TRIM(event, f) AUDIO_BUZZER(audioEvent(AU_TRIM_MOVE, f), { if (!IS_KEY_FIRST(event)) warble = true; beep(1); }) #define AUDIO_PLAY(p) audioEvent(p) -#define AUDIO_VARIO(f, t) audioQueue.playTone(f, t, 0, PLAY_BACKGROUND) +#define AUDIO_VARIO(fq, t, p, f) audioQueue.playTone(fq, t, p, f) #if defined(PCBTARANIS) #define AUDIO_A1_ORANGE() audioEvent(AU_A1_ORANGE) diff --git a/radio/src/maths.cpp b/radio/src/maths.cpp index 727814ded..67456c95d 100644 --- a/radio/src/maths.cpp +++ b/radio/src/maths.cpp @@ -114,7 +114,53 @@ void getGpsDistance() } #endif -#if defined(FRSKY) && defined(VARIO) +#if defined(CPUARM) + +void varioWakeup() +{ + if (isFunctionActive(FUNCTION_VARIO)) { + int varioFreq, varioDuration, varioPause=0; + uint8_t varioFlags; + +#if 1 + #warning "Ele stick for vario tests" + int verticalSpeed = getValue(MIXSRC_Ele); +#else + int verticalSpeed = frskyData.hub.varioSpeed; +#endif + + int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; + int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; + int varioMax = (10+(int)g_model.frsky.varioMax) * 100; + int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; + + if (verticalSpeed > varioMax) + verticalSpeed = varioMax; + else if (verticalSpeed < varioMin) + verticalSpeed = varioMin; + + if (verticalSpeed > varioCenterMin) { + varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * verticalSpeed) / varioMax); + int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / (varioMax * varioMax); + if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax) + varioDuration = varioPeriod / 4; + else + varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100; + varioPause = varioPeriod - varioDuration; + varioFlags = PLAY_BACKGROUND; + } + else { + varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-BEEP_MIN_FREQ) * verticalSpeed) / varioMin); + varioDuration = 80; // TODO 20 // continuous beep: we will enter again here before the tone ends + varioFlags = PLAY_BACKGROUND|PLAY_NOW; + } + + AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags); + } +} + +#elif defined(FRSKY) && defined(VARIO) + void varioWakeup() { static tmr10ms_t s_varioTmr; @@ -123,12 +169,7 @@ void varioWakeup() if (isFunctionActive(FUNCTION_VARIO)) { #if defined(AUDIO) cli(); -#if 1 - #warning "Ele stick for vario tests" - int verticalSpeed = getValue(MIXSRC_Ele); -#else int verticalSpeed = frskyData.hub.varioSpeed; -#endif sei(); #if defined(PCBSTD) @@ -140,10 +181,10 @@ void varioWakeup() verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); if ((int16_t)(s_varioTmr-tmr10ms) < 0) { - uint8_t varioBeepFreq = (verticalSpeed * 10 + 16000) >> 8; - uint8_t varioBeepTime = (1600 - verticalSpeed) / 100; - s_varioTmr = tmr10ms + (varioBeepTime*2); - AUDIO_VARIO(varioBeepFreq, varioBeepTime); + uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8; + uint8_t varioDuration = (1600 - verticalSpeed) / 100; + s_varioTmr = tmr10ms + (varioDuration*2); + AUDIO_VARIO(varioFreq, varioDuration); } } #else @@ -158,34 +199,17 @@ void varioWakeup() verticalSpeed = varioMin; if ((int16_t)(s_varioTmr - tmr10ms) < 0) { -#if defined(CPUARM) - s_varioTmr = tmr10ms; - int varioBeepFreq, varioBeepTime; - if (verticalSpeed > varioCenterMin) { - varioBeepFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * verticalSpeed) / varioMax); - int period = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * verticalSpeed * verticalSpeed) / (varioMax * varioMax); - if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax) - varioBeepTime = period / 4; - else - varioBeepTime = period * (85 - ((verticalSpeed-varioCenterMin) * 25 / (varioCenterMax-varioCenterMin))) / 100; - } - else { - varioBeepFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-BEEP_MIN_FREQ) * verticalSpeed) / varioMin); - varioBeepTime = 80; // TODO 20 // continuous beep: we will enter again here before the tone ends - } -#else - uint8_t varioBeepFreq, varioBeepTime; + uint8_t varioFreq, varioDuration; if (verticalSpeed > 0) { - varioBeepFreq = (verticalSpeed * 4 + 8000) >> 7; - varioBeepTime = (8000 - verticalSpeed * 5) / 100; + varioFreq = (verticalSpeed * 4 + 8000) >> 7; + varioDuration = (8000 - verticalSpeed * 5) / 100; } else { - varioBeepFreq = (verticalSpeed * 3 + 8000) >> 7; - varioBeepTime = 20; + varioFreq = (verticalSpeed * 3 + 8000) >> 7; + varioDuration = 20; } - s_varioTmr = tmr10ms + (varioBeepTime/2); -#endif - AUDIO_VARIO(varioBeepFreq, varioBeepTime); + s_varioTmr = tmr10ms + (varioDuration/2); + AUDIO_VARIO(varioFreq, varioDuration); } #endif @@ -217,4 +241,5 @@ void varioWakeup() s_varioTmr = tmr10ms; } } + #endif