kopia lustrzana https://github.com/mobilinkd/tnc3-firmware
383 wiersze
18 KiB
C++
383 wiersze
18 KiB
C++
// Copyright 2020 Rob Riggs <rob@mobilinkd.com>
|
|
// All rights reserved.
|
|
|
|
#include "Fsk9600Demodulator.hpp"
|
|
#include "Goertzel.h"
|
|
#include "AudioInput.hpp"
|
|
#include "GPIO.hpp"
|
|
#include "Log.h"
|
|
|
|
namespace mobilinkd { namespace tnc {
|
|
|
|
hdlc::IoFrame* Fsk9600Demodulator::operator()(const q15_t* samples)
|
|
{
|
|
hdlc::IoFrame* result = nullptr;
|
|
|
|
auto filtered = demod_filter.filter(const_cast<q15_t* >(samples));
|
|
|
|
for (size_t i = 0; i != ADC_BLOCK_SIZE; ++i)
|
|
{
|
|
auto sample = filtered[i];
|
|
|
|
bool bit = sample >= 0;
|
|
auto pll = pll_(bit);
|
|
|
|
if (pll.sample)
|
|
{
|
|
locked_ = pll.locked;
|
|
|
|
// We will only ever get one frame because there are
|
|
// not enough bits in a block for more than one.
|
|
if (result) {
|
|
auto tmp = hdlc_decoder_(nrzi_.decode(lfsr_(bit)), locked_);
|
|
if (tmp) hdlc::release(tmp);
|
|
} else {
|
|
result = hdlc_decoder_(nrzi_.decode(lfsr_(bit)), locked_);
|
|
#ifdef KISS_LOGGING
|
|
if (result) {
|
|
INFO("samples = %ld, mean = %d, dev = %d",
|
|
snr_.samples, int(snr_.mean), int(snr_.stdev()));
|
|
INFO("SNR = %dmB", int(snr_.SNR() * 100.0f));
|
|
snr_.reset();
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#ifdef KISS_LOGGING
|
|
if (hdlc_decoder_.active())
|
|
{
|
|
if (!decoding_)
|
|
{
|
|
snr_.reset();
|
|
decoding_ = true;
|
|
}
|
|
snr_.capture(float(abs(sample)));
|
|
} else {
|
|
decoding_ = false;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/*
|
|
* 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 Fsk9600Demodulator::readTwist()
|
|
{
|
|
DEBUG("enter Fsk9600Demodulator::readTwist");
|
|
|
|
float g120 = 0.0f;
|
|
float g4800 = 0.0f;
|
|
|
|
GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf120(120.0, 0);
|
|
GoertzelFilter<ADC_BLOCK_SIZE, SAMPLE_RATE> gf4800(4800.0, 0);
|
|
|
|
const uint32_t AVG_SAMPLES = 160;
|
|
|
|
startADC(416, ADC_BLOCK_SIZE);
|
|
|
|
for (uint32_t i = 0; i != AVG_SAMPLES; ++i)
|
|
{
|
|
uint32_t count = 0;
|
|
while (count < ADC_BLOCK_SIZE)
|
|
{
|
|
osEvent evt = osMessageGet(adcInputQueueHandle, osWaitForever);
|
|
if (evt.status != osEventMessage)
|
|
continue;
|
|
|
|
auto block = (audio::adc_pool_type::chunk_type*) evt.value.p;
|
|
uint16_t* data = (uint16_t*) block->buffer;
|
|
gf120(data, ADC_BLOCK_SIZE);
|
|
gf4800(data, ADC_BLOCK_SIZE);
|
|
|
|
audio::adcPool.deallocate(block);
|
|
|
|
count += ADC_BLOCK_SIZE;
|
|
}
|
|
|
|
g120 += (gf120 / count);
|
|
g4800 += (gf4800 / count);
|
|
|
|
gf120.reset();
|
|
gf4800.reset();
|
|
}
|
|
|
|
IDemodulator::stopADC();
|
|
|
|
g120 = 10.0f * log10f(g120 / AVG_SAMPLES);
|
|
g4800 = 10.0f * log10f(g4800 / AVG_SAMPLES);
|
|
|
|
auto result = g120 - g4800;
|
|
|
|
INFO("9600 Twist = %d / 100 (%d - %d)", int(result * 100), int(g120 * 100),
|
|
int(g4800 * 100));
|
|
|
|
DEBUG("exit Fsk9600Demodulator::readTwist");
|
|
return result;
|
|
}
|
|
|
|
uint32_t Fsk9600Demodulator::readBatteryLevel()
|
|
{
|
|
DEBUG("enter Fsk9600Demodulator::readBatteryLevel");
|
|
|
|
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();
|
|
|
|
gpio::BAT_DIVIDER::on();
|
|
|
|
// Restore battery charging state.
|
|
if (!usb_ce) gpio::USB_CE::off();
|
|
|
|
INFO("Vref = %lu", vrefint);
|
|
INFO("Vbat = %lu (raw)", vbat);
|
|
|
|
// Order of operations is important to avoid underflow.
|
|
vbat *= 6600;
|
|
vbat /= (VREF + 1);
|
|
|
|
uint32_t vref = ((vrefint * 3300) + (VREF / 2)) / VREF;
|
|
|
|
INFO("Vref = %lumV", vref)
|
|
INFO("Vbat = %lumV", vbat);
|
|
|
|
DEBUG("exit Fsk9600Demodulator::readBatteryLevel");
|
|
return vbat;
|
|
}
|
|
|
|
const Fsk9600Demodulator::bpf_bank_type Fsk9600Demodulator::bpf_bank = {{
|
|
// -3dB
|
|
{{
|
|
1, 0, 0, 0, 0, 0, -1, -1, -1, -1, 0, 0,
|
|
0, 0, 1, 2, 2, 3, 2, 2, 0, 0, -2, -3,
|
|
-4, -4, -3, -1, 3, 9, 17, 27, 38, 48, 58, 65,
|
|
68, 67, 60, 46, 25, -1, -32, -67, -102, -135, -160, -175,
|
|
-174, -153, -110, -41, 54, 178, 327, 500, 692, 897, 1109, 1318,
|
|
1518, 1700, 1857, 1981, 2067, 2110, 2110, 2067, 1981, 1857, 1700, 1518,
|
|
1318, 1109, 897, 692, 500, 327, 178, 54, -41, -110, -153, -174,
|
|
-175, -160, -135, -102, -67, -32, -1, 25, 46, 60, 67, 68,
|
|
65, 58, 48, 38, 27, 17, 9, 3, -1, -3, -4, -4,
|
|
-3, -2, 0, 0, 2, 2, 3, 2, 2, 1, 0, 0,
|
|
0, 0, -1, -1, -1, -1, 0, 0, 0, 0, 0, 1
|
|
}},
|
|
// -2dB
|
|
{{
|
|
1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1,
|
|
0, 0, 1, 1, 2, 2, 1, 0, 0, -1, -3, -5,
|
|
-6, -6, -5, -1, 3, 10, 20, 31, 42, 54, 64, 72,
|
|
75, 72, 63, 46, 21, -11, -49, -92, -135, -175, -208, -229,
|
|
-233, -216, -173, -102, 0, 131, 293, 481, 691, 915, 1147, 1378,
|
|
1598, 1799, 1971, 2108, 2203, 2251, 2251, 2203, 2108, 1971, 1799, 1598,
|
|
1378, 1147, 915, 691, 481, 293, 131, 0, -102, -173, -216, -233,
|
|
-229, -208, -175, -135, -92, -49, -11, 21, 46, 63, 72, 75,
|
|
72, 64, 54, 42, 31, 20, 10, 3, -1, -5, -6, -6,
|
|
-5, -3, -1, 0, 0, 1, 2, 2, 1, 1, 0, 0,
|
|
-1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, 1
|
|
}},
|
|
// -1dB
|
|
{{
|
|
1, 0, 0, 0, 0, -1, -1, -2, -2, -2, -2, -1,
|
|
-1, 0, 0, 0, 1, 1, 0, 0, -1, -3, -5, -7,
|
|
-8, -8, -6, -2, 3, 11, 22, 34, 48, 60, 72, 80,
|
|
82, 78, 66, 45, 15, -22, -68, -119, -171, -221, -262, -291,
|
|
-300, -286, -244, -170, -62, 79, 255, 460, 689, 936, 1191, 1445,
|
|
1688, 1909, 2100, 2251, 2356, 2410, 2410, 2356, 2251, 2100, 1909, 1688,
|
|
1445, 1191, 936, 689, 460, 255, 79, -62, -170, -244, -286, -300,
|
|
-291, -262, -221, -171, -119, -68, -22, 15, 45, 66, 78, 82,
|
|
80, 72, 60, 48, 34, 22, 11, 3, -2, -6, -8, -8,
|
|
-7, -5, -3, -1, 0, 0, 1, 1, 0, 0, 0, -1,
|
|
-1, -2, -2, -2, -2, -1, -1, 0, 0, 0, 0, 1
|
|
}},
|
|
// 0dB
|
|
{{
|
|
1, 0, 0, 0, -1, -1, -2, -2, -3, -3, -3, -2,
|
|
-2, -1, 0, 0, 0, 0, 0, -1, -3, -5, -7, -9,
|
|
-10, -10, -7, -3, 3, 13, 25, 39, 53, 68, 80, 88,
|
|
91, 85, 70, 45, 9, -35, -90, -150, -212, -272, -323, -359,
|
|
-376, -366, -324, -247, -132, 21, 212, 436, 688, 959, 1240, 1520,
|
|
1789, 2034, 2244, 2412, 2528, 2587, 2587, 2528, 2412, 2244, 2034, 1789,
|
|
1520, 1240, 959, 688, 436, 212, 21, -132, -247, -324, -366, -376,
|
|
-359, -323, -272, -212, -150, -90, -35, 9, 45, 70, 85, 91,
|
|
88, 80, 68, 53, 39, 25, 13, 3, -3, -7, -10, -10,
|
|
-9, -7, -5, -3, -1, 0, 0, 0, 0, 0, -1, -2,
|
|
-2, -3, -3, -3, -2, -2, -1, -1, 0, 0, 0, 1
|
|
}},
|
|
// 1dB
|
|
{{
|
|
1, 1, 0, 0, -1, -2, -2, -3, -3, -4, -4, -3,
|
|
-3, -2, -1, -1, -1, -1, -1, -3, -5, -7, -9, -11,
|
|
-12, -12, -9, -4, 3, 14, 28, 43, 60, 76, 89, 98,
|
|
100, 93, 75, 45, 3, -50, -114, -185, -258, -329, -390, -437,
|
|
-460, -454, -413, -333, -209, -43, 164, 409, 686, 984, 1295, 1604,
|
|
1901, 2173, 2406, 2592, 2721, 2786, 2786, 2721, 2592, 2406, 2173, 1901,
|
|
1604, 1295, 984, 686, 409, 164, -43, -209, -333, -413, -454, -460,
|
|
-437, -390, -329, -258, -185, -114, -50, 3, 45, 75, 93, 100,
|
|
98, 89, 76, 60, 43, 28, 14, 3, -4, -9, -12, -12,
|
|
-11, -9, -7, -5, -3, -1, -1, -1, -1, -1, -2, -3,
|
|
-3, -4, -4, -3, -3, -2, -2, -1, 0, 0, 1, 1
|
|
}},
|
|
// 2dB
|
|
{{
|
|
1, 1, 0, 0, -1, -2, -3, -4, -4, -5, -5, -4,
|
|
-4, -3, -3, -2, -2, -2, -3, -5, -7, -9, -12, -14,
|
|
-15, -14, -11, -5, 3, 15, 31, 49, 67, 85, 100, 109,
|
|
110, 101, 80, 45, -3, -66, -141, -223, -309, -393, -467, -523,
|
|
-555, -554, -513, -429, -297, -116, 110, 379, 684, 1013, 1356, 1699,
|
|
2028, 2329, 2588, 2794, 2937, 3010, 3010, 2937, 2794, 2588, 2329, 2028,
|
|
1699, 1356, 1013, 684, 379, 110, -116, -297, -429, -513, -554, -555,
|
|
-523, -467, -393, -309, -223, -141, -66, -3, 45, 80, 101, 110,
|
|
109, 100, 85, 67, 49, 31, 15, 3, -5, -11, -14, -15,
|
|
-14, -12, -9, -7, -5, -3, -2, -2, -2, -3, -3, -4,
|
|
-4, -5, -5, -4, -4, -3, -2, -1, 0, 0, 1, 1
|
|
}},
|
|
// 3dB
|
|
{{
|
|
2, 1, 0, 0, -1, -3, -4, -4, -5, -6, -6, -5,
|
|
-5, -4, -4, -3, -3, -4, -5, -7, -9, -12, -15, -17,
|
|
-18, -17, -13, -6, 3, 17, 35, 55, 75, 95, 111, 121,
|
|
122, 111, 86, 45, -12, -85, -171, -267, -367, -465, -552, -620,
|
|
-661, -665, -626, -537, -395, -199, 49, 346, 681, 1045, 1425, 1805,
|
|
2170, 2504, 2792, 3021, 3179, 3261, 3261, 3179, 3021, 2792, 2504, 2170,
|
|
1805, 1425, 1045, 681, 346, 49, -199, -395, -537, -626, -665, -661,
|
|
-620, -552, -465, -367, -267, -171, -85, -12, 45, 86, 111, 122,
|
|
121, 111, 95, 75, 55, 35, 17, 3, -6, -13, -17, -18,
|
|
-17, -15, -12, -9, -7, -5, -4, -3, -3, -4, -4, -5,
|
|
-5, -6, -6, -5, -4, -4, -3, -1, 0, 0, 1, 2
|
|
}},
|
|
// 4dB
|
|
{{
|
|
2, 1, 0, -1, -2, -3, -4, -5, -6, -7, -7, -7,
|
|
-6, -6, -5, -5, -5, -6, -7, -9, -12, -15, -18, -20,
|
|
-21, -20, -16, -8, 3, 19, 39, 61, 85, 107, 125, 135,
|
|
135, 122, 92, 44, -21, -105, -205, -316, -432, -546, -648, -729,
|
|
-780, -791, -752, -658, -505, -291, -18, 308, 679, 1082, 1502, 1924,
|
|
2330, 2701, 3021, 3275, 3452, 3542, 3542, 3452, 3275, 3021, 2701, 2330,
|
|
1924, 1502, 1082, 679, 308, -18, -291, -505, -658, -752, -791, -780,
|
|
-729, -648, -546, -432, -316, -205, -105, -21, 44, 92, 122, 135,
|
|
135, 125, 107, 85, 61, 39, 19, 3, -8, -16, -20, -21,
|
|
-20, -18, -15, -12, -9, -7, -6, -5, -5, -5, -6, -6,
|
|
-7, -7, -7, -6, -5, -4, -3, -2, -1, 0, 1, 2
|
|
}},
|
|
// 5dB
|
|
{{
|
|
2, 1, 0, -1, -2, -4, -5, -6, -7, -8, -8, -8,
|
|
-8, -8, -7, -7, -7, -8, -9, -11, -14, -18, -21, -24,
|
|
-25, -23, -19, -9, 3, 21, 44, 69, 95, 119, 139, 151,
|
|
150, 134, 99, 44, -31, -128, -243, -371, -505, -636, -756, -852,
|
|
-914, -931, -894, -795, -629, -394, -94, 265, 676, 1122, 1589, 2058,
|
|
2508, 2921, 3277, 3560, 3757, 3858, 3858, 3757, 3560, 3277, 2921, 2508,
|
|
2058, 1589, 1122, 676, 265, -94, -394, -629, -795, -894, -931, -914,
|
|
-852, -756, -636, -505, -371, -243, -128, -31, 44, 99, 134, 150,
|
|
151, 139, 119, 95, 69, 44, 21, 3, -9, -19, -23, -25,
|
|
-24, -21, -18, -14, -11, -9, -8, -7, -7, -7, -8, -8,
|
|
-8, -8, -8, -7, -6, -5, -4, -2, -1, 0, 1, 2
|
|
}},
|
|
// 6dB
|
|
{{
|
|
2, 1, 0, -1, -3, -4, -6, -7, -9, -10, -10, -10,
|
|
-10, -9, -9, -9, -9, -10, -12, -14, -18, -21, -25, -28,
|
|
-29, -27, -22, -11, 3, 24, 49, 77, 107, 134, 156, 168,
|
|
167, 148, 107, 44, -43, -154, -286, -432, -586, -738, -876, -989,
|
|
-1064, -1089, -1053, -947, -767, -510, -180, 218, 672, 1168, 1687, 2208,
|
|
2709, 3169, 3565, 3881, 4100, 4212, 4212, 4100, 3881, 3565, 3169, 2709,
|
|
2208, 1687, 1168, 672, 218, -180, -510, -767, -947, -1053, -1089, -1064,
|
|
-989, -876, -738, -586, -432, -286, -154, -43, 44, 107, 148, 167,
|
|
168, 156, 134, 107, 77, 49, 24, 3, -11, -22, -27, -29,
|
|
-28, -25, -21, -18, -14, -12, -10, -9, -9, -9, -9, -10,
|
|
-10, -10, -10, -9, -7, -6, -4, -3, -1, 0, 1, 2
|
|
}},
|
|
// 7dB
|
|
{{
|
|
3, 1, 0, -1, -3, -5, -7, -9, -10, -11, -12, -12,
|
|
-12, -11, -11, -11, -11, -12, -14, -17, -21, -26, -30, -33,
|
|
-34, -32, -25, -13, 3, 27, 55, 87, 120, 150, 174, 188,
|
|
185, 163, 116, 43, -56, -183, -334, -501, -678, -852, -1012, -1143,
|
|
-1232, -1266, -1231, -1119, -923, -641, -276, 164, 668, 1219, 1796, 2376,
|
|
2934, 3446, 3888, 4240, 4484, 4609, 4609, 4484, 4240, 3888, 3446, 2934,
|
|
2376, 1796, 1219, 668, 164, -276, -641, -923, -1119, -1231, -1266, -1232,
|
|
-1143, -1012, -852, -678, -501, -334, -183, -56, 43, 116, 163, 185,
|
|
188, 174, 150, 120, 87, 55, 27, 3, -13, -25, -32, -34,
|
|
-33, -30, -26, -21, -17, -14, -12, -11, -11, -11, -11, -12,
|
|
-12, -12, -11, -10, -9, -7, -5, -3, -1, 0, 1, 3
|
|
}},
|
|
// 8dB
|
|
{{
|
|
3, 1, 0, -2, -4, -6, -8, -10, -12, -13, -14, -14,
|
|
-14, -14, -13, -13, -14, -15, -17, -21, -25, -30, -35, -38,
|
|
-39, -36, -29, -16, 3, 30, 62, 97, 134, 168, 195, 209,
|
|
206, 180, 127, 43, -71, -216, -388, -579, -780, -980, -1164, -1316,
|
|
-1421, -1464, -1431, -1311, -1097, -787, -383, 104, 664, 1277, 1919, 2565,
|
|
3187, 3758, 4251, 4643, 4916, 5055, 5055, 4916, 4643, 4251, 3758, 3187,
|
|
2565, 1919, 1277, 664, 104, -383, -787, -1097, -1311, -1431, -1464, -1421,
|
|
-1316, -1164, -980, -780, -579, -388, -216, -71, 43, 127, 180, 206,
|
|
209, 195, 168, 134, 97, 62, 30, 3, -16, -29, -36, -39,
|
|
-38, -35, -30, -25, -21, -17, -15, -14, -13, -13, -14, -14,
|
|
-14, -14, -13, -12, -10, -8, -6, -4, -2, 0, 1, 3
|
|
}},
|
|
// 9dB
|
|
{{
|
|
3, 2, 0, -2, -4, -7, -9, -12, -14, -15, -16, -17,
|
|
-17, -16, -16, -16, -17, -18, -21, -25, -30, -35, -40, -44,
|
|
-45, -42, -33, -18, 3, 33, 69, 109, 151, 189, 218, 234,
|
|
230, 199, 138, 42, -88, -253, -448, -666, -895, -1123, -1334, -1510,
|
|
-1633, -1687, -1656, -1527, -1293, -951, -504, 37, 659, 1341, 2056, 2777,
|
|
3470, 4107, 4658, 5095, 5400, 5556, 5556, 5400, 5095, 4658, 4107, 3470,
|
|
2777, 2056, 1341, 659, 37, -504, -951, -1293, -1527, -1656, -1687, -1633,
|
|
-1510, -1334, -1123, -895, -666, -448, -253, -88, 42, 138, 199, 230,
|
|
234, 218, 189, 151, 109, 69, 33, 3, -18, -33, -42, -45,
|
|
-44, -40, -35, -30, -25, -21, -18, -17, -16, -16, -16, -17,
|
|
-17, -16, -15, -14, -12, -9, -7, -4, -2, 0, 2, 3
|
|
}}
|
|
}};
|
|
|
|
}} // mobilinkd::tnc
|
|
|