From 9e76b24e7ae2b78731ee33f214d5f0c0611f1c69 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 13 Jun 2016 23:12:28 +0100 Subject: [PATCH] Rewrite the GPS data collector. --- YSFGateway/GPS.cpp | 92 ++++++++++++++++++++++----------------- YSFGateway/GPS.h | 4 +- YSFGateway/YSFGateway.cpp | 2 +- 3 files changed, 53 insertions(+), 45 deletions(-) diff --git a/YSFGateway/GPS.cpp b/YSFGateway/GPS.cpp index ec82153..5c68d08 100644 --- a/YSFGateway/GPS.cpp +++ b/YSFGateway/GPS.cpp @@ -20,6 +20,7 @@ #include "YSFPayload.h" #include "YSFDefines.h" #include "Utils.h" +#include "CRC.h" #include "Log.h" #include @@ -33,8 +34,6 @@ const unsigned char LONG_GPS[] = {0x47U, 0x64U}; CGPS::CGPS(const std::string& callsign, const std::string& suffix, const std::string& password, const std::string& address, unsigned int port) : m_writer(callsign, suffix, password, address, port), m_buffer(NULL), -m_dt1(false), -m_dt2(false), m_sent(false) { assert(!callsign.empty()); @@ -42,7 +41,7 @@ m_sent(false) assert(!address.empty()); assert(port > 0U); - m_buffer = new unsigned char[20U]; + m_buffer = new unsigned char[300U]; } CGPS::~CGPS() @@ -60,72 +59,85 @@ bool CGPS::open() return m_writer.open(); } -void CGPS::data(const unsigned char* source, const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn) +void CGPS::data(const unsigned char* source, const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft) { if (m_sent) return; - // In case we had the data but not the source callsign - if (m_dt1 && m_dt2) { - transmitGPS(source); + if (fi != YSF_FI_COMMUNICATIONS) return; - } CYSFPayload payload; - if (fi == YSF_FI_COMMUNICATIONS && dt == YSF_DT_VD_MODE1) { - if (fn == 3U && !m_dt1) { - bool valid = payload.readVDMode1Data(data, m_buffer); - if (valid) { - m_dt1 = true; - m_dt2 = true; + if (dt == YSF_DT_VD_MODE1) { + if (fn == 0U || fn == 1U || fn == 2U) + return; + + bool valid = payload.readVDMode1Data(data, m_buffer + (fn - 3U) * 20U); + if (!valid) + return; + + if (fn == ft) { + bool valid = false; - // If no GPS data then mark it as complete for this transmission - if (::memcmp(m_buffer + 1U, NULL_GPS, 2U) == 0) { - CUtils::dump("Null GPS data received", m_buffer, 20U); - m_sent = true; + // Find the end marker + for (unsigned int i = (fn - 2U) * 20U; i > 0U; i--) { + if (m_buffer[i] == 0x03U) { + unsigned char crc = CCRC::addCRC(m_buffer, i + 1U); + if (crc == m_buffer[i + 1U]) + valid = true; + + break; } + } + if (valid) { if (::memcmp(m_buffer + 1U, SHRT_GPS, 2U) == 0) { - CUtils::dump("Short GPS data received", m_buffer, 20U); + CUtils::dump("Short GPS data received", m_buffer, (fn - 2U) * 20U); transmitGPS(source); } if (::memcmp(m_buffer + 1U, LONG_GPS, 2U) == 0) { - CUtils::dump("Long GPS data received", m_buffer, 20U); + CUtils::dump("Long GPS data received", m_buffer, (fn - 2U) * 20U); transmitGPS(source); } + + m_sent = true; } } - } else if (fi == YSF_FI_COMMUNICATIONS && dt == YSF_DT_VD_MODE2) { - if (fn == 6U && !m_dt1) { - bool valid = payload.readVDMode2Data(data, m_buffer + 0U); - if (valid) { - m_dt1 = true; - - // If no GPS data then mark it as complete for this transmission - if (::memcmp(m_buffer + 1U, NULL_GPS, 2U) == 0) { - CUtils::dump("Null GPS data received", m_buffer, 20U); - m_sent = true; - m_dt2 = true; + } else if (dt == YSF_DT_VD_MODE2) { + if (fn != 6U && fn != 7U) + return; + + bool valid = payload.readVDMode2Data(data, m_buffer + (fn - 6U) * 10U); + if (!valid) + return; + + if (fn == ft) { + bool valid = false; + + // Find the end marker + for (unsigned int i = (fn - 5U) * 10U; i > 0U; i--) { + if (m_buffer[i] == 0x03U) { + unsigned char crc = CCRC::addCRC(m_buffer, i + 1U); + if (crc == m_buffer[i + 1U]) + valid = true; + break; } } - } - - if (fn == 7U && !m_dt2) { - bool valid = payload.readVDMode2Data(data, m_buffer + 10U); - if (valid) { - m_dt2 = true; + if (valid) { if (::memcmp(m_buffer + 1U, SHRT_GPS, 2U) == 0) { - CUtils::dump("Short GPS data received", m_buffer, 20U); + CUtils::dump("Short GPS data received", m_buffer, (fn - 5U) * 10U); transmitGPS(source); } if (::memcmp(m_buffer + 1U, LONG_GPS, 2U) == 0) { - CUtils::dump("Long GPS data received", m_buffer, 20U); + CUtils::dump("Long GPS data received", m_buffer, (fn - 5U) * 10U); transmitGPS(source); } + + m_sent = true; } } } @@ -133,8 +145,6 @@ void CGPS::data(const unsigned char* source, const unsigned char* data, unsigned void CGPS::reset() { - m_dt1 = false; - m_dt2 = false; m_sent = false; } diff --git a/YSFGateway/GPS.h b/YSFGateway/GPS.h index 968b61f..379f3b7 100644 --- a/YSFGateway/GPS.h +++ b/YSFGateway/GPS.h @@ -32,7 +32,7 @@ public: bool open(); - void data(const unsigned char* source, const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn); + void data(const unsigned char* source, const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft); void clock(unsigned int ms); @@ -43,8 +43,6 @@ public: private: CAPRSWriter m_writer; unsigned char* m_buffer; - bool m_dt1; - bool m_dt2; bool m_sent; void transmitGPS(const unsigned char* source); diff --git a/YSFGateway/YSFGateway.cpp b/YSFGateway/YSFGateway.cpp index d0f58de..311a752 100644 --- a/YSFGateway/YSFGateway.cpp +++ b/YSFGateway/YSFGateway.cpp @@ -251,7 +251,7 @@ int CYSFGateway::run() } if (m_gps != NULL) - m_gps->data(buffer + 14U, buffer + 35U, fi, dt, fn); + m_gps->data(buffer + 14U, buffer + 35U, fi, dt, fn, ft); } if (networkEnabled && m_linked && !m_exclude)