mirror
/
tnc3-firmware
kopia lustrzana https://github.com/mobilinkd/tnc3-firmware
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
tnc3-firmware/TNC/AudioInput.cpp

1059 wiersze
28 KiB
C++
Czysty Wina Historia

// Copyright 2018 Rob Riggs <rob@mobilinkd.com>
// All rights reserved.
#include "AudioInput.hpp"
#include "AfskDemodulator.hpp"
#include "AudioLevel.hpp"
#include "Log.h"
#include "KissHardware.hpp"
#include "GPIO.hpp"
#include "HdlcFrame.hpp"
#include "memory.hpp"
#include "IirFilter.hpp"
#include "FilterCoefficients.hpp"
#include "PortInterface.hpp"
#include "Goertzel.h"
#include "DCD.h"
#include "arm_math.h"
#include "stm32l4xx_hal.h"
#include <algorithm>
#include <numeric>
#include <cstring>
#include <cstdint>
#include <atomic>
extern osMessageQId ioEventQueueHandle;
extern "C" void SystemClock_Config(void);
// 1kB
typedef mobilinkd::tnc::memory::Pool<
8, mobilinkd::tnc::audio::ADC_BUFFER_SIZE * 2> adc_pool_type;
adc_pool_type adcPool;
// DMA Conversion first half complete.
extern "C" void HAL_ADC_ConvHalfCpltCallback(ADC_HandleTypeDef*) {
using namespace mobilinkd::tnc::audio;
auto block = adcPool.allocate();
if (!block) return;
memmove(block->buffer, adc_buffer, ADC_BUFFER_SIZE * 2);
auto status = osMessagePut(adcInputQueueHandle, (uint32_t) block, 0);
if (status != osOK) adcPool.deallocate(block);
}
// DMA Conversion second half complete.
extern "C" void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef*) {
using namespace mobilinkd::tnc::audio;
auto block = adcPool.allocate();
if (!block) return;
memmove(block->buffer, adc_buffer + DMA_TRANSFER_SIZE, ADC_BUFFER_SIZE * 2);
auto status = osMessagePut(adcInputQueueHandle, (uint32_t) block, 0);
if (status != osOK) adcPool.deallocate(block);
}
extern "C" void HAL_ADC_ErrorCallback(ADC_HandleTypeDef* /* hadc */) {
using namespace mobilinkd::tnc::audio;
// __HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_EOC | ADC_FLAG_EOS | ADC_FLAG_OVR));
// HAL_DMA_Start(hadc->DMA_Handle, (uint32_t)&hadc->Instance->DR, (uint32_t)adc_buffer, ADC_BUFFER_SIZE * 2);
}
extern "C" void startAudioInputTask(void const*) {
using namespace mobilinkd::tnc::audio;
DEBUG("startAudioInputTask");
adcPool.init();
uint8_t adcState = mobilinkd::tnc::audio::IDLE;
while (true) {
osEvent event = osMessageGet(audioInputQueueHandle, osWaitForever);
if (event.status != osEventMessage) continue;
adcState = event.value.v;
switch (adcState) {
case STOPPED:
DEBUG("STOPPED");
// stop();
break;
case DEMODULATOR:
DEBUG("DEMODULATOR");
demodulatorTask();
break;
case STREAM_AMPLIFIED_INPUT_LEVEL:
DEBUG("STREAM_AMPLIFIED_INPUT_LEVEL");
streamAmplifiedInputLevels();
break;
case POLL_AMPLIFIED_INPUT_LEVEL:
DEBUG("POLL_AMPLIFIED_INPUT_LEVEL");
pollAmplifiedInputLevel();
break;
case POLL_BATTERY_LEVEL:
DEBUG("POLL_BATTERY_LEVEL");
pollBatteryLevel();
break;
case POLL_TWIST_LEVEL:
DEBUG("POLL_TWIST_LEVEL");
pollInputTwist();
break;
case STREAM_AVERAGE_TWIST_LEVEL:
DEBUG("STREAM_AVERAGE_TWIST_LEVEL");
// streamAverageInputTwist();
break;
case STREAM_INSTANT_TWIST_LEVEL:
DEBUG("STREAM_INSTANT_TWIST_LEVEL");
// streamInstantInputTwist();
break;
case AUTO_ADJUST_INPUT_LEVEL:
DEBUG("AUTO_ADJUST_INPUT_LEVEL");
autoAudioInputLevel();
break;
case CONFIGURE_INPUT_LEVELS:
DEBUG("CONFIGURE_INPUT_LEVELS");
setAudioInputLevels();
break;
case UPDATE_SETTINGS:
DEBUG("UPDATE_SETTINGS");
setAudioInputLevels();
break;
case IDLE:
DEBUG("IDLE");
break;
default:
break;
}
}
}
namespace mobilinkd { namespace tnc { namespace audio {
/*
FIR filter designed with
http://t-filter.appspot.com
sampling frequency: 26400 Hz
* 0 Hz - 800 Hz
gain = 0
desired attenuation = -40 dB
actual attenuation = -41.713187739640446 dB
* 1100 Hz - 2300 Hz
gain = 1
desired ripple = 3 dB
actual ripple = 1.9403554103597218 dB
* 2600 Hz - 13200 Hz
gain = 0
desired attenuation = -40 dB
actual attenuation = -41.713187739640446 dB
*/
#define FILTER_TAP_NUM 121
const float taps_0dB_121[] = {
0.00404434702588704,
-0.0003678805989470367,
-0.0011037474176397869,
-0.0023718433790735397,
-0.004074774206090812,
-0.005873042355767296,
-0.007185024927682025,
-0.00733586918005499,
-0.005849936137673611,
-0.0026340821242355635,
0.001829866887380395,
0.006596559367932984,
0.010513363703436297,
0.012581963716759209,
0.012278717176141912,
0.009711068794837135,
0.005595156551222992,
0.0010151465722928203,
-0.0028911100291917724,
-0.00525632756952279,
-0.005713225280738633,
-0.004481173891886628,
-0.0022979447479362165,
-0.00020042687909196258,
0.0007698191454281245,
-0.00010521900913954391,
-0.0029591195718788473,
-0.00722329412770922,
-0.01171525034180743,
-0.014940096229612041,
-0.015533747313789608,
-0.012735965353880947,
-0.006719733175529481,
0.0013488096154956725,
0.00958058819325866,
0.015905894530456915,
0.018729580500272548,
0.01748781569748245,
0.012897342590446394,
0.006795493031300682,
0.0015881037163025886,
-0.0005374299552534915,
0.0016047331197704602,
0.007673729209166328,
0.015724242346878387,
0.022631203832237833,
0.024935722640487233,
0.01989507252801227,
0.00645078432931977,
-0.014172629267618218,
-0.038442876378037664,
-0.06113274585379875,
-0.07647113797360244,
-0.07957164384983365,
-0.06778356575105521,
-0.04159845430900078,
-0.0048373906972505945,
0.03598583922416374,
0.0730150987796154,
0.09880455186134979,
0.10804223448811107,
0.09880455186134979,
0.0730150987796154,
0.03598583922416374,
-0.0048373906972505945,
-0.04159845430900078,
-0.06778356575105521,
-0.07957164384983365,
-0.07647113797360244,
-0.06113274585379875,
-0.038442876378037664,
-0.014172629267618218,
0.006450784329319771,
0.01989507252801227,
0.024935722640487233,
0.022631203832237833,
0.015724242346878387,
0.007673729209166332,
0.0016047331197704602,
-0.0005374299552534915,
0.0015881037163025886,
0.0067954930313006805,
0.012897342590446394,
0.017487815697482458,
0.01872958050027255,
0.015905894530456915,
0.00958058819325866,
0.0013488096154956762,
-0.006719733175529481,
-0.012735965353880947,
-0.015533747313789608,
-0.014940096229612034,
-0.01171525034180743,
-0.00722329412770922,
-0.0029591195718788473,
-0.00010521900913954758,
0.0007698191454281245,
-0.00020042687909196258,
-0.0022979447479362165,
-0.004481173891886626,
-0.005713225280738633,
-0.005256327569522788,
-0.0028911100291917724,
0.0010151465722928203,
0.005595156551222992,
0.009711068794837135,
0.012278717176141915,
0.012581963716759209,
0.010513363703436297,
0.006596559367932984,
0.001829866887380395,
-0.0026340821242355635,
-0.005849936137673611,
-0.0073358691800549875,
-0.007185024927682024,
-0.005873042355767296,
-0.004074774206090811,
-0.0023718433790735397,
-0.0011037474176397869,
-0.00036788059894704404,
0.00404434702588704
};
/*
FIR filter designed with
http://t-filter.appspot.com
sampling frequency: 26400 Hz
* 0 Hz - 600 Hz
gain = 0
desired attenuation = -40 dB
actual attenuation = -41.59537969202882 dB
* 1100 Hz - 2300 Hz
gain = 1
desired ripple = 3 dB
actual ripple = 1.9670775534013671 dB
* 2800 Hz - 13200 Hz
gain = 0
desired attenuation = -40 dB
actual attenuation = -41.59537969202882 dB
*/
// #define FILTER_TAP_NUM 73
const float taps_0dB_73[] = {
0.0010893641938196257,
0.0029403198794405202,
-0.0037231874681753637,
-0.005078116094780293,
-0.008797286521082463,
-0.011317340935878852,
-0.012200463385017889,
-0.01036925371439487,
-0.005637326405238566,
0.0014334055832832988,
0.009462055516437227,
0.016585173167785613,
0.020968649539195763,
0.021402512805125434,
0.017768177789191805,
0.011189277365350641,
0.003796773667470304,
-0.0019035128640327481,
-0.003853114272608765,
-0.0012626334798333488,
0.004945485136075468,
0.012177421685799305,
0.016832103112238102,
0.01536679512873413,
0.005573647945409955,
-0.012436210925634471,
-0.03581550676890827,
-0.05935854007614169,
-0.07666569528110105,
-0.08180873487685453,
-0.07107700533422828,
-0.0442469908102239,
-0.005044918510227134,
0.03944927956419565,
0.0803589200537307,
0.10908069178917619,
0.11940367705752576,
0.1090806917891762,
0.0803589200537307,
0.03944927956419565,
-0.005044918510227135,
-0.0442469908102239,
-0.07107700533422828,
-0.08180873487685453,
-0.07666569528110105,
-0.05935854007614169,
-0.03581550676890827,
-0.012436210925634473,
0.005573647945409955,
0.015366795128734134,
0.016832103112238102,
0.012177421685799305,
0.004945485136075468,
-0.0012626334798333488,
-0.003853114272608765,
-0.001903512864032745,
0.003796773667470304,
0.011189277365350641,
0.017768177789191805,
0.021402512805125434,
0.020968649539195763,
0.016585173167785607,
0.009462055516437227,
0.0014334055832832988,
-0.005637326405238568,
-0.01036925371439487,
-0.012200463385017889,
-0.011317340935878852,
-0.008797286521082463,
-0.005078116094780293,
-0.0037231874681753637,
0.0029403198794405202,
0.0010893641938196242
};
uint32_t adc_buffer[ADC_BUFFER_SIZE]; // Two samples per element.
typedef FirFilter<ADC_BUFFER_SIZE, FILTER_TAP_NUM> audio_filter_type;
audio_filter_type audio_filter;
mobilinkd::tnc::afsk1200::emphasis_filter_type filter_1;
mobilinkd::tnc::afsk1200::emphasis_filter_type filter_2;
mobilinkd::tnc::afsk1200::emphasis_filter_type filter_3;
mobilinkd::tnc::afsk1200::Demodulator& getDemod1(const TFirCoefficients<9>& f) __attribute__((noinline));
mobilinkd::tnc::afsk1200::Demodulator& getDemod2(const TFirCoefficients<9>& f) __attribute__((noinline));
mobilinkd::tnc::afsk1200::Demodulator& getDemod3(const TFirCoefficients<9>& f) __attribute__((noinline));
mobilinkd::tnc::afsk1200::Demodulator& getDemod1(const TFirCoefficients<9>& f) {
filter_1.init(f);
static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_1);
return instance;
}
mobilinkd::tnc::afsk1200::Demodulator& getDemod2(const TFirCoefficients<9>& f) {
filter_2.init(f);
static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_2);
return instance;
}
mobilinkd::tnc::afsk1200::Demodulator& getDemod3(const TFirCoefficients<9>& f) {
filter_3.init(f);
static mobilinkd::tnc::afsk1200::Demodulator instance(26400, filter_3);
return instance;
}
void demodulatorTask() {
DEBUG("enter demodulatorTask");
audio_filter.init(taps_0dB_121);
// rx_twist is 6dB for discriminator input and 0db for de-emphasized input.
auto twist = kiss::settings().rx_twist;
mobilinkd::tnc::afsk1200::Demodulator& demod1 = getDemod1(*filter::fir::AfskFilters[twist + 3]);
mobilinkd::tnc::afsk1200::Demodulator& demod2 = getDemod2(*filter::fir::AfskFilters[twist + 6]);
mobilinkd::tnc::afsk1200::Demodulator& demod3 = getDemod3(*filter::fir::AfskFilters[twist + 9]);
startADC(AUDIO_IN);
mobilinkd::tnc::hdlc::IoFrame* frame = 0;
uint16_t last_fcs = 0;
uint32_t last_counter = 0;
uint32_t counter = 0;
bool dcd_status{false};
while (true) {
osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
if (peek.status == osEventMessage) break;
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) {
continue;
}
++counter;
auto block = (adc_pool_type::chunk_type*) evt.value.p;
auto samples = (int16_t*) block->buffer;
float* audio = audio_filter(samples);
adcPool.deallocate(block);
#if 1
frame = demod1(audio, ADC_BUFFER_SIZE);
if (frame) {
if (frame->fcs() != last_fcs or counter > last_counter + 2) {
auto save_fcs = frame->fcs();
if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
last_fcs = save_fcs;
last_counter = counter;
} else {
hdlc::release(frame);
}
}
else {
hdlc::release(frame);
}
}
#endif
#if 1
frame = demod2(audio, ADC_BUFFER_SIZE);
if (frame) {
if (frame->fcs() != last_fcs or counter > last_counter + 2) {
auto save_fcs = frame->fcs();
if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
last_fcs = save_fcs;
last_counter = counter;
} else {
hdlc::release(frame);
}
}
else {
hdlc::release(frame);
}
}
#endif
#if 1
frame = demod3(audio, ADC_BUFFER_SIZE);
if (frame) {
if (frame->fcs() != last_fcs or counter > last_counter + 2) {
auto save_fcs = frame->fcs();
if (osMessagePut(ioEventQueueHandle, (uint32_t) frame, 1) == osOK) {
last_fcs = save_fcs;
last_counter = counter;
} else {
hdlc::release(frame);
}
}
else {
hdlc::release(frame);
}
}
#endif
bool new_dcd_status = demod1.locked() or demod2.locked() or demod3.locked();
if (new_dcd_status xor dcd_status) {
dcd_status = new_dcd_status;
if (dcd_status) {
dcd_on();
} else {
dcd_off();
}
}
}
stopADC();
dcd_off();
DEBUG("exit demodulatorTask");
}
void streamLevels(uint32_t channel, uint8_t cmd) {
// Stream out Vpp, Vavg, Vmin, Vmax as four 16-bit values, left justified.
uint8_t data[9];
INFO("streamLevels: start");
startADC(channel);
while (true) {
osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
if (peek.status == osEventMessage) break;
uint16_t count = 0;
uint32_t accum = 0;
uint16_t vmin = std::numeric_limits<uint16_t>::max();
uint16_t vmax = std::numeric_limits<uint16_t>::min();
while (count < 2640) {
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
count += ADC_BUFFER_SIZE;
auto block = (adc_pool_type::chunk_type*) evt.value.p;
auto start = (uint16_t*) block->buffer;
auto end = start + ADC_BUFFER_SIZE;
vmin = std::min(vmin, *std::min_element(start, end));
vmax = std::max(vmax, *std::max_element(start, end));
accum = std::accumulate(start, end, accum);
adcPool.deallocate(block);
}
uint16_t pp = (vmax - vmin) << 4;
uint16_t avg = (accum / count) << 4;
vmin <<= 4;
vmax <<= 4;
data[0] = cmd;
data[1] = (pp >> 8) & 0xFF; // Vpp
data[2] = (pp & 0xFF);
data[3] = (avg >> 8) & 0xFF; // Vavg (DC level)
data[4] = (avg & 0xFF);
data[5] = (vmin >> 8) & 0xFF; // Vmin
data[6] = (vmin & 0xFF);
data[7] = (vmax >> 8) & 0xFF; // Vmax
data[8] = (vmax & 0xFF);
ioport->write(data, 9, 6, 10);
}
stopADC();
DEBUG("exit streamLevels");
}
levels_type readLevels(uint32_t channel, uint32_t samples) {
DEBUG("enter readLevels");
// Return Vpp, Vavg, Vmin, Vmax as four 16-bit values, right justified.
uint16_t count = 0;
uint32_t accum = 0;
uint16_t vmin = std::numeric_limits<uint16_t>::max();
uint16_t vmax = std::numeric_limits<uint16_t>::min();
INFO("readLevels: start");
startADC(channel);
while (count < samples) {
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
auto block = (adc_pool_type::chunk_type*) evt.value.p;
auto start = (uint16_t*) block->buffer;
auto end = start + ADC_BUFFER_SIZE;
vmin = std::min(vmin, *std::min_element(start, end));
vmax = std::max(vmax, *std::max_element(start, end));
accum = std::accumulate(start, end, accum);
adcPool.deallocate(block);
count += ADC_BUFFER_SIZE;
}
stopADC();
uint16_t pp = vmax - vmin;
uint16_t avg = accum / count;
DEBUG("exit readLevels");
return levels_type(pp, avg, vmin, vmax);
}
/**
* This provides 100Hz resolution to the Goerztel filter.
*/
constexpr uint32_t TWIST_SAMPLE_SIZE = 88;
/*
* Return twist as a the difference in dB between mark and space. The
* expected values are about 0dB for discriminator output and about 5.5dB
* for de-emphasized audio.
*/
float readTwist()
{
DEBUG("enter readTwist");
constexpr uint32_t channel = AUDIO_IN;
float g1200 = 0.0f;
float g2200 = 0.0f;
GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf1200(1200.0, 0);
GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf2200(2200.0, 0);
const uint32_t AVG_SAMPLES = 20;
startADC(channel);
for (uint32_t i = 0; i != AVG_SAMPLES; ++i)
{
uint32_t count = 0;
while (count < TWIST_SAMPLE_SIZE)
{
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
auto block = (adc_pool_type::chunk_type*) evt.value.p;
uint16_t* data = (uint16_t*) block->buffer;
gf1200(data, ADC_BUFFER_SIZE);
gf2200(data, ADC_BUFFER_SIZE);
adcPool.deallocate(block);
count += ADC_BUFFER_SIZE;
}
g1200 += (gf1200 / count);
g2200 += (gf2200 / count);
gf1200.reset();
gf2200.reset();
}
stopADC();
g1200 = 10.0f * log10f(g1200 / AVG_SAMPLES);
g2200 = 10.0f * log10f(g2200 / AVG_SAMPLES);
auto result = g1200 - g2200;
INFO("Twist = %d / 100 (%d - %d)", int(result*100), int(g1200), int(g2200));
DEBUG("exit readTwist");
return result;
}
/*
* Get the input twist level as a pair of numbers -- the relative dB
* level of the Bell 202 mark and space tones.
*
* This is intended to measure noise levels on an empty channel.
*
* When de-emphasis is applied, the noise at 1200Hz will be about 5.5dB
* higher than at 2200Hz. When de-emphasis is not applied (discriminator
* output), the levels should be about the same.
*
* This is used to adjust the demodulator filters so that the proper
* input twist is applied to the signal. In general, properly modulated
* signals are expected to be pre-emphasized so that they are equal
* when de-emphasis is applied.
*
* If no de-emphasis is detected, the de-emphasis has to be applied in
* the demodulator.
*
* This takes about 5 seconds to complete as it averages 100 50ms samples
* to get a reasonable sampling of the noise.
*/
void pollInputTwist()
{
DEBUG("enter pollInputTwist");
constexpr uint32_t channel = AUDIO_IN;
float g1200 = 0.0f;
float g2200 = 0.0f;
GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf1200(1200.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, SAMPLE_RATE> gf2200(2200.0);
const uint32_t AVG_SAMPLES = 100;
startADC(channel);
for (uint32_t i = 0; i != AVG_SAMPLES; ++i) {
uint32_t count = 0;
while (count < TWIST_SAMPLE_SIZE) {
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
count += ADC_BUFFER_SIZE;
auto block = (adc_pool_type::chunk_type*) evt.value.p;
uint16_t* data = (uint16_t*) block->buffer;
gf1200(data, ADC_BUFFER_SIZE);
gf2200(data, ADC_BUFFER_SIZE);
adcPool.deallocate(block);
}
g1200 += 10.0 * log10(gf1200);
g2200 += 10.0 * log10(gf2200);
gf1200.reset();
gf2200.reset();
}
stopADC();
DEBUG("pollInputTwist: MARK=%d, SPACE=%d (x100)",
int(g1200 * 100.0 / AVG_SAMPLES), int(g2200 * 100.0 / AVG_SAMPLES));
int16_t g1200i = int16_t(g1200 * 256 / AVG_SAMPLES);
int16_t g2200i = int16_t(g2200 * 256 / AVG_SAMPLES);
uint8_t buffer[5];
buffer[0] = kiss::hardware::POLL_INPUT_TWIST;
buffer[1] = (g1200i >> 8) & 0xFF;
buffer[2] = g1200i & 0xFF;
buffer[3] = (g2200i >> 8) & 0xFF;
buffer[4] = g2200i & 0xFF;
ioport->write(buffer, 5, 6, 10);
DEBUG("exit pollInputTwist");
}
#if 0
void streamAverageInputTwist()
{
DEBUG("enter streamAverageInputTwist");
constexpr uint32_t channel = AUDIO_IN;
startADC(channel);
uint32_t acount = 0;
float g700 = 0.0f;
float g1200 = 0.0f;
float g1700 = 0.0f;
float g2200 = 0.0f;
float g2700 = 0.0f;
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf700(700.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1200(1200.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1700(1700.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2200(2200.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2700(2700.0);
while (true) {
osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
if (peek.status == osEventMessage) break;
acount++;
uint32_t count = 0;
while (count < TWIST_SAMPLE_SIZE) {
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
count += ADC_BUFFER_SIZE;
auto block = (adc_pool_type::chunk_type*) evt.value.v;
uint16_t* data = (uint16_t*) block->buffer;
gf700(data, ADC_BUFFER_SIZE);
gf1200(data, ADC_BUFFER_SIZE);
gf1700(data, ADC_BUFFER_SIZE);
gf2200(data, ADC_BUFFER_SIZE);
gf2700(data, ADC_BUFFER_SIZE);
adcPool.deallocate(block);
}
g700 += 10.0 * log10(gf700);
g1200 += 10.0 * log10(gf1200);
g1700 += 10.0 * log10(gf1700);
g2200 += 10.0 * log10(gf2200);
g2700 += 10.0 * log10(gf2700);
char* buffer = 0;
// @TODO: Make re-entrant printf work (or convert to fixed-point).
int len = asiprintf_r(
&buffer,
"_%f, %f, %f, %f, %f\r\n",
g700 / acount,
g1200 / acount,
g1700 / acount,
g2200 / acount,
g2700 / acount);
if (len > 0) {
buffer[0] = kiss::hardware::POLL_INPUT_TWIST;
ioport->write((uint8_t*)buffer, len - 1, 6, 10);
free(buffer);
}
gf700.reset();
gf1200.reset();
gf1700.reset();
gf2200.reset();
gf2700.reset();
}
stopADC();
DEBUG("exit streamAverageInputTwist");
}
void streamInstantInputTwist()
{
DEBUG("enter streamInstantInputTwist");
constexpr uint32_t channel = AUDIO_IN;
startADC(channel);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf700(700.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1200(1200.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf1700(1700.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2200(2200.0);
GoertzelFilter<TWIST_SAMPLE_SIZE, 26400> gf2700(2700.0);
while (true) {
osEvent peek = osMessagePeek(audioInputQueueHandle, 0);
if (peek.status == osEventMessage) break;
uint32_t count = 0;
while (count < TWIST_SAMPLE_SIZE) {
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
if (evt.status != osEventMessage) continue;
count += ADC_BUFFER_SIZE;
auto block = (adc_pool_type::chunk_type*) evt.value.v;
uint16_t* data = (uint16_t*) block->buffer;
gf700(data, ADC_BUFFER_SIZE);
gf1200(data, ADC_BUFFER_SIZE);
gf1700(data, ADC_BUFFER_SIZE);
gf2200(data, ADC_BUFFER_SIZE);
gf2700(data, ADC_BUFFER_SIZE);
adcPool.deallocate(block);
}
char* buffer = 0;
// @TODO: Make re-entrant printf work (or convert to fixed-point).
int len = asiprintf_r(
&buffer,
"_%f, %f, %f, %f, %f\r\n",
10.0 * log10(gf700),
10.0 * log10(gf1200),
10.0 * log10(gf1700),
10.0 * log10(gf2200),
10.0 * log10(gf2700));
if (len > 0) {
buffer[0] = kiss::hardware::POLL_INPUT_TWIST;
ioport->write((uint8_t*)buffer, len - 1, 6, 10);
free(buffer);
}
gf700.reset();
gf1200.reset();
gf1700.reset();
gf2200.reset();
gf2700.reset();
}
stopADC();
DEBUG("exit streamInstantInputTwist");
}
#endif
void streamAmplifiedInputLevels() {
DEBUG("enter streamAmplifiedInputLevels");
streamLevels(AUDIO_IN, kiss::hardware::POLL_INPUT_LEVEL);
DEBUG("exit streamAmplifiedInputLevels");
}
void pollAmplifiedInputLevel() {
DEBUG("enter pollAmplifiedInputLevel");
uint16_t Vpp, Vavg, Vmin, Vmax;
std::tie(Vpp, Vavg, Vmin, Vmax) = readLevels(AUDIO_IN);
Vpp <<= 4;
Vavg <<= 4;
Vmin <<= 4;
Vmax <<= 4;
uint8_t data[9];
data[0] = kiss::hardware::POLL_INPUT_LEVEL;
data[1] = (Vpp >> 8) & 0xFF; // Vpp
data[2] = (Vpp & 0xFF);
data[3] = (Vavg >> 8) & 0xFF; // Vavg (DC level)
data[4] = (Vavg & 0xFF);
data[5] = (Vmin >> 8) & 0xFF; // Vmin
data[6] = (Vmin & 0xFF);
data[7] = (Vmax >> 8) & 0xFF; // Vmax
data[8] = (Vmax & 0xFF);
ioport->write(data, 9, 6, 10);
DEBUG("exit pollAmplifiedInputLevel");
}
void pollBatteryLevel() {
DEBUG("enter pollBatteryLevel");
ADC_ChannelConfTypeDef sConfig;
sConfig.Channel = ADC_CHANNEL_VREFINT;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.SamplingTime = ADC_SAMPLETIME_247CYCLES_5;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
CxxErrorHandler();
htim6.Init.Period = 48000;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
if (HAL_TIM_Base_Start(&htim6) != HAL_OK)
CxxErrorHandler();
if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
if (HAL_ADC_PollForConversion(&hadc1, 3) != HAL_OK) CxxErrorHandler();
auto vrefint = HAL_ADC_GetValue(&hadc1);
if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
// Disable battery charging while measuring battery voltage.
auto usb_ce = gpio::USB_CE::get();
gpio::USB_CE::on();
gpio::BAT_DIVIDER::off();
HAL_Delay(1);
sConfig.Channel = ADC_CHANNEL_15;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
CxxErrorHandler();
uint32_t vbat = 0;
if (HAL_ADC_Start(&hadc1) != HAL_OK) CxxErrorHandler();
for (size_t i = 0; i != 8; ++i)
{
if (HAL_ADC_PollForConversion(&hadc1, 1) != HAL_OK) CxxErrorHandler();
vbat += HAL_ADC_GetValue(&hadc1);
}
vbat /= 8;
if (HAL_ADC_Stop(&hadc1) != HAL_OK) CxxErrorHandler();
if (HAL_TIM_Base_Stop(&htim6) != HAL_OK)
CxxErrorHandler();
htim6.Init.Period = 1817;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK) CxxErrorHandler();
HAL_Delay(1);
gpio::BAT_DIVIDER::on();
if (!usb_ce) gpio::USB_CE::off(); // Restore battery charging state.
INFO("Vref = %lu", vrefint);
INFO("Vbat = %lu (raw)", vbat);
// Order of operations is important to avoid underflow.
vbat *= 6600;
vbat /= (vref + 1);
INFO("Vbat = %lumV", vbat);
uint8_t data[3];
data[0] = kiss::hardware::GET_BATTERY_LEVEL;
data[1] = (vbat >> 8) & 0xFF;
data[2] = (vbat & 0xFF);
ioport->write(data, 3, 6, 10);
DEBUG("exit pollBatteryLevel");
}
#if 0
void stop() {
osDelay(100);
#if 0
auto restore = SysTick->CTRL;
kiss::settings().input_offset += 6;
setAudioInputLevels();
kiss::settings().input_offset -= 6;
DEBUG("Stop");
// __disable_irq();
vTaskSuspendAll();
SysTick->CTRL = 0;
HAL_COMP_Init(&hcomp1);
HAL_COMP_Start_IT(&hcomp1);
while (adcState == STOPPED) {
// PWR_MAINREGULATOR_ON / PWR_LOWPOWERREGULATOR_ON
HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
}
SystemClock_Config();
SysTick->CTRL = restore;
// __enable_irq();
HAL_COMP_Stop_IT(&hcomp1);
HAL_COMP_DeInit(&hcomp1);
xTaskResumeAll();
setAudioInputLevels();
// adcState = DEMODULATOR;
DEBUG("Wake");
#endif
}
#endif
}}} // mobilinkd::tnc::audio
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik