// Copyright 2020 Rob Riggs // 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(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() { TNC_DEBUG("enter Fsk9600Demodulator::readTwist"); float g120 = 0.0f; float g4800 = 0.0f; GoertzelFilter gf120(120.0, 0); GoertzelFilter 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)); TNC_DEBUG("exit Fsk9600Demodulator::readTwist"); return result; } uint32_t Fsk9600Demodulator::readBatteryLevel() { #ifndef NUCLEOTNC TNC_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 = BATTERY_ADC_CHANNEL; if (HAL_ADC_ConfigChannel(&BATTERY_ADC_HANDLE, &sConfig) != HAL_OK) CxxErrorHandler(); uint32_t vbat = 0; if (HAL_ADC_Start(&BATTERY_ADC_HANDLE) != HAL_OK) CxxErrorHandler(); for (size_t i = 0; i != 8; ++i) { if (HAL_ADC_PollForConversion(&BATTERY_ADC_HANDLE, 1) != HAL_OK) CxxErrorHandler(); vbat += HAL_ADC_GetValue(&BATTERY_ADC_HANDLE); } vbat /= 8; if (HAL_ADC_Stop(&BATTERY_ADC_HANDLE) != 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); TNC_DEBUG("exit Fsk9600Demodulator::readBatteryLevel"); return vbat; #else return 0; #endif } 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