drivers: gnss: Add GNSS parsing utilities

This commit adds parsing utilites for common string
representations of values contained in GNSS messages.

These utilites both parse and validate the integrity of
the data.

Unit tests are also added to validate the parsing
utilities.

Signed-off-by: Bjarki Arge Andreasen <bjarkix123@gmail.com>

drivers/gnss/CMakeLists.txt

@@ -4,3 +4,4 @@
 zephyr_library()
 zephyr_library_sources(gnss_publish.c)
 zephyr_library_sources_ifdef(CONFIG_GNSS_DUMP gnss_dump.c)
+zephyr_library_sources_ifdef(CONFIG_GNSS_PARSE gnss_parse.c)

drivers/gnss/Kconfig

@@ -36,6 +36,11 @@ config GNSS_DUMP_TO_LOG_BUF_SIZE
 
 endif
 
+config GNSS_PARSE
+	bool "GNSS parsing utilities"
+	help
+	  Enable GNSS parsing utilities.
+
 module = GNSS
 module-str = gnss
 source "subsys/logging/Kconfig.template.log_config"

drivers/gnss/gnss_parse.c

@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2023 Trackunit Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/kernel.h>
+
+#include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+
+#include "gnss_parse.h"
+
+#define GNSS_PARSE_NANO_KNOTS_IN_MMS               (1943840LL)
+#define GNSS_PARSE_NANO                            (1000000000LL)
+#define GNSS_PARSE_MICRO                           (1000000LL)
+#define GNSS_PARSE_MILLI                           (1000LL)
+
+int gnss_parse_dec_to_nano(const char *str, int64_t *nano)
+{
+	int64_t sum = 0;
+	int8_t decimal = -1;
+	int8_t pos = 0;
+	int8_t start = 0;
+	int64_t increment;
+
+	__ASSERT(str != NULL, "str argument must be provided");
+	__ASSERT(str != NULL, "nano argument must be provided");
+
+	/* Find decimal */
+	while (str[pos] != '\0') {
+		/* Verify if char is decimal */
+		if (str[pos] == '.') {
+			decimal = pos;
+			break;
+		}
+
+		/* Advance position */
+		pos++;
+	}
+
+	/* Determine starting position based on decimal location */
+	pos = decimal < 0 ? pos - 1 : decimal - 1;
+
+	/* Skip sign if it exists */
+	start = str[0] == '-' ? 1 : 0;
+
+	/* Add whole value to sum */
+	increment = GNSS_PARSE_NANO;
+	while (start <= pos) {
+		/* Verify char is decimal */
+		if (str[pos] < '0' || str[pos] > '9') {
+			return -EINVAL;
+		}
+
+		/* Add value to sum */
+		sum += (str[pos] - '0') * increment;
+
+		/* Update increment */
+		increment *= 10;
+
+		/* Degrement position */
+		pos--;
+	}
+
+	/* Check if decimal was found */
+	if (decimal < 0) {
+		/* Set sign of sum */
+		sum = start == 1 ? -sum : sum;
+
+		*nano = sum;
+		return 0;
+	}
+
+	/* Convert decimal part to nano fractions and add it to sum */
+	pos = decimal + 1;
+	increment = GNSS_PARSE_NANO / 10LL;
+	while (str[pos] != '\0') {
+		/* Verify char is decimal */
+		if (str[pos] < '0' || str[pos] > '9') {
+			return -EINVAL;
+		}
+
+		/* Add value to micro_degrees */
+		sum += (str[pos] - '0') * increment;
+
+		/* Update unit */
+		increment /= 10;
+
+		/* Increment position */
+		pos++;
+	}
+
+	/* Set sign of sum */
+	sum = start == 1 ? -sum : sum;
+
+	*nano = sum;
+	return 0;
+}
+
+int gnss_parse_dec_to_micro(const char *str, uint64_t *micro)
+{
+	int ret;
+
+	__ASSERT(str != NULL, "str argument must be provided");
+	__ASSERT(micro != NULL, "micro argument must be provided");
+
+	ret = gnss_parse_dec_to_nano(str, micro);
+	if (ret < 0) {
+		return ret;
+	}
+
+	*micro = (*micro) / GNSS_PARSE_MILLI;
+	return 0;
+}
+
+
+int gnss_parse_dec_to_milli(const char *str, int64_t *milli)
+{
+	int ret;
+
+	__ASSERT(str != NULL, "str argument must be provided");
+	__ASSERT(milli != NULL, "milli argument must be provided");
+
+	ret = gnss_parse_dec_to_nano(str, milli);
+	if (ret < 0) {
+		return ret;
+	}
+
+	(*milli) = (*milli) / GNSS_PARSE_MICRO;
+	return 0;
+}
+
+int gnss_parse_atoi(const char *str, uint8_t base, int32_t *integer)
+{
+	char *end;
+
+	__ASSERT(str != NULL, "str argument must be provided");
+	__ASSERT(integer != NULL, "integer argument must be provided");
+
+	*integer = (int32_t)strtol(str, &end, (int)base);
+
+	if ('\0' != (*end)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}

drivers/gnss/gnss_parse.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2023 Trackunit Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_DRIVERS_GNSS_GNSS_PARSE_H_
+#define ZEPHYR_DRIVERS_GNSS_GNSS_PARSE_H_
+
+#include <zephyr/types.h>
+
+/**
+ * @brief Parse decimal string to nano parts
+ *
+ * @example "-1231.3512" -> -1231351200000
+ *
+ * @param str The decimal string to be parsed
+ * @param nano Destination for parsed decimal
+ *
+ * @retval -EINVAL if str could not be parsed
+ * @retval 0 if str successfully parsed
+ */
+int gnss_parse_dec_to_nano(const char *str, int64_t *nano);
+
+/**
+ * @brief Parse decimal string to micro parts
+ *
+ * @example "-1231.3512" -> -1231351200
+ *
+ * @param str The decimal string to be parsed
+ * @param milli Destination for parsed decimal
+ *
+ * @retval -EINVAL if str could not be parsed
+ * @retval 0 if str successfully parsed
+ */
+int gnss_parse_dec_to_micro(const char *str, uint64_t *micro);
+
+/**
+ * @brief Parse decimal string to milli parts
+ *
+ * @example "-1231.3512" -> -1231351
+ *
+ * @param str The decimal string to be parsed
+ * @param milli Destination for parsed decimal
+ *
+ * @retval -EINVAL if str could not be parsed
+ * @retval 0 if str successfully parsed
+ */
+int gnss_parse_dec_to_milli(const char *str, int64_t *milli);
+
+/**
+ * @brief Parse integer string of configurable base to integer
+ *
+ * @example "-1231" -> -1231
+ *
+ * @param str Decimal string to be parsed
+ * @param base Base of decimal string to be parsed
+ * @param integer Destination for parsed integer
+ *
+ * @retval -EINVAL if str could not be parsed
+ * @retval 0 if str successfully parsed
+ */
+int gnss_parse_atoi(const char *str, uint8_t base, int32_t *integer);
+
+#endif /* ZEPHYR_DRIVERS_GNSS_GNSS_PARSE_H_ */

tests/drivers/gnss/gnss_parse/CMakeLists.txt

@@ -0,0 +1,14 @@
+# Copyright (c) 2023 Trackunit Corporation
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+
+project(gnss_parse)
+
+target_sources(app PRIVATE
+	src/main.c
+)
+
+target_include_directories(app PRIVATE ${ZEPHYR_BASE}/drivers/gnss)

tests/drivers/gnss/gnss_parse/prj.conf

@@ -0,0 +1,7 @@
+# Copyright (c) 2023 Trackunit Corporation
+# SPDX-License-Identifier: Apache-2.0
+
+CONFIG_GNSS=y
+CONFIG_GNSS_PARSE=y
+CONFIG_ZTEST=y
+CONFIG_ZTEST_STACK_SIZE=4096

tests/drivers/gnss/gnss_parse/src/main.c

@@ -0,0 +1,99 @@
+/*
+ * Copyright 2023 Trackunit Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/ztest.h>
+
+#include "gnss_parse.h"
+
+struct test_atoi_sample {
+	const char *str;
+	uint8_t base;
+	int32_t value;
+};
+
+static const struct test_atoi_sample atoi_samples[] = {
+	{.str = "10", .base = 10, .value = 10},
+	{.str = "1", .base = 10, .value = 1},
+	{.str = "002", .base = 10, .value = 2},
+	{.str = "-10", .base = 10, .value = -10},
+	{.str = "-1", .base = 10, .value = -1},
+	{.str = "-002", .base = 10, .value = -2},
+	{.str = "30000000", .base = 10, .value = 30000000},
+	{.str = "-30000000", .base = 10, .value = -30000000},
+	{.str = "00", .base = 16, .value = 0},
+	{.str = "20", .base = 16, .value = 32},
+	{.str = "42", .base = 16, .value = 66},
+	{.str = "122", .base = 16, .value = 290},
+	{.str = "0122", .base = 16, .value = 290},
+};
+
+ZTEST(gnss_parse, test_atoi)
+{
+	int32_t value;
+
+	for (size_t i = 0; i < ARRAY_SIZE(atoi_samples); i++) {
+		zassert_ok(gnss_parse_atoi(atoi_samples[i].str, atoi_samples[i].base, &value),
+			   "Parse failed");
+
+		zassert_equal(atoi_samples[i].value, value, "Parsed value is incorrect");
+	}
+
+	zassert_equal(gnss_parse_atoi("a10", 10, &value), -EINVAL,
+		      "Parse should fail due to invalid base 10 chars");
+
+	zassert_equal(gnss_parse_atoi("h#1c", 16, &value), -EINVAL,
+		      "Parse should fail due to invalid base 16 chars");
+}
+
+struct test_dec_sample {
+	const char *str;
+	int64_t value;
+};
+
+static const struct test_dec_sample dec_to_nano_samples[] = {
+	{.str = "10", .value = 10000000000},
+	{.str = "1", .value = 1000000000},
+	{.str = "002", .value = 2000000000},
+	{.str = "-10", .value = -10000000000},
+	{.str = "-1", .value = -1000000000},
+	{.str = "-002", .value = -2000000000},
+	{.str = "30000000", .value = 30000000000000000},
+	{.str = "-30000000", .value = -30000000000000000},
+	{.str = "0.10", .value = 100000000},
+	{.str = "-0.10", .value = -100000000},
+	{.str = "1", .value = 1000000000},
+	{.str = "002.000", .value = 2000000000},
+	{.str = "-002.000", .value = -2000000000},
+	{.str = "0.989812343", .value = 989812343},
+	{.str = "-0.989812343", .value = -989812343},
+	{.str = "0.112211", .value = 112211000},
+	{.str = "-0.112211", .value = -112211000},
+	{.str = "000000000.112211000000000000", .value = 112211000},
+	{.str = "-000000000.11221100000000000", .value = -112211000},
+};
+
+ZTEST(gnss_parse, test_dec_to_nano)
+{
+	int64_t value;
+
+	for (volatile size_t i = 0; i < ARRAY_SIZE(dec_to_nano_samples); i++) {
+		zassert_ok(gnss_parse_dec_to_nano(dec_to_nano_samples[i].str, &value),
+			   "Parse failed");
+
+		zassert_equal(dec_to_nano_samples[i].value, value, "Parsed value is incorrect");
+	}
+
+	zassert_equal(gnss_parse_dec_to_nano("-0s02..000", &value), -EINVAL,
+		      "Parse should fail due to double dot");
+
+	zassert_equal(gnss_parse_dec_to_nano("--002.000", &value), -EINVAL,
+		      "Parse should fail due to double -");
+
+	zassert_equal(gnss_parse_dec_to_nano("-00s2.000", &value), -EINVAL,
+		      "Parse should fail due to invalid char");
+}
+
+ZTEST_SUITE(gnss_parse, NULL, NULL, NULL, NULL, NULL);

tests/drivers/gnss/gnss_parse/testcase.yaml

@@ -0,0 +1,9 @@
+# Copyright (c) 2023 Trackunit Corporation
+# SPDX-License-Identifier: Apache-2.0
+
+tests:
+  drivers.gnss.gnss_parse:
+    tags:
+      - drivers
+      - gnss
+      - parse