VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-21 23:55:13 -05:00
Code Issues Projects Releases Wiki Activity

M17: implemented GNSS data

Browse Source
This commit is contained in:
f4exb 2022-07-16 03:48:33 +02:00
parent 823cffdae1
commit 1e34a2b5a4
15 changed files with 339 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
modems/m17/LinkSetupFrame.h
View File

@ -4,6 +4,7 @@
#include <array>
#include <cstdint>
#include <cmath>
#include <string_view> // Don't have std::span in C++17.
#include <stdexcept>
#include <algorithm>
@ -15,6 +16,7 @@ struct LinkSetupFrame
{
using call_t = std::array<char,10>; // NUL-terminated C-string.
using encoded_call_t = std::array<uint8_t, 6>;
using gnss_t = std::array<uint8_t, 14>;
using frame_t = std::array<uint8_t, 30>;
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()
{}

4
modems/m17/M17Demodulator.cpp
View File

@ -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;
}

56
modems/m17/M17Modulator.h
View File

@ -114,7 +114,7 @@ public:
return baseband;
}
static std::array<int8_t, 368> make_lsf(lsf_t& lsf, const std::string& src, const std::string& dest, uint8_t can = 10, bool streamElsePacket = false)
std::array<int8_t, 368> 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<float, 150> rrc_taps;

2
plugins/channelrx/demodm17/m17demod.h
View File

@ -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<uint8_t, 14>& 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(); }

2
plugins/channelrx/demodm17/m17demodbaseband.h
View File

@ -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<uint8_t, 14>& 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(); }

52
plugins/channelrx/demodm17/m17demodgui.cpp
View File

@ -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<uint8_t, 14>& 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);

3
plugins/channelrx/demodm17/m17demodgui.h
View File

@ -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<uint8_t, 14>& 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*);

111
plugins/channelrx/demodm17/m17demodgui.ui
View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>522</width>
<height>420</height>
<height>450</height>
</rect>
</property>
<property name="sizePolicy">
@ -19,13 +19,13 @@
<property name="minimumSize">
<size>
<width>522</width>
<height>420</height>
<height>450</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>560</width>
<height>420</height>
<height>450</height>
</size>
</property>
<property name="font">
@ -717,6 +717,111 @@
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="metaLayout">
<item>
<widget class="QLabel" name="qrbLabel">
<property name="minimumSize">
<size>
<width>0</width>
<height>30</height>
</size>
</property>
<property name="toolTip">
<string>QRB is distance to station (km) when GNSS data is available</string>
</property>
<property name="text">
<string>QRB</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="qrbText">
<property name="minimumSize">
<size>
<width>70</width>
<height>0</height>
</size>
</property>
<property name="font">
<font>
<family>Liberation Mono</family>
<pointsize>9</pointsize>
</font>
</property>
<property name="toolTip">
<string>Distance to station (km)</string>
</property>
<property name="frameShape">
<enum>QFrame::Box</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Sunken</enum>
</property>
<property name="text">
<string>...</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="qtfLabel">
<property name="toolTip">
<string>QRB is bearing to station (degrees) when GNSS data is available</string>
</property>
<property name="text">
<string>QTF</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="qtfText">
<property name="minimumSize">
<size>
<width>60</width>
<height>0</height>
</size>
</property>
<property name="font">
<font>
<family>Liberation Mono</family>
<pointsize>9</pointsize>
</font>
</property>
<property name="toolTip">
<string>Bearing to station (degrees)</string>
</property>
<property name="frameShape">
<enum>QFrame::Box</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Sunken</enum>
</property>
<property name="text">
<string>...</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<widget class="QTabWidget" name="digitalTabWidget">
<property name="minimumSize">

3
plugins/channelrx/demodm17/m17demodprocessor.cpp
View File

@ -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];

3
plugins/channelrx/demodm17/m17demodprocessor.h
View File

@ -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<uint8_t, 14>& 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<uint8_t, 14> 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;

2
plugins/channelrx/demodm17/m17demodsink.h
View File

@ -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<uint8_t, 14>& 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(); }

1
plugins/channeltx/modm17/m17modgui.cpp
View File

@ -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<int>::of(&QSpinBox::valueChanged), this, &M17ModGUI::on_can_valueChanged);
QObject::connect(ui->insertPosition, &ButtonSwitch::toggled, this, &M17ModGUI::on_insertPosition_toggled);
}
void M17ModGUI::updateAbsoluteCenterFrequency()

20
plugins/channeltx/modm17/m17modprocessor.cpp
View File

@ -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<uint8_t, 30> lsf;
std::array<int8_t, 368> lsf_frame = modemm17::M17Modulator::make_lsf(lsf, sourceCall.toStdString(), destCall.toStdString(), can);
std::array<int8_t, 368> 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<uint8_t, 30> lsf;
std::array<int8_t, 368> lsf_frame = modemm17::M17Modulator::make_lsf(lsf, sourceCall.toStdString(), destCall.toStdString(), can, true);
std::array<int8_t, 368> lsf_frame = m_m17Modulator.make_lsf(lsf, true);
output_baseband(modemm17::M17Modulator::LSF_SYNC_WORD, lsf_frame);
// Prepare LICH

40
plugins/channeltx/modm17/m17modprocessor.h
View File

@ -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();

19
plugins/channeltx/modm17/m17modsource.cpp
View File

@ -546,6 +546,25 @@ void M17ModSource::applySettings(const M17ModSettings& settings, const QList<QSt
}
}
if (settingsKeys.contains("insertPosition") || force)
{
if (settings.m_insertPosition != m_settings.m_insertPosition)
{
if (settings.m_insertPosition)
{
const MainSettings& mainSettings = MainCore::instance()->getSettings();
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 {