From 8f9c47017d144b2f19467e0b0c857e03431cfc66 Mon Sep 17 00:00:00 2001 From: cfif Date: Wed, 4 Dec 2024 13:10:49 +0300 Subject: [PATCH] Init --- Inc/Nmea0183Parser.h | 77 +++++++++++++ Src/Nmea1083Parser.c | 259 +++++++++++++++++++++++++++++++++++++++++++ Tst/CMakeLists.txt | 8 ++ Tst/Src/test.c | 69 ++++++++++++ Tst/modular.json | 16 +++ modular.json | 22 ++++ 6 files changed, 451 insertions(+) create mode 100644 Inc/Nmea0183Parser.h create mode 100644 Src/Nmea1083Parser.c create mode 100644 Tst/CMakeLists.txt create mode 100644 Tst/Src/test.c create mode 100644 Tst/modular.json create mode 100644 modular.json diff --git a/Inc/Nmea0183Parser.h b/Inc/Nmea0183Parser.h new file mode 100644 index 0000000..aedf6e0 --- /dev/null +++ b/Inc/Nmea0183Parser.h @@ -0,0 +1,77 @@ +// +// Created by xemon on 01.09.22. +// + +#ifndef NMEA_0183_PARSER_NMEA_0183_PARSER_H +#define NMEA_0183_PARSER_NMEA_0183_PARSER_H + +#include "stdint.h" +#include "stddef.h" +#include "stdbool.h" + +typedef struct { + uint8_t hour; + uint8_t minute; + uint8_t second; + uint16_t millisecond; +} tNmeaTimeRmc; + +typedef struct { + uint8_t day; + uint8_t month; + uint8_t year; +} tNmeaDateRmc; + +typedef struct { + double latitude; + char nsIndicator; + double longitude; + double horizontDiluitPrecis; + double altitude; + char ewIndicator; +} tNmeaLocationRmc; + +typedef struct { + double declination; + double nsat; + double mode; +} tNmeaMagneticRmc; + +typedef struct { + char status; + + tNmeaDateRmc date; + tNmeaTimeRmc time; + tNmeaLocationRmc location; + tNmeaMagneticRmc magnetic; + + double knotVelocity; + double kmhVelocity; + double headingAngle; +} tNmeaRmc; + +typedef struct { + double lat; + double lon; +} tLocationPointInDegDouble; + +typedef struct { + float lat; + float lon; +} tLocationPointInDeg; + +uint32_t iNmea0183TimestampFromRmc(tNmeaRmc *env); + +tLocationPointInDegDouble iNmea0183_DegLocationDoubleFromRmc(tNmeaRmc *env); + +tLocationPointInDeg iNmea0183_DegLocationFromRmc(tNmeaRmc *env); + +void vNmea0183Sign(char *str, size_t *strLen); + +bool bNmea0183ParseRMC(char *rmcString, size_t len, tNmeaRmc *result); + +bool bNmea0183IsRmcString(char *nmeaString, size_t len); + +bool bNmea0183IsValidString(char *nmeaString, size_t len); + +#endif //NMEA_0183_PARSER_NMEA_0183_PARSER_H diff --git a/Src/Nmea1083Parser.c b/Src/Nmea1083Parser.c new file mode 100644 index 0000000..9d93fbe --- /dev/null +++ b/Src/Nmea1083Parser.c @@ -0,0 +1,259 @@ +// +// Created by xemon on 01.09.22. +// +#include "Nmea0183Parser.h" +#include "AsciiStringAssmeblingUtils.h" +#include "AsciiStringParsingUtils.h" +#include "time.h" +#include +//#include "printf.h" + +double specifAtCommandGNSS; +/* +static char *strnstr(const char *haystack, const char *needle, size_t len) +{ + int i; + size_t needle_len; + + if (0 == (needle_len = strnlen(needle, len))) + return (char *)haystack; + + for (i=0; i<=(int)(len-needle_len); i++) + { + if ((haystack[0] == needle[0]) && + (0 == strncmp(haystack, needle, needle_len))) + return (char *)haystack; + + haystack++; + } + return NULL; +} +*/ +#define Nmea0183ParseShortCharsDecimalNumber(STR, LEN) (uint16_t) iAsciiStringParseUnsignedLongDecimalNumber(STR,((STR)+(LEN))) + +#define Nmea0183ParseDouble(STR, LEN) dAsciiStringParseDouble(STR,((STR)+(LEN))) + +#define Nmea0183SkipSpace(STR, STR_END) xAsciiStringSkipSpace(STR,STR_END) + +#define Nmea0183SkipToChar(STR, STR_END, SYMBOL) xAsciiStringSeekChar(STR,STR_END,SYMBOL) + +#define Nmea0183ParseNextPortion(BEGIN, DIV, STR_END) iAsciiStringMoveToNextParsingBlock(BEGIN,DIV,STR_END,',') + +#define Nmea0183ParseHexByte(STR) iAsciiStringParseHexByte(STR) + +bool Nmea0183ParseTime(char *utcString, char const *utcStringEnd, tNmeaTimeRmc *time) { + + uint32_t len = utcStringEnd - utcString; + uint8_t charsLeft = len - 7; + + if (len > 10 || len < 9) { + return false; + } + + if (utcString[6] != '.') { + return false; + } + + time->hour = Nmea0183ParseShortCharsDecimalNumber(utcString + 0, 2); + time->minute = Nmea0183ParseShortCharsDecimalNumber(utcString + 2, 2); + time->second = Nmea0183ParseShortCharsDecimalNumber(utcString + 4, 2); + + time->millisecond = Nmea0183ParseShortCharsDecimalNumber(utcString + 7, charsLeft); + + charsLeft = 3 - charsLeft; + + while (charsLeft) { + time->millisecond *= 10; + --charsLeft; + } + + return true; +} + +bool Nmea0183ParseDate(char *utcString, char const *utcStringEnd, tNmeaDateRmc *date) { + + uint32_t len = utcStringEnd - utcString; + + if (len != 6) { + return false; + } + + date->day = Nmea0183ParseShortCharsDecimalNumber(utcString + 0, 2); + date->month = Nmea0183ParseShortCharsDecimalNumber(utcString + 2, 2); + date->year = Nmea0183ParseShortCharsDecimalNumber(utcString + 4, 2); + + return true; +} + +uint8_t uNmea0183Checksum(const char *str, size_t len) { + char *end = (char *) (str + len); + + unsigned char result; + result = 0; + str++; + + while ((str != end) && (*str != '*') && (*str != '\0')) + result ^= *str++; + + return result; +} + + +void vNmea0183Sign(char *str, size_t *strLen) { + uint8_t crc = uNmea0183Checksum(str, *strLen); + vAsciiStringAddChar(str, strLen, '*'); + vAsciiStringAddByteAsHex(str, strLen, crc); +} + +bool bNmea0183IsRmcString(char *nmeaString, size_t len) { + if (nmeaString[0] != '$') { + return false; + } + + return strnstr(nmeaString, "RMC", len); +} + +uint32_t iNmea0183TimestampFromRmc(tNmeaRmc *env) { + struct tm time; + time.tm_hour = env->time.hour; + time.tm_min = env->time.minute; + time.tm_sec = env->time.second; + + time.tm_mday = env->date.day; + time.tm_mon = env->date.month - 1; + if(env->date.year>60){ + time.tm_year = (1900 - 1900) + env->date.year; + }else{ + time.tm_year = (2000 - 1900) + env->date.year; + } + + return mktime(&time); +} + +static double nmeaLocationToDeg(double dec) { + double _dec = dec; + int deg = (int) (_dec / 100); + int min = (int) (_dec) - (deg * 100); + + double sec = (double) (_dec - min - 100 * deg) * 60.0; + + return deg + min / 60.0 + sec / 3600.0; +} + + +tLocationPointInDegDouble iNmea0183_DegLocationDoubleFromRmc(tNmeaRmc *env) { + tLocationPointInDegDouble result; + result.lat = nmeaLocationToDeg(env->location.latitude); + result.lon = nmeaLocationToDeg(env->location.longitude); + + if (env->location.nsIndicator == 'S' || env->location.nsIndicator == 's') { + result.lat *= -1.0; + } + + if (env->location.ewIndicator == 'W' || env->location.ewIndicator == 'w') { + result.lon *= -1.0; + } + return result; +} + +tLocationPointInDeg iNmea0183_DegLocationFromRmc(tNmeaRmc *env) { + tLocationPointInDeg result; + result.lat = (float) nmeaLocationToDeg(env->location.latitude); + result.lon = (float) nmeaLocationToDeg(env->location.longitude); + + if (env->location.nsIndicator == 'S' || env->location.nsIndicator == 's') { + result.lat *= -1.0f; + } + + if (env->location.ewIndicator == 'W' || env->location.ewIndicator == 'w') { + result.lon *= -1.0f; + } + return result; +} + + +bool bNmea0183IsValidString(char *nmeaString, size_t len) { + if (specifAtCommandGNSS == 1) { + char *dataEndPos = strnstr((char *) nmeaString, "*", len); + uint8_t checksumCalculated = uNmea0183Checksum(nmeaString, dataEndPos - nmeaString); + ++dataEndPos; + uint8_t checksumInString = Nmea0183ParseHexByte(dataEndPos); + return checksumCalculated == checksumInString; + } else { + return 1; + } +} + +bool bNmea0183ParseRMC(char *rmcString, size_t len, tNmeaRmc *result) { + + char *end = rmcString + len; + char *front = rmcString; + char *nextDivider = front - 1; + + result->status = 'V'; + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + if (!Nmea0183ParseTime(front, nextDivider, &result->time)) { + return false; + } + } else { + result->time.second = 0; + result->time.minute = 0; + result->time.hour = 0; + result->time.millisecond = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->status = *front; + } else { + result->status = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->location.latitude = Nmea0183ParseDouble(front, nextDivider - front); + } else { + result->location.latitude = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->location.nsIndicator = *front; + } else { + result->location.nsIndicator = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->location.longitude = Nmea0183ParseDouble(front, nextDivider - front); + } else { + result->location.longitude = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->location.ewIndicator = *front; + } else { + result->location.ewIndicator = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->knotVelocity = Nmea0183ParseDouble(front, nextDivider - front); + } else { + result->knotVelocity = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + result->headingAngle = Nmea0183ParseDouble(front, nextDivider - front); + } else { + result->headingAngle = 0; + } + + if (Nmea0183ParseNextPortion(&front, &nextDivider, end) > 0) { + if (!Nmea0183ParseDate(front, nextDivider, &result->date)) { + return false; + } + } else { + result->date.year = 0; + result->date.month = 0; + result->date.day = 0; + } + + + return true; +} diff --git a/Tst/CMakeLists.txt b/Tst/CMakeLists.txt new file mode 100644 index 0000000..4a7275c --- /dev/null +++ b/Tst/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.17) +PROJECT(nmea_parser_test) + +set(CMAKE_CXX_STANDARD 17) + +include(modular.cmake) + +add_executable(test ${SOURCES}) \ No newline at end of file diff --git a/Tst/Src/test.c b/Tst/Src/test.c new file mode 100644 index 0000000..6f748d4 --- /dev/null +++ b/Tst/Src/test.c @@ -0,0 +1,69 @@ +// +// Created by xemon on 02.09.22. +// + +#include "printf.h" + +#include "Nmea0183Parser.h" +#include "stdio.h" + + +void testRmc(char *rmcStr, size_t rmcStrLen) { + printf("string %*s\n", (int) rmcStrLen, rmcStr); + if (bNmea0183IsValidString(rmcStr, rmcStrLen)) { + printf("it's valid nmea string\n"); + if (bNmea0183IsRmcString(rmcStr, rmcStrLen)) { + printf("it's rmc string\n"); + tNmeaRmc rmc; + if (bNmea0183ParseRMC(rmcStr + 7, rmcStrLen - 7, &rmc)) { + printf("RMC string parsed OK!\n"); + if (rmc.location.ewIndicator) { + printf("lat %c %f\n", rmc.location.ewIndicator, rmc.location.latitude); + } else { + printf("no lat\n"); + } + if (rmc.location.nsIndicator) { + printf("lon %c %f\n", rmc.location.nsIndicator, rmc.location.longitude); + } else { + printf("no lon\n"); + } + + printf("time %02i:%02i:%02i [%03i]\n", + rmc.time.hour, + rmc.time.minute, + rmc.time.second, + rmc.time.millisecond + ); + + printf("date %i %i 20%02i\n", + rmc.date.day, + rmc.date.month, + rmc.date.year + ); + } else { + printf("Can't parse rmc string!\n"); + } + } else { + printf("it's not rmc string\n"); + } + } else { + printf("it's invalid nmea string\n"); + } + +} + + + + + +int main() { + + char gprmc0[] = "$GPSACP: 115235.000,5244.8321N,04126.7108E,0.6,201.7,3,0.0,0.0,0.0,121022,05,08"; + char gprmc1[] = "$GPRMC,125504.049,A,5542.2389,N,03741.6063,E,0.06,25.82,200906,,,*17"; + printf("\n"); +// testAcp(gprmc0, sizeof(gprmc0) - 1); + + printf("\n"); + testRmc(gprmc1, sizeof(gprmc1) - 1); + return 0; +} \ No newline at end of file diff --git a/Tst/modular.json b/Tst/modular.json new file mode 100644 index 0000000..236f353 --- /dev/null +++ b/Tst/modular.json @@ -0,0 +1,16 @@ +{ + "dep": [ + { + "type": "local", + "dir": "../" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file diff --git a/modular.json b/modular.json new file mode 100644 index 0000000..18b71d6 --- /dev/null +++ b/modular.json @@ -0,0 +1,22 @@ +{ + "dep": [ + { + "type": "git", + "provider": "NAVIGATOR_UVEOS_NATION_TELIT", + "repo": "AsciiStringAssemblingUtils" + }, + { + "type": "git", + "provider": "NAVIGATOR_UVEOS_NATION_TELIT", + "repo": "AsciiStringParsingUtils" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file