Перенос на новую организацию GONEC
This commit is contained in:
cfif
commit
a3fc0f61d5
|
|
@ -0,0 +1,64 @@
|
||||||
|
//
|
||||||
|
// Created by cfif on 27.09.22.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifndef GNSSPRO04THREAD_H
|
||||||
|
#define GNSSPRO04THREAD_H
|
||||||
|
|
||||||
|
#include <Nmea0183Parser.h>
|
||||||
|
#include <GnssPro04Nmea.h>
|
||||||
|
#include <cmsis_os.h>
|
||||||
|
#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
|
||||||
|
|
@ -0,0 +1,314 @@
|
||||||
|
//
|
||||||
|
// Created by cfif on 27.09.22.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <sys/select.h>
|
||||||
|
#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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,17 @@
|
||||||
|
{
|
||||||
|
"dep": [
|
||||||
|
{
|
||||||
|
"type": "git",
|
||||||
|
"provider": "GONEC",
|
||||||
|
"repo": "GnssPro04Nmea"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"cmake": {
|
||||||
|
"inc_dirs": [
|
||||||
|
"Inc"
|
||||||
|
],
|
||||||
|
"srcs": [
|
||||||
|
"Src/**.c"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue