diff --git a/drivers/gnss/CMakeLists.txt b/drivers/gnss/CMakeLists.txt index f19be7c0055..d30723c1915 100644 --- a/drivers/gnss/CMakeLists.txt +++ b/drivers/gnss/CMakeLists.txt @@ -5,3 +5,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) +zephyr_library_sources_ifdef(CONFIG_GNSS_NMEA0183 gnss_nmea0183.c) diff --git a/drivers/gnss/Kconfig b/drivers/gnss/Kconfig index f07145a934a..fcb45122aa4 100644 --- a/drivers/gnss/Kconfig +++ b/drivers/gnss/Kconfig @@ -41,6 +41,12 @@ config GNSS_PARSE help Enable GNSS parsing utilities. +config GNSS_NMEA0183 + bool "NMEA0183 parsing utilities" + select GNSS_PARSE + help + Enable NMEA0183 parsing utilities. + module = GNSS module-str = gnss source "subsys/logging/Kconfig.template.log_config" diff --git a/drivers/gnss/gnss_nmea0183.c b/drivers/gnss/gnss_nmea0183.c new file mode 100644 index 00000000000..f69dd347a9f --- /dev/null +++ b/drivers/gnss/gnss_nmea0183.c @@ -0,0 +1,678 @@ +/* + * Copyright (c) 2023 Trackunit Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +#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 > UINT16_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; +} diff --git a/drivers/gnss/gnss_nmea0183.h b/drivers/gnss/gnss_nmea0183.h new file mode 100644 index 00000000000..6a3fa177f44 --- /dev/null +++ b/drivers/gnss/gnss_nmea0183.h @@ -0,0 +1,178 @@ +/* + * Copyright (c) 2023 Trackunit Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_GNSS_GNSS_NMEA0183_H_ +#define ZEPHYR_DRIVERS_GNSS_GNSS_NMEA0183_H_ + +#include + +/** + * @brief Compute NMEA0183 checksum + * + * @example "PAIR002" -> 0x38 + * + * @param str String from which checksum is computed + * + * @retval checksum + */ +uint8_t gnss_nmea0183_checksum(const char *str); + +/** + * @brief Encapsulate str in NMEA0183 message format + * + * @example "PAIR%03u", 2 -> "$PAIR002*38" + * + * @param str Destination for encapsulated string + * @param size Size of destination for encapsulated string + * @param fmt Format of string to encapsulate + * @param ... Arguments + * + * @retval checksum + */ +int gnss_nmea0183_snprintk(char *str, size_t size, const char *fmt, ...); + +/** + * @brief Computes and validates checksum + * + * @param argv Array of arguments split by ',' including message id and checksum + * @param argc Number of arguments in argv + * + * @retval true if message is intact + * @retval false if message is corrupted + */ +bool gnss_nmea0183_validate_message(char **argv, uint16_t argc); + +/** + * @brief Parse a ddmm.mmmm formatted angle to nano degrees + * + * @example "5610.9928" -> 56183214000 + * + * @param ddmm_mmmm String representation of angle in ddmm.mmmm format + * @param ndeg Result in nano degrees + * + * @retval -EINVAL if ddmm_mmmm argument is invalid + * @retval 0 if parsed successfully + */ +int gnss_nmea0183_ddmm_mmmm_to_ndeg(const char *ddmm_mmmm, int64_t *ndeg); + +/** + * @brief Parse knots to millimeters pr second + * + * @example "15.231" -> 7835 + * + * @param str String representation of speed in knots + * @param mms Destination for speed in millimeters pr second + * + * @retval -EINVAL if str could not be parsed or if speed is negative + * @retval 0 if parsed successfully + */ +int gnss_nmea0183_knots_to_mms(const char *str, int64_t *mms); + +/** + * @brief Parse hhmmss.sss to struct gnss_time + * + * @example "133243.012" -> { .hour = 13, .minute = 32, .ms = 43012 } + * @example "133243" -> { .hour = 13, .minute = 32, .ms = 43000 } + * + * @param str String representation of hours, minutes, seconds and subseconds + * @param utc Destination for parsed time + * + * @retval -EINVAL if str could not be parsed + * @retval 0 if parsed successfully + */ +int gnss_nmea0183_parse_hhmmss(const char *hhmmss, struct gnss_time *utc); + +/** + * @brief Parse ddmmyy to unsigned integers + * + * @example "041122" -> { .mday = 4, .month = 11, .year = 22 } + * + * @param str String representation of speed in knots + * @param utc Destination for parsed time + * + * @retval -EINVAL if str could not be parsed + * @retval 0 if parsed successfully + */ +int gnss_nmea0183_parse_ddmmyy(const char *ddmmyy, struct gnss_time *utc); + +/** + * @brief Parses NMEA0183 RMC message + * + * @details Parses the time, date, latitude, longitude, speed, and bearing + * from the NMEA0183 RMC message provided as an array of strings split by ',' + * + * @param argv Array of arguments split by ',' including message id and checksum + * @param argc Number of arguments in argv' + * @param data Destination for data parsed from NMEA0183 RMC message + * + * @retval 0 if successful + * @retval -EINVAL if input is invalid + */ +int gnss_nmea0183_parse_rmc(const char **argv, uint16_t argc, struct gnss_data *data); + +/** + * @brief Parses NMEA0183 GGA message + * + * @details Parses the GNSS fix quality and status, number of satellites used for + * fix, HDOP, and altitude (geoid separation) from the NMEA0183 GGA message provided + * as an array of strings split by ',' + * + * @param argv Array of arguments split by ',' including message id and checksum + * @param argc Number of arguments in argv' + * @param data Destination for data parsed from NMEA0183 GGA message + * + * @retval 0 if successful + * @retval -EINVAL if input is invalid + */ +int gnss_nmea0183_parse_gga(const char **argv, uint16_t argc, struct gnss_data *data); + +/** GSV header structure */ +struct gnss_nmea0183_gsv_header { + /** Indicates the system of the space-vehicles contained in the message */ + enum gnss_system system; + /** Number of GSV messages in total */ + uint16_t number_of_messages; + /** Number of this GSV message */ + uint16_t message_number; + /** Number of visible space-vehicles */ + uint16_t number_of_svs; +}; + +/** + * @brief Parses header of NMEA0183 GSV message + * + * @details The GSV messages are part of a list of messages sent in ascending + * order, split by GNSS system. + * + * @param argv Array of arguments split by ',' including message id and checksum + * @param argc Number of arguments in argv + * @param header Destination for parsed NMEA0183 GGA message header + * + * @retval 0 if successful + * @retval -EINVAL if input is invalid + */ +int gnss_nmea0183_parse_gsv_header(const char **argv, uint16_t argc, + struct gnss_nmea0183_gsv_header *header); + +/** + * @brief Parses space-vehicles in NMEA0183 GSV message + * + * @details The NMEA0183 GSV message contains up to 4 space-vehicles which follow + * the header. + * + * @param argv Array of arguments split by ',' including message id and checksum + * @param argc Number of arguments in argv + * @param satellites Destination for parsed satellites from NMEA0183 GGA message + * @param size Size of destination for parsed satellites from NMEA0183 GGA message + * + * @retval Number of parsed space-vehicles stored at destination if successful + * @retval -ENOMEM if all space-vehicles in message could not be stored at destination + * @retval -EINVAL if input is invalid + */ +int gnss_nmea0183_parse_gsv_svs(const char **argv, uint16_t argc, + struct gnss_satellite *satellites, uint16_t size); + +#endif /* ZEPHYR_DRIVERS_GNSS_GNSS_NMEA0183_H_ */ diff --git a/tests/drivers/gnss/gnss_nmea0183/CMakeLists.txt b/tests/drivers/gnss/gnss_nmea0183/CMakeLists.txt new file mode 100644 index 00000000000..73183c90acc --- /dev/null +++ b/tests/drivers/gnss/gnss_nmea0183/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_nmea0183) + +target_sources(app PRIVATE + src/main.c +) + +target_include_directories(app PRIVATE ${ZEPHYR_BASE}/drivers/gnss) diff --git a/tests/drivers/gnss/gnss_nmea0183/prj.conf b/tests/drivers/gnss/gnss_nmea0183/prj.conf new file mode 100644 index 00000000000..d2cc9d2b645 --- /dev/null +++ b/tests/drivers/gnss/gnss_nmea0183/prj.conf @@ -0,0 +1,7 @@ +# Copyright (c) 2023 Trackunit Corporation +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_GNSS=y +CONFIG_GNSS_NMEA0183=y +CONFIG_ZTEST=y +CONFIG_ZTEST_STACK_SIZE=4096 diff --git a/tests/drivers/gnss/gnss_nmea0183/src/main.c b/tests/drivers/gnss/gnss_nmea0183/src/main.c new file mode 100644 index 00000000000..680382f7dbc --- /dev/null +++ b/tests/drivers/gnss/gnss_nmea0183/src/main.c @@ -0,0 +1,739 @@ +/* + * Copyright 2023 Trackunit Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include "gnss_nmea0183.h" + +#define TEST_DDMM_MMMM_MAX_ROUNDING_ERROR_NDEG (1) + +struct test_ddmm_mmmm_sample { + const char *ddmm_mmmm; + int64_t ndeg; +}; + +/* + * The conversion from ddmm.mmmm to decimal nano degree is + * ((1/60) * mm.mmmm * 1E9) + (dd * 1E9) + */ +static const struct test_ddmm_mmmm_sample ddmm_mmmm_samples[] = { + {.ddmm_mmmm = "00.0", .ndeg = 0}, + {.ddmm_mmmm = "000.0", .ndeg = 0}, + {.ddmm_mmmm = "9000.0000", .ndeg = 90000000000}, + {.ddmm_mmmm = "4530.0000", .ndeg = 45500000000}, + {.ddmm_mmmm = "4530.3000", .ndeg = 45505000000}, + {.ddmm_mmmm = "4530.3001", .ndeg = 45505001667}, + {.ddmm_mmmm = "4530.9999", .ndeg = 45516665000}, + {.ddmm_mmmm = "18000.0000", .ndeg = 180000000000} +}; + +ZTEST(gnss_nmea0183, test_ddmm_mmmm) +{ + int64_t min_ndeg; + int64_t max_ndeg; + int64_t ndeg; + + for (size_t i = 0; i < ARRAY_SIZE(ddmm_mmmm_samples); i++) { + zassert_ok(gnss_nmea0183_ddmm_mmmm_to_ndeg(ddmm_mmmm_samples[i].ddmm_mmmm, &ndeg), + "Parse failed"); + + min_ndeg = ddmm_mmmm_samples[i].ndeg - TEST_DDMM_MMMM_MAX_ROUNDING_ERROR_NDEG; + max_ndeg = ddmm_mmmm_samples[i].ndeg + TEST_DDMM_MMMM_MAX_ROUNDING_ERROR_NDEG; + zassert_true(ndeg >= min_ndeg, "Parsed value falls below max rounding error"); + zassert_true(ndeg <= max_ndeg, "Parsed value is above max rounding error"); + } + + /* Minutes can only go from 0 to 59.9999 */ + zassert_equal(gnss_nmea0183_ddmm_mmmm_to_ndeg("99.0000", &ndeg), -EINVAL, + "Parse should fail"); + + zassert_equal(gnss_nmea0183_ddmm_mmmm_to_ndeg("60.0000", &ndeg), -EINVAL, + "Parse should fail"); + + /* Missing dot */ + zassert_equal(gnss_nmea0183_ddmm_mmmm_to_ndeg("18000", &ndeg), -EINVAL, + "Parse should fail"); + + /* Invalid chars */ + zassert_equal(gnss_nmea0183_ddmm_mmmm_to_ndeg("900#.0a000", &ndeg), -EINVAL, + "Parse should fail"); + + /* Negative angle */ + zassert_equal(gnss_nmea0183_ddmm_mmmm_to_ndeg("-18000.0", &ndeg), -EINVAL, + "Parse should fail"); +} + +struct test_knots_to_mms_sample { + const char *str; + int64_t value; +}; + +static const struct test_knots_to_mms_sample knots_to_mms_samples[] = { + {.str = "1", .value = 514}, + {.str = "2.2", .value = 1131}, + {.str = "003241.12543", .value = 1667364} +}; + +ZTEST(gnss_nmea0183, test_knots_to_mms) +{ + int64_t mms; + + for (size_t i = 0; i < ARRAY_SIZE(knots_to_mms_samples); i++) { + zassert_ok(gnss_nmea0183_knots_to_mms(knots_to_mms_samples[i].str, &mms), + "Parse failed"); + + zassert_equal(knots_to_mms_samples[i].value, mms, + "Parsed value falls below max rounding error"); + } +} + +struct test_hhmmss_sample { + const char *str; + uint8_t hour; + uint8_t minute; + uint16_t millisecond; +}; + +static const struct test_hhmmss_sample hhmmss_samples[] = { + {.str = "000102", .hour = 0, .minute = 1, .millisecond = 2000}, + {.str = "235959.999", .hour = 23, .minute = 59, .millisecond = 59999}, + {.str = "000000.0", .hour = 0, .minute = 0, .millisecond = 0} +}; + +ZTEST(gnss_nmea0183, test_hhmmss) +{ + struct gnss_time utc; + int ret; + + for (size_t i = 0; i < ARRAY_SIZE(hhmmss_samples); i++) { + zassert_ok(gnss_nmea0183_parse_hhmmss(hhmmss_samples[i].str, &utc), + "Parse failed"); + + zassert_equal(hhmmss_samples[i].hour, utc.hour, "Failed to parse hour"); + zassert_equal(hhmmss_samples[i].minute, utc.minute, "Failed to parse minute"); + zassert_equal(hhmmss_samples[i].millisecond, utc.millisecond, + "Failed to parse millisecond"); + } + + ret = gnss_nmea0183_parse_hhmmss("-101010", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_hhmmss("01010", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_hhmmss("246060.999", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_hhmmss("99a9c9", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_hhmmss("12121212", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); +} + +struct test_ddmmyy_sample { + const char *str; + uint8_t month_day; + uint8_t month; + uint16_t century_year; +}; + +static const struct test_ddmmyy_sample ddmmyy_samples[] = { + {.str = "010203", .month_day = 1, .month = 2, .century_year = 3}, + {.str = "311299", .month_day = 31, .month = 12, .century_year = 99}, + {.str = "010100", .month_day = 1, .month = 1, .century_year = 0} +}; + +ZTEST(gnss_nmea0183, test_ddmmyy) +{ + struct gnss_time utc; + int ret; + + for (size_t i = 0; i < ARRAY_SIZE(ddmmyy_samples); i++) { + zassert_ok(gnss_nmea0183_parse_ddmmyy(ddmmyy_samples[i].str, &utc), + "Parse failed"); + + zassert_equal(ddmmyy_samples[i].month_day, utc.month_day, + "Failed to parse monthday"); + + zassert_equal(ddmmyy_samples[i].month, utc.month, "Failed to parse month"); + zassert_equal(ddmmyy_samples[i].century_year, utc.century_year, + "Failed to parse year"); + } + + ret = gnss_nmea0183_parse_ddmmyy("000000", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_ddmmyy("-12123", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_ddmmyy("01010", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_ddmmyy("999999", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_ddmmyy("99a9c9", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); + + ret = gnss_nmea0183_parse_ddmmyy("12121212", &utc); + zassert_equal(ret, -EINVAL, "Should fail to parse invalid value"); +} + +/* "$GNRMC,160833.099,V,,,,,,,090923,,,N,V*27" */ +const char *rmc_argv_no_fix[15] = { + "$GNRMC", + "160833.099", + "V", + "", + "", + "", + "", + "", + "", + "090923", + "", + "", + "N", + "V", + "27" +}; + +static struct gnss_data data; + +ZTEST(gnss_nmea0183, test_parse_rmc_no_fix) +{ + int ret; + + /* Corrupt data */ + memset(&data, 0xFF, sizeof(data)); + + ret = gnss_nmea0183_parse_rmc(rmc_argv_no_fix, ARRAY_SIZE(rmc_argv_no_fix), &data); + zassert_ok(ret, "NMEA0183 RMC message parse should succeed"); +} + +/* "$GNGGA,160834.099,,,,,0,0,,,M,,M,,*5E" */ +const char *gga_argv_no_fix[16] = { + "$GNGGA", + "160834.099", + "", + "", + "", + "", + "0", + "0", + "", + "", + "M", + "", + "M", + "", + "5E" +}; + +ZTEST(gnss_nmea0183, test_parse_gga_no_fix) +{ + int ret; + + /* Corrupt data */ + memset(&data, 0xFF, sizeof(data)); + + ret = gnss_nmea0183_parse_gga(gga_argv_no_fix, ARRAY_SIZE(gga_argv_no_fix), &data); + zassert_ok(ret, "NMEA0183 GGA message parse should succeed"); + zassert_equal(data.info.fix_quality, GNSS_FIX_QUALITY_INVALID, + "Incorrectly parsed fix quality"); + + zassert_equal(data.info.fix_status, GNSS_FIX_STATUS_NO_FIX, + "Incorrectly parsed fix status"); +} + +/* "$GNRMC,160849.000,A,5709.736602,N,00957.660738,E,0.33,0.00,090923,,,A,V*03" */ +const char *rmc_argv_fix[15] = { + "$GNRMC", + "160849.000", + "A", + "5709.736602", + "N", + "00957.660738", + "E", + "0.33", + "33.31", + "090923", + "", + "", + "A", + "V", + "03", +}; + +ZTEST(gnss_nmea0183, test_parse_rmc_fix) +{ + int ret; + + /* Corrupt data */ + memset(&data, 0xFF, sizeof(data)); + + ret = gnss_nmea0183_parse_rmc(rmc_argv_fix, ARRAY_SIZE(rmc_argv_fix), &data); + zassert_ok(ret, "NMEA0183 RMC message parse should succeed"); + zassert_equal(data.nav_data.latitude, 57162276699, "Incorrectly parsed latitude"); + zassert_equal(data.nav_data.longitude, 9961012299, "Incorrectly parsed longitude"); + zassert_equal(data.nav_data.speed, 169, "Incorrectly parsed speed"); + zassert_equal(data.nav_data.bearing, 33310, "Incorrectly parsed speed"); + zassert_equal(data.utc.hour, 16, "Incorrectly parsed hour"); + zassert_equal(data.utc.minute, 8, "Incorrectly parsed minute"); + zassert_equal(data.utc.millisecond, 49000, "Incorrectly parsed millisecond"); + zassert_equal(data.utc.month_day, 9, "Incorrectly parsed month day"); + zassert_equal(data.utc.month, 9, "Incorrectly parsed month"); + zassert_equal(data.utc.century_year, 23, "Incorrectly parsed century year"); +} + +/* "$GNGGA,160858.000,5709.734778,N,00957.659514,E,1,6,1.41,15.234,M,42.371,M,,*72" */ +const char *gga_argv_fix[16] = { + "$GNGGA", + "160858.000", + "5709.734778", + "N", + "00957.659514", + "E", + "1", + "6", + "1.41", + "15.234", + "M", + "42.371", + "M", + "", + "", + "72", +}; + +ZTEST(gnss_nmea0183, test_parse_gga_fix) +{ + int ret; + + /* Corrupt data */ + memset(&data, 0xFF, sizeof(data)); + + ret = gnss_nmea0183_parse_gga(gga_argv_fix, ARRAY_SIZE(gga_argv_fix), &data); + zassert_ok(ret, "NMEA0183 GGA message parse should succeed"); + zassert_equal(data.info.fix_quality, GNSS_FIX_QUALITY_GNSS_SPS, + "Incorrectly parsed fix quality"); + + zassert_equal(data.info.fix_status, GNSS_FIX_STATUS_GNSS_FIX, + "Incorrectly parsed fix status"); + + zassert_equal(data.info.satellites_cnt, 6, + "Incorrectly parsed number of satelites"); + + zassert_equal(data.info.hdop, 1410, "Incorrectly parsed HDOP"); + zassert_equal(data.nav_data.altitude, 42371, "Incorrectly parsed altitude"); +} + +ZTEST(gnss_nmea0183, test_snprintk) +{ + int ret; + char buf[sizeof("$PAIR002,3*27")]; + + ret = gnss_nmea0183_snprintk(buf, sizeof(buf), "PAIR%03u,%u", 2, 3); + zassert_equal(ret, (sizeof("$PAIR002,3*27") - 1), "Failed to format NMEA0183 message"); + zassert_ok(strcmp(buf, "$PAIR002,3*27"), "Incorrectly formatted NMEA0183 message"); + + ret = gnss_nmea0183_snprintk(buf, sizeof(buf) - 1, "PAIR%03u,%u", 2, 3); + zassert_equal(ret, -ENOMEM, "Should fail with -ENOMEM as buffer is too small"); +} + +/* $GPGSV,8,1,25,21,44,141,47,15,14,049,44,6,31,255,46,3,25,280,44*75 */ +const char *gpgsv_8_1_25[21] = { + "$GPGSV", + "8", + "1", + "25", + "21", + "44", + "141", + "47", + "15", + "14", + "049", + "44", + "6", + "31", + "255", + "46", + "3", + "25", + "280", + "44", + "75", +}; + +static const struct gnss_nmea0183_gsv_header gpgsv_8_1_25_header = { + .system = GNSS_SYSTEM_GPS, + .number_of_messages = 8, + .message_number = 1, + .number_of_svs = 25 +}; + +static const struct gnss_satellite gpgsv_8_1_25_sats[] = { + {.prn = 21, .elevation = 44, .azimuth = 141, .snr = 47, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 15, .elevation = 14, .azimuth = 49, .snr = 44, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 6, .elevation = 31, .azimuth = 255, .snr = 46, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 3, .elevation = 25, .azimuth = 280, .snr = 44, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, +}; + +/* $GPGSV,8,2,25,18,61,057,48,22,68,320,52,27,34,268,47,24,32,076,45*76 */ +const char *gpgsv_8_2_25[21] = { + "$GPGSV", + "8", + "2", + "25", + "18", + "61", + "057", + "48", + "22", + "68", + "320", + "52", + "27", + "34", + "268", + "47", + "24", + "32", + "076", + "45", + "76", +}; + +static const struct gnss_nmea0183_gsv_header gpgsv_8_2_25_header = { + .system = GNSS_SYSTEM_GPS, + .number_of_messages = 8, + .message_number = 2, + .number_of_svs = 25 +}; + +static const struct gnss_satellite gpgsv_8_2_25_sats[] = { + {.prn = 18, .elevation = 61, .azimuth = 57, .snr = 48, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 22, .elevation = 68, .azimuth = 320, .snr = 52, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 27, .elevation = 34, .azimuth = 268, .snr = 47, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 24, .elevation = 32, .azimuth = 76, .snr = 45, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, +}; + +/* $GPGSV,8,3,25,14,51,214,49,19,23,308,46*7E */ +const char *gpgsv_8_3_25[13] = { + "$GPGSV", + "8", + "3", + "25", + "14", + "51", + "214", + "49", + "19", + "23", + "308", + "46", + "7E", +}; + +static const struct gnss_nmea0183_gsv_header gpgsv_8_3_25_header = { + .system = GNSS_SYSTEM_GPS, + .number_of_messages = 8, + .message_number = 3, + .number_of_svs = 25 +}; + +static const struct gnss_satellite gpgsv_8_3_25_sats[] = { + {.prn = 14, .elevation = 51, .azimuth = 214, .snr = 49, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, + {.prn = 19, .elevation = 23, .azimuth = 308, .snr = 46, + .system = GNSS_SYSTEM_GPS, .is_tracked = true}, +}; + +/* $GPGSV,8,4,25,51,44,183,49,46,41,169,43,48,36,220,45*47 */ +const char *gpgsv_8_4_25[17] = { + "$GPGSV", + "8", + "4", + "25", + "51", + "44", + "183", + "49", + "46", + "41", + "169", + "43", + "48", + "36", + "220", + "45", + "47", +}; + +static const struct gnss_nmea0183_gsv_header gpgsv_8_4_25_header = { + .system = GNSS_SYSTEM_GPS, + .number_of_messages = 8, + .message_number = 4, + .number_of_svs = 25 +}; + +static const struct gnss_satellite gpgsv_8_4_25_sats[] = { + {.prn = (51 + 87), .elevation = 44, .azimuth = 183, .snr = 49, + .system = GNSS_SYSTEM_SBAS, .is_tracked = true}, + {.prn = (46 + 87), .elevation = 41, .azimuth = 169, .snr = 43, + .system = GNSS_SYSTEM_SBAS, .is_tracked = true}, + {.prn = (48 + 87), .elevation = 36, .azimuth = 220, .snr = 45, + .system = GNSS_SYSTEM_SBAS, .is_tracked = true}, +}; + +/* $GLGSV,8,5,25,82,49,219,52,76,22,051,41,83,37,316,51,67,57,010,51*6C */ +const char *glgsv_8_5_25[21] = { + "$GLGSV", + "8", + "5", + "25", + "82", + "49", + "219", + "52", + "76", + "22", + "051", + "41", + "83", + "37", + "316", + "51", + "67", + "57", + "010", + "51", + "6C", +}; + +static const struct gnss_nmea0183_gsv_header glgsv_8_5_25_header = { + .system = GNSS_SYSTEM_GLONASS, + .number_of_messages = 8, + .message_number = 5, + .number_of_svs = 25 +}; + +static const struct gnss_satellite glgsv_8_5_25_sats[] = { + {.prn = (82 - 64), .elevation = 49, .azimuth = 219, .snr = 52, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (76 - 64), .elevation = 22, .azimuth = 51, .snr = 41, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (83 - 64), .elevation = 37, .azimuth = 316, .snr = 51, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (67 - 64), .elevation = 57, .azimuth = 10, .snr = 51, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, +}; + +/* $GLGSV,8,6,25,77,24,108,44,81,10,181,46,78,1,152,34,66,18,060,45*50 */ +const char *glgsv_8_6_25[21] = { + "$GLGSV", + "8", + "6", + "25", + "77", + "24", + "108", + "44", + "81", + "10", + "181", + "46", + "78", + "1", + "152", + "34", + "66", + "18", + "060", + "45", + "50", +}; + +static const struct gnss_nmea0183_gsv_header glgsv_8_6_25_header = { + .system = GNSS_SYSTEM_GLONASS, + .number_of_messages = 8, + .message_number = 6, + .number_of_svs = 25 +}; + +static const struct gnss_satellite glgsv_8_6_25_sats[] = { + {.prn = (77 - 64), .elevation = 24, .azimuth = 108, .snr = 44, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (81 - 64), .elevation = 10, .azimuth = 181, .snr = 46, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (78 - 64), .elevation = 1, .azimuth = 152, .snr = 34, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, + {.prn = (66 - 64), .elevation = 18, .azimuth = 60, .snr = 45, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, +}; + +/* $GLGSV,8,7,25,68,37,284,50*5C */ +const char *glgsv_8_7_25[9] = { + "$GLGSV", + "8", + "7", + "25", + "68", + "37", + "284", + "50", + "5C", +}; + +static const struct gnss_nmea0183_gsv_header glgsv_8_7_25_header = { + .system = GNSS_SYSTEM_GLONASS, + .number_of_messages = 8, + .message_number = 7, + .number_of_svs = 25 +}; + +static const struct gnss_satellite glgsv_8_7_25_sats[] = { + {.prn = (68 - 64), .elevation = 37, .azimuth = 284, .snr = 50, + .system = GNSS_SYSTEM_GLONASS, .is_tracked = true}, +}; + +/* $GBGSV,8,8,25,111,35,221,47,112,4,179,39,114,48,290,48*11 */ +const char *gbgsv_8_8_25[17] = { + "$GBGSV", + "8", + "8", + "25", + "111", + "35", + "221", + "47", + "112", + "4", + "179", + "39", + "114", + "48", + "290", + "48", + "11", +}; + +static const struct gnss_nmea0183_gsv_header gbgsv_8_8_25_header = { + .system = GNSS_SYSTEM_BEIDOU, + .number_of_messages = 8, + .message_number = 8, + .number_of_svs = 25 +}; + +static const struct gnss_satellite gbgsv_8_8_25_sats[] = { + {.prn = (111 - 100), .elevation = 35, .azimuth = 221, .snr = 47, + .system = GNSS_SYSTEM_BEIDOU, .is_tracked = true}, + {.prn = (112 - 100), .elevation = 4, .azimuth = 179, .snr = 39, + .system = GNSS_SYSTEM_BEIDOU, .is_tracked = true}, + {.prn = (114 - 100), .elevation = 48, .azimuth = 290, .snr = 48, + .system = GNSS_SYSTEM_BEIDOU, .is_tracked = true}, +}; + +struct test_gsv_sample { + const char **argv; + uint16_t argc; + const struct gnss_nmea0183_gsv_header *header; + const struct gnss_satellite *satellites; + uint16_t number_of_svs; +}; + +static const struct test_gsv_sample gsv_samples[] = { + {.argv = gpgsv_8_1_25, .argc = ARRAY_SIZE(gpgsv_8_1_25), .header = &gpgsv_8_1_25_header, + .satellites = gpgsv_8_1_25_sats, .number_of_svs = ARRAY_SIZE(gpgsv_8_1_25_sats)}, + {.argv = gpgsv_8_2_25, .argc = ARRAY_SIZE(gpgsv_8_2_25), .header = &gpgsv_8_2_25_header, + .satellites = gpgsv_8_2_25_sats, .number_of_svs = ARRAY_SIZE(gpgsv_8_2_25_sats)}, + {.argv = gpgsv_8_3_25, .argc = ARRAY_SIZE(gpgsv_8_3_25), .header = &gpgsv_8_3_25_header, + .satellites = gpgsv_8_3_25_sats, .number_of_svs = ARRAY_SIZE(gpgsv_8_3_25_sats)}, + {.argv = gpgsv_8_4_25, .argc = ARRAY_SIZE(gpgsv_8_4_25), .header = &gpgsv_8_4_25_header, + .satellites = gpgsv_8_4_25_sats, .number_of_svs = ARRAY_SIZE(gpgsv_8_4_25_sats)}, + {.argv = glgsv_8_5_25, .argc = ARRAY_SIZE(glgsv_8_5_25), .header = &glgsv_8_5_25_header, + .satellites = glgsv_8_5_25_sats, .number_of_svs = ARRAY_SIZE(glgsv_8_5_25_sats)}, + {.argv = glgsv_8_6_25, .argc = ARRAY_SIZE(glgsv_8_6_25), .header = &glgsv_8_6_25_header, + .satellites = glgsv_8_6_25_sats, .number_of_svs = ARRAY_SIZE(glgsv_8_6_25_sats)}, + {.argv = glgsv_8_7_25, .argc = ARRAY_SIZE(glgsv_8_7_25), .header = &glgsv_8_7_25_header, + .satellites = glgsv_8_7_25_sats, .number_of_svs = ARRAY_SIZE(glgsv_8_7_25_sats)}, + {.argv = gbgsv_8_8_25, .argc = ARRAY_SIZE(gbgsv_8_8_25), .header = &gbgsv_8_8_25_header, + .satellites = gbgsv_8_8_25_sats, .number_of_svs = ARRAY_SIZE(gbgsv_8_8_25_sats)}, +}; + +ZTEST(gnss_nmea0183, test_gsv_parse_headers) +{ + struct gnss_nmea0183_gsv_header header; + int ret; + + for (uint16_t i = 0; i < ARRAY_SIZE(gsv_samples); i++) { + ret = gnss_nmea0183_parse_gsv_header(gsv_samples[i].argv, gsv_samples[i].argc, + &header); + + zassert_ok(ret, "Failed to parse GSV header"); + + zassert_equal(header.system, gsv_samples[i].header->system, + "Failed to parse GNSS system"); + + zassert_equal(header.number_of_messages, + gsv_samples[i].header->number_of_messages, + "Failed to parse number of messages"); + + zassert_equal(header.message_number, gsv_samples[i].header->message_number, + "Failed to parse message number"); + + zassert_equal(header.number_of_svs, gsv_samples[i].header->number_of_svs, + "Failed to parse number of space vehicles"); + } +} + +ZTEST(gnss_nmea0183, test_gsv_parse_satellites) +{ + struct gnss_satellite satellites[4]; + int ret; + + for (uint16_t i = 0; i < ARRAY_SIZE(gsv_samples); i++) { + ret = gnss_nmea0183_parse_gsv_svs(gsv_samples[i].argv, gsv_samples[i].argc, + satellites, ARRAY_SIZE(satellites)); + + zassert_equal(ret, gsv_samples[i].number_of_svs, + "Incorrect number of satellites parsed"); + + for (uint16_t u = 0; u < gsv_samples[i].number_of_svs; u++) { + zassert_equal(gsv_samples[i].satellites[u].prn, + satellites[u].prn, + "Failed to parse satellite prn"); + zassert_equal(gsv_samples[i].satellites[u].snr, + satellites[u].snr, + "Failed to parse satellite snr"); + zassert_equal(gsv_samples[i].satellites[u].elevation, + satellites[u].elevation, + "Failed to parse satellite elevation"); + zassert_equal(gsv_samples[i].satellites[u].azimuth, + satellites[u].azimuth, + "Failed to parse satellite azimuth"); + zassert_equal(gsv_samples[i].satellites[u].system, + satellites[u].system, + "Failed to parse satellite system"); + zassert_equal(gsv_samples[i].satellites[u].is_tracked, + satellites[u].is_tracked, + "Failed to parse satellite is_tracked"); + } + } +} + +ZTEST_SUITE(gnss_nmea0183, NULL, NULL, NULL, NULL, NULL); diff --git a/tests/drivers/gnss/gnss_nmea0183/testcase.yaml b/tests/drivers/gnss/gnss_nmea0183/testcase.yaml new file mode 100644 index 00000000000..b63e7f24dbf --- /dev/null +++ b/tests/drivers/gnss/gnss_nmea0183/testcase.yaml @@ -0,0 +1,10 @@ +# Copyright (c) 2023 Trackunit Corporation +# SPDX-License-Identifier: Apache-2.0 + +tests: + drivers.gnss.gnss_nmea0183: + tags: + - drivers + - gnss + - parse + - nmea0183