From a3fc0f61d5b648aa40612df0aaa00ca4abf685a1 Mon Sep 17 00:00:00 2001 From: cfif Date: Fri, 24 Jan 2025 13:22:33 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=B5=D1=80=D0=B5=D0=BD=D0=BE=D1=81=20?= =?UTF-8?q?=D0=BD=D0=B0=20=D0=BD=D0=BE=D0=B2=D1=83=D1=8E=20=D0=BE=D1=80?= =?UTF-8?q?=D0=B3=D0=B0=D0=BD=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8E=20GONEC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Inc/Gnss.h | 64 +++++++++++ Src/Gnss.c | 314 +++++++++++++++++++++++++++++++++++++++++++++++++++ modular.json | 17 +++ 3 files changed, 395 insertions(+) create mode 100644 Inc/Gnss.h create mode 100644 Src/Gnss.c create mode 100644 modular.json diff --git a/Inc/Gnss.h b/Inc/Gnss.h new file mode 100644 index 0000000..e1029a8 --- /dev/null +++ b/Inc/Gnss.h @@ -0,0 +1,64 @@ +// +// Created by cfif on 27.09.22. +// + +#ifndef GNSSPRO04THREAD_H +#define GNSSPRO04THREAD_H + +#include +#include +#include +#include "ModemGnss.h" +#include "RtcIO.h" +#include "LoggerInterface.h" + +typedef struct { + struct { + char data[256]; + size_t len; + } rxBuffer; + + tGnssPro04Nmea nmea; + tNmeaRmc currentRmc; + tNmeaRmc lastGoodRmc; + + uint32_t GMTcorr_v; + tRtcIO *rtcIo; + bool flagSetOneGnss; + + uint32_t lastRmcUpdate; + + tLoggerInterface *logger; + + osMutexId_t rmcAccess; + + struct { + osThreadId_t id; + uint32_t stack[512]; + StaticTask_t controlBlock; + osThreadAttr_t attr; + } thread; +} tGnss; + +// Получение данных с GNSS о местоположении и времени +void Gnss_GetNavDataTracert (tGnss *env, tTracertNavData *location); + +void Gnss_Init(tGnss *env, tSerialPortIO *gnssIo); + +void Gnss_InitDevice(tGnss *env); + +void Gnss_StartThread(tGnss *env); + +void Gnss_GetTime(tGnss *env, uint32_t *timestamp); + +// Получение данных с GNSS о местоположении +void Gnss_GetNavDataNmeaRmc (tGnss *env, tModemNavData *location); + +// latitude and longitude are in degrees, minutes and fraction of minutes +char cConvertDecToMilliArcSecFromDegMin(int32_t deg, int32_t min, int32_t *mArcs, double *gradus); + +uint32_t cConvertFrDeg(double frDeg, uint8_t latLonFlag); + +void Gnss_GetNavDataMsdNmeaRmc(tGnss *env, tNavDataMsd *location); + +#endif //GNSSPRO04THREAD_H diff --git a/Src/Gnss.c b/Src/Gnss.c new file mode 100644 index 0000000..8c390d5 --- /dev/null +++ b/Src/Gnss.c @@ -0,0 +1,314 @@ +// +// Created by cfif on 27.09.22. +// + +#include +#include +#include "CmsisRtosThreadUtils.h" +#include "Gnss.h" +#include "SystemDelayInterface.h" +#include "AsciiStringAssmeblingUtils.h" +#include "Nmea0183Parser.h" +#include "string.h" +#include "math.h" + +#define LOGGER env->logger +#define LOG_SIGN "GONEC" + +void Gnss_GetTime(tGnss *env, uint32_t *timestamp); + +void Gnss_Init(tGnss *env, tSerialPortIO *gnssIo) { + + memset(env, 0, sizeof(tGnss)); + + vGnssPro04NmeaInit(&env->nmea, gnssIo); + vAsciiStringInit(env->rxBuffer.data, &env->rxBuffer.len, sizeof(env->rxBuffer.data)); + env->rmcAccess = osMutexNew(NULL); + + //Инициализируем поток + InitThreadAtrStatic(&env->thread.attr, "Gnss", env->thread.controlBlock, env->thread.stack, osPriorityNormal); + env->thread.id = 0; + + env->flagSetOneGnss = true; + +} + +void Gnss_InitDevice(tGnss *env) { + vGnssPro04NmeaSetRate(&env->nmea, 1); + vGnssPro04NmeaSetCoordType(&env->nmea, GNNS_PRO04_COORD_W84); + vGnssPro04NmeaSetStartType(&env->nmea, GNNS_PRO04_STARTUP_HOT); + vGnssPro04NmeaSetStandard(&env->nmea, GNNS_PRO04_NMEA_STANDARD_2); + + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_DTM, false); + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_VTG, false); + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GSV, false); + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GSA, false); + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_GGA, false); + vGnssPro04NmeaSetMessageEnabled(&env->nmea, GNNS_PRO04_MESAGE_RMC, true); +} + +static _Noreturn void Gnss_Thread(tGnss *env) { + + Gnss_InitDevice(env); + + // Очистка физического буфера UART, вычитываем данные + SerialPortClearRxBuffer(env->nmea.io); + + tNmeaRmc tmpRmc; + for (;;) { + + // Очистка входного буфера + vAsciiStringClean(env->rxBuffer.data, &env->rxBuffer.len); + + // Чтение данных, до тех пор пока не встретится '\n' + env->rxBuffer.len = SerialPortReceiveTo( + env->nmea.io, + (uint8_t *) env->rxBuffer.data, + sizeof(env->rxBuffer.data), + '\n', + SystemWaitForever + ); + + // Поиск строки RMC + if (bNmea0183IsRmcString(env->rxBuffer.data, env->rxBuffer.len)) { + if (bNmea0183IsValidString(env->rxBuffer.data, env->rxBuffer.len)) { + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + if (bNmea0183ParseRMC(env->rxBuffer.data + 7, env->rxBuffer.len - 7, &tmpRmc)) { + + if (tmpRmc.status == 'A') { + env->lastGoodRmc = tmpRmc; + } else if (env->currentRmc.status == 'A') { + env->lastGoodRmc = env->currentRmc; + } else if (env->lastGoodRmc.status != 'A' && !env->lastGoodRmc.location.nsIndicator) { + env->lastGoodRmc = env->currentRmc; + } + + env->currentRmc = tmpRmc; + env->lastRmcUpdate = SystemGetMs(); +/* + if (env->flagSetOneGnss) { + + time_t set_timestamp = iNmea0183TimestampFromRmc(&env->currentRmc); + + srand(set_timestamp); + + if (set_timestamp > (time_t) 1644683158) {` + env->flagSetOneGnss = false; + set_timestamp += 3600 * env->GMTcorr_v; + env->rtcIo->set(&env->rtcIo, &set_timestamp); + } + + } + */ + + } + + osMutexRelease(env->rmcAccess); + } + } + } + } +} + +void Gnss_StartThread(tGnss *env) { + if (!env->thread.id) { + env->thread.id = osThreadNew((osThreadFunc_t) (Gnss_Thread), (void *) (env), &env->thread.attr); + } else { + osThreadResume(env->thread.id); + } +} + +uint32_t cConvertFrDeg(double frDeg, uint8_t latLonFlag) { + + uint8_t direction = frDeg > 0 ? 1 : 0; + + int8_t fullDeg = (int8_t) (frDeg); + double frMin = (frDeg - fullDeg) * 60.0; + + int8_t fullMin = (int8_t) frMin; + + int16_t frOfMin = (int16_t) ((frMin - fullMin) * 8192); + + uint8_t absFullDeg = (uint8_t) abs((int) fullDeg); + uint8_t absFullMin = abs(fullMin); + uint16_t absFrOfMin = abs(frOfMin); + + uint32_t result = + (0b01 << 29) | + ((latLonFlag & 0b1) << 28) | + ((direction) << 27) | + ((absFullDeg) << 19) | + ((absFullMin & 0b111111) << 13) | + ((absFrOfMin & 0b11111111111111)) << 0; + + return result; +} + +// latitude and longitude are in degrees, minutes and fraction of minutes +char cConvertDecToMilliArcSecFromDegMin(int32_t deg, int32_t min, int32_t *mArcs, double *gradus) { + double sec = 0.0; + + if (gradus != NULL) *gradus = deg + min / 60.0 + sec / 3600.0; + + if (mArcs != NULL) *mArcs = (int) (deg * 3600 + min * 60) * 1000 + (int) (sec * 1000); // mArcseconds + + return 0; +} + +// latitude and longitude are in degrees, minutes and fraction of minutes +char cConvertDecToMilliArcSec(double dec, int32_t *mArcs, double *gradus) { + int deg = 0, min = 0; + double sec = 0.0; + double _dec = dec; + + deg = (int) (_dec / 100); + min = (int) (_dec) - (deg * 100); + + sec = (double) (_dec - min - 100 * deg) * 60.0; + + if (gradus != NULL) *gradus = deg + min / 60.0 + sec / 3600.0; + + if (mArcs != NULL) *mArcs = (int) (deg * 3600 + min * 60) * 1000 + (int) (sec * 1000); // mArcseconds + + return 0; +} + + +// Получение данных с GNSS о местоположении и времени +void Gnss_GetNavDataTracert(tGnss *env, tTracertNavData *location) { + + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + + uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc); + + uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc); + + tNmeaRmc *rmc = &env->currentRmc; + + if (env->currentRmc.status != 'A') { + +// LoggerInfoStatic(LOGGER, LOG_SIGN, "Координаты не достоверны (tracert)"); + + rmc = &env->lastGoodRmc; + } + + + + +// if (env->currentRmc.status != 'A' && +// (!env->currentRmc.location.nsIndicator) && +// (env->currentRmc.location.nsIndicator) && +// ((timestamp - lastGoodTimestamp) < 10000)) { +// rmc = &env->lastGoodRmc; +// } + + cConvertDecToMilliArcSec(rmc->location.latitude, NULL, &location->gnss_latitude); + if (rmc->location.nsIndicator == 'S') + location->gnss_latitude *= -1; + + cConvertDecToMilliArcSec(rmc->location.longitude, NULL, &location->gnss_longitude); + if (rmc->location.ewIndicator == 'W') + location->gnss_longitude *= -1; + + location->valid = (rmc->status == 'A') ? true : false; + + location->curse = lround(rmc->headingAngle); + location->speed = lround(rmc->knotVelocity * 1.852); + + + osMutexRelease(env->rmcAccess); + } + +/* + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + + tModemNavData loc; + Gnss_GetNavDataNmeaRmc(env, &loc); + + location->gnss_latitude = loc.gnss_latitude; + location->gnss_longitude = loc.gnss_longitude; + + location->curse = lround(env->currentRmc.headingAngle); + location->speed = lround(env->currentRmc.kmhVelocity); + + location->valid = loc.valid; + + osMutexRelease(env->rmcAccess); + } + */ +} + + +// Получение данных с GNSS о местоположении +void Gnss_GetNavDataNmeaRmc(tGnss *env, tModemNavData *location) { + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + + uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc); + + uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc); + + tNmeaRmc *rmc = &env->currentRmc; + + if (env->currentRmc.status != 'A') { +// LoggerInfoStatic(LOGGER, LOG_SIGN, "Координаты не достоверны (modem)"); + + rmc = &env->lastGoodRmc; + } + +// if (env->currentRmc.status != 'A' && +// (!env->currentRmc.location.nsIndicator) && +// (env->currentRmc.location.nsIndicator) && +// ((timestamp - lastGoodTimestamp) < 10000)) { +// rmc = &env->lastGoodRmc; +// } + + cConvertDecToMilliArcSec(rmc->location.latitude, NULL, &location->gnss_latitude); + if (rmc->location.nsIndicator == 'S') + location->gnss_latitude *= -1; + + cConvertDecToMilliArcSec(rmc->location.longitude, NULL, &location->gnss_longitude); + if (rmc->location.ewIndicator == 'W') + location->gnss_longitude *= -1; + + location->valid = (rmc->status == 'A') ? true : false; + + osMutexRelease(env->rmcAccess); + } +} + + +// Получение данных с GNSS о местоположении +void Gnss_GetNavDataMsdNmeaRmc(tGnss *env, tNavDataMsd *location) { + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + + uint32_t timestamp = iNmea0183TimestampFromRmc(&env->currentRmc); + + uint32_t lastGoodTimestamp = iNmea0183TimestampFromRmc(&env->lastGoodRmc); + + tNmeaRmc *rmc = &env->currentRmc; + + if (env->currentRmc.status != 'A') { + rmc = &env->lastGoodRmc; + } + + cConvertDecToMilliArcSec(rmc->location.latitude, &location->latitude, NULL); + cConvertDecToMilliArcSec(rmc->location.longitude, &location->longitude, NULL); + + location->direction = (uint16_t) rmc->headingAngle; + + location->valid = (rmc->status == 'A') ? true : false; + + osMutexRelease(env->rmcAccess); + + } +} + +// Получение времени +void Gnss_GetTime(tGnss *env, uint32_t *timestamp) { + if (osMutexAcquire(env->rmcAccess, 2000) == osOK) { + + *timestamp = iNmea0183TimestampFromRmc(&env->currentRmc); + + osMutexRelease(env->rmcAccess); + } +} diff --git a/modular.json b/modular.json new file mode 100644 index 0000000..5d84837 --- /dev/null +++ b/modular.json @@ -0,0 +1,17 @@ +{ + "dep": [ + { + "type": "git", + "provider": "GONEC", + "repo": "GnssPro04Nmea" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file