1
0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-10-31 15:07:12 -04:00
sdrangel/plugins/channelrx/demodm17/m17demodprocessor.cpp

595 lines
18 KiB
C++
Raw Normal View History

///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2022 Edouard Griffiths, F4EXB //
// //
// This program is free software; you can redistribute it and/or modify //
// it under the terms of the GNU General Public License as published by //
// the Free Software Foundation as version 3 of the License, or //
// (at your option) any later version. //
// //
// This program is distributed in the hope that it will be useful, //
// but WITHOUT ANY WARRANTY; without even the implied warranty of //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
// GNU General Public License V3 for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with this program. If not, see <http://www.gnu.org/licenses/>. //
///////////////////////////////////////////////////////////////////////////////////
2022-06-16 20:25:34 -04:00
// #include <boost/crc.hpp>
// #include <boost/program_options.hpp>
// #include <boost/optional.hpp>
#include <codec2/codec2.h>
#include <QDebug>
#include "audio/audiofifo.h"
2022-07-03 04:06:12 -04:00
#include "util/ax25.h"
2022-07-19 23:47:48 -04:00
#include "ax25_frame.h"
2022-06-19 00:39:05 -04:00
#include "m17demod.h"
#include "m17demodprocessor.h"
M17DemodProcessor* M17DemodProcessor::m_this = nullptr;
M17DemodProcessor::M17DemodProcessor() :
m_packetFrameCounter(0),
m_displayLSF(true),
m_noiseBlanker(true),
m_demod(handle_frame),
m_audioFifo(nullptr),
m_audioMute(false),
2022-06-19 00:39:05 -04:00
m_volume(1.0f),
m_demodInputMessageQueue(nullptr)
{
m_this = this;
m_codec2 = ::codec2_create(CODEC2_MODE_3200);
2022-06-08 22:49:41 -04:00
m_audioBuffer.resize(12000);
m_audioBufferFill = 0;
m_srcCall = "";
m_destCall = "";
m_typeInfo = "";
m_metadata.fill(0);
2022-07-15 21:48:33 -04:00
m_hasGNSS = false;
m_crc = 0;
m_lsfCount = 0;
setUpsampling(6); // force upsampling of audio to 48k
m_demod.diagnostics(diagnostic_callback);
}
M17DemodProcessor::~M17DemodProcessor()
{
codec2_destroy(m_codec2);
}
void M17DemodProcessor::pushSample(qint16 sample)
{
m_demod(sample / 22000.0f);
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::handle_frame(modemm17::M17FrameDecoder::output_buffer_t const& frame, int viterbi_cost)
{
2022-07-04 17:03:07 -04:00
using FrameType = modemm17::M17FrameDecoder::FrameType;
bool result = true;
switch (frame.type)
{
case FrameType::LSF:
result = m_this->decode_lsf(frame.lsf);
break;
case FrameType::LICH:
result = m_this->decode_lich(frame.lich);
break;
case FrameType::STREAM:
result = m_this->demodulate_audio(frame.stream, viterbi_cost);
break;
case FrameType::BASIC_PACKET:
result = m_this->decode_packet(frame.packet);
break;
case FrameType::FULL_PACKET:
result = m_this->decode_packet(frame.packet);
break;
case FrameType::BERT:
result = m_this->decode_bert(frame.bert);
break;
}
return result;
}
void M17DemodProcessor::diagnostic_callback(
bool dcd,
float evm,
float deviation,
float offset,
int status,
2022-07-06 01:01:42 -04:00
int sync_word_type,
float clock,
int sample_index,
int sync_index,
int clock_index,
int viterbi_cost)
{
bool debug = false;
bool quiet = true;
m_this->m_dcd = dcd;
m_this->m_evm = evm;
m_this->m_deviation = deviation;
m_this->m_offset = offset;
m_this->m_status = status;
2022-07-06 01:01:42 -04:00
m_this->m_syncWordType = sync_word_type;
m_this->m_clock = clock;
m_this->m_sampleIndex = sample_index;
m_this->m_syncIndex = sync_index;
m_this->m_clockIndex = clock_index;
m_this->m_viterbiCost = viterbi_cost;
if (debug)
{
std::ostringstream oss;
oss << "dcd: " << std::setw(1) << int(dcd)
<< ", evm: " << std::setfill(' ') << std::setprecision(4) << std::setw(8) << evm * 100 <<"%"
<< ", deviation: " << std::setprecision(4) << std::setw(8) << deviation
<< ", freq offset: " << std::setprecision(4) << std::setw(8) << offset
<< ", locked: " << std::boolalpha << std::setw(6) << (status != 0) << std::dec
<< ", clock: " << std::setprecision(7) << std::setw(8) << clock
<< ", sample: " << std::setw(1) << sample_index << ", " << sync_index << ", " << clock_index
<< ", cost: " << viterbi_cost;
qDebug() << "M17DemodProcessor::diagnostic_callback: " << oss.str().c_str();
}
if (m_this->m_prbs.sync() && !quiet)
{
std::ostringstream oss;
auto ber = double(m_this->m_prbs.errors()) / double(m_this->m_prbs.bits());
char buffer[40];
snprintf(buffer, 40, "BER: %-1.6lf (%u bits)", ber, m_this->m_prbs.bits());
oss << buffer;
qDebug() << "M17DemodProcessor::diagnostic_callback: " << oss.str().c_str();
}
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::decode_lich(modemm17::M17FrameDecoder::lich_buffer_t const& lich)
{
uint8_t fragment_number = lich[5]; // Get fragment number.
fragment_number = (fragment_number >> 5) & 7;
qDebug("M17DemodProcessor::handle_frame: LICH: %d", (int) fragment_number);
return true;
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::decode_lsf(modemm17::M17FrameDecoder::lsf_buffer_t const& lsf)
{
2022-07-04 17:03:07 -04:00
modemm17::LinkSetupFrame::encoded_call_t encoded_call;
std::ostringstream oss;
std::copy(lsf.begin() + 6, lsf.begin() + 12, encoded_call.begin());
2022-07-04 17:03:07 -04:00
modemm17::LinkSetupFrame::call_t src = modemm17::LinkSetupFrame::decode_callsign(encoded_call);
m_srcCall = QString(src.data());
std::copy(lsf.begin(), lsf.begin() + 6, encoded_call.begin());
2022-07-04 17:03:07 -04:00
modemm17::LinkSetupFrame::call_t dest = modemm17::LinkSetupFrame::decode_callsign(encoded_call);
m_destCall = QString(dest.data());
uint16_t type = (lsf[12] << 8) | lsf[13];
decode_type(type);
2022-07-15 21:48:33 -04:00
m_hasGNSS = ((lsf[13] >> 5) & 3) == 1;
std::copy(lsf.begin()+14, lsf.begin()+28, m_metadata.begin());
m_crc = (lsf[28] << 8) | lsf[29];
if (m_displayLSF)
{
oss << "SRC: " << m_srcCall.toStdString().c_str();
oss << ", DEST: " << m_destCall.toStdString().c_str();
oss << ", " << m_typeInfo.toStdString().c_str();
oss << ", META: ";
for (size_t i = 0; i != 14; ++i) {
oss << std::hex << std::setw(2) << std::setfill('0') << int(m_metadata[i]);
}
oss << ", CRC: " << std::hex << std::setw(4) << std::setfill('0') << m_crc;
oss << std::dec;
}
m_currentPacket.clear();
m_packetFrameCounter = 0;
if (!lsf[111]) // LSF type bit 0
{
uint8_t packet_type = (lsf[109] << 1) | lsf[110];
switch (packet_type)
{
case 1: // RAW -- ignore LSF.
break;
case 2: // ENCAPSULATED
append_packet(m_currentPacket, lsf);
break;
default:
oss << " LSF for reserved packet type";
append_packet(m_currentPacket, lsf);
}
}
m_lsfCount++;
2022-06-08 22:49:41 -04:00
qDebug() << "M17DemodProcessor::decode_lsf: " << m_lsfCount << ":" << oss.str().c_str();
return true;
}
void M17DemodProcessor::decode_type(uint16_t type)
{
2022-06-08 22:49:41 -04:00
m_streamElsePacket = type & 1; // bit 0
if (m_streamElsePacket)
{
m_typeInfo = "STR:"; // Stream mode
switch ((type & 6) >> 1) // bits 1..2
{
case 0:
m_typeInfo += "UNK";
break;
case 1:
m_typeInfo += "D/D";
break;
case 2:
m_typeInfo += "V/V";
break;
case 3:
m_typeInfo += "V/D";
break;
}
}
else
{
m_typeInfo = "PKT:"; // Packet mode
switch ((type & 6) >> 1) // bits 1..2
{
case 0:
m_typeInfo += "UNK";
break;
case 1:
m_typeInfo += "RAW";
break;
case 2:
m_typeInfo += "ENC";
break;
case 3:
m_typeInfo += "UNK";
break;
}
}
m_typeInfo += QString(" CAN:%1").arg(int((type & 0x780) >> 7), 2, 10, QChar('0')); // Channel Access number (bits 7..10)
}
void M17DemodProcessor::resetInfo()
{
m_srcCall = "";
m_destCall = "";
m_typeInfo = "";
2022-06-08 22:49:41 -04:00
m_streamElsePacket = true;
m_metadata.fill(0);
m_crc = 0;
m_lsfCount = 0;
}
void M17DemodProcessor::setDCDOff()
{
m_demod.dcd_off();
}
2022-07-04 17:03:07 -04:00
void M17DemodProcessor::append_packet(std::vector<uint8_t>& result, modemm17::M17FrameDecoder::lsf_buffer_t in)
{
uint8_t out = 0;
size_t b = 0;
for (auto c : in)
{
out = (out << 1) | c;
if (++b == 8)
{
result.push_back(out);
out = 0;
b = 0;
}
}
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::decode_packet(modemm17::M17FrameDecoder::packet_buffer_t const& packet_segment)
{
2022-06-16 20:25:34 -04:00
// qDebug() << tr("M17DemodProcessor::decode_packet: 0x%1").arg((int) packet_segment[25], 2, 16, QChar('0'));
if (packet_segment[25] & 0x80) // last frame of packet.
{
size_t packet_size = (packet_segment[25] & 0x7F) >> 2;
2022-06-08 22:49:41 -04:00
packet_size = std::min(packet_size, size_t(25)); // on last frame this is the remainder byte count
m_packetFrameCounter = 0;
2022-06-16 20:25:34 -04:00
for (size_t i = 0; i < packet_size; ++i) {
m_currentPacket.push_back(packet_segment[i]);
}
2022-06-16 20:25:34 -04:00
if (m_currentPacket.size() < 3)
{
2022-06-08 22:49:41 -04:00
qDebug() << "M17DemodProcessor::decode_packet: too small:" << m_currentPacket.size();
return false;
}
2022-06-16 20:25:34 -04:00
else
{
qDebug() << "M17DemodProcessor::decode_packet: last chunk size:" << packet_size << " packet size:" << m_currentPacket.size();
}
2022-07-27 12:15:47 -04:00
modemm17::CRC16 crc16(0x5935, 0xFFFF);
2022-06-16 20:25:34 -04:00
crc16.reset();
for (std::vector<uint8_t>::const_iterator it = m_currentPacket.begin(); it != m_currentPacket.end() - 2; ++it) {
crc16(*it);
}
2022-06-08 22:49:41 -04:00
2022-06-16 20:25:34 -04:00
uint16_t calcChecksum = crc16.get_bytes()[0] + (crc16.get_bytes()[1]<<8);
uint16_t xmitChecksum = m_currentPacket.back() + (m_currentPacket.end()[-2]<<8);
2022-06-08 22:49:41 -04:00
if (calcChecksum == xmitChecksum) // (checksum == 0x0f47)
{
2022-06-08 22:49:41 -04:00
uint8_t protocol = m_currentPacket.front();
m_stdPacketProtocol = protocol < (int) StdPacketUnknown ? (StdPacketProtocol) protocol : StdPacketUnknown;
qDebug() << "M17DemodProcessor::decode_packet: protocol: " << m_stdPacketProtocol;
if (m_stdPacketProtocol == StdPacketAX25)
{
std::string ax25;
ax25.reserve(m_currentPacket.size());
for (auto c : m_currentPacket) {
ax25.push_back(char(c));
}
2022-07-04 17:03:07 -04:00
modemm17::ax25_frame frame(ax25);
2022-06-08 22:49:41 -04:00
std::ostringstream oss;
2022-07-04 17:03:07 -04:00
modemm17::write(oss, frame); // TODO: get details
2022-06-08 22:49:41 -04:00
qDebug() << "M17DemodProcessor::decode_packet: AX25:" << oss.str().c_str();
}
else if (m_stdPacketProtocol == StdPacketSMS)
{
std::ostringstream oss;
for (std::vector<uint8_t>::const_iterator it = m_currentPacket.begin()+1; it < m_currentPacket.end()-2; ++it) {
oss << *it;
}
2022-07-03 04:06:12 -04:00
qDebug() << "M17DemodProcessor::decode SMS_packet: "
<< " Src:" << getSrcCall()
<< " Dest:" << getDestcCall()
2022-06-19 00:39:05 -04:00
<< " SMS:" << oss.str().c_str();
if (m_demodInputMessageQueue)
{
M17Demod::MsgReportSMS *msg = M17Demod::MsgReportSMS::create(
getSrcCall(),
getDestcCall(),
QString(oss.str().c_str())
);
m_demodInputMessageQueue->push(msg);
}
}
2022-07-03 04:06:12 -04:00
else if (m_stdPacketProtocol == StdPacketAPRS)
{
AX25Packet ax25;
QByteArray packet = QByteArray(reinterpret_cast<const char*>(&m_currentPacket[1]), m_currentPacket.size()-3);
if (ax25.decode(packet))
{
qDebug() << "M17DemodProcessor::decode APRS_packet: "
<< " Src:" << getSrcCall()
<< " Dest:" << getDestcCall()
<< " From:" << ax25.m_from
<< " To: " << ax25.m_to
<< " Via: " << ax25.m_via
<< " Type: " << ax25.m_type
<< " PID: " << ax25.m_pid
<< " Data: " << ax25.m_dataASCII;
if (m_demodInputMessageQueue)
{
M17Demod::MsgReportAPRS *msg = M17Demod::MsgReportAPRS::create(
getSrcCall(),
getDestcCall(),
ax25.m_from,
ax25.m_to,
ax25.m_via,
ax25.m_type,
ax25.m_pid,
ax25.m_dataASCII
);
msg->getPacket() = packet;
m_demodInputMessageQueue->push(msg);
}
}
}
return true;
}
2022-06-16 20:25:34 -04:00
QString ccrc = tr("0x%1").arg(calcChecksum, 4, 16, QChar('0'));
QString xcrc = tr("0x%1").arg(xmitChecksum, 4, 16, QChar('0'));
qWarning() << "M17DemodProcessor::decode_packet: Packet checksum error: " << ccrc << "vs" << xcrc;
return false;
}
size_t frame_number = (packet_segment[25] & 0x7F) >> 2;
if (frame_number != m_packetFrameCounter)
{
qWarning() << "M17DemodProcessor::decode_packet: Packet frame sequence error. Got "
<< frame_number << ", expected " << m_packetFrameCounter;
return false;
}
2022-06-16 20:25:34 -04:00
if (m_packetFrameCounter == 0) {
m_currentPacket.clear();
}
2022-06-16 20:25:34 -04:00
for (size_t i = 0; i < 25; ++i) {
m_currentPacket.push_back(packet_segment[i]);
}
2022-06-16 20:25:34 -04:00
m_packetFrameCounter++;
return true;
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::decode_bert(modemm17::M17FrameDecoder::bert_buffer_t const& bert)
{
for (int j = 0; j != 24; ++j)
{
auto b = bert[j];
for (int i = 0; i != 8; ++i)
{
m_prbs.validate(b & 0x80);
b <<= 1;
}
}
auto b = bert[24];
for (int i = 0; i != 5; ++i)
{
m_prbs.validate(b & 0x80);
b <<= 1;
}
return true;
}
2022-07-04 17:03:07 -04:00
bool M17DemodProcessor::demodulate_audio(modemm17::M17FrameDecoder::audio_buffer_t const& audio, int viterbi_cost)
{
bool result = true;
std::array<int16_t, 160> buf; // 8k audio
// First two bytes are the frame counter + EOS indicator.
if (viterbi_cost < 70 && (audio[0] & 0x80))
{
if (m_displayLSF) {
qDebug() << "M17DemodProcessor::demodulate_audio: EOS";
}
result = false;
}
if (m_audioFifo && !m_audioMute)
{
if (m_noiseBlanker && viterbi_cost > 80)
{
buf.fill(0);
processAudio(buf); // first block expanded
processAudio(buf); // second block expanded
}
else
{
codec2_decode(m_codec2, buf.data(), audio.data() + 2); // first 8 bytes block input
processAudio(buf);
codec2_decode(m_codec2, buf.data(), audio.data() + 10); // second 8 bytes block input
processAudio(buf);
}
}
return result;
}
void M17DemodProcessor::setUpsampling(int upsampling)
{
m_upsampling = upsampling < 1 ? 1 : upsampling > 6 ? 6 : upsampling;
}
void M17DemodProcessor::setVolume(float volume)
{
m_volume = volume;
setVolumeFactors();
}
void M17DemodProcessor::processAudio(const std::array<int16_t, 160>& in)
{
if (m_upsampling > 1) {
upsample(m_upsampling, in.data(), in.size());
} else {
noUpsample(in.data(), in.size());
}
if (m_audioBufferFill >= m_audioBuffer.size() - 960)
{
uint res = m_audioFifo->write((const quint8*)&m_audioBuffer[0], m_audioBufferFill);
if (res != m_audioBufferFill) {
qDebug("M17DemodProcessor::processAudio: %u/%u audio samples written", res, m_audioBufferFill);
}
m_audioBufferFill = 0;
}
}
void M17DemodProcessor::upsample(int upsampling, const int16_t *in, int nbSamplesIn)
{
for (int i = 0; i < nbSamplesIn; i++)
{
float cur = m_upsamplingFilter.usesHP() ? m_upsamplingFilter.runHP((float) in[i]) : (float) in[i];
float prev = m_upsamplerLastValue;
qint16 upsample;
for (int j = 1; j <= upsampling; j++)
{
upsample = (qint16) m_upsamplingFilter.runLP(cur*m_upsamplingFactors[j] + prev*m_upsamplingFactors[upsampling-j]);
2022-06-08 22:49:41 -04:00
m_audioBuffer[m_audioBufferFill].l = m_compressor.compress(upsample);
m_audioBuffer[m_audioBufferFill].r = m_compressor.compress(upsample);
if (m_audioBufferFill < m_audioBuffer.size() - 1) {
++m_audioBufferFill;
}
}
m_upsamplerLastValue = cur;
}
if (m_audioBufferFill >= m_audioBuffer.size() - 1) {
qDebug("M17DemodProcessor::upsample(%d): audio buffer is full check its size", upsampling);
}
}
void M17DemodProcessor::noUpsample(const int16_t *in, int nbSamplesIn)
{
for (int i = 0; i < nbSamplesIn; i++)
{
float cur = m_upsamplingFilter.usesHP() ? m_upsamplingFilter.runHP((float) in[i]) : (float) in[i];
m_audioBuffer[m_audioBufferFill].l = cur*m_upsamplingFactors[0];
m_audioBuffer[m_audioBufferFill].r = cur*m_upsamplingFactors[0];
if (m_audioBufferFill < m_audioBuffer.size() - 1) {
++m_audioBufferFill;
}
}
if (m_audioBufferFill >= m_audioBuffer.size() - 1) {
qDebug("M17DemodProcessor::noUpsample: audio buffer is full check its size");
}
}
void M17DemodProcessor::setVolumeFactors()
{
m_upsamplingFactors[0] = m_volume;
for (int i = 1; i <= m_upsampling; i++) {
m_upsamplingFactors[i] = (i*m_volume) / (float) m_upsampling;
}
}
2022-06-08 22:49:41 -04:00
M17DemodProcessor::StdPacketProtocol M17DemodProcessor::getStdPacketProtocol() const
{
if (m_streamElsePacket) {
return StdPacketUnknown;
} else {
return m_stdPacketProtocol;
}
}