|
-
-
- #include <Arduino.h>
- #include "analyze_tonedetect.h"
- #include "utility/dspinst.h"
-
- #if defined(__ARM_ARCH_7EM__)
-
- static inline int32_t multiply_32x32_rshift30(int32_t a, int32_t b) __attribute__((always_inline));
- static inline int32_t multiply_32x32_rshift30(int32_t a, int32_t b)
- {
- return ((int64_t)a * (int64_t)b) >> 30;
- }
-
-
-
- void AudioAnalyzeToneDetect::update(void)
- {
- audio_block_t *block;
- int32_t q0, q1, q2, coef;
- const int16_t *p, *end;
- uint16_t n;
-
- block = receiveReadOnly();
- if (!block) return;
- if (!enabled) {
- release(block);
- return;
- }
- p = block->data;
- end = p + AUDIO_BLOCK_SAMPLES;
- n = count;
- coef = coefficient;
- q1 = s1;
- q2 = s2;
- do {
-
- #ifdef TONE_DETECT_FAST
- q0 = (*p++) + (multiply_32x32_rshift32_rounded(coef, q1) << 2) - q2;
- #else
- q0 = (*p++) + multiply_32x32_rshift30(coef, q1) - q2;
-
- #endif
- q2 = q1;
- q1 = q0;
- if (--n == 0) {
- out1 = q1;
- out2 = q2;
- q1 = 0;
- q2 = 0;
- new_output = true;
- n = length;
- }
- } while (p < end);
- count = n;
- s1 = q1;
- s2 = q2;
- release(block);
- }
-
- void AudioAnalyzeToneDetect::set_params(int32_t coef, uint16_t cycles, uint16_t len)
- {
- __disable_irq();
- coefficient = coef;
- ncycles = cycles;
- length = len;
- count = len;
- s1 = 0;
- s2 = 0;
- enabled = true;
- __enable_irq();
-
- }
-
- float AudioAnalyzeToneDetect::read(void)
- {
- int32_t coef, q1, q2, power;
- uint16_t len;
-
- __disable_irq();
- coef = coefficient;
- q1 = out1;
- q2 = out2;
- len = length;
- __enable_irq();
- #ifdef TONE_DETECT_FAST
- power = multiply_32x32_rshift32_rounded(q2, q2);
- power = multiply_accumulate_32x32_rshift32_rounded(power, q1, q1);
- power = multiply_subtract_32x32_rshift32_rounded(power,
- multiply_32x32_rshift30(q1, q2), coef);
- power <<= 4;
- #else
- int64_t power64;
- power64 = (int64_t)q2 * (int64_t)q2;
- power64 += (int64_t)q1 * (int64_t)q1;
- power64 -= (((int64_t)q1 * (int64_t)q2) >> 30) * (int64_t)coef;
- power = power64 >> 28;
- #endif
- return sqrtf((float)power) / (float)len;
- }
-
-
- AudioAnalyzeToneDetect::operator bool()
- {
- int32_t coef, q1, q2, power, trigger;
- uint16_t len;
-
- __disable_irq();
- coef = coefficient;
- q1 = out1;
- q2 = out2;
- len = length;
- __enable_irq();
- #ifdef TONE_DETECT_FAST
- power = multiply_32x32_rshift32_rounded(q2, q2);
- power = multiply_accumulate_32x32_rshift32_rounded(power, q1, q1);
- power = multiply_subtract_32x32_rshift32_rounded(power,
- multiply_32x32_rshift30(q1, q2), coef);
- power <<= 4;
- #else
- int64_t power64;
- power64 = (int64_t)q2 * (int64_t)q2;
- power64 += (int64_t)q1 * (int64_t)q1;
- power64 -= (((int64_t)q1 * (int64_t)q2) >> 30) * (int64_t)coef;
- power = power64 >> 28;
- #endif
- trigger = (uint32_t)len * thresh;
- trigger = multiply_32x32_rshift32(trigger, trigger);
-
-
- return (power >= trigger);
-
-
- }
-
-
- #elif defined(KINETISL)
-
- void AudioAnalyzeToneDetect::update(void)
- {
- audio_block_t *block;
- block = receiveReadOnly();
- if (block) release(block);
- }
-
- #endif
|