607 lines
26 KiB
C++
607 lines
26 KiB
C++
/**
|
|
* \file lwgps.c
|
|
* \brief GPS main file
|
|
*/
|
|
|
|
/*
|
|
* Copyright (c) 2020 Tilen MAJERLE
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person
|
|
* obtaining a copy of this software and associated documentation
|
|
* files (the "Software"), to deal in the Software without restriction,
|
|
* including without limitation the rights to use, copy, modify, merge,
|
|
* publish, distribute, sublicense, and/or sell copies of the Software,
|
|
* and to permit persons to whom the Software is furnished to do so,
|
|
* subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be
|
|
* included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
|
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
|
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
|
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
|
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
|
* OTHER DEALINGS IN THE SOFTWARE.
|
|
*
|
|
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
|
*
|
|
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
|
* Version: v2.1.0
|
|
*/
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include "lwgps.h"
|
|
|
|
#define FLT(x) ((lwgps_float_t)(x))
|
|
#define D2R(x) FLT(FLT(x) * FLT(0.01745329251994)) /*!< Degrees to radians */
|
|
#define R2D(x) FLT(FLT(x) * FLT(57.29577951308232))/*!< Radians to degrees */
|
|
#define EARTH_RADIUS FLT(6371.0) /*!< Earth radius in units of kilometers */
|
|
|
|
#define CRC_ADD(_gh, ch) (_gh)->p.crc_calc ^= (uint8_t)(ch)
|
|
#define TERM_ADD(_gh, ch) do { \
|
|
if ((_gh)->p.term_pos < (sizeof((_gh)->p.term_str) - 1)) { \
|
|
(_gh)->p.term_str[(_gh)->p.term_pos] = (ch);\
|
|
(_gh)->p.term_str[++(_gh)->p.term_pos] = 0; \
|
|
} \
|
|
} while (0)
|
|
#define TERM_NEXT(_gh) do { (_gh)->p.term_str[((_gh)->p.term_pos = 0)] = 0; ++(_gh)->p.term_num; } while (0)
|
|
|
|
#define CIN(x) ((x) >= '0' && (x) <= '9')
|
|
#define CIHN(x) (((x) >= '0' && (x) <= '9') || ((x) >= 'a' && (x) <= 'f') || ((x) >= 'A' && (x) <= 'F'))
|
|
#define CTN(x) ((x) - '0')
|
|
#define CHTN(x) (((x) >= '0' && (x) <= '9') ? ((x) - '0') : (((x) >= 'a' && (x) <= 'z') ? ((x) - 'a' + 10) : (((x) >= 'A' && (x) <= 'Z') ? ((x) - 'A' + 10) : 0)))
|
|
|
|
/**
|
|
* \brief Parse number as integer
|
|
* \param[in] gh: GPS handle
|
|
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
|
|
* \return Parsed integer
|
|
*/
|
|
static int32_t
|
|
prv_parse_number(lwgps_t* gh, const char* t) {
|
|
int32_t res = 0;
|
|
uint8_t minus;
|
|
|
|
if (t == NULL) {
|
|
t = gh->p.term_str;
|
|
}
|
|
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
|
|
|
|
minus = (*t == '-' ? (++t, 1) : 0);
|
|
for (; t != NULL && CIN(*t); ++t) {
|
|
res = 10 * res + CTN(*t);
|
|
}
|
|
return minus ? -res : res;
|
|
}
|
|
|
|
/**
|
|
* \brief Parse number as double and convert it to \ref lwgps_float_t
|
|
* \param[in] gh: GPS handle
|
|
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
|
|
* \return Parsed double in \ref lwgps_float_t format
|
|
*/
|
|
static lwgps_float_t
|
|
prv_parse_float_number(lwgps_t* gh, const char* t) {
|
|
lwgps_float_t res;
|
|
|
|
if (t == NULL) {
|
|
t = gh->p.term_str;
|
|
}
|
|
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
|
|
|
|
#if LWGPS_CFG_DOUBLE
|
|
res = strtod(t, NULL); /* Parse string to double */
|
|
#else /* LWGPS_CFG_DOUBLE */
|
|
res = strtof(t, NULL); /* Parse string to float */
|
|
#endif /* !LWGPS_CFG_DOUBLE */
|
|
|
|
return FLT(res); /* Return casted value, based on float size */
|
|
}
|
|
|
|
/**
|
|
* \brief Parse latitude/longitude NMEA format to double
|
|
*
|
|
* NMEA output for latitude is ddmm.sss and longitude is dddmm.sss
|
|
* \param[in] gh: GPS handle
|
|
* \return Latitude/Longitude value in degrees
|
|
*/
|
|
static lwgps_float_t
|
|
prv_parse_lat_long(lwgps_t* gh) {
|
|
lwgps_float_t ll, deg, min;
|
|
|
|
ll = prv_parse_float_number(gh, NULL); /* Parse value as double */
|
|
deg = FLT((int)((int)ll / 100)); /* Get absolute degrees value, interested in integer part only */
|
|
min = ll - (deg * FLT(100)); /* Get remaining part from full number, minutes */
|
|
ll = deg + (min / FLT(60.0)); /* Calculate latitude/longitude */
|
|
|
|
return ll;
|
|
}
|
|
|
|
/**
|
|
* \brief Parse received term
|
|
* \param[in] gh: GPS handle
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
static uint8_t
|
|
prv_parse_term(lwgps_t* gh) {
|
|
if (gh->p.term_num == 0) { /* Check string type */
|
|
if (0) {
|
|
#if LWGPS_CFG_STATEMENT_GPGGA
|
|
} else if (!strncmp(gh->p.term_str, "$GPGGA", 6) || !strncmp(gh->p.term_str, "$GNGGA", 6)) {
|
|
gh->p.stat = STAT_GGA;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSA
|
|
} else if (!strncmp(gh->p.term_str, "$GPGSA", 6) || !strncmp(gh->p.term_str, "$GNGSA", 6)) {
|
|
gh->p.stat = STAT_GSA;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSV
|
|
} else if (!strncmp(gh->p.term_str, "$GPGSV", 6) || !strncmp(gh->p.term_str, "$GNGSV", 6)) {
|
|
gh->p.stat = STAT_GSV;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
|
#if LWGPS_CFG_STATEMENT_GPRMC
|
|
} else if (!strncmp(gh->p.term_str, "$GPRMC", 6) || !strncmp(gh->p.term_str, "$GNRMC", 6)) {
|
|
gh->p.stat = STAT_RMC;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
|
#if LWGPS_CFG_STATEMENT_PUBX
|
|
} else if (!strncmp(gh->p.term_str, "$PUBX", 5)) {
|
|
gh->p.stat = STAT_UBX;
|
|
#endif /* LWGPS_CFG_STATEMENT_PUBX */
|
|
} else {
|
|
gh->p.stat = STAT_UNKNOWN; /* Invalid statement for library */
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/* Start parsing terms */
|
|
if (gh->p.stat == STAT_UNKNOWN) {
|
|
#if LWGPS_CFG_STATEMENT_GPGGA
|
|
} else if (gh->p.stat == STAT_GGA) { /* Process GPGGA statement */
|
|
switch (gh->p.term_num) {
|
|
case 1: /* Process UTC time */
|
|
gh->p.data.gga.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
|
gh->p.data.gga.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
|
gh->p.data.gga.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
|
break;
|
|
case 2: /* Latitude */
|
|
gh->p.data.gga.latitude = prv_parse_lat_long(gh); /* Parse latitude */
|
|
break;
|
|
case 3: /* Latitude north/south information */
|
|
if (gh->p.term_str[0] == 'S' || gh->p.term_str[0] == 's') {
|
|
gh->p.data.gga.latitude = -gh->p.data.gga.latitude;
|
|
}
|
|
break;
|
|
case 4: /* Longitude */
|
|
gh->p.data.gga.longitude = prv_parse_lat_long(gh); /* Parse longitude */
|
|
break;
|
|
case 5: /* Longitude east/west information */
|
|
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
|
|
gh->p.data.gga.longitude = -gh->p.data.gga.longitude;
|
|
}
|
|
break;
|
|
case 6: /* Fix status */
|
|
gh->p.data.gga.fix = (uint8_t)prv_parse_number(gh, NULL);
|
|
break;
|
|
case 7: /* Satellites in use */
|
|
gh->p.data.gga.sats_in_use = (uint8_t)prv_parse_number(gh, NULL);
|
|
break;
|
|
case 9: /* Altitude */
|
|
gh->p.data.gga.altitude = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 11: /* Altitude above ellipsoid */
|
|
gh->p.data.gga.geo_sep = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSA
|
|
} else if (gh->p.stat == STAT_GSA) { /* Process GPGSA statement */
|
|
switch (gh->p.term_num) {
|
|
case 2: /* Process fix mode */
|
|
gh->p.data.gsa.fix_mode = (uint8_t)prv_parse_number(gh, NULL);
|
|
break;
|
|
case 15: /* Process PDOP */
|
|
gh->p.data.gsa.dop_p = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 16: /* Process HDOP */
|
|
gh->p.data.gsa.dop_h = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 17: /* Process VDOP */
|
|
gh->p.data.gsa.dop_v = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
default:
|
|
/* Parse satellite IDs */
|
|
if (gh->p.term_num >= 3 && gh->p.term_num <= 14) {
|
|
gh->p.data.gsa.satellites_ids[gh->p.term_num - 3] = (uint8_t)prv_parse_number(gh, NULL);
|
|
}
|
|
break;
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSV
|
|
} else if (gh->p.stat == STAT_GSV) { /* Process GPGSV statement */
|
|
switch (gh->p.term_num) {
|
|
case 2: /* Current GPGSV statement number */
|
|
gh->p.data.gsv.stat_num = (uint8_t)prv_parse_number(gh, NULL);
|
|
break;
|
|
case 3: /* Process satellites in view */
|
|
gh->p.data.gsv.sats_in_view = (uint8_t)prv_parse_number(gh, NULL);
|
|
break;
|
|
default:
|
|
#if LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
|
|
if (gh->p.term_num >= 4 && gh->p.term_num <= 19) { /* Check current term number */
|
|
uint8_t index, term_num = gh->p.term_num - 4; /* Normalize term number from 4-19 to 0-15 */
|
|
uint16_t value;
|
|
|
|
index = ((gh->p.data.gsv.stat_num - 1) << 0x02) + (term_num >> 2); /* Get array index */
|
|
if (index < sizeof(gh->sats_in_view_desc) / sizeof(gh->sats_in_view_desc[0])) {
|
|
value = (uint16_t)prv_parse_number(gh, NULL); /* Parse number as integer */
|
|
switch (term_num & 0x03) {
|
|
case 0:
|
|
gh->sats_in_view_desc[index].num = value;
|
|
break;
|
|
case 1:
|
|
gh->sats_in_view_desc[index].elevation = value;
|
|
break;
|
|
case 2:
|
|
gh->sats_in_view_desc[index].azimuth = value;
|
|
break;
|
|
case 3:
|
|
gh->sats_in_view_desc[index].snr = value;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSV_SAT_DET */
|
|
break;
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
|
#if LWGPS_CFG_STATEMENT_GPRMC
|
|
} else if (gh->p.stat == STAT_RMC) { /* Process GPRMC statement */
|
|
switch (gh->p.term_num) {
|
|
case 2: /* Process valid status */
|
|
gh->p.data.rmc.is_valid = (gh->p.term_str[0] == 'A');
|
|
break;
|
|
case 7: /* Process ground speed in knots */
|
|
gh->p.data.rmc.speed = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 8: /* Process true ground coarse */
|
|
gh->p.data.rmc.course = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 9: /* Process date */
|
|
gh->p.data.rmc.date = (uint8_t)(10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]));
|
|
gh->p.data.rmc.month = (uint8_t)(10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]));
|
|
gh->p.data.rmc.year = (uint8_t)(10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]));
|
|
break;
|
|
case 10: /* Process magnetic variation */
|
|
gh->p.data.rmc.variation = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 11: /* Process magnetic variation east/west */
|
|
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
|
|
gh->p.data.rmc.variation = -gh->p.data.rmc.variation;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
|
#if LWGPS_CFG_STATEMENT_PUBX
|
|
} else if (gh->p.stat == STAT_UBX) { /* Disambiguate generic PUBX statement */
|
|
if (gh->p.term_str[0] == '0' && gh->p.term_str[1] == '4') {
|
|
gh->p.stat = STAT_UBX_TIME;
|
|
}
|
|
#if LWGPS_CFG_STATEMENT_PUBX_TIME
|
|
} else if (gh->p.stat == STAT_UBX_TIME) { /* Process PUBX (uBlox) TIME statement */
|
|
switch (gh->p.term_num) {
|
|
case 2: /* Process UTC time; ignore fractions of seconds */
|
|
gh->p.data.time.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
|
gh->p.data.time.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
|
gh->p.data.time.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
|
break;
|
|
case 3: /* Process UTC date */
|
|
gh->p.data.time.date = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
|
|
gh->p.data.time.month = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
|
|
gh->p.data.time.year = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
|
|
break;
|
|
case 4: /* Process UTC TimeOfWeek */
|
|
gh->p.data.time.utc_tow = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 5: /* Process UTC WeekNumber */
|
|
gh->p.data.time.utc_wk = prv_parse_number(gh, NULL);
|
|
break;
|
|
case 6: /* Process UTC leap seconds */
|
|
/*
|
|
* Accomodate a 2- or 3-digit leap second count
|
|
* a trailing 'D' means this is the firmware's default value.
|
|
*/
|
|
if (gh->p.term_str[2] == 'D' || gh->p.term_str[2] == '\0') {
|
|
gh->p.data.time.leap_sec = 10 * CTN(gh->p.term_str[0])
|
|
+ CTN(gh->p.term_str[1]);
|
|
} else {
|
|
gh->p.data.time.leap_sec = 100 * CTN(gh->p.term_str[0])
|
|
+ 10 * CTN(gh->p.term_str[1])
|
|
+ CTN(gh->p.term_str[2]);
|
|
}
|
|
break;
|
|
case 7: /* Process clock bias */
|
|
gh->p.data.time.clk_bias = prv_parse_number(gh, NULL);
|
|
break;
|
|
case 8: /* Process clock drift */
|
|
gh->p.data.time.clk_drift = prv_parse_float_number(gh, NULL);
|
|
break;
|
|
case 9: /* Process time pulse granularity */
|
|
gh->p.data.time.tp_gran = prv_parse_number(gh, NULL);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
|
|
#endif /* LWGPS_CFG_STATEMENT_PUBX */
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* \brief Compare calculated CRC with received CRC
|
|
* \param[in] gh: GPS handle
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
static uint8_t
|
|
prv_check_crc(lwgps_t* gh) {
|
|
uint8_t crc;
|
|
crc = (uint8_t)((CHTN(gh->p.term_str[0]) & 0x0F) << 0x04) | (CHTN(gh->p.term_str[1]) & 0x0F); /* Convert received CRC from string (hex) to number */
|
|
return gh->p.crc_calc == crc; /* They must match! */
|
|
}
|
|
|
|
/**
|
|
* \brief Copy temporary memory to user memory
|
|
* \param[in] gh: GPS handle
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
static uint8_t
|
|
prv_copy_from_tmp_memory(lwgps_t* gh) {
|
|
if (0) {
|
|
#if LWGPS_CFG_STATEMENT_GPGGA
|
|
} else if (gh->p.stat == STAT_GGA) {
|
|
gh->latitude = gh->p.data.gga.latitude;
|
|
gh->longitude = gh->p.data.gga.longitude;
|
|
gh->altitude = gh->p.data.gga.altitude;
|
|
gh->geo_sep = gh->p.data.gga.geo_sep;
|
|
gh->sats_in_use = gh->p.data.gga.sats_in_use;
|
|
gh->fix = gh->p.data.gga.fix;
|
|
gh->hours = gh->p.data.gga.hours;
|
|
gh->minutes = gh->p.data.gga.minutes;
|
|
gh->seconds = gh->p.data.gga.seconds;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSA
|
|
} else if (gh->p.stat == STAT_GSA) {
|
|
gh->dop_h = gh->p.data.gsa.dop_h;
|
|
gh->dop_p = gh->p.data.gsa.dop_p;
|
|
gh->dop_v = gh->p.data.gsa.dop_v;
|
|
gh->fix_mode = gh->p.data.gsa.fix_mode;
|
|
memcpy(gh->satellites_ids, gh->p.data.gsa.satellites_ids, sizeof(gh->satellites_ids));
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
|
|
#if LWGPS_CFG_STATEMENT_GPGSV
|
|
} else if (gh->p.stat == STAT_GSV) {
|
|
gh->sats_in_view = gh->p.data.gsv.sats_in_view;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
|
|
#if LWGPS_CFG_STATEMENT_GPRMC
|
|
} else if (gh->p.stat == STAT_RMC) {
|
|
gh->course = gh->p.data.rmc.course;
|
|
gh->is_valid = gh->p.data.rmc.is_valid;
|
|
gh->speed = gh->p.data.rmc.speed;
|
|
gh->variation = gh->p.data.rmc.variation;
|
|
gh->date = gh->p.data.rmc.date;
|
|
gh->month = gh->p.data.rmc.month;
|
|
gh->year = gh->p.data.rmc.year;
|
|
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
|
|
#if LWGPS_CFG_STATEMENT_PUBX_TIME
|
|
} else if (gh->p.stat == STAT_UBX_TIME) {
|
|
gh->hours = gh->p.data.time.hours;
|
|
gh->minutes = gh->p.data.time.minutes;
|
|
gh->seconds = gh->p.data.time.seconds;
|
|
gh->date = gh->p.data.time.date;
|
|
gh->month = gh->p.data.time.month;
|
|
gh->year = gh->p.data.time.year;
|
|
gh->utc_tow = gh->p.data.time.utc_tow;
|
|
gh->utc_wk = gh->p.data.time.utc_wk;
|
|
gh->leap_sec = gh->p.data.time.leap_sec;
|
|
gh->clk_bias = gh->p.data.time.clk_bias;
|
|
gh->clk_drift = gh->p.data.time.clk_drift;
|
|
gh->tp_gran = gh->p.data.time.tp_gran;
|
|
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* \brief Init GPS handle
|
|
* \param[in] gh: GPS handle structure
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
uint8_t
|
|
lwgps_init(lwgps_t* gh) {
|
|
memset(gh, 0x00, sizeof(*gh)); /* Reset structure */
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* \brief Process NMEA data from GPS receiver
|
|
* \param[in] gh: GPS handle structure
|
|
* \param[in] data: Received data
|
|
* \param[in] len: Number of bytes to process
|
|
* \param[in] evt_fn: Event function to notify application layer.
|
|
* This parameter is available only if \ref LWGPS_CFG_STATUS is enabled
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
uint8_t
|
|
#if LWGPS_CFG_STATUS || __DOXYGEN__
|
|
lwgps_process(lwgps_t* gh, const void* data, size_t len, lwgps_process_fn evt_fn) {
|
|
#else /* LWGPS_CFG_STATUS */
|
|
lwgps_process(lwgps_t* gh, const void* data, size_t len) {
|
|
#endif /* !LWGPS_CFG_STATUS */
|
|
const uint8_t* d =(uint8_t*) data;
|
|
|
|
for (; len > 0; ++d, --len) { /* Process all bytes */
|
|
if (*d == '$') { /* Check for beginning of NMEA line */
|
|
memset(&gh->p, 0x00, sizeof(gh->p));/* Reset private memory */
|
|
TERM_ADD(gh, *d); /* Add character to term */
|
|
} else if (*d == ',') { /* Term separator character */
|
|
prv_parse_term(gh); /* Parse term we have currently in memory */
|
|
CRC_ADD(gh, *d); /* Add character to CRC computation */
|
|
TERM_NEXT(gh); /* Start with next term */
|
|
} else if (*d == '*') { /* Start indicates end of data for CRC computation */
|
|
prv_parse_term(gh); /* Parse term we have currently in memory */
|
|
gh->p.star = 1; /* STAR detected */
|
|
TERM_NEXT(gh); /* Start with next term */
|
|
} else if (*d == '\r') {
|
|
if (prv_check_crc(gh)) { /* Check for CRC result */
|
|
/* CRC is OK, in theory we can copy data from statements to user data */
|
|
prv_copy_from_tmp_memory(gh); /* Copy memory from temporary to user memory */
|
|
#if LWGPS_CFG_STATUS
|
|
if (evt_fn != NULL) {
|
|
evt_fn(gh->p.stat);
|
|
}
|
|
} else if (evt_fn != NULL) {
|
|
evt_fn(STAT_CHECKSUM_FAIL);
|
|
#endif /* LWGPS_CFG_STATUS */
|
|
}
|
|
} else {
|
|
if (!gh->p.star) { /* Add to CRC only if star not yet detected */
|
|
CRC_ADD(gh, *d); /* Add to CRC */
|
|
}
|
|
TERM_ADD(gh, *d); /* Add character to term */
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* \brief Calculate distance and bearing between `2` latitude and longitude coordinates
|
|
* \param[in] las: Latitude start coordinate, in units of degrees
|
|
* \param[in] los: Longitude start coordinate, in units of degrees
|
|
* \param[in] lae: Latitude end coordinate, in units of degrees
|
|
* \param[in] loe: Longitude end coordinate, in units of degrees
|
|
* \param[out] d: Pointer to output distance in units of meters
|
|
* \param[out] b: Pointer to output bearing between start and end coordinate in relation to north in units of degrees
|
|
* \return `1` on success, `0` otherwise
|
|
*/
|
|
uint8_t
|
|
lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae, lwgps_float_t loe, lwgps_float_t* d, lwgps_float_t* b) {
|
|
lwgps_float_t df, dfi, a;
|
|
|
|
if (d == NULL && b == NULL) {
|
|
return 0;
|
|
}
|
|
|
|
/* Convert degrees to radians */
|
|
df = D2R(lae - las);
|
|
dfi = D2R(loe - los);
|
|
las = D2R(las);
|
|
los = D2R(los);
|
|
lae = D2R(lae);
|
|
loe = D2R(loe);
|
|
|
|
/*
|
|
* Calculate distance
|
|
*
|
|
* Calculated distance is absolute value in meters between 2 points on earth.
|
|
*/
|
|
if (d != NULL) {
|
|
/*
|
|
* a = sin(df / 2)^2 + cos(las) * cos(lae) * sin(dfi / 2)^2
|
|
* *d = RADIUS * 2 * atan(sqrt(a) / sqrt(1 - a)) * 1000 (for meters)
|
|
*/
|
|
#if LWGPS_CFG_DOUBLE
|
|
a = FLT(sin(df * 0.5) * sin(df * 0.5) + sin(dfi * 0.5) * sin(dfi * 0.5) * cos(las) * cos(lae));
|
|
*d = FLT(EARTH_RADIUS * 2.0 * atan2(sqrt(a), sqrt(1.0 - a)) * 1000.0);
|
|
#else /* LWGPS_CFG_DOUBLE */
|
|
a = FLT(sinf(df * 0.5f) * sinf(df * 0.5f) + sinf(dfi * 0.5f) * sinf(dfi * 0.5f) * cosf(las) * cosf(lae));
|
|
*d = FLT(EARTH_RADIUS * 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)) * 1000.0f);
|
|
#endif /* !LWGPS_CFG_DOUBLE */
|
|
}
|
|
|
|
/*
|
|
* Calculate bearing
|
|
*
|
|
* Bearing is calculated from point 1 to point 2.
|
|
* Result will tell us in which direction (according to north) we should move,
|
|
* to reach point 2.
|
|
*
|
|
* Example:
|
|
* Bearing is 0 => move to north
|
|
* Bearing is 90 => move to east
|
|
* Bearing is 180 => move to south
|
|
* Bearing is 270 => move to west
|
|
*/
|
|
if (b != NULL) {
|
|
#if LWGPS_CFG_DOUBLE
|
|
df = FLT(sin(loe - los) * cos(lae));
|
|
dfi = FLT(cos(las) * sin(lae) - sin(las) * cos(lae) * cos(loe - los));
|
|
|
|
*b = R2D(atan2(df, dfi)); /* Calculate bearing and convert to degrees */
|
|
#else /* LWGPS_CFG_DOUBLE */
|
|
df = FLT(sinf(loe - los) * cosf(lae));
|
|
dfi = FLT(cosf(las) * sinf(lae) - sinf(las) * cosf(lae) * cosf(loe - los));
|
|
|
|
*b = R2D(atan2f(df, dfi)); /* Calculate bearing and convert to degrees */
|
|
#endif /* !LWGPS_CFG_DOUBLE */
|
|
if (*b < 0) { /* Check for negative angle */
|
|
*b += FLT(360); /* Make bearing always positive */
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/**
|
|
* \brief Convert NMEA GPS speed (in knots = nautical mile per hour) to different speed format
|
|
* \param[in] sik: Speed in knots, received from GPS NMEA statement
|
|
* \param[in] ts: Target speed to convert to from knots
|
|
* \return Speed calculated from knots
|
|
*/
|
|
lwgps_float_t
|
|
lwgps_to_speed(lwgps_float_t sik, lwgps_speed_t ts) {
|
|
switch (ts) {
|
|
case lwgps_speed_kps:
|
|
return FLT(sik * FLT(0.000514));
|
|
case lwgps_speed_kph:
|
|
return FLT(sik * FLT(1.852));
|
|
case lwgps_speed_mps:
|
|
return FLT(sik * FLT(0.5144));
|
|
case lwgps_speed_mpm:
|
|
return FLT(sik * FLT(30.87));
|
|
|
|
case lwgps_speed_mips:
|
|
return FLT(sik * FLT(0.0003197));
|
|
case lwgps_speed_mph:
|
|
return FLT(sik * FLT(1.151));
|
|
case lwgps_speed_fps:
|
|
return FLT(sik * FLT(1.688));
|
|
case lwgps_speed_fpm:
|
|
return FLT(sik * FLT(101.3));
|
|
|
|
case lwgps_speed_mpk:
|
|
return FLT(sik * FLT(32.4));
|
|
case lwgps_speed_spk:
|
|
return FLT(sik * FLT(1944.0));
|
|
case lwgps_speed_sp100m:
|
|
return FLT(sik * FLT(194.4));
|
|
case lwgps_speed_mipm:
|
|
return FLT(sik * FLT(52.14));
|
|
case lwgps_speed_spm:
|
|
return FLT(sik * FLT(3128.0));
|
|
case lwgps_speed_sp100y:
|
|
return FLT(sik * FLT(177.7));
|
|
|
|
case lwgps_speed_smph:
|
|
return FLT(sik * FLT(1.0));
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|