Init
This commit is contained in:
cfif
commit
aebb16e566
|
|
@ -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_NEW",
|
||||
"repo": "GnssPro04Nmea"
|
||||
}
|
||||
],
|
||||
"cmake": {
|
||||
"inc_dirs": [
|
||||
"Inc"
|
||||
],
|
||||
"srcs": [
|
||||
"Src/**.c"
|
||||
]
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue