315 lines
9.9 KiB
C
315 lines
9.9 KiB
C
//
|
||
// 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);
|
||
}
|
||
}
|