kopia lustrzana https://github.com/meshtastic/firmware
72 wiersze
3.1 KiB
C++
72 wiersze
3.1 KiB
C++
#include "NMEAWPL.h"
|
|
#include "GeoCoord.h"
|
|
|
|
/* -------------------------------------------
|
|
* 1 2 3 4 5 6
|
|
* | | | | | |
|
|
* $--WPL,llll.ll,a,yyyyy.yy,a,c--c*hh<CR><LF>
|
|
*
|
|
* Field Number:
|
|
* 1 Latitude
|
|
* 2 N or S (North or South)
|
|
* 3 Longitude
|
|
* 4 E or W (East or West)
|
|
* 5 Waypoint name
|
|
* 6 Checksum
|
|
* -------------------------------------------
|
|
*/
|
|
|
|
uint32_t printWPL(char *buf, size_t bufsz, const Position &pos, const char *name)
|
|
{
|
|
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
|
|
uint32_t len = snprintf(buf, bufsz, "$GNWPL,%02d%07.4f,%c,%03d%07.4f,%c,%s", geoCoord.getDMSLatDeg(),
|
|
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(),
|
|
geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6,
|
|
geoCoord.getDMSLonCP(), name);
|
|
uint32_t chk = 0;
|
|
for (uint32_t i = 1; i < len; i++) {
|
|
chk ^= buf[i];
|
|
}
|
|
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
|
|
return len;
|
|
}
|
|
|
|
/* -------------------------------------------
|
|
* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
|
|
* | | | | | | | | | | | | | | |
|
|
* $--GGA,hhmmss.ss,ddmm.mm,a,ddmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh<CR><LF>
|
|
*
|
|
* Field Number:
|
|
* 1 UTC of this position report, hh is hours, mm is minutes, ss.ss is seconds.
|
|
* 2 Latitude
|
|
* 3 N or S (North or South)
|
|
* 4 Longitude
|
|
* 5 E or W (East or West)
|
|
* 6 GPS Quality Indicator (non null)
|
|
* 7 Number of satellites in use, 00 - 12
|
|
* 8 Horizontal Dilution of precision (meters)
|
|
* 9 Antenna Altitude above/below mean-sea-level (geoid) (in meters)
|
|
* 10 Units of antenna altitude, meters
|
|
* 11 Geoidal separation, the difference between the WGS-84 earth ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
|
|
* below ellipsoid 12 Units of geoidal separation, meters 13 Age of differential GPS data, time in seconds since last SC104 type 1
|
|
* or 9 update, null field when DGPS is not used 14 Differential reference station ID, 0000-1023 15 Checksum
|
|
* -------------------------------------------
|
|
*/
|
|
|
|
uint32_t printGGA(char *buf, size_t bufsz, const Position &pos)
|
|
{
|
|
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
|
|
uint32_t len =
|
|
snprintf(buf, bufsz, "$GNGGA,%06u.%03u,%02d%07.4f,%c,%03d%07.4f,%c,%u,%02u,%04u,%04d,%c,%04d,%c,%d,%04d", pos.time / 1000,
|
|
pos.time % 1000, geoCoord.getDMSLatDeg(), (abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6,
|
|
geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
|
|
(abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), pos.fix_type,
|
|
pos.sats_in_view, pos.HDOP, geoCoord.getAltitude(), 'M', pos.altitude_geoidal_separation, 'M', 0, 0);
|
|
|
|
uint32_t chk = 0;
|
|
for (uint32_t i = 1; i < len; i++) {
|
|
chk ^= buf[i];
|
|
}
|
|
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
|
|
return len;
|
|
} |