Skip to content

Commit

Permalink
NMEA GxGGA output: work around issue with 'always 0.0,M' geoid separa…
Browse files Browse the repository at this point in the history 
…tion value
  • Loading branch information
@lyusupov
lyusupov committed Sep 13, 2018
1 parent 4ff8ccf commit 0ef8557
Show file tree
Hide file tree
Showing 3 changed files with 105 additions and 70 deletions.
31 changes: 10 additions & 21 deletions software/firmware/source/SoftRF/GNSSHelper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -588,29 +588,18 @@ void PickGNSSFix()
write_size += sentence_size + strlen(csum_ptr);
}

Serial.write((uint8_t *) &GNSSbuf[ndx], write_size);
Serial.write('\n');

if (SoC->Bluetooth) {
SoC->Bluetooth->write((uint8_t *) &GNSSbuf[ndx], write_size);
SoC->Bluetooth->write((uint8_t *) "\n", 1);
}

if (settings->nmea_u) {
// ASSERT(sizeof(UDPpacketBuffer) > sizeof(GNSSbuf))
memcpy(UDPpacketBuffer, &GNSSbuf[ndx], write_size);
UDPpacketBuffer[write_size] = '\n';
SoC->WiFi_transmit_UDP(NMEA_DST_PORT, (byte *)UDPpacketBuffer, write_size + 1);
/*
* Work around issue with "always 0.0,M" GGA geoid separation value
* given by some Chinese GNSS chipsets
*/
if (hw_info.model == SOFTRF_MODEL_PRIME_MK2 &&
!strncmp((char *) &GNSSbuf[ndx+3], "GGA,", strlen("GGA,")) &&
gnss.separation.meters() == 0.0) {
NMEA_GGA();
} else {
NMEA_Out(&GNSSbuf[ndx], write_size, true);
}

#if defined(AIRCONNECT_IS_ACTIVE)
for (uint8_t acc_ndx = 0; acc_ndx < MAX_AIRCONNECT_CLIENTS; acc_ndx++) {
if (AirConnectClient[acc_ndx] && AirConnectClient[acc_ndx].connected()){
AirConnectClient[acc_ndx].write(&GNSSbuf[ndx], GNSS_cnt - ndx + 1);
AirConnectClient[acc_ndx].write('\n');
}
}
#endif
break;
}
}
Expand Down
142 changes: 93 additions & 49 deletions software/firmware/source/SoftRF/NMEAHelper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,43 @@ void NMEA_loop()
#endif
}

void NMEA_Out(byte *buf, size_t size, bool nl)
{
Serial.write(buf, size);
if (nl)
Serial.write('\n');

if (SoC->Bluetooth) {
SoC->Bluetooth->write(buf, size);
if (nl)
SoC->Bluetooth->write((byte *) "\n", 1);
}

if (settings->nmea_u) {
size_t udp_size = size;

if (size >= sizeof(UDPpacketBuffer))
udp_size = sizeof(UDPpacketBuffer) - 1;
memcpy(UDPpacketBuffer, buf, udp_size);

if (nl)
UDPpacketBuffer[udp_size] = '\n';

SoC->WiFi_transmit_UDP(NMEA_DST_PORT, (byte *) UDPpacketBuffer,
nl ? udp_size + 1 : udp_size);
}

#if defined(AIRCONNECT_IS_ACTIVE)
for (uint8_t acc_ndx = 0; acc_ndx < MAX_AIRCONNECT_CLIENTS; acc_ndx++) {
if (AirConnectClient[acc_ndx] && AirConnectClient[acc_ndx].connected()){
AirConnectClient[acc_ndx].write(buf, size);
if (nl)
AirConnectClient[acc_ndx].write('\n');
}
}
#endif
}

void NMEA_Export()
{
int bearing;
Expand Down Expand Up @@ -159,22 +196,7 @@ void NMEA_Export()
csum_ptr = NMEABuffer + strlen(NMEABuffer);
snprintf(csum_ptr, sizeof(NMEABuffer) - strlen(NMEABuffer), "%02X\r\n", cs);

Serial.write((uint8_t *) NMEABuffer, strlen(NMEABuffer));
if (SoC->Bluetooth) {
SoC->Bluetooth->write((uint8_t *) NMEABuffer, strlen(NMEABuffer));
}

if (settings->nmea_u) {
SoC->WiFi_transmit_UDP(NMEA_DST_PORT, (byte *) NMEABuffer, strlen(NMEABuffer));
}

#if defined(AIRCONNECT_IS_ACTIVE)
for (uint8_t acc_ndx = 0; acc_ndx < MAX_AIRCONNECT_CLIENTS; acc_ndx++) {
if (AirConnectClient[acc_ndx] && AirConnectClient[acc_ndx].connected()){
AirConnectClient[acc_ndx].write(NMEABuffer, strlen(NMEABuffer));
}
}
#endif
NMEA_Out((byte *) NMEABuffer, strlen(NMEABuffer), false);

/* Most close traffic is treated as highest priority target */
if (distance < HP_distance) {
Expand Down Expand Up @@ -209,23 +231,7 @@ void NMEA_Export()
csum_ptr = NMEABuffer + strlen(NMEABuffer);
snprintf(csum_ptr, sizeof(NMEABuffer) - strlen(NMEABuffer), "%02X\r\n", cs);

Serial.write((uint8_t *) NMEABuffer, strlen(NMEABuffer));

if (SoC->Bluetooth) {
SoC->Bluetooth->write((uint8_t *) NMEABuffer, strlen(NMEABuffer));
}

if (settings->nmea_u) {
SoC->WiFi_transmit_UDP(NMEA_DST_PORT, (byte *) NMEABuffer, strlen(NMEABuffer));
}

#if defined(AIRCONNECT_IS_ACTIVE)
for (uint8_t acc_ndx = 0; acc_ndx < MAX_AIRCONNECT_CLIENTS; acc_ndx++) {
if (AirConnectClient[acc_ndx] && AirConnectClient[acc_ndx].connected()){
AirConnectClient[acc_ndx].write(NMEABuffer, strlen(NMEABuffer));
}
}
#endif
NMEA_Out((byte *) NMEABuffer, strlen(NMEABuffer), false);
}
}

