view Small_CPU/Src/GNSS.c @ 972:12426391edd9
Evo_2_23 tip
Disabled development debug warnings
author |
Ideenmodellierer |
date |
Sun, 19 Jan 2025 21:03:42 +0100 (4 days ago) |
parents |
3420e3ba698d |
children |
|
line source
/*
* GNSS.c
*
* Created on: 03.10.2020
* Author: SimpleMethod
*
*Copyright 2020 SimpleMethod
*
*Permission is hereby granted, free of charge, to any person obtaining a copy of
*this software and associated documentation files (the "Software"), to deal in
*the Software without restriction, including without limitation the rights to
*use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
*of the Software, and to permit persons to whom the Software is furnished to do
*so, subject to the following conditions:
*
*The above copyright notice and this permission notice shall be included in all
*copies or substantial portions of the Software.
*
*THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
*IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
*AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
*LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
*OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
*THE SOFTWARE.
******************************************************************************
*/
#include <string.h>
#include "GNSS.h"
#include "data_exchange.h"
#include "rtc.h"
union u_Short uShort;
union i_Short iShort;
union u_Long uLong;
union i_Long iLong;
/*!
* Structure initialization.
* @param GNSS Pointer to main GNSS structure.
* @param huart Pointer to uart handle.
*/
void GNSS_Init(GNSS_StateHandle *GNSS, UART_HandleTypeDef *huart) {
GNSS->huart = huart;
GNSS->year = 0;
GNSS->month = 0;
GNSS->day = 0;
GNSS->hour = 0;
GNSS->min = 0;
GNSS->sec = 0;
GNSS->fixType = 0;
GNSS->lon = 0;
GNSS->lat = 0;
GNSS->height = 0;
GNSS->hMSL = 0;
GNSS->hAcc = 0;
GNSS->vAcc = 0;
GNSS->gSpeed = 0;
GNSS->headMot = 0;
}
/*!
* Parse data to unique chip ID standard.
* Look at: 32.19.1.1 u-blox 8 Receiver description
* @param GNSS Pointer to main GNSS structure.
*/
void GNSS_ParseUniqID(GNSS_StateHandle *GNSS) {
for (int var = 0; var < 4; var++) {
GNSS->uniqueID[var] = GNSS_Handle.uartWorkingBuffer[10 + var];
}
}
/*!
* Parse data to navigation position velocity time solution standard.
* Look at: 32.17.15.1 u-blox 8 Receiver description.
* @param GNSS Pointer to main GNSS structure.
*/
void GNSS_ParsePVTData(GNSS_StateHandle *GNSS) {
static float searchCnt = 1.0;
RTC_TimeTypeDef sTimeNow;
uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10];
GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10];
uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11];
GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11];
GNSS->year = uShort.uShort;
GNSS->month = GNSS_Handle.uartWorkingBuffer[12];
GNSS->day = GNSS_Handle.uartWorkingBuffer[13];
GNSS->hour = GNSS_Handle.uartWorkingBuffer[14];
GNSS->min = GNSS_Handle.uartWorkingBuffer[15];
GNSS->sec = GNSS_Handle.uartWorkingBuffer[16];
GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26];
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30];
}
GNSS->lon = iLong.iLong;
GNSS->fLon=(float)iLong.iLong/10000000.0;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34];
GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34];
}
GNSS->lat = iLong.iLong;
GNSS->fLat=(float)iLong.iLong/10000000.0;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38];
}
GNSS->height = iLong.iLong;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
}
GNSS->hMSL = iLong.iLong;
for (int var = 0; var < 4; ++var) {
uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46];
}
GNSS->hAcc = uLong.uLong;
for (int var = 0; var < 4; ++var) {
uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50];
}
GNSS->vAcc = uLong.uLong;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
}
GNSS->gSpeed = iLong.iLong;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70];
}
GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options.
if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0))
{
GNSS->fLat = searchCnt++;
}
if(GNSS->alive & GNSS_ALIVE_STATE_ALIVE) /* alive */
{
GNSS->alive &= ~GNSS_ALIVE_STATE_ALIVE;
}
else
{
GNSS->alive |= GNSS_ALIVE_STATE_ALIVE;
}
if((GNSS_Handle.uartWorkingBuffer[17] & 0x03) == 0x03) /* date/time valid */
{
GNSS->alive |= GNSS_ALIVE_STATE_TIME;
}
else
{
GNSS->alive &= ~GNSS_ALIVE_STATE_TIME;
}
if(GNSS->fixType >= 2)
{
RTC_GetTime(&sTimeNow);
GNSS->alive |= GNSS_ALIVE_BACKUP_POS;
GNSS->last_fLat = GNSS->fLat;
GNSS->last_fLon = GNSS->fLon;
GNSS->last_hour = sTimeNow.Hours;
}
}
/*!
* Parse data to UTC time solution standard.
* Look at: 32.17.30.1 u-blox 8 Receiver description.
* @param GNSS Pointer to main GNSS structure.
*/
void GNSS_ParseNavSatData(GNSS_StateHandle *GNSS) {
uint8_t loop = 0;
uint8_t searchIndex = 0;
uint8_t statIndex = 0; /* only 4 state information will be forwarded */
uint8_t signalQuality = 0;
GNSS->numSat = GNSS_Handle.uartWorkingBuffer[11];
memset(GNSS->statSat, 0, sizeof(GNSS->statSat));
if(GNSS->numSat > 0)
{
searchIndex = 0;
while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get good signal quality */
{
signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
if(signalQuality > 4)
{
GNSS->statSat[statIndex++] = signalQuality;
}
if(statIndex == 4) break;
searchIndex++;
}
searchIndex = 0;
while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get medium signal quality */
{
signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
if((signalQuality > 2) && (signalQuality <= 4))
{
GNSS->statSat[statIndex++] = signalQuality;
}
if(statIndex == 4) break;
searchIndex++;
}
searchIndex = 0;
while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get poor signal quality */
{
signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
if(signalQuality <= 2)
{
GNSS->statSat[statIndex++] = signalQuality;
}
if(statIndex == 4) break;
searchIndex++;
}
loop++;
}
}
void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) {
uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18];
uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19];
GNSS->year = uShort.uShort;
GNSS->month = GNSS_Handle.uartWorkingBuffer[20];
GNSS->day = GNSS_Handle.uartWorkingBuffer[21];
GNSS->hour = GNSS_Handle.uartWorkingBuffer[22];
GNSS->min = GNSS_Handle.uartWorkingBuffer[23];
GNSS->sec = GNSS_Handle.uartWorkingBuffer[24];
}
/*!
* Parse data to geodetic position solution standard.
* Look at: 32.17.14.1 u-blox 8 Receiver description.
* @param GNSS Pointer to main GNSS structure.
*/
void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) {
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10];
}
GNSS->lon = iLong.iLong;
GNSS->fLon=(float)iLong.iLong/10000000.0;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14];
}
GNSS->lat = iLong.iLong;
GNSS->fLat=(float)iLong.iLong/10000000.0;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18];
}
GNSS->height = iLong.iLong;
for (int var = 0; var < 4; ++var) {
iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22];
}
GNSS->hMSL = iLong.iLong;
for (int var = 0; var < 4; ++var) {
uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26];
}
GNSS->hAcc = uLong.uLong;
for (int var = 0; var < 4; ++var) {
uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
}
GNSS->vAcc = uLong.uLong;
}