blob: 515606fbefe5b88f33b9b8ce6265dd8919023988 [file] [log] [blame]
/*
* Copyright (c) 2023 Trackunit Corporation
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel.h>
#include <string.h>
#include <stdarg.h>
#include <stdarg.h>
#include "gnss_nmea0183.h"
#include "gnss_parse.h"
#define GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE (1000000000000ULL)
#define GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE (GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE / 60ULL)
#define GNSS_NMEA0183_PICO_DEGREES_IN_NANO_DEGREE (1000ULL)
#define GNSS_NMEA0183_NANO_KNOTS_IN_MMS (1943861LL)
#define GNSS_NMEA0183_MESSAGE_SIZE_MIN (6)
#define GNSS_NMEA0183_MESSAGE_CHECKSUM_SIZE (3)
#define GNSS_NMEA0183_GSV_HDR_ARG_CNT (4)
#define GNSS_NMEA0183_GSV_SV_ARG_CNT (4)
#define GNSS_NMEA0183_GSV_PRN_GPS_RANGE (32)
#define GNSS_NMEA0183_GSV_PRN_SBAS_OFFSET (87)
#define GNSS_NMEA0183_GSV_PRN_GLONASS_OFFSET (64)
#define GNSS_NMEA0183_GSV_PRN_BEIDOU_OFFSET (100)
struct gsv_header_args {
const char *message_id;
const char *number_of_messages;
const char *message_number;
const char *numver_of_svs;
};
struct gsv_sv_args {
const char *prn;
const char *elevation;
const char *azimuth;
const char *snr;
};
static int gnss_system_from_gsv_header_args(const struct gsv_header_args *args,
enum gnss_system *sv_system)
{
switch (args->message_id[2]) {
case 'A':
*sv_system = GNSS_SYSTEM_GALILEO;
break;
case 'B':
*sv_system = GNSS_SYSTEM_BEIDOU;
break;
case 'P':
*sv_system = GNSS_SYSTEM_GPS;
break;
case 'L':
*sv_system = GNSS_SYSTEM_GLONASS;
break;
case 'Q':
*sv_system = GNSS_SYSTEM_QZSS;
break;
default:
return -EINVAL;
}
return 0;
}
static void align_satellite_with_gnss_system(enum gnss_system sv_system,
struct gnss_satellite *satellite)
{
switch (sv_system) {
case GNSS_SYSTEM_GPS:
if (satellite->prn > GNSS_NMEA0183_GSV_PRN_GPS_RANGE) {
satellite->system = GNSS_SYSTEM_SBAS;
satellite->prn += GNSS_NMEA0183_GSV_PRN_SBAS_OFFSET;
break;
}
satellite->system = GNSS_SYSTEM_GPS;
break;
case GNSS_SYSTEM_GLONASS:
satellite->system = GNSS_SYSTEM_GLONASS;
satellite->prn -= GNSS_NMEA0183_GSV_PRN_GLONASS_OFFSET;
break;
case GNSS_SYSTEM_GALILEO:
satellite->system = GNSS_SYSTEM_GALILEO;
break;
case GNSS_SYSTEM_BEIDOU:
satellite->system = GNSS_SYSTEM_BEIDOU;
satellite->prn -= GNSS_NMEA0183_GSV_PRN_BEIDOU_OFFSET;
break;
case GNSS_SYSTEM_QZSS:
satellite->system = GNSS_SYSTEM_QZSS;
break;
case GNSS_SYSTEM_IRNSS:
case GNSS_SYSTEM_IMES:
case GNSS_SYSTEM_SBAS:
break;
}
}
uint8_t gnss_nmea0183_checksum(const char *str)
{
uint8_t checksum = 0;
size_t end;
__ASSERT(str != NULL, "str argument must be provided");
end = strlen(str);
for (size_t i = 0; i < end; i++) {
checksum = checksum ^ str[i];
}
return checksum;
}
int gnss_nmea0183_snprintk(char *str, size_t size, const char *fmt, ...)
{
va_list ap;
uint8_t checksum;
int pos;
int len;
__ASSERT(str != NULL, "str argument must be provided");
__ASSERT(fmt != NULL, "fmt argument must be provided");
if (size < GNSS_NMEA0183_MESSAGE_SIZE_MIN) {
return -ENOMEM;
}
str[0] = '$';
va_start(ap, fmt);
pos = vsnprintk(&str[1], size - 1, fmt, ap) + 1;
va_end(ap);
if (pos < 0) {
return -EINVAL;
}
len = pos + GNSS_NMEA0183_MESSAGE_CHECKSUM_SIZE;
if ((size - 1) < len) {
return -ENOMEM;
}
checksum = gnss_nmea0183_checksum(&str[1]);
pos = snprintk(&str[pos], size - pos, "*%02X", checksum);
if (pos != 3) {
return -EINVAL;
}
str[len] = '\0';
return len;
}
int gnss_nmea0183_ddmm_mmmm_to_ndeg(const char *ddmm_mmmm, int64_t *ndeg)
{
uint64_t pico_degrees = 0;
int8_t decimal = -1;
int8_t pos = 0;
uint64_t increment;
__ASSERT(ddmm_mmmm != NULL, "ddmm_mmmm argument must be provided");
__ASSERT(ndeg != NULL, "ndeg argument must be provided");
/* Find decimal */
while (ddmm_mmmm[pos] != '\0') {
/* Verify if char is decimal */
if (ddmm_mmmm[pos] == '.') {
decimal = pos;
break;
}
/* Advance position */
pos++;
}
/* Verify decimal was found and placed correctly */
if (decimal < 1) {
return -EINVAL;
}
/* Validate potential degree fraction is within bounds */
if (decimal > 1 && ddmm_mmmm[decimal - 2] > '5') {
return -EINVAL;
}
/* Convert minute fraction to pico degrees and add it to pico_degrees */
pos = decimal + 1;
increment = (GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE / 10);
while (ddmm_mmmm[pos] != '\0') {
/* Verify char is decimal */
if (ddmm_mmmm[pos] < '0' || ddmm_mmmm[pos] > '9') {
return -EINVAL;
}
/* Add increment to pico_degrees */
pico_degrees += (ddmm_mmmm[pos] - '0') * increment;
/* Update unit */
increment /= 10;
/* Increment position */
pos++;
}
/* Convert minutes and degrees to pico_degrees */
pos = decimal - 1;
increment = GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE;
while (pos >= 0) {
/* Check if digit switched from minutes to degrees */
if ((decimal - pos) == 3) {
/* Reset increment to degrees */
increment = GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE;
}
/* Verify char is decimal */
if (ddmm_mmmm[pos] < '0' || ddmm_mmmm[pos] > '9') {
return -EINVAL;
}
/* Add increment to pico_degrees */
pico_degrees += (ddmm_mmmm[pos] - '0') * increment;
/* Update unit */
increment *= 10;
/* Decrement position */
pos--;
}
/* Convert to nano degrees */
*ndeg = (int64_t)(pico_degrees / GNSS_NMEA0183_PICO_DEGREES_IN_NANO_DEGREE);
return 0;
}
bool gnss_nmea0183_validate_message(char **argv, uint16_t argc)
{
int32_t tmp = 0;
uint8_t checksum = 0;
size_t len;
__ASSERT(argv != NULL, "argv argument must be provided");
/* Message must contain message id and checksum */
if (argc < 2) {
return false;
}
/* First argument should start with '$' which is not covered by checksum */
if ((argc < 1) || (argv[0][0] != '$')) {
return false;
}
len = strlen(argv[0]);
for (uint16_t u = 1; u < len; u++) {
checksum ^= argv[0][u];
}
checksum ^= ',';
/* Cover all except last argument which contains the checksum*/
for (uint16_t i = 1; i < (argc - 1); i++) {
len = strlen(argv[i]);
for (uint16_t u = 0; u < len; u++) {
checksum ^= argv[i][u];
}
checksum ^= ',';
}
if ((gnss_parse_atoi(argv[argc - 1], 16, &tmp) < 0) ||
(tmp > UINT8_MAX) ||
(tmp < 0)) {
return false;
}
return checksum == (uint8_t)tmp;
}
int gnss_nmea0183_knots_to_mms(const char *str, int64_t *mms)
{
int ret;
__ASSERT(str != NULL, "str argument must be provided");
__ASSERT(mms != NULL, "mms argument must be provided");
ret = gnss_parse_dec_to_nano(str, mms);
if (ret < 0) {
return ret;
}
*mms = (*mms) / GNSS_NMEA0183_NANO_KNOTS_IN_MMS;
return 0;
}
int gnss_nmea0183_parse_hhmmss(const char *hhmmss, struct gnss_time *utc)
{
int64_t i64;
int32_t i32;
char part[3] = {0};
__ASSERT(hhmmss != NULL, "hhmmss argument must be provided");
__ASSERT(utc != NULL, "utc argument must be provided");
if (strlen(hhmmss) < 6) {
return -EINVAL;
}
memcpy(part, hhmmss, 2);
if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
(i32 < 0) ||
(i32 > 23)) {
return -EINVAL;
}
utc->hour = (uint8_t)i32;
memcpy(part, &hhmmss[2], 2);
if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
(i32 < 0) ||
(i32 > 59)) {
return -EINVAL;
}
utc->minute = (uint8_t)i32;
if ((gnss_parse_dec_to_milli(&hhmmss[4], &i64) < 0) ||
(i64 < 0) ||
(i64 > 59999)) {
return -EINVAL;
}
utc->millisecond = (uint16_t)i64;
return 0;
}
int gnss_nmea0183_parse_ddmmyy(const char *ddmmyy, struct gnss_time *utc)
{
int32_t i32;
char part[3] = {0};
__ASSERT(ddmmyy != NULL, "ddmmyy argument must be provided");
__ASSERT(utc != NULL, "utc argument must be provided");
if (strlen(ddmmyy) != 6) {
return -EINVAL;
}
memcpy(part, ddmmyy, 2);
if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
(i32 < 1) ||
(i32 > 31)) {
return -EINVAL;
}
utc->month_day = (uint8_t)i32;
memcpy(part, &ddmmyy[2], 2);
if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
(i32 < 1) ||
(i32 > 12)) {
return -EINVAL;
}
utc->month = (uint8_t)i32;
memcpy(part, &ddmmyy[4], 2);
if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
(i32 < 0) ||
(i32 > 99)) {
return -EINVAL;
}
utc->century_year = (uint8_t)i32;
return 0;
}
int gnss_nmea0183_parse_rmc(const char **argv, uint16_t argc, struct gnss_data *data)
{
int64_t tmp;
__ASSERT(argv != NULL, "argv argument must be provided");
__ASSERT(data != NULL, "data argument must be provided");
if (argc < 10) {
return -EINVAL;
}
/* Validate GNSS has fix */
if (argv[2][0] == 'V') {
return 0;
}
if (argv[2][0] != 'A') {
return -EINVAL;
}
/* Parse UTC time */
if ((gnss_nmea0183_parse_hhmmss(argv[1], &data->utc) < 0)) {
return -EINVAL;
}
/* Validate cardinal directions */
if (((argv[4][0] != 'N') && (argv[4][0] != 'S')) ||
((argv[6][0] != 'E') && (argv[6][0] != 'W'))) {
return -EINVAL;
}
/* Parse coordinates */
if ((gnss_nmea0183_ddmm_mmmm_to_ndeg(argv[3], &data->nav_data.latitude) < 0) ||
(gnss_nmea0183_ddmm_mmmm_to_ndeg(argv[5], &data->nav_data.longitude) < 0)) {
return -EINVAL;
}
/* Align sign of coordinates with cardinal directions */
data->nav_data.latitude = argv[4][0] == 'N'
? data->nav_data.latitude
: -data->nav_data.latitude;
data->nav_data.longitude = argv[6][0] == 'E'
? data->nav_data.longitude
: -data->nav_data.longitude;
/* Parse speed */
if ((gnss_nmea0183_knots_to_mms(argv[7], &tmp) < 0) ||
(tmp > UINT32_MAX)) {
return -EINVAL;
}
data->nav_data.speed = (uint32_t)tmp;
/* Parse bearing */
if ((gnss_parse_dec_to_milli(argv[8], &tmp) < 0) ||
(tmp > 359999) ||
(tmp < 0)) {
return -EINVAL;
}
data->nav_data.bearing = (uint32_t)tmp;
/* Parse UTC date */
if ((gnss_nmea0183_parse_ddmmyy(argv[9], &data->utc) < 0)) {
return -EINVAL;
}
return 0;
}
static int parse_gga_fix_quality(const char *str, enum gnss_fix_quality *fix_quality)
{
__ASSERT(str != NULL, "str argument must be provided");
__ASSERT(fix_quality != NULL, "fix_quality argument must be provided");
if ((str[1] != ((char)'\0')) || (str[0] < ((char)'0')) || (((char)'6') < str[0])) {
return -EINVAL;
}
(*fix_quality) = (enum gnss_fix_quality)(str[0] - ((char)'0'));
return 0;
}
static enum gnss_fix_status fix_status_from_fix_quality(enum gnss_fix_quality fix_quality)
{
enum gnss_fix_status fix_status = GNSS_FIX_STATUS_NO_FIX;
switch (fix_quality) {
case GNSS_FIX_QUALITY_GNSS_SPS:
case GNSS_FIX_QUALITY_GNSS_PPS:
fix_status = GNSS_FIX_STATUS_GNSS_FIX;
break;
case GNSS_FIX_QUALITY_DGNSS:
case GNSS_FIX_QUALITY_RTK:
case GNSS_FIX_QUALITY_FLOAT_RTK:
fix_status = GNSS_FIX_STATUS_DGNSS_FIX;
break;
case GNSS_FIX_QUALITY_ESTIMATED:
fix_status = GNSS_FIX_STATUS_ESTIMATED_FIX;
break;
default:
break;
}
return fix_status;
}
int gnss_nmea0183_parse_gga(const char **argv, uint16_t argc, struct gnss_data *data)
{
int32_t tmp32;
int64_t tmp64;
__ASSERT(argv != NULL, "argv argument must be provided");
__ASSERT(data != NULL, "data argument must be provided");
if (argc < 12) {
return -EINVAL;
}
/* Parse fix quality and status */
if (parse_gga_fix_quality(argv[6], &data->info.fix_quality) < 0) {
return -EINVAL;
}
data->info.fix_status = fix_status_from_fix_quality(data->info.fix_quality);
/* Validate GNSS has fix */
if (data->info.fix_status == GNSS_FIX_STATUS_NO_FIX) {
return 0;
}
/* Parse number of satellites */
if ((gnss_parse_atoi(argv[7], 10, &tmp32) < 0) ||
(tmp32 > UINT16_MAX) ||
(tmp32 < 0)) {
return -EINVAL;
}
data->info.satellites_cnt = (uint16_t)tmp32;
/* Parse HDOP */
if ((gnss_parse_dec_to_milli(argv[8], &tmp64) < 0) ||
(tmp64 > UINT32_MAX) ||
(tmp64 < 0)) {
return -EINVAL;
}
data->info.hdop = (uint16_t)tmp64;
/* Parse altitude */
if ((gnss_parse_dec_to_milli(argv[9], &tmp64) < 0) ||
(tmp64 > INT32_MAX) ||
(tmp64 < INT32_MIN)) {
return -EINVAL;
}
data->nav_data.altitude = (int32_t)tmp64;
return 0;
}
static int parse_gsv_svs(struct gnss_satellite *satellites, const struct gsv_sv_args *svs,
uint16_t svs_size)
{
int32_t i32;
for (uint16_t i = 0; i < svs_size; i++) {
/* Parse PRN */
if ((gnss_parse_atoi(svs[i].prn, 10, &i32) < 0) ||
(i32 < 0) || (i32 > UINT16_MAX)) {
return -EINVAL;
}
satellites[i].prn = (uint16_t)i32;
/* Parse elevation */
if ((gnss_parse_atoi(svs[i].elevation, 10, &i32) < 0) ||
(i32 < 0) || (i32 > 90)) {
return -EINVAL;
}
satellites[i].elevation = (uint8_t)i32;
/* Parse azimuth */
if ((gnss_parse_atoi(svs[i].azimuth, 10, &i32) < 0) ||
(i32 < 0) || (i32 > 359)) {
return -EINVAL;
}
satellites[i].azimuth = (uint16_t)i32;
/* Parse SNR */
if (strlen(svs[i].snr) == 0) {
satellites[i].snr = 0;
satellites[i].is_tracked = false;
continue;
}
if ((gnss_parse_atoi(svs[i].snr, 10, &i32) < 0) ||
(i32 < 0) || (i32 > 99)) {
return -EINVAL;
}
satellites[i].snr = (uint16_t)i32;
satellites[i].is_tracked = true;
}
return 0;
}
int gnss_nmea0183_parse_gsv_header(const char **argv, uint16_t argc,
struct gnss_nmea0183_gsv_header *header)
{
const struct gsv_header_args *args = (const struct gsv_header_args *)argv;
int i32;
__ASSERT(argv != NULL, "argv argument must be provided");
__ASSERT(header != NULL, "header argument must be provided");
if (argc < 4) {
return -EINVAL;
}
/* Parse GNSS sv_system */
if (gnss_system_from_gsv_header_args(args, &header->system) < 0) {
return -EINVAL;
}
/* Parse number of messages */
if ((gnss_parse_atoi(args->number_of_messages, 10, &i32) < 0) ||
(i32 < 0) || (i32 > UINT16_MAX)) {
return -EINVAL;
}
header->number_of_messages = (uint16_t)i32;
/* Parse message number */
if ((gnss_parse_atoi(args->message_number, 10, &i32) < 0) ||
(i32 < 0) || (i32 > UINT16_MAX)) {
return -EINVAL;
}
header->message_number = (uint16_t)i32;
/* Parse message number */
if ((gnss_parse_atoi(args->numver_of_svs, 10, &i32) < 0) ||
(i32 < 0) || (i32 > UINT16_MAX)) {
return -EINVAL;
}
header->number_of_svs = (uint16_t)i32;
return 0;
}
int gnss_nmea0183_parse_gsv_svs(const char **argv, uint16_t argc,
struct gnss_satellite *satellites, uint16_t size)
{
const struct gsv_header_args *header_args = (const struct gsv_header_args *)argv;
const struct gsv_sv_args *sv_args = (const struct gsv_sv_args *)(argv + 4);
uint16_t sv_args_size;
enum gnss_system sv_system;
__ASSERT(argv != NULL, "argv argument must be provided");
__ASSERT(satellites != NULL, "satellites argument must be provided");
if (argc < 9) {
return 0;
}
sv_args_size = (argc - GNSS_NMEA0183_GSV_HDR_ARG_CNT) / GNSS_NMEA0183_GSV_SV_ARG_CNT;
if (size < sv_args_size) {
return -ENOMEM;
}
if (parse_gsv_svs(satellites, sv_args, sv_args_size) < 0) {
return -EINVAL;
}
if (gnss_system_from_gsv_header_args(header_args, &sv_system) < 0) {
return -EINVAL;
}
for (uint16_t i = 0; i < sv_args_size; i++) {
align_satellite_with_gnss_system(sv_system, &satellites[i]);
}
return (int)sv_args_size;
}