kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
857 wiersze
39 KiB
C++
857 wiersze
39 KiB
C++
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "hal.h"
|
|
#include "gps.h"
|
|
// #include "ctrl.h"
|
|
|
|
#include "nmea.h"
|
|
#include "ubx.h"
|
|
#ifdef WITH_MAVLINK
|
|
#include "mavlink.h"
|
|
#include "atmosphere.h"
|
|
#endif
|
|
|
|
#include "ogn.h"
|
|
|
|
// #include "ctrl.h"
|
|
// #include "knob.h"
|
|
|
|
#include "lowpass2.h"
|
|
|
|
// #define DEBUG_PRINT
|
|
|
|
#ifdef DEBUG_PRINT
|
|
static char Line[128];
|
|
#endif
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
// void Debug_Print(uint8_t Byte) { while(!UART1_TxEmpty()) taskYIELD(); UART1_TxChar(Byte); }
|
|
|
|
static NMEA_RxMsg NMEA; // NMEA sentences catcher
|
|
#ifdef WITH_GPS_UBX
|
|
static UBX_RxMsg UBX; // UBX messages catcher
|
|
#endif
|
|
#ifdef WITH_MAVLINK
|
|
static MAV_RxMsg MAV; // MAVlink message catcher
|
|
#endif
|
|
|
|
uint16_t GPS_PosPeriod = 0;
|
|
|
|
const uint8_t PosPipeIdxMask = GPS_PosPipeSize-1;
|
|
static GPS_Position Position[GPS_PosPipeSize]; // GPS position pipe
|
|
static uint8_t PosIdx; // Pipe index, increments with every GPS position received
|
|
|
|
static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started
|
|
|
|
uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock
|
|
uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format
|
|
int32_t GPS_Altitude = 0; // [0.1m] last valid altitude
|
|
int32_t GPS_Latitude = 0; //
|
|
int32_t GPS_Longitude = 0; //
|
|
int16_t GPS_GeoidSepar= 0; // [0.1m]
|
|
uint16_t GPS_LatCosine = 3000; //
|
|
|
|
Status GPS_Status;
|
|
|
|
static union
|
|
{ uint8_t Flags;
|
|
struct
|
|
{ bool Spare:1; //
|
|
bool Active:1; // has started
|
|
bool GxRMC:1; // GPRMC or GNRMC registered
|
|
bool GxGGA:1; // GPGGA or GNGGA registered
|
|
bool GxGSA:1; // GPGSA or GNGSA registered
|
|
bool Complete:1; // all GPS data is supplied and thus ready for processing
|
|
} ;
|
|
} GPS_Burst;
|
|
// for the autobaud on the GPS port
|
|
const int GPS_BurstTimeout = 200; // [ms]
|
|
|
|
static const uint8_t BaudRates=7; // number of possible baudrates choices
|
|
static uint8_t BaudRateIdx=0; // actual choice
|
|
static const uint32_t BaudRate[BaudRates] = { 4800, 9600, 19200, 38400, 57600, 115200, 230400 } ; // list of baudrate the autobaud scans through
|
|
|
|
uint32_t GPS_getBaudRate (void) { return BaudRate[BaudRateIdx]; }
|
|
uint32_t GPS_nextBaudRate(void) { BaudRateIdx++; if(BaudRateIdx>=BaudRates) BaudRateIdx=0; return GPS_getBaudRate(); }
|
|
|
|
const uint32_t GPS_TargetBaudRate = 57600; // BaudRate[4]; // [bps] must be one of the baud rates known by the autbaud
|
|
const uint8_t GPS_TargetDynModel = 7; // for UBX GPS's: 6 = airborne with >1g, 7 = with >2g
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
int16_t GPS_AverageSpeed(void) // get average speed based on stored GPS positions
|
|
{ uint8_t Count=0;
|
|
int16_t Speed=0;
|
|
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++) // loop over GPS positions
|
|
{ GPS_Position *Pos = Position+Idx;
|
|
if( !Pos->hasGPS || !Pos->isValid() ) continue; // skip invalid positions
|
|
Speed += Pos->Speed +abs(Pos->ClimbRate); Count++;
|
|
}
|
|
if(Count==0) return -1;
|
|
if(Count>1) Speed/=Count;
|
|
return Speed; } // [0.1m/s]
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
static void GPS_PPS_On(void) // called on rising edge of PPS
|
|
{ static TickType_t PrevTickCount=0;
|
|
TickType_t TickCount = xTaskGetTickCount(); // [ms] TickCount now
|
|
TickType_t Delta = TickCount-PrevTickCount; // [ms] time difference to the previous PPS
|
|
PrevTickCount = TickCount; // [ms]
|
|
if(abs((int)Delta-1000)>10) return; // [ms] filter out difference away from 1.00sec
|
|
TimeSync_HardPPS(TickCount);
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
Format_String(CONS_UART_Write, " -> PPS\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
GPS_Status.PPS=1;
|
|
LED_PCB_Flash(50);
|
|
// uint8_t Sec=GPS_Sec; Sec++; if(Sec>=60) Sec=0; GPS_Sec=Sec;
|
|
// GPS_UnixTime++;
|
|
// #ifdef WITH_MAVLINK
|
|
// static MAV_SYSTEM_TIME MAV_Time;
|
|
// MAV_Time.time_unix_usec = (uint64_t)1000000*TimeSync_Time();
|
|
// MAV_Time.time_boot_ms = TickCount;
|
|
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
// MAV_RxMsg::Send(sizeof(MAV_Time), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_SYSTEM_TIME, (const uint8_t *)&MAV_Time, CONS_UART_Write);
|
|
// xSemaphoreGive(CONS_Mutex);
|
|
// #endif
|
|
}
|
|
|
|
static void GPS_PPS_Off(void) // called on falling edge of PPS
|
|
{ }
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
static void GPS_LockStart(void) // called when GPS catches a lock
|
|
{
|
|
|
|
#ifdef WITH_BEEPER
|
|
if(KNOB_Tick>12)
|
|
{ Play(Play_Vol_1 | Play_Oct_1 | 0x00, 100);
|
|
Play(Play_Vol_0 | Play_Oct_1 | 0x00, 100);
|
|
Play(Play_Vol_1 | Play_Oct_1 | 0x02, 100);
|
|
Play(Play_Vol_0 | Play_Oct_1 | 0x02, 100); }
|
|
#endif
|
|
|
|
}
|
|
|
|
static void GPS_LockEnd(void) // called when GPS looses a lock
|
|
{
|
|
|
|
#ifdef WITH_BEEPER
|
|
if(KNOB_Tick>12)
|
|
{ Play(Play_Vol_1 | Play_Oct_1 | 0x02, 100);
|
|
Play(Play_Vol_0 | Play_Oct_1 | 0x02, 100);
|
|
Play(Play_Vol_1 | Play_Oct_1 | 0x00, 100);
|
|
Play(Play_Vol_0 | Play_Oct_1 | 0x00, 100); }
|
|
#endif
|
|
|
|
}
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
static void GPS_BurstStart(void) // when GPS starts sending the data on the serial port
|
|
{ Burst_TickCount=xTaskGetTickCount();
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
Format_String(CONS_UART_Write, " -> GPS_BurstStart() GPS:");
|
|
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
#ifdef WITH_GPS_CONFIG
|
|
static uint16_t QueryWait=0;
|
|
if(GPS_Status.NMEA || GPS_Status.UBX) // if there is communication with the GPS already
|
|
{ if(QueryWait)
|
|
{ QueryWait--; }
|
|
else
|
|
{ if(!GPS_Status.ModeConfig) // if GPS navigation mode is not done yet
|
|
{ // Format_String(CONS_UART_Write, "CFG_NAV5 query...\n");
|
|
#ifdef WITH_GPS_UBX
|
|
UBX_RxMsg::Send(0x06, 0x24, GPS_UART_Write); // send the query for the navigation mode setting
|
|
if(!GPS_Status.NMEA) // if NMEA sentences are not there
|
|
{ UBX_CFG_MSG CFG_MSG; // send CFG_MSG to enable the NMEA sentences
|
|
CFG_MSG.msgClass = 0xF0; // NMEA class
|
|
CFG_MSG.rate = 1; // send every measurement event
|
|
// CFG_MSG.rate[0] = 1;
|
|
// CFG_MSG.rate[1] = 1;
|
|
// CFG_MSG.rate[2] = 1;
|
|
// CFG_MSG.rate[3] = 1;
|
|
// CFG_MSG.rate[4] = 1;
|
|
// CFG_MSG.rate[5] = 1;
|
|
CFG_MSG.msgID = 0x00; // ID for GGA
|
|
UBX_RxMsg::Send(0x06, 0x01, GPS_UART_Write, (uint8_t *)(&CFG_MSG), sizeof(CFG_MSG));
|
|
CFG_MSG.msgID = 0x02; // ID for RMC
|
|
UBX_RxMsg::Send(0x06, 0x01, GPS_UART_Write, (uint8_t *)(&CFG_MSG), sizeof(CFG_MSG));
|
|
CFG_MSG.msgID = 0x04; // ID for GSA
|
|
UBX_RxMsg::Send(0x06, 0x01, GPS_UART_Write, (uint8_t *)(&CFG_MSG), sizeof(CFG_MSG));
|
|
}
|
|
#endif
|
|
}
|
|
if(!GPS_Status.BaudConfig) // if GPS baud config is not done yet
|
|
{ // Format_String(CONS_UART_Write, "CFG_PRT query...\n");
|
|
#ifdef WITH_GPS_UBX
|
|
// uint8_t UART1_Port=1;
|
|
// UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write, &UART1_Port, 1); // send the query for the port config to have a template configuration packet
|
|
UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write); // send the query for the port config to have a template configuration packet
|
|
#endif
|
|
#ifdef WITH_GPS_MTK
|
|
char GPS_Cmd[36];
|
|
strcpy(GPS_Cmd, "$PMTK251,"); // MTK command to change the baud rate
|
|
uint8_t Len = strlen(GPS_Cmd);
|
|
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
|
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
|
GPS_Cmd[Len++]='\r'; // this is apparently needed but it should not, as ESP32 does auto-CR ??
|
|
GPS_Cmd[Len++]='\n';
|
|
GPS_Cmd[Len]=0;
|
|
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "GPS <- ");
|
|
Format_String(CONS_UART_Write, GPS_Cmd, Len, 0);
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
#endif
|
|
#ifdef WITH_GPS_SRF
|
|
char GPS_Cmd[36];
|
|
strcpy(GPS_Cmd, "$PSRF100,1,"); // SiRF command to change the baud rate
|
|
Len = strlen(GPS_Cmd);
|
|
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
|
strcpy(GPS_Cmd+Len, ",8,1,0");
|
|
Len = strlen(GPS_Cmd);
|
|
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
|
GPS_Cmd[Len++]='\r'; // this is apparently needed but it should not, as ESP32 does auto-CR ??
|
|
GPS_Cmd[Len++]='\n';
|
|
GPS_Cmd[Len]=0;
|
|
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
|
#endif
|
|
}
|
|
QueryWait=60;
|
|
}
|
|
}
|
|
else { QueryWait=0; }
|
|
#endif // WITH_GPS_CONFIG
|
|
}
|
|
|
|
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
|
|
{
|
|
#ifdef WITH_MAVLINK
|
|
GPS_Position *GPS = Position+PosIdx;
|
|
if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro)
|
|
{ int32_t StdAlt1 = Atmosphere::StdAltitude((GPS->Pressure+2)/4); // [0.1m] we try to fix the cheap chinese ArduPilot with baro chip cs5607 instead of cs5611
|
|
int32_t StdAlt2 = Atmosphere::StdAltitude((GPS->Pressure+1)/2); // [0.1m] the cx5607 is very close but gives pressure is twice as larger units
|
|
int32_t Alt = GPS->Altitude; // [0.1m] thus it appears to give pressure readout lower by a factor of two.
|
|
int32_t Delta1 = StdAlt1-Alt; // [0.1m] Here we check which pressure fits better the GPS altitude
|
|
int32_t Delta2 = StdAlt2-Alt; // [0.1m]
|
|
if(fabs(Delta1)<fabs(Delta2)) { GPS->StdAltitude=StdAlt1; } //
|
|
else { GPS->Pressure*=2; GPS->StdAltitude=StdAlt2; }
|
|
}
|
|
#endif
|
|
#ifdef DEBUG_PRINT
|
|
Position[PosIdx].PrintLine(Line); // print out the GPS position in a single-line format
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
Format_String(CONS_UART_Write, " -> GPS_BurstComplete() GPS:");
|
|
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
|
|
Format_String(CONS_UART_Write, "\nGPS");
|
|
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
|
Format_String(CONS_UART_Write, Line);
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
if(Position[PosIdx].hasGPS) // GPS position data complete
|
|
{ Position[PosIdx].isReady=1; // mark this record as ready for processing => producing packets for transmission
|
|
if(Position[PosIdx].isTimeValid()) // if time is valid already
|
|
{ if(Position[PosIdx].isDateValid()) // if date is valid as well
|
|
{ uint32_t UnixTime=Position[PosIdx].getUnixTime();
|
|
GPS_FatTime=Position[PosIdx].getFatTime();
|
|
#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message
|
|
TimeSync_SoftPPS(Burst_TickCount, UnixTime, Parameters.PPSdelay);
|
|
#endif
|
|
}
|
|
}
|
|
if(Position[PosIdx].isValid()) // position is complete and locked
|
|
{ if(Parameters.manGeoidSepar) // if GeoidSepar is "manual" - this implies the GPS does not correct for it
|
|
{ Position[PosIdx].GeoidSeparation = Parameters.GeoidSepar; // copy the manually set GeoidSepar
|
|
Position[PosIdx].Altitude -= Parameters.GeoidSepar; } // correct the Altitude - we likely need a separate flag for this
|
|
Position[PosIdx].calcLatitudeCosine();
|
|
GPS_TimeSinceLock++;
|
|
GPS_Altitude=Position[PosIdx].Altitude;
|
|
GPS_Latitude=Position[PosIdx].Latitude;
|
|
GPS_Longitude=Position[PosIdx].Longitude;
|
|
GPS_GeoidSepar=Position[PosIdx].GeoidSeparation;
|
|
GPS_LatCosine=Position[PosIdx].LatitudeCosine;
|
|
// GPS_FreqPlan=Position[PosIdx].getFreqPlan();
|
|
if(GPS_TimeSinceLock==1) // if we just acquired the lock a moment ago
|
|
{ GPS_LockStart(); }
|
|
if(GPS_TimeSinceLock>1) // if the lock is more persistant
|
|
{ uint8_t PrevIdx=(PosIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
|
int16_t TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx]);
|
|
for( ; ; )
|
|
{ if(TimeDiff>=95) break;
|
|
uint8_t PrevIdx2=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
|
if(PrevIdx2==PosIdx) break;
|
|
if(!Position[PrevIdx2].isValid()) break;
|
|
TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx2]);
|
|
PrevIdx=PrevIdx2; }
|
|
TimeDiff=Position[PosIdx].calcDifferences(Position[PrevIdx]);
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "calcDiff() => ");
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx);
|
|
Format_String(CONS_UART_Write, "->");
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)PrevIdx);
|
|
CONS_UART_Write(' ');
|
|
Format_SignDec(CONS_UART_Write, TimeDiff, 3, 2);
|
|
Format_String(CONS_UART_Write, "s\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
LED_PCB_Flash(100); }
|
|
}
|
|
else // complete but no valid lock
|
|
{ if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; }
|
|
}
|
|
// #ifdef WITH_MAVLINK
|
|
// static MAV_GPS_RAW_INT MAV_Position;
|
|
// Position[PosIdx].Encode(MAV_Position);
|
|
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
// MAV_RxMsg::Send(sizeof(MAV_Position), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_GPS_RAW_INT, (const uint8_t *)&MAV_Position, CONS_UART_Write);
|
|
// xSemaphoreGive(CONS_Mutex);
|
|
// #endif
|
|
}
|
|
else // posiiton not complete, no GPS lock
|
|
{ if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; }
|
|
}
|
|
uint8_t NextPosIdx = (PosIdx+1)&PosPipeIdxMask; // next position to be recorded
|
|
if( Position[PosIdx].isTimeValid() && Position[NextPosIdx].isTimeValid() )
|
|
{ int16_t Period = Position[PosIdx].calcTimeDiff(Position[NextPosIdx]);
|
|
if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1);
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write,"GPS");
|
|
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].Sec, 2);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].FracSec, 2);
|
|
Format_String(CONS_UART_Write, "s ");
|
|
Format_SignDec(CONS_UART_Write, Period, 3, 2);
|
|
Format_String(CONS_UART_Write, "s\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
|
|
}
|
|
Position[NextPosIdx].Clear(); // clear the next position
|
|
// int8_t Sec = Position[PosIdx].Sec; //
|
|
// Sec++; if(Sec>=60) Sec=0;
|
|
// Position[NextPosIdx].Sec=Sec; // set the correct time for the next position
|
|
Position[NextPosIdx].copyTime(Position[PosIdx]); // copy time from current position
|
|
Position[NextPosIdx].incrTime(); // increment time by 1 sec
|
|
// Position[NextPosIdx].copyDate(Position[PosIdx]);
|
|
PosIdx=NextPosIdx; // advance the index
|
|
}
|
|
|
|
static void GPS_BurstEnd(void) // when GPS stops sending the data on the serial port
|
|
{ }
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac) // return GPS position closest to the given Sec.Frac
|
|
{ int16_t TargetTime = Frac+(int16_t)Sec*100;
|
|
BestIdx=0; BestRes=0x7FFF;
|
|
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)
|
|
{ GPS_Position *Pos=Position+Idx;
|
|
if(!Pos->isReady) continue;
|
|
int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100);
|
|
if(Diff<(-3000)) Diff+=6000;
|
|
else if(Diff>3000) Diff-=6000;
|
|
if(fabs(Diff)<fabs(BestRes)) { BestRes=Diff; BestIdx=Idx; }
|
|
}
|
|
return BestRes==0x7FFF ? 0:Position+BestIdx; }
|
|
|
|
GPS_Position *GPS_getPosition(void) // return most recent GPS_Position which has time/position data
|
|
{ uint8_t PrevIdx=PosIdx;
|
|
GPS_Position *PrevPos = Position+PrevIdx;
|
|
if(PrevPos->isReady) return PrevPos;
|
|
PrevIdx=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
|
PrevPos = Position+PrevIdx;
|
|
if(PrevPos->isReady) return PrevPos;
|
|
return 0; }
|
|
|
|
GPS_Position *GPS_getPosition(int8_t Sec) // return the GPS_Position which corresponds to given Sec (may be incomplete and not valid)
|
|
{ for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)
|
|
{ int8_t PosSec = Position[Idx].Sec; if(Position[Idx].FracSec>=50) { PosSec++; if(PosSec>=60) PosSec-=60; }
|
|
if(Sec==PosSec) return Position+Idx; }
|
|
return 0; }
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
static void GPS_NMEA(void) // when GPS gets a correct NMEA sentence
|
|
{ GPS_Status.NMEA=1;
|
|
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
|
LED_PCB_Flash(2); // Flash the LED for 2 ms
|
|
Position[PosIdx].ReadNMEA(NMEA); // read position elements from NMEA
|
|
if(NMEA.isGxRMC()) GPS_Burst.GxRMC=1;
|
|
if(NMEA.isGxGGA()) GPS_Burst.GxGGA=1;
|
|
if(NMEA.isGxGSA()) GPS_Burst.GxGSA=1;
|
|
if(Button_SleepRequest)
|
|
{
|
|
#ifdef WITH_GPS_MTK
|
|
#ifdef WITH_GPS_ENABLE
|
|
GPS_DISABLE();
|
|
#endif
|
|
Format_String(GPS_UART_Write, /* "$PMTK161,0*28\r\n", */ "$PMTK225,4*2F\r\n", 15, 0); // 225 or "$PMTK161,0*28\r\n" request to the GPS to enter sleep
|
|
#endif
|
|
}
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
Format_String(CONS_UART_Write, " -> ");
|
|
Format_Bytes(CONS_UART_Write, NMEA.Data, 6);
|
|
CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, GPS_Burst.Flags);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
#ifndef WITH_GPS_NMEA_PASS
|
|
// these NMEA from GPS we want to pass to the console
|
|
if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isGPTXT() )
|
|
// we would need to patch the GGA here for the GPS which does not calc. nor correct for GeoidSepar
|
|
#endif
|
|
{ // if(CONS_UART_Free()>=128)
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, (const char *)NMEA.Data, 0, NMEA.Len);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex); }
|
|
#ifdef WITH_SDLOG
|
|
if(Log_Free()>=128)
|
|
{ xSemaphoreTake(Log_Mutex, portMAX_DELAY);
|
|
Format_String(Log_Write, (const char *)NMEA.Data, 0, NMEA.Len);
|
|
Log_Write('\n');
|
|
xSemaphoreGive(Log_Mutex); }
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#ifdef WITH_GPS_UBX
|
|
#ifdef DEBUG_PRINT
|
|
static void DumpUBX(void)
|
|
{ Format_String(CONS_UART_Write, "UBX: ");
|
|
Format_UnsDec(CONS_UART_Write, xTaskGetTickCount(), 6, 3);
|
|
CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, UBX.Class);
|
|
CONS_UART_Write(':'); Format_Hex(CONS_UART_Write, UBX.ID);
|
|
CONS_UART_Write('_'); Format_UnsDec(CONS_UART_Write, (uint16_t)UBX.Bytes);
|
|
for(uint8_t Idx=0; Idx<UBX.Bytes; Idx++)
|
|
{ CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, UBX.Byte[Idx]); }
|
|
Format_String(CONS_UART_Write, "\n"); }
|
|
#endif // DEBUG_PRINT
|
|
|
|
static void GPS_UBX(void) // when GPS gets an UBX packet
|
|
{ GPS_Status.UBX=1;
|
|
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
|
LED_PCB_Flash(2);
|
|
// DumpUBX();
|
|
// Position[PosIdx].ReadUBX(UBX);
|
|
#ifdef WITH_GPS_UBX_PASS
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY); // send ther UBX packet to the console
|
|
UBX.Send(CONS_UART_Write);
|
|
// DumpUBX();
|
|
// Format_String(CONS_UART_Write, "UBX");
|
|
// Format_Hex(CONS_UART_Write, UBX.Class);
|
|
// Format_Hex(CONS_UART_Write, UBX.ID);
|
|
xSemaphoreGive(CONS_Mutex); }
|
|
#endif
|
|
#ifdef WITH_GPS_CONFIG
|
|
if(UBX.isCFG_PRT()) // if port configuration
|
|
{ class UBX_CFG_PRT *CFG = (class UBX_CFG_PRT *)UBX.Word; // create pointer to the packet content
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskGPS: CFG_PRT\n");
|
|
DumpUBX();
|
|
Format_Hex(CONS_UART_Write, CFG->portID);
|
|
CONS_UART_Write(':');
|
|
Format_UnsDec(CONS_UART_Write, CFG->baudRate);
|
|
Format_String(CONS_UART_Write, "bps\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
if(CFG->baudRate==GPS_TargetBaudRate) GPS_Status.BaudConfig=1; // if baudrate same as our target then declare the baud config is done
|
|
else // otherwise use the received packet as the template
|
|
{ CFG->baudRate=GPS_TargetBaudRate; // set the baudrate to our target
|
|
CFG->outProtoMask|=0x02; // enable NMEA protocol
|
|
UBX.RecalcCheck(); // reclaculate the check sum
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, GPS_TargetBaudRate);
|
|
Format_String(CONS_UART_Write, "bps\n");
|
|
DumpUBX();
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
UBX.Send(GPS_UART_Write); // send this UBX packet to the GPS
|
|
}
|
|
}
|
|
if(UBX.isCFG_NAV5()) // Navigation config
|
|
{ class UBX_CFG_NAV5 *CFG = (class UBX_CFG_NAV5 *)UBX.Word;
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskGPS: CFG_NAV5 ");
|
|
Format_Hex(CONS_UART_Write, CFG->dynModel);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
if(CFG->dynModel==GPS_TargetDynModel) GPS_Status.ModeConfig=1; // dynamic model = 6 => Airborne with >1g acceleration
|
|
else
|
|
{ CFG->dynModel=GPS_TargetDynModel; CFG->mask = 0x01; //
|
|
UBX.RecalcCheck(); // reclaculate the check sum
|
|
UBX.Send(GPS_UART_Write); // send this UBX packet
|
|
}
|
|
}
|
|
#ifdef DEBUG_PRINT
|
|
if(UBX.isACK())
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskGPS: ACK_ ");
|
|
Format_Hex(CONS_UART_Write, UBX.ID);
|
|
CONS_UART_Write(' ');
|
|
Format_Hex(CONS_UART_Write, UBX.Byte[0]);
|
|
CONS_UART_Write(':');
|
|
Format_Hex(CONS_UART_Write, UBX.Byte[1]);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
/*
|
|
if(UBX.Byte[0]==0x06 && UBX.Byte[1]==0x00 && UBX.ID==0) // negative ACK to CFG-PRT
|
|
{ static char GPS_Cmd[36];
|
|
strcpy(GPS_Cmd, "$PUBX,41,1,0007,0003,"); // $PUBX command to change the baud rate
|
|
uint8_t Len = strlen(GPS_Cmd);
|
|
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
|
GPS_Cmd[Len++]=','; GPS_Cmd[Len++]='0';
|
|
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
|
GPS_Cmd[Len++]='\n';
|
|
GPS_Cmd[Len]=0;
|
|
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
|
Format_String(CONS_UART_Write, GPS_Cmd, Len, 0);
|
|
}
|
|
*/
|
|
/*
|
|
{ UBX_CFG_PRT Cmd =
|
|
{ portID:1, // 0 = I2C, 1 = UART1, 2 = UART2, 3 = USB, 4 = SPI
|
|
reserved0:0,
|
|
txReady:0,
|
|
mode:0x08D0, // 00 10x x 11 x 1 xxxx => 0x08D0
|
|
baudRate:GPS_TargetBaudRate, // [bps]
|
|
inProtoMask:7, // bit 0:UBX, bit 1:NMEA
|
|
outProtoMask:3, // bit 0:UBX, bit 1:NMEA
|
|
reserved4:0,
|
|
reserved5:0,
|
|
} ;
|
|
UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write, (uint8_t*)&Cmd, sizeof(Cmd));
|
|
}
|
|
*/
|
|
}
|
|
#endif
|
|
#endif // WITH_GPS_CONFIG
|
|
}
|
|
#endif // WITH_GPS_UBX
|
|
|
|
#ifdef WITH_MAVLINK
|
|
static int64_t MAV_TimeOfs_ms=0; // [ms] diff. between UTC time and boot time reported in MAV messages
|
|
|
|
static uint64_t MAV_getUnixTime(void) // [ms] extract time from the MAVlink message
|
|
{ int32_t TimeCorr_ms = (int32_t)Parameters.TimeCorr*1000; // [ms] apparently ArduPilot needs some time correction, as it "manually" converts from GPS to UTC time
|
|
uint8_t MsgID = MAV.getMsgID();
|
|
if(MsgID==MAV_ID_SYSTEM_TIME) return ((const MAV_SYSTEM_TIME *)MAV.getPayload())->time_unix_usec/1000 + TimeCorr_ms;
|
|
if(MsgID==MAV_ID_GLOBAL_POSITION_INT) return ((const MAV_GLOBAL_POSITION_INT *)MAV.getPayload())->time_boot_ms + MAV_TimeOfs_ms;
|
|
if(MsgID==MAV_ID_SCALED_PRESSURE) return ((const MAV_SCALED_PRESSURE *)MAV.getPayload())->time_boot_ms + MAV_TimeOfs_ms;
|
|
uint64_t UnixTime_ms = 0;
|
|
// if(MsgID==MAV_ID_RAW_IMU) UnixTime_ms = ((const MAV_RAW_IMU *)MAV.getPayload())->time_usec/1000;
|
|
if(MsgID==MAV_ID_GPS_RAW_INT) UnixTime_ms = ((const MAV_GPS_RAW_INT *)MAV.getPayload())->time_usec/1000;
|
|
if(UnixTime_ms==0) return UnixTime_ms;
|
|
if(UnixTime_ms<1000000000000) UnixTime_ms += MAV_TimeOfs_ms;
|
|
else UnixTime_ms += TimeCorr_ms;
|
|
return UnixTime_ms; }
|
|
|
|
static void GPS_MAV(void) // when GPS gets an MAV packet
|
|
{ TickType_t TickCount=xTaskGetTickCount();
|
|
GPS_Status.MAV=1;
|
|
LED_PCB_Flash(2);
|
|
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
|
uint8_t MsgID = MAV.getMsgID();
|
|
uint64_t UnixTime_ms = MAV_getUnixTime(); // get the time from the MAVlink message
|
|
if( (MsgID!=MAV_ID_SYSTEM_TIME) && UnixTime_ms)
|
|
{ if(Position[PosIdx].hasTime)
|
|
{ uint64_t PrevUnixTime_ms = Position[PosIdx].getUnixTime_ms();
|
|
int32_t TimeDiff_ms = UnixTime_ms-PrevUnixTime_ms;
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_TimeDiff: ");
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)MsgID, 3); CONS_UART_Write(' ');
|
|
Format_SignDec(CONS_UART_Write, TimeDiff_ms, 3);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
if(TimeDiff_ms>GPS_BurstTimeout) GPS_BurstComplete();
|
|
}
|
|
}
|
|
if(MsgID==MAV_ID_HEARTBEAT)
|
|
{ const MAV_HEARTBEAT *Heartbeat = (const MAV_HEARTBEAT *)MAV.getPayload();
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_HEARTBEAT: ");
|
|
Format_Hex(CONS_UART_Write, Heartbeat->system_status);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
} else if(MsgID==MAV_ID_SYSTEM_TIME)
|
|
{ const MAV_SYSTEM_TIME *SysTime = (const MAV_SYSTEM_TIME *)MAV.getPayload();
|
|
uint32_t UnixTime = UnixTime_ms/1000; // [ s] Unix Time
|
|
uint32_t UnixFrac = UnixTime_ms-(uint64_t)UnixTime*1000; // [ms] Second fraction of the Unix time
|
|
MAV_TimeOfs_ms=UnixTime_ms-SysTime->time_boot_ms; // [ms] difference between the Unix Time and the Ardupilot time-since-boot
|
|
TimeSync_SoftPPS(TickCount-UnixFrac, UnixTime, 70);
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_SYSTEM_TIME: ");
|
|
Format_UnsDec(CONS_UART_Write, UnixTime, 10);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, UnixFrac, 3);
|
|
CONS_UART_Write(' ');
|
|
Format_SignDec(CONS_UART_Write, MAV_TimeOfs_ms, 13, 3);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
} else if(MsgID==MAV_ID_GLOBAL_POSITION_INT) // position based on GPS and inertial sensors
|
|
{ const MAV_GLOBAL_POSITION_INT *Pos = (const MAV_GLOBAL_POSITION_INT *)MAV.getPayload();
|
|
Position[PosIdx].Read(Pos, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure
|
|
#ifdef DEBUG_PRINT
|
|
Position[PosIdx].PrintLine(Line);
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_GLOBAL_POSITION_INT: ");
|
|
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
|
Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
|
Format_String(CONS_UART_Write, Line);
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
} else if(MsgID==MAV_ID_GPS_RAW_INT) // position form the GPS
|
|
{ const MAV_GPS_RAW_INT *RawGPS = (const MAV_GPS_RAW_INT *)MAV.getPayload();
|
|
Position[PosIdx].Read(RawGPS, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure
|
|
#ifdef DEBUG_PRINT
|
|
Position[PosIdx].PrintLine(Line);
|
|
uint32_t UnixTime = (UnixTime_ms+500)/1000;
|
|
int32_t TimeDiff = (int64_t)UnixTime_ms-(int64_t)UnixTime*1000;
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_GPS_RAW_INT: ");
|
|
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
|
CONS_UART_Write(' ');
|
|
Format_SignDec(CONS_UART_Write, TimeDiff, 4, 3);
|
|
CONS_UART_Write(abs(TimeDiff)<250 ? '*':' ');
|
|
Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
|
Format_String(CONS_UART_Write, Line);
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
} else if(MsgID==MAV_ID_SCALED_PRESSURE)
|
|
{ const MAV_SCALED_PRESSURE *Press = (const MAV_SCALED_PRESSURE *)MAV.getPayload();
|
|
// uint64_t UnixTime_ms = Press->time_boot_ms + MAV_TimeOfs_ms;
|
|
Position[PosIdx].Read(Press, UnixTime_ms);
|
|
#ifdef DEBUG_PRINT
|
|
Position[PosIdx].PrintLine(Line);
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_SCALED_PRESSURE: ");
|
|
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
|
Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
|
Format_String(CONS_UART_Write, Line);
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
} else if(MsgID==MAV_ID_SYS_STATUS)
|
|
{ const MAV_SYS_STATUS *Status = (const MAV_SYS_STATUS *)MAV.getPayload();
|
|
#ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV_SYS_STATUS: ");
|
|
Format_UnsDec(CONS_UART_Write, Status->battery_voltage, 4, 3);
|
|
Format_String(CONS_UART_Write, "V ");
|
|
Format_SignDec(CONS_UART_Write, Status->battery_current, 3, 2);
|
|
Format_String(CONS_UART_Write, "A\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
#endif
|
|
// } else if(MsgID==MAV_ID_STATUSTEXT)
|
|
// {
|
|
}
|
|
#ifdef DEBUG_PRINT
|
|
else
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "MAV: MsgID=");
|
|
Format_UnsDec(CONS_UART_Write, (uint16_t)MAV.getMsgID(), 3);
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
}
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
// Baud setting for SIRF GPS:
|
|
// 9600/8/N/1 $PSRF100,1,9600,8,1,0*0D<cr><lf>
|
|
// 19200/8/N/1 $PSRF100,1,19200,8,1,0*38<cr><lf>
|
|
// 38400/8/N/1 $PSRF100,1,38400,8,1,0*3D<cr><lf>
|
|
// $PSRF100,1,57600,8,1,0*36
|
|
// $PSRF100,1,115200,8,1,0*05
|
|
|
|
// static const char *SiRF_SetBaudrate_57600 = "$PSRF100,1,57600,8,1,0*36\r\n";
|
|
// static const char *SiRF_SetBaudrate_115200 = "$PSRF100,1,115200,8,1,0*05\r\n";
|
|
|
|
|
|
// Baud setting for MTK GPS:
|
|
// $PMTK251,38400*27<CR><LF>
|
|
// $PMTK251,57600*2C<CR><LF>
|
|
// $PMTK251,115200*1F<CR><LF>
|
|
|
|
// Baud setting for UBX GPS:
|
|
// $PUBX,41,1,0007,0003,9600,0*10<CR><LF>
|
|
// $PUBX,41,1,0007,0003,38400,0*20<CR><LF>
|
|
|
|
// static const char *MTK_SetBaudrate_115200 = "$PMTK251,115200*1F\r\n";
|
|
|
|
|
|
// Baud setting for UBX GPS:
|
|
// "$PUBX,41,1,0003,0001,19200,0*23\r\n"
|
|
// "$PUBX,41,1,0003,0001,38400,0*26\r\n"
|
|
// "$PUBX,41,1,0003,0001,57600,0*2D\r\n"
|
|
// static const char *UBX_SetBaudrate_115200 = "$PUBX,41,1,0003,0001,115200,0*1E\r\n";
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
#endif
|
|
void vTaskGPS(void* pvParameters)
|
|
{
|
|
GPS_Status.Flags = 0;
|
|
|
|
// PPS_TickCount=0;
|
|
Burst_TickCount=0;
|
|
|
|
vTaskDelay(5);
|
|
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskGPS:");
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
|
|
GPS_Burst.Flags=0;
|
|
bool PPS=0;
|
|
int LineIdle=0; // [ms] counts idle time for the GPS data
|
|
int NoValidData=0; // [ms] count time without valid data (to decide to change baudrate)
|
|
NMEA.Clear();
|
|
#ifdef WITH_GPS_UBX
|
|
UBX.Clear(); // scans GPS input for NMEA and UBX frames
|
|
#endif
|
|
#ifdef WITH_MAVLINK
|
|
MAV.Clear();
|
|
#endif
|
|
for(uint8_t Idx=0; Idx<4; Idx++)
|
|
Position[Idx].Clear();
|
|
PosIdx=0;
|
|
|
|
TickType_t RefTick = xTaskGetTickCount();
|
|
for( ; ; ) // main task loop: every milisecond (RTOS time tick)
|
|
{ vTaskDelay(1); // wait for the next time tick (but apparently it can wait more than one OS tick)
|
|
TickType_t NewTick = xTaskGetTickCount();
|
|
TickType_t Delta = NewTick-RefTick;
|
|
RefTick = NewTick;
|
|
/*
|
|
#ifdef DEBUG_PRINT
|
|
if(Delta>1)
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time(RefTick)%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(RefTick),3);
|
|
Format_String(CONS_UART_Write, " -> ");
|
|
Format_UnsDec(CONS_UART_Write, Delta);
|
|
Format_String(CONS_UART_Write, "t\n");
|
|
xSemaphoreGive(CONS_Mutex); }
|
|
#endif
|
|
*/
|
|
#ifdef WITH_GPS_PPS
|
|
if(GPS_PPS_isOn()) { if(!PPS) { PPS=1; GPS_PPS_On(); } } // monitor GPS PPS signal
|
|
else { if( PPS) { PPS=0; GPS_PPS_Off(); } } // and call handling calls
|
|
#endif
|
|
LineIdle+=Delta; // count idle time
|
|
NoValidData+=Delta; // count time without any valid NMEA nor UBX packet
|
|
uint16_t Bytes=0;
|
|
uint16_t MaxBytesPerTick = 1+(GPS_getBaudRate()+2500)/5000;
|
|
for( ; ; ) // loop over bytes in the GPS UART buffer
|
|
{ uint8_t Byte; int Err=GPS_UART_Read(Byte); if(Err<=0) break; // get Byte from serial port, if no bytes then break this loop
|
|
Bytes++;
|
|
LineIdle=0; // if there was a byte: restart idle counting
|
|
NMEA.ProcessByte(Byte); // process through the NMEA interpreter
|
|
#ifdef WITH_GPS_UBX
|
|
UBX.ProcessByte(Byte);
|
|
#endif
|
|
#ifdef WITH_MAVLINK
|
|
MAV.ProcessByte(Byte);
|
|
#endif
|
|
if(NMEA.isComplete()) // NMEA completely received ?
|
|
{ if(NMEA.isChecked()) { GPS_NMEA(); NoValidData=0; } // NMEA check sum is correct ?
|
|
NMEA.Clear(); break; }
|
|
#ifdef WITH_GPS_UBX
|
|
if(UBX.isComplete()) { GPS_UBX(); NoValidData=0; UBX.Clear(); break; }
|
|
#endif
|
|
#ifdef WITH_MAVLINK
|
|
if(MAV.isComplete()) { GPS_MAV(); NoValidData=0; MAV.Clear(); break; }
|
|
#endif
|
|
if(Bytes>=MaxBytesPerTick) break;
|
|
}
|
|
/*
|
|
#ifdef DEBUG_PRINT
|
|
if(Bytes)
|
|
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time(RefTick)%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(RefTick),3);
|
|
Format_String(CONS_UART_Write, "..");
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
|
CONS_UART_Write('.');
|
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
Format_String(CONS_UART_Write, " -> ");
|
|
Format_UnsDec(CONS_UART_Write, Bytes);
|
|
Format_String(CONS_UART_Write, "B\n");
|
|
xSemaphoreGive(CONS_Mutex); }
|
|
#endif
|
|
*/
|
|
if(LineIdle==0) // if any bytes were received ?
|
|
{ if(!GPS_Burst.Active) GPS_BurstStart(); // burst started
|
|
GPS_Burst.Active=1;
|
|
if( (!GPS_Burst.Complete) && (GPS_Burst.GxGGA && GPS_Burst.GxRMC && GPS_Burst.GxGSA) )
|
|
{ GPS_Burst.Complete=1; GPS_BurstComplete(); }
|
|
}
|
|
else if(LineIdle>=GPS_BurstTimeout) // if GPS sends no more data for 10 time ticks
|
|
{ if(GPS_Burst.Active) // if still in burst
|
|
{ if(!GPS_Burst.Complete) GPS_BurstComplete();
|
|
GPS_BurstEnd(); } // burst just ended
|
|
else if(LineIdle>=1500) // if idle for more than 1.5 sec
|
|
{ GPS_Status.Flags=0; }
|
|
GPS_Burst.Flags=0;
|
|
}
|
|
|
|
if(NoValidData>=2000) // if no valid data from GPS for 1sec
|
|
{ GPS_Status.Flags=0; GPS_Burst.Flags=0; // assume GPS state is unknown
|
|
uint32_t NewBaudRate = GPS_nextBaudRate(); // switch to the next baud rate
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskGPS: ");
|
|
Format_UnsDec(CONS_UART_Write, NewBaudRate);
|
|
Format_String(CONS_UART_Write, "bps\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
GPS_UART_SetBaudrate(NewBaudRate);
|
|
NoValidData=0;
|
|
}
|
|
}
|
|
}
|