drivers/gnss/gnss_nmea0183.c - third_party/github/zephyrproject-rtos/zephyr - Git at Google

 /*
  * 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[11], &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;
 }
	/*
	* 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[11], &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;
	}