From 4cfea4a520278fd712dd8cb3444f1c4e70cd1153 Mon Sep 17 00:00:00 2001 From: Bjarki Arge Andreasen Date: Fri, 25 Aug 2023 12:45:00 +0200 Subject: [PATCH] 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 --- drivers/gnss/CMakeLists.txt | 1 + drivers/gnss/Kconfig | 5 + drivers/gnss/gnss_parse.c | 149 +++++++++++++++++++ drivers/gnss/gnss_parse.h | 65 ++++++++ tests/drivers/gnss/gnss_parse/CMakeLists.txt | 14 ++ tests/drivers/gnss/gnss_parse/prj.conf | 7 + tests/drivers/gnss/gnss_parse/src/main.c | 99 ++++++++++++ tests/drivers/gnss/gnss_parse/testcase.yaml | 9 ++ 8 files changed, 349 insertions(+) create mode 100644 drivers/gnss/gnss_parse.c create mode 100644 drivers/gnss/gnss_parse.h create mode 100644 tests/drivers/gnss/gnss_parse/CMakeLists.txt create mode 100644 tests/drivers/gnss/gnss_parse/prj.conf create mode 100644 tests/drivers/gnss/gnss_parse/src/main.c create mode 100644 tests/drivers/gnss/gnss_parse/testcase.yaml diff --git a/drivers/gnss/CMakeLists.txt b/drivers/gnss/CMakeLists.txt index eeaf40721b..f19be7c005 100644 --- a/drivers/gnss/CMakeLists.txt +++ b/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) diff --git a/drivers/gnss/Kconfig b/drivers/gnss/Kconfig index 2bde6428be..f07145a934 100644 --- a/drivers/gnss/Kconfig +++ b/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" diff --git a/drivers/gnss/gnss_parse.c b/drivers/gnss/gnss_parse.c new file mode 100644 index 0000000000..5cde6a630a --- /dev/null +++ b/drivers/gnss/gnss_parse.c @@ -0,0 +1,149 @@ +/* + * Copyright (c) 2023 Trackunit Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +#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; +} diff --git a/drivers/gnss/gnss_parse.h b/drivers/gnss/gnss_parse.h new file mode 100644 index 0000000000..443363cfa6 --- /dev/null +++ b/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 + +/** + * @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_ */ diff --git a/tests/drivers/gnss/gnss_parse/CMakeLists.txt b/tests/drivers/gnss/gnss_parse/CMakeLists.txt new file mode 100644 index 0000000000..ec07dff7dc --- /dev/null +++ b/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) diff --git a/tests/drivers/gnss/gnss_parse/prj.conf b/tests/drivers/gnss/gnss_parse/prj.conf new file mode 100644 index 0000000000..78ac36dd3d --- /dev/null +++ b/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 diff --git a/tests/drivers/gnss/gnss_parse/src/main.c b/tests/drivers/gnss/gnss_parse/src/main.c new file mode 100644 index 0000000000..9bf1c0e4eb --- /dev/null +++ b/tests/drivers/gnss/gnss_parse/src/main.c @@ -0,0 +1,99 @@ +/* + * Copyright 2023 Trackunit Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#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); diff --git a/tests/drivers/gnss/gnss_parse/testcase.yaml b/tests/drivers/gnss/gnss_parse/testcase.yaml new file mode 100644 index 0000000000..82939ba446 --- /dev/null +++ b/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