Expand All @@ -252,7 +258,8 @@ void NMEA_Position()
info.longitude = ((int) ThisAircraft.longitude) * 100.0;
info.longitude += (ThisAircraft.longitude - (int) ThisAircraft.longitude) * 60.0;
info.speed = ThisAircraft.speed * _GPS_KMPH_PER_KNOT;
info.elevation = ThisAircraft.altitude;
info.elevation = ThisAircraft.altitude; /* above MSL */
info.height = LookupSeparation(ThisAircraft.latitude, ThisAircraft.longitude);
info.track = ThisAircraft.course;

#if 0
Expand All @@ -269,6 +276,7 @@ void NMEA_Position()
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_LON);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_SPEED);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_ELV);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_HEIGHT);

nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_TRACK);
#if 0
Expand Down Expand Up @@ -327,21 +335,57 @@ void NMEA_Position()
}

if (gen_sz) {
Serial.write((uint8_t *) nmealib_buf.buffer, gen_sz);
if (SoC->Bluetooth) {
SoC->Bluetooth->write((uint8_t *) nmealib_buf.buffer, gen_sz);
}
if (settings->nmea_u) {
SoC->WiFi_transmit_UDP(NMEA_DST_PORT, (byte *) nmealib_buf.buffer, gen_sz);
}

#if defined(AIRCONNECT_IS_ACTIVE)
for (uint8_t acc_ndx = 0; acc_ndx < MAX_AIRCONNECT_CLIENTS; acc_ndx++) {
if (AirConnectClient[acc_ndx] && AirConnectClient[acc_ndx].connected()){
AirConnectClient[acc_ndx].write((char *) nmealib_buf.buffer, gen_sz);
}
}
#endif
NMEA_Out((byte *) nmealib_buf.buffer, gen_sz, false);
}
}
}

void NMEA_GGA()
{
NmeaInfo info;

float latitude = gnss.location.lat();
float longitude = gnss.location.lng();

nmeaInfoClear(&info);

info.utc.hour = gnss.time.hour();
info.utc.min = gnss.time.minute();
info.utc.sec = gnss.time.second();
info.utc.hsec = gnss.time.centisecond();

info.latitude = ((int) latitude) * 100.0;
info.latitude += (latitude - (int) latitude) * 60.0;
info.longitude = ((int) longitude) * 100.0;
info.longitude += (longitude - (int) longitude) * 60.0;

info.sig = (NmeaSignal) gnss.location.Quality();
info.satellites.inViewCount = gnss.satellites.value();

info.hdop = gnss.hdop.hdop();

info.elevation = gnss.altitude.meters(); /* above MSL */
info.height = gnss.separation.meters();

if (info.height == 0.0) {
info.height = LookupSeparation(latitude, longitude);
info.elevation -= info.height;
}

nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_UTCTIME);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_LAT);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_LON);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_SIG);
/* Should be SATINUSECOUNT, but it seems to be a bug in NMEALib */
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_SATINVIEWCOUNT);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_HDOP);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_ELV);
nmeaInfoSetPresent(&info.present, NMEALIB_PRESENT_HEIGHT);

size_t gen_sz = nmeaSentenceFromInfo(&nmealib_buf, &info, (NmeaSentence)
NMEALIB_SENTENCE_GPGGA );

if (gen_sz) {
NMEA_Out((byte *) nmealib_buf.buffer, gen_sz, false);
}
}
2 changes: 2 additions & 0 deletions software/firmware/source/SoftRF/NMEAHelper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@ void NMEA_setup(void);
void NMEA_loop(void);
void NMEA_Export(void);
void NMEA_Position(void);
void NMEA_Out(byte *, size_t, bool);
void NMEA_GGA(void);

#if defined(AIRCONNECT_IS_ACTIVE)
#define MAX_AIRCONNECT_CLIENTS 2
Expand Down

0 comments on commit 0ef8557

Please sign in to comment.