MMDVM/MMDVMHost
1
0
Fork 0
mirror of https://github.com/g4klx/MMDVMHost synced 2025-12-22 08:05:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Handle 2's complement values in the embedded GPS data properly.

Browse Source
This commit is contained in:
Jonathan Naylor
2017-11-26 17:56:20 +00:00
parent 64eb59bb52
commit a215ce2982
2 changed files with 13 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
DMRSlot.cpp
View File

@@ -29,6 +29,7 @@
#include <cassert> #include <cassert>
#include <ctime> #include <ctime>
#include <algorithm> #include <algorithm>
#include <cstdint>
unsigned int CDMRSlot::m_colorCode = 0U; unsigned int CDMRSlot::m_colorCode = 0U;
@@ -601,7 +602,7 @@ bool CDMRSlot::writeModem(unsigned char *data, unsigned int len)
if (m_dumpTAData) { if (m_dumpTAData) {
::sprintf(text, "DMR Slot %u, Embedded GPS Info", m_slotNo); ::sprintf(text, "DMR Slot %u, Embedded GPS Info", m_slotNo);
CUtils::dump(2U, text, data, 9U); CUtils::dump(2U, text, data, 9U);
logGPSposition(data); logGPSPosition(data);
} }
if (m_network != NULL) if (m_network != NULL)
m_network->writePosition(m_rfLC->getSrcId(), data); m_network->writePosition(m_rfLC->getSrcId(), data);
@@ -1390,7 +1391,7 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData)
if (m_dumpTAData) { if (m_dumpTAData) {
::sprintf(text, "DMR Slot %u, Embedded GPS Info", m_slotNo); ::sprintf(text, "DMR Slot %u, Embedded GPS Info", m_slotNo);
CUtils::dump(2U, text, data, 9U); CUtils::dump(2U, text, data, 9U);
logGPSposition(data); logGPSPosition(data);
} }
break; break;
case FLCO_TALKER_ALIAS_HEADER: case FLCO_TALKER_ALIAS_HEADER:
@@ -1608,7 +1609,7 @@ void CDMRSlot::writeNetwork(const CDMRData& dmrData)
} }
void CDMRSlot::logGPSposition(const unsigned char* data) void CDMRSlot::logGPSPosition(const unsigned char* data)
{ {
unsigned int errorI = (data[2U] & 0x0E) >> 1U; unsigned int errorI = (data[2U] & 0x0E) >> 1U;
@@ -1636,25 +1637,23 @@ void CDMRSlot::logGPSposition(const unsigned char* data)
::strcpy(errorS, "> 200km"); ::strcpy(errorS, "> 200km");
break; break;
default: default:
::strcpy(errorS, "not known or position invalid"); ::strcpy(errorS, "not known");
break; break;
} }
long longitudeI = (data[3U] << 16) + (data[4U] << 8) + data[5U]; int32_t longitudeI = ((data[2U] & 0x01U) << 31) | (data[3U] << 23) | (data[4U] << 15) | (data[5U] << 7);
if ((data[2U] & 0x01U) == 0x01U) longitudeI >>= 7;
longitudeI = -longitudeI;
long latitudeI = ((data[6U] & 0x7FU) << 16) + (data[7U] << 8) + data[8U]; int32_t latitudeI = (data[6U] << 24) | (data[7U] << 16) | (data[8U] << 8);
if ((data[6U] & 0x80U) == 0x80U) latitudeI >>= 8;
latitudeI = -latitudeI;
float longitude = 360.0F / 33554432.0F; // 360/2^25 steps float longitude = 360.0F / 33554432.0F; // 360/2^25 steps
float latitude = 180.0F / 16777216.0F; // 180/2^24 steps float latitude = 180.0F / 16777216.0F; // 180/2^24 steps
longitude *= longitudeI; longitude *= float(longitudeI);
latitude *= latitudeI; latitude *= float(latitudeI);
LogMessage("GPS position [%08f,%09f] (Position error %s)",latitude,longitude,errorS); LogMessage("GPS position [%f,%f] (Position error %s)", latitude, longitude, errorS);
} }
void CDMRSlot::clock() void CDMRSlot::clock()

2
DMRSlot.h
View File

@@ -124,7 +124,7 @@ private:
static unsigned char m_id2; static unsigned char m_id2;
static ACTIVITY_TYPE m_activity2; static ACTIVITY_TYPE m_activity2;
void logGPSposition(const unsigned char* data); void logGPSPosition(const unsigned char* data);
void writeQueueRF(const unsigned char* data); void writeQueueRF(const unsigned char* data);
void writeQueueNet(const unsigned char* data); void writeQueueNet(const unsigned char* data);