From 1e34a2b5a4b652e1c5efc513944382bc7d8c4524 Mon Sep 17 00:00:00 2001 From: f4exb Date: Sat, 16 Jul 2022 03:48:33 +0200 Subject: [PATCH] M17: implemented GNSS data --- modems/m17/LinkSetupFrame.h | 49 ++++++++ modems/m17/M17Demodulator.cpp | 4 +- modems/m17/M17Modulator.h | 56 +++++---- plugins/channelrx/demodm17/m17demod.h | 2 + plugins/channelrx/demodm17/m17demodbaseband.h | 2 + plugins/channelrx/demodm17/m17demodgui.cpp | 52 ++++++++ plugins/channelrx/demodm17/m17demodgui.h | 3 + plugins/channelrx/demodm17/m17demodgui.ui | 111 +++++++++++++++++- .../channelrx/demodm17/m17demodprocessor.cpp | 3 + .../channelrx/demodm17/m17demodprocessor.h | 3 + plugins/channelrx/demodm17/m17demodsink.h | 2 + plugins/channeltx/modm17/m17modgui.cpp | 1 + plugins/channeltx/modm17/m17modprocessor.cpp | 20 +++- plugins/channeltx/modm17/m17modprocessor.h | 40 +++++++ plugins/channeltx/modm17/m17modsource.cpp | 19 +++ 15 files changed, 339 insertions(+), 28 deletions(-) diff --git a/modems/m17/LinkSetupFrame.h b/modems/m17/LinkSetupFrame.h index e1e30c867..6efa046cf 100644 --- a/modems/m17/LinkSetupFrame.h +++ b/modems/m17/LinkSetupFrame.h @@ -4,6 +4,7 @@ #include #include +#include #include // Don't have std::span in C++17. #include #include @@ -15,6 +16,7 @@ struct LinkSetupFrame { using call_t = std::array; // NUL-terminated C-string. using encoded_call_t = std::array; + using gnss_t = std::array; using frame_t = std::array; static constexpr encoded_call_t BROADCAST_ADDRESS = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; @@ -119,6 +121,53 @@ struct LinkSetupFrame return result; } + static gnss_t encode_gnss(float lat, float lon, float alt) + { + gnss_t result; + result.fill(0); + double lat_int, lat_frac; + double lon_int, lon_frac; + uint16_t lat_dec, lon_dec; + + lat_frac = modf(lat, &lat_int); + lon_frac = modf(lon, &lon_int); + + bool north = lat_int >= 0; + bool east = lon_int >= 0; + + result[2] = (int) abs(lat_int); + lat_dec = abs(lat_frac) * 65536.0f; + result[3] = lat_dec >> 8; + result[4] = lat_dec & 0xFF; + result[5] = (int) abs(lon_int); + lon_dec = abs(lon_frac) * 65536.0f; + result[6] = lon_dec >> 8; + result[7] = lon_dec & 0xFF; + result[8] = (north ? 0 : 1) | ((east ? 0 : 1)<<1) | (1<<2); + + uint16_t alt_enc = (alt * 3.28084f) + 1500; + result[9] = alt_enc >> 8; + result[10] = alt_enc & 0xFF; + + return result; + } + + static void decode_gnss(const gnss_t& gnss_enc, float& lat, float& lon, float& alt) + { + bool north = (gnss_enc[8] & 1) != 0; + bool east = (gnss_enc[8] & 2) != 0; + uint32_t lat_int = gnss_enc[2]; + uint16_t lat_frac = (gnss_enc[3] << 8) + gnss_enc[4]; + uint32_t lon_int = gnss_enc[5]; + uint16_t lon_frac = (gnss_enc[6] << 8) + gnss_enc[7]; + lat = lat_int + (lat_frac / 65536.0f); + lat = north ? lat : -lat; + lon = lon_int + (lon_frac / 65536.0f); + lat = east ? lon : -lon; + uint16_t alt_enc = (gnss_enc[9] << 8) + gnss_enc[10]; + alt = (alt_enc - 1500) / 3.28084f; + } + LinkSetupFrame() {} diff --git a/modems/m17/M17Demodulator.cpp b/modems/m17/M17Demodulator.cpp index 8c2a55d44..63abff8ba 100644 --- a/modems/m17/M17Demodulator.cpp +++ b/modems/m17/M17Demodulator.cpp @@ -212,9 +212,7 @@ void M17Demodulator::do_lsf_sync() { sync_triggered = preamble_sync.triggered(correlator); - if (sync_triggered > 0.1) - { - qDebug() << "modemm17::M17Demodulator::do_lsf_sync: preamble:" << sync_triggered; + if (sync_triggered > 0.1) { return; } diff --git a/modems/m17/M17Modulator.h b/modems/m17/M17Modulator.h index 164131572..f3e8598d4 100644 --- a/modems/m17/M17Modulator.h +++ b/modems/m17/M17Modulator.h @@ -114,7 +114,7 @@ public: return baseband; } - static std::array make_lsf(lsf_t& lsf, const std::string& src, const std::string& dest, uint8_t can = 10, bool streamElsePacket = false) + std::array make_lsf(lsf_t& lsf, bool streamElsePacket = false) { lsf.fill(0); @@ -122,25 +122,21 @@ public: PolynomialInterleaver<45, 92, 368> interleaver; CRC16<0x5935, 0xFFFF> crc; - modemm17::LinkSetupFrame::call_t callsign; - callsign.fill(0); - std::copy(src.begin(), src.end(), callsign.begin()); - auto encoded_src = modemm17::LinkSetupFrame::encode_callsign(callsign); + auto rit = std::copy(dest_.begin(), dest_.end(), lsf.begin()); + std::copy(source_.begin(), source_.end(), rit); - modemm17::LinkSetupFrame::encoded_call_t encoded_dest = {0xff,0xff,0xff,0xff,0xff,0xff}; + lsf[12] = can_ >> 1; + lsf[13] = (streamElsePacket ? 5 : 4) | ((can_ & 1) << 7); - if (!dest.empty()) + if (gnss_on_) { - callsign.fill(0); - std::copy(dest.begin(), dest.end(), callsign.begin()); - encoded_dest = modemm17::LinkSetupFrame::encode_callsign(callsign); + lsf[13] |= (1<<5); + std::copy(gnss_.begin(), gnss_.end(), &lsf[14]); + } + else + { + lsf[13] |= (3<<5); } - - auto rit = std::copy(encoded_dest.begin(), encoded_dest.end(), lsf.begin()); - std::copy(encoded_src.begin(), encoded_src.end(), rit); - - lsf[12] = can >> 1; - lsf[13] = (streamElsePacket ? 5 : 4) | ((can & 1) << 7); crc.reset(); @@ -313,9 +309,6 @@ public: packet_assembly[25] = 0x80 | ((packet_size+2)<<2); // sent packet size includes CRC packet_assembly[packet_size] = crc_.get_bytes()[1]; packet_assembly[packet_size+1] = crc_.get_bytes()[0]; - qDebug() << QString("modemm17::M17Modulator::make_packet_frame: %1:%2") - .arg((int) crc_.get_bytes()[1], 2, 16, QChar('0')) - .arg((int) crc_.get_bytes()[0], 2, 16, QChar('0')); } else { @@ -465,7 +458,10 @@ public: dest_(encode_callsign(dest)), can_(10), rrc(makeFirFilter(rrc_taps)) - { } + { + gnss_.fill(0); + gnss_on_ = false; + } /** * Set the source identifier (callsign) for the transmitter. @@ -489,9 +485,29 @@ public: can_ = can & 0xF; } + /** + * Set GNSS data + */ + void set_gnss(float lat, float lon, float alt) + { + gnss_ = LinkSetupFrame::encode_gnss(lat, lon, alt); + gnss_on_ = true; + } + + /** + * Reset GNSS data + */ + void reset_gnss() + { + gnss_.fill(0); + gnss_on_ = false; + } + private: LinkSetupFrame::encoded_call_t source_; LinkSetupFrame::encoded_call_t dest_; + LinkSetupFrame::gnss_t gnss_; + bool gnss_on_; uint8_t can_; BaseFirFilter<150> rrc; static const std::array rrc_taps; diff --git a/plugins/channelrx/demodm17/m17demod.h b/plugins/channelrx/demodm17/m17demod.h index 2917ee759..65c169033 100644 --- a/plugins/channelrx/demodm17/m17demod.h +++ b/plugins/channelrx/demodm17/m17demod.h @@ -255,6 +255,8 @@ public: const QString& getSrcCall() const { return m_basebandSink->getSrcCall(); } const QString& getDestcCall() const { return m_basebandSink->getDestcCall(); } const QString& getTypeInfo() const { return m_basebandSink->getTypeInfo(); } + const std::array& getMeta() const { return m_basebandSink->getMeta(); } + bool getHasGNSS() const { return m_basebandSink->getHasGNSS(); } bool getStreamElsePacket() const { return m_basebandSink->getStreamElsePacket(); } uint16_t getCRC() const { return m_basebandSink->getCRC(); } int getStdPacketProtocol() const { return m_basebandSink->getStdPacketProtocol(); } diff --git a/plugins/channelrx/demodm17/m17demodbaseband.h b/plugins/channelrx/demodm17/m17demodbaseband.h index f40546062..ad5e46936 100644 --- a/plugins/channelrx/demodm17/m17demodbaseband.h +++ b/plugins/channelrx/demodm17/m17demodbaseband.h @@ -115,6 +115,8 @@ public: const QString& getSrcCall() const { return m_sink.getSrcCall(); } const QString& getDestcCall() const { return m_sink.getDestcCall(); } const QString& getTypeInfo() const { return m_sink.getTypeInfo(); } + const std::array& getMeta() const {return m_sink.getMeta(); } + bool getHasGNSS() const { return m_sink.getHasGNSS(); } bool getStreamElsePacket() const { return m_sink.getStreamElsePacket(); } uint16_t getCRC() const { return m_sink.getCRC(); } int getStdPacketProtocol() const { return m_sink.getStdPacketProtocol(); } diff --git a/plugins/channelrx/demodm17/m17demodgui.cpp b/plugins/channelrx/demodm17/m17demodgui.cpp index 4a9ff80f4..96c432d0d 100644 --- a/plugins/channelrx/demodm17/m17demodgui.cpp +++ b/plugins/channelrx/demodm17/m17demodgui.cpp @@ -744,6 +744,18 @@ void M17DemodGUI::tick() ui->typeText->setText(m_m17Demod->getTypeInfo()); ui->crcText->setText(tr("%1").arg(m_m17Demod->getCRC(), 4, 16, QChar('0'))); + if (m_m17Demod->getHasGNSS()) + { + const MainSettings& mainSettings = MainCore::instance()->getSettings(); + float lat, lon; + getLatLonFromGNSSMeta(m_m17Demod->getMeta(), lat, lon); + float qrb = distance(mainSettings.getLatitude(), mainSettings.getLongitude(), lat, lon); + float qtf = bearing(mainSettings.getLatitude(), mainSettings.getLongitude(), lat, lon); + qDebug("M17DemodGUI::tick: GNSS: lat: %f lon: %f", lat, lon); + ui->qrbText->setText(tr("%1").arg(qrb, 0, 'f', 1)); + ui->qtfText->setText(tr("%1").arg(qtf, 0, 'f', 1)); + } + if (ui->activateStatusLog->isChecked() && (m_m17Demod->getLSFCount() != 0)) { QString s = tr("Src: %1 Dst: %2 Typ: %3 CRC: %4") @@ -985,6 +997,46 @@ QLineSeries *M17DemodGUI::addBERSeriesRate(bool total, qreal& min, qreal& max) return series; } +void M17DemodGUI::getLatLonFromGNSSMeta(const std::array& meta, float& lat, float& lon) +{ + int latInt = meta[2]; + int latFrac = (meta[3]<<8) + meta[4]; + lat = latInt + latFrac / 65536.0f; + int lonInt = meta[5]; + int lonFrac = (meta[6]<<8) + meta[7]; + lon = lonInt + lonFrac / 65536.0f; + lat = (meta[8] & 1) == 1 ? -lat : lat; + lon = ((meta[8]>>1) & 1) == 1 ? -lon : lon; +} + +float M17DemodGUI::bearing(float latFrom, float lonFrom, float latTo, float lonTo) +{ + double lat1 = latFrom * (M_PI / 180.0); + double lon1 = lonFrom * (M_PI / 180.0); + double lat2 = latTo * (M_PI / 180.0); + double lon2 = lonTo * (M_PI / 180.0); + double dLon = lon2 - lon1; + double y = sin(dLon) * cos(lat2); + double x = (cos(lat1)*sin(lat2)) - + (sin(lat1)*cos(lat2)*cos(dLon)); + double bear_rad = atan2(y,x); + + if (bear_rad > 0) { + return bear_rad * (180.0 / M_PI); + } else { + return 360.0 + (bear_rad * (180.0 / M_PI)); + } +} + +float M17DemodGUI::distance(float latFrom, float lonFrom, float latTo, float lonTo) +{ + double lat1 = latFrom * (M_PI / 180.0); + double lon1 = lonFrom * (M_PI / 180.0); + double lat2 = latTo * (M_PI / 180.0); + double lon2 = lonTo * (M_PI / 180.0); + return acos(sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(lon2-lon1)) * 6371.0; +} + void M17DemodGUI::makeUIConnections() { QObject::connect(ui->deltaFrequency, &ValueDialZ::changed, this, &M17DemodGUI::on_deltaFrequency_changed); diff --git a/plugins/channelrx/demodm17/m17demodgui.h b/plugins/channelrx/demodm17/m17demodgui.h index a94a06856..aca4afd2a 100644 --- a/plugins/channelrx/demodm17/m17demodgui.h +++ b/plugins/channelrx/demodm17/m17demodgui.h @@ -135,6 +135,9 @@ private: void packetReceived(QByteArray packet); QLineSeries *addBERSeries(bool total, uint32_t& min, uint32_t& max); QLineSeries *addBERSeriesRate(bool total, qreal& min, qreal& max); + static void getLatLonFromGNSSMeta(const std::array& meta, float& lat, float& lon); + static float bearing(float latFrom, float lonFrom, float latTo, float lonTo); + static float distance(float lat1, float lon1, float lat2, float lon2); void leaveEvent(QEvent*); void enterEvent(QEvent*); diff --git a/plugins/channelrx/demodm17/m17demodgui.ui b/plugins/channelrx/demodm17/m17demodgui.ui index bb42fd145..f1f65ce4c 100644 --- a/plugins/channelrx/demodm17/m17demodgui.ui +++ b/plugins/channelrx/demodm17/m17demodgui.ui @@ -7,7 +7,7 @@ 0 0 522 - 420 + 450 @@ -19,13 +19,13 @@ 522 - 420 + 450 560 - 420 + 450 @@ -717,6 +717,111 @@ + + + + + + + 0 + 30 + + + + QRB is distance to station (km) when GNSS data is available + + + QRB + + + + + + + + 70 + 0 + + + + + Liberation Mono + 9 + + + + Distance to station (km) + + + QFrame::Box + + + QFrame::Sunken + + + ... + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + QRB is bearing to station (degrees) when GNSS data is available + + + QTF + + + + + + + + 60 + 0 + + + + + Liberation Mono + 9 + + + + Bearing to station (degrees) + + + QFrame::Box + + + QFrame::Sunken + + + ... + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + diff --git a/plugins/channelrx/demodm17/m17demodprocessor.cpp b/plugins/channelrx/demodm17/m17demodprocessor.cpp index bebed35ea..3ff0acc9c 100644 --- a/plugins/channelrx/demodm17/m17demodprocessor.cpp +++ b/plugins/channelrx/demodm17/m17demodprocessor.cpp @@ -49,6 +49,7 @@ M17DemodProcessor::M17DemodProcessor() : m_destCall = ""; m_typeInfo = ""; m_metadata.fill(0); + m_hasGNSS = false; m_crc = 0; m_lsfCount = 0; setUpsampling(6); // force upsampling of audio to 48k @@ -173,6 +174,8 @@ bool M17DemodProcessor::decode_lsf(modemm17::M17FrameDecoder::lsf_buffer_t const uint16_t type = (lsf[12] << 8) | lsf[13]; decode_type(type); + 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]; diff --git a/plugins/channelrx/demodm17/m17demodprocessor.h b/plugins/channelrx/demodm17/m17demodprocessor.h index 45244e091..52160b679 100644 --- a/plugins/channelrx/demodm17/m17demodprocessor.h +++ b/plugins/channelrx/demodm17/m17demodprocessor.h @@ -61,6 +61,8 @@ public: const QString& getSrcCall() const { return m_srcCall; } const QString& getDestcCall() const { return m_destCall; } const QString& getTypeInfo() const { return m_typeInfo; } + const std::array& getMeta() const { return m_metadata; } + bool getHasGNSS() const { return m_hasGNSS; } bool getStreamElsePacket() const { return m_streamElsePacket; } uint16_t getCRC() const { return m_crc; } StdPacketProtocol getStdPacketProtocol() const; @@ -141,6 +143,7 @@ private: QString m_typeInfo; bool m_streamElsePacket; std::array m_metadata; + bool m_hasGNSS; uint16_t m_crc; uint32_t m_lsfCount; // Incremented each time a new LSF is decoded. Reset when lock is lost. StdPacketProtocol m_stdPacketProtocol; diff --git a/plugins/channelrx/demodm17/m17demodsink.h b/plugins/channelrx/demodm17/m17demodsink.h index 334f14c8b..bd0bc5081 100644 --- a/plugins/channelrx/demodm17/m17demodsink.h +++ b/plugins/channelrx/demodm17/m17demodsink.h @@ -115,6 +115,8 @@ public: const QString& getSrcCall() const { return m_m17DemodProcessor.getSrcCall(); } const QString& getDestcCall() const { return m_m17DemodProcessor.getDestcCall(); } const QString& getTypeInfo() const { return m_m17DemodProcessor.getTypeInfo(); } + const std::array& getMeta() const { return m_m17DemodProcessor.getMeta(); } + bool getHasGNSS() const { return m_m17DemodProcessor.getHasGNSS(); } bool getStreamElsePacket() const { return m_m17DemodProcessor.getStreamElsePacket(); } uint16_t getCRC() const { return m_m17DemodProcessor.getCRC(); } int getStdPacketProtocol() const { return (int) m_m17DemodProcessor.getStdPacketProtocol(); } diff --git a/plugins/channeltx/modm17/m17modgui.cpp b/plugins/channeltx/modm17/m17modgui.cpp index 4f4d14934..497fd08a6 100644 --- a/plugins/channeltx/modm17/m17modgui.cpp +++ b/plugins/channeltx/modm17/m17modgui.cpp @@ -848,6 +848,7 @@ void M17ModGUI::makeUIConnections() QObject::connect(ui->source, &QLineEdit::editingFinished, this, &M17ModGUI::on_source_editingFinished); QObject::connect(ui->destination, &QLineEdit::editingFinished, this, &M17ModGUI::on_destination_editingFinished); QObject::connect(ui->can, QOverload::of(&QSpinBox::valueChanged), this, &M17ModGUI::on_can_valueChanged); + QObject::connect(ui->insertPosition, &ButtonSwitch::toggled, this, &M17ModGUI::on_insertPosition_toggled); } void M17ModGUI::updateAbsoluteCenterFrequency() diff --git a/plugins/channeltx/modm17/m17modprocessor.cpp b/plugins/channeltx/modm17/m17modprocessor.cpp index 970f96719..2d276a4b6 100644 --- a/plugins/channeltx/modm17/m17modprocessor.cpp +++ b/plugins/channeltx/modm17/m17modprocessor.cpp @@ -31,6 +31,8 @@ MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgStopAudio, Message) MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgStartBERT, Message) MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgSendBERTFrame, Message) MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgStopBERT, Message) +MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgSetGNSS, Message) +MESSAGE_CLASS_DEFINITION(M17ModProcessor::MsgStopGNSS, Message) M17ModProcessor::M17ModProcessor() : m_m17Modulator("MYCALL", ""), @@ -129,6 +131,19 @@ bool M17ModProcessor::handleMessage(const Message& cmd) send_eot(); // EOT return true; } + else if (MsgSetGNSS::match(cmd)) + { + qDebug("M17ModProcessor::handleMessage: MsgSetGNSS"); + MsgSetGNSS& notif = (MsgSetGNSS&) cmd; + m_m17Modulator.set_gnss(notif.getLat(), notif.getLon(), notif.getAlt()); + return true; + } + else if (MsgStopGNSS::match(cmd)) + { + qDebug("M17ModProcessor::handleMessage: MsgStopGNSS"); + m_m17Modulator.reset_gnss(); + return true; + } return false; } @@ -161,12 +176,13 @@ void M17ModProcessor::processPacket(const QString& sourceCall, const QString& de qDebug("M17ModProcessor::processPacket: %s to %s: %s", qPrintable(sourceCall), qPrintable(destCall), qPrintable(packetBytes)); m_m17Modulator.source(sourceCall.toStdString()); m_m17Modulator.dest(destCall.toStdString()); + m_m17Modulator.can(can); send_preamble(); // preamble // LSF std::array lsf; - std::array lsf_frame = modemm17::M17Modulator::make_lsf(lsf, sourceCall.toStdString(), destCall.toStdString(), can); + std::array lsf_frame = m_m17Modulator.make_lsf(lsf); output_baseband(modemm17::M17Modulator::LSF_SYNC_WORD, lsf_frame); // Packets @@ -205,7 +221,7 @@ void M17ModProcessor::audioStart(const QString& sourceCall, const QString& destC // LSF std::array lsf; - std::array lsf_frame = modemm17::M17Modulator::make_lsf(lsf, sourceCall.toStdString(), destCall.toStdString(), can, true); + std::array lsf_frame = m_m17Modulator.make_lsf(lsf, true); output_baseband(modemm17::M17Modulator::LSF_SYNC_WORD, lsf_frame); // Prepare LICH diff --git a/plugins/channeltx/modm17/m17modprocessor.h b/plugins/channeltx/modm17/m17modprocessor.h index f99cd0577..521eaebb6 100644 --- a/plugins/channeltx/modm17/m17modprocessor.h +++ b/plugins/channeltx/modm17/m17modprocessor.h @@ -228,6 +228,46 @@ public: { } }; + class MsgSetGNSS : public Message { + MESSAGE_CLASS_DECLARATION + + public: + float getLat() const { return m_lat; } + float getLon() const { return m_lon; } + float getAlt() const { return m_alt; } + + static MsgSetGNSS* create(float lat, float lon, float alt) { + return new MsgSetGNSS(lat, lon, alt); + } + + private: + float m_lat; + float m_lon; + float m_alt; + + MsgSetGNSS(float lat, float lon, float alt) : + Message(), + m_lat(lat), + m_lon(lon), + m_alt(alt) + { } + }; + + class MsgStopGNSS : public Message { + MESSAGE_CLASS_DECLARATION + + public: + static MsgStopGNSS* create() { + return new MsgStopGNSS(); + } + + private: + + MsgStopGNSS() : + Message() + { } + }; + M17ModProcessor(); ~M17ModProcessor(); diff --git a/plugins/channeltx/modm17/m17modsource.cpp b/plugins/channeltx/modm17/m17modsource.cpp index fc3e59138..eb2a84af1 100644 --- a/plugins/channeltx/modm17/m17modsource.cpp +++ b/plugins/channeltx/modm17/m17modsource.cpp @@ -546,6 +546,25 @@ void M17ModSource::applySettings(const M17ModSettings& settings, const QListgetSettings(); + M17ModProcessor::MsgSetGNSS *msg = M17ModProcessor::MsgSetGNSS::create( + mainSettings.getLatitude(), mainSettings.getLongitude(), mainSettings.getAltitude()); + m_processor->getInputMessageQueue()->push(msg); + } + else + { + M17ModProcessor::MsgStopGNSS *msg = M17ModProcessor::MsgStopGNSS::create(); + m_processor->getInputMessageQueue()->push(msg); + } + } + } + if (force) { m_settings = settings; } else {