1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00

fixed gpsSol access race conditions

This commit is contained in:
Roman Lut 2023-05-19 18:15:09 +02:00
parent 46150ad52d
commit 4d888e438b
8 changed files with 207 additions and 183 deletions

View file

@ -3513,35 +3513,36 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (dataSize >= 14) { if (dataSize >= 14) {
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) { if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
gpsSol.fixType = sbufReadU8(src); gpsSolDRV.fixType = sbufReadU8(src);
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.flags.hasNewData = true; gpsSolDRV.flags.hasNewData = true;
gpsSol.numSat = sbufReadU8(src); gpsSolDRV.numSat = sbufReadU8(src);
if (gpsSol.fixType != GPS_NO_FIX) { if (gpsSolDRV.fixType != GPS_NO_FIX) {
gpsSol.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSol.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
gpsSol.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
gpsSol.flags.validTime = false; gpsSolDRV.flags.validTime = false;
gpsSol.llh.lat = sbufReadU32(src); gpsSolDRV.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src); gpsSolDRV.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU32(src); gpsSolDRV.llh.alt = sbufReadU32(src);
gpsSol.groundSpeed = (int16_t)sbufReadU16(src); gpsSolDRV.groundSpeed = (int16_t)sbufReadU16(src);
gpsSol.groundCourse = (int16_t)sbufReadU16(src); gpsSolDRV.groundCourse = (int16_t)sbufReadU16(src);
gpsSol.velNED[X] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[X] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Y] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[Y] = (int16_t)sbufReadU16(src);
gpsSol.velNED[Z] = (int16_t)sbufReadU16(src); gpsSolDRV.velNED[Z] = (int16_t)sbufReadU16(src);
gpsSol.eph = 100; gpsSolDRV.eph = 100;
gpsSol.epv = 100; gpsSolDRV.epv = 100;
ENABLE_STATE(GPS_FIX); ENABLE_STATE(GPS_FIX);
} else { } else {
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
} }
// Feed data to navigation // Feed data to navigation
gpsProcessNewDriverData();
gpsProcessNewSolutionData(); gpsProcessNewSolutionData();
} else { } else {
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3); sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);

View file

@ -82,7 +82,13 @@ typedef struct {
// GPS public data // GPS public data
gpsReceiverData_t gpsState; gpsReceiverData_t gpsState;
gpsStatistics_t gpsStats; gpsStatistics_t gpsStats;
gpsSolutionData_t gpsSol;
//it is not safe to access gpsSolDRV which is filled in gps thread by driver.
//gpsSolDRV can be accessed only after driver signalled that data is ready
//we copy gpsSolDRV to gpsSol, process by "Disable GPS logic condition" and "GPS Fix estimation" features
//and use it in the rest of code.
gpsSolutionData_t gpsSolDRV; //filled by driver
gpsSolutionData_t gpsSol; //used in the rest of the code
// Map gpsBaudRate_e index to baudRate_e // Map gpsBaudRate_e index to baudRate_e
baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 }; baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT] = { BAUD_115200, BAUD_57600, BAUD_38400, BAUD_19200, BAUD_9600, BAUD_230400 };
@ -195,8 +201,6 @@ void processDisableGPSFix(void)
gpsSol.llh.lat = last_lat; gpsSol.llh.lat = last_lat;
gpsSol.llh.lon = last_lon; gpsSol.llh.lon = last_lon;
gpsSol.llh.alt = last_alt; gpsSol.llh.alt = last_alt;
DISABLE_STATE(GPS_FIX);
} else { } else {
last_lat = gpsSol.llh.lat; last_lat = gpsSol.llh.lat;
last_lon = gpsSol.llh.lon; last_lon = gpsSol.llh.lon;
@ -204,6 +208,7 @@ void processDisableGPSFix(void)
} }
} }
//called after gpsSolDRV is copied to gpsSol and processed by "Disable GPS Fix logical condition"
void updateEstimatedGPSFix(void) void updateEstimatedGPSFix(void)
{ {
static uint32_t lastUpdateMs = 0; static uint32_t lastUpdateMs = 0;
@ -215,16 +220,15 @@ void updateEstimatedGPSFix(void)
int32_t dt = t - lastUpdateMs; int32_t dt = t - lastUpdateMs;
lastUpdateMs = t; lastUpdateMs = t;
if (STATE(GPS_FIX) || !canEstimateGPSFix()) { bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats;
DISABLE_STATE(GPS_ESTIMATED_FIX);
if (sensorHasFix || !canEstimateGPSFix()) {
estimated_lat = gpsSol.llh.lat; estimated_lat = gpsSol.llh.lat;
estimated_lon = gpsSol.llh.lon; estimated_lon = gpsSol.llh.lon;
estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt; estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
return; return;
} }
ENABLE_STATE(GPS_ESTIMATED_FIX);
gpsSol.fixType = GPS_FIX_3D; gpsSol.fixType = GPS_FIX_3D;
gpsSol.hdop = 99; gpsSol.hdop = 99;
gpsSol.flags.hasNewData = true; gpsSol.flags.hasNewData = true;
@ -283,28 +287,42 @@ void updateEstimatedGPSFix(void)
} }
void gpsProcessNewDriverData(void)
{
gpsSol = gpsSolDRV;
processDisableGPSFix();
updateEstimatedGPSFix();
}
//called after:
//1)driver copies gpsSolDRV to gpsSol
//2)gpsSol is processed by "Disable GPS logical switch"
//3)gpsSol is processed by GPS Fix estimator
void gpsProcessNewSolutionData(void) void gpsProcessNewSolutionData(void)
{ {
// Set GPS fix flag only if we have 3D fix if ( gpsSol.numSat == 99 ) {
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) { ENABLE_STATE(GPS_ESTIMATED_FIX);
ENABLE_STATE(GPS_FIX);
}
else {
/* When no fix available - reset flags as well */
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_ESTIMATED_FIX);
// Set GPS fix flag only if we have 3D fix
if (gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats) {
ENABLE_STATE(GPS_FIX);
}
else {
/* When no fix available - reset flags as well */
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX);
}
} }
// Set sensor as ready and available // Set sensor as ready and available
sensorsSet(SENSOR_GPS); sensorsSet(SENSOR_GPS);
processDisableGPSFix();
updateEstimatedGPSFix();
// Pass on GPS update to NAV and IMU // Pass on GPS update to NAV and IMU
onNewGPSData(); onNewGPSData();
@ -322,29 +340,27 @@ void gpsProcessNewSolutionData(void)
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat; gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
} }
static void gpsResetSolution(void) static void gpsResetSolution(gpsSolutionData_t* gpsSol)
{ {
gpsSol.eph = 9999; gpsSol->eph = 9999;
gpsSol.epv = 9999; gpsSol->epv = 9999;
gpsSol.numSat = 0; gpsSol->numSat = 0;
gpsSol.hdop = 9999; gpsSol->hdop = 9999;
gpsSol.fixType = GPS_NO_FIX; gpsSol->fixType = GPS_NO_FIX;
gpsSol.flags.validVelNE = false; gpsSol->flags.validVelNE = false;
gpsSol.flags.validVelD = false; gpsSol->flags.validVelD = false;
gpsSol.flags.validMag = false; gpsSol->flags.validEPE = false;
gpsSol.flags.validEPE = false; gpsSol->flags.validTime = false;
gpsSol.flags.validTime = false;
} }
void gpsTryEstimateOnTimeout(void) void gpsTryEstimateOnTimeout(void)
{ {
gpsResetSolution(); gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
processDisableGPSFix();
updateEstimatedGPSFix(); updateEstimatedGPSFix();
if (STATE(GPS_ESTIMATED_FIX)) { if (STATE(GPS_ESTIMATED_FIX)) {
@ -369,7 +385,8 @@ void gpsInit(void)
gpsStats.timeouts = 0; gpsStats.timeouts = 0;
// Reset solution, timeout and prepare to start // Reset solution, timeout and prepare to start
gpsResetSolution(); gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
gpsSetProtocolTimeout(gpsState.baseTimeoutMs); gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
gpsSetState(GPS_UNKNOWN); gpsSetState(GPS_UNKNOWN);
@ -477,7 +494,8 @@ bool gpsUpdate(void)
gpsSol.fixType = GPS_NO_FIX; gpsSol.fixType = GPS_NO_FIX;
// Reset solution // Reset solution
gpsResetSolution(); gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
// Call GPS protocol reset handler // Call GPS protocol reset handler
gpsProviders[gpsState.gpsConfig->provider].restart(); gpsProviders[gpsState.gpsConfig->provider].restart();

View file

@ -118,7 +118,6 @@ typedef struct gpsSolutionData_s {
bool gpsHeartbeat; // Toggle each update bool gpsHeartbeat; // Toggle each update
bool validVelNE; bool validVelNE;
bool validVelD; bool validVelD;
bool validMag;
bool validEPE; // EPH/EPV values are valid - actual accuracy bool validEPE; // EPH/EPV values are valid - actual accuracy
bool validTime; bool validTime;
} flags; } flags;
@ -127,7 +126,6 @@ typedef struct gpsSolutionData_s {
uint8_t numSat; uint8_t numSat;
gpsLocation_t llh; gpsLocation_t llh;
int16_t magData[3];
int16_t velNED[3]; int16_t velNED[3];
int16_t groundSpeed; int16_t groundSpeed;

View file

@ -62,37 +62,39 @@ void gpsFakeSet(
int16_t velNED_Z, int16_t velNED_Z,
time_t time) time_t time)
{ {
gpsSol.fixType = fixType; gpsSolDRV.fixType = fixType;
gpsSol.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100; gpsSolDRV.hdop = gpsSol.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSol.numSat = numSat; gpsSolDRV.numSat = numSat;
gpsSol.llh.lat = lat; gpsSolDRV.llh.lat = lat;
gpsSol.llh.lon = lon; gpsSolDRV.llh.lon = lon;
gpsSol.llh.alt = alt; gpsSolDRV.llh.alt = alt;
gpsSol.groundSpeed = groundSpeed; gpsSolDRV.groundSpeed = groundSpeed;
gpsSol.groundCourse = groundCourse; gpsSolDRV.groundCourse = groundCourse;
gpsSol.velNED[X] = velNED_X; gpsSolDRV.velNED[X] = velNED_X;
gpsSol.velNED[Y] = velNED_Y; gpsSolDRV.velNED[Y] = velNED_Y;
gpsSol.velNED[Z] = velNED_Z; gpsSolDRV.velNED[Z] = velNED_Z;
gpsSol.eph = 100; gpsSolDRV.eph = 100;
gpsSol.epv = 100; gpsSolDRV.epv = 100;
gpsSol.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSol.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
gpsSol.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
gpsSol.flags.hasNewData = true; gpsSolDRV.flags.hasNewData = true;
if (time) { if (time) {
struct tm* gTime = gmtime(&time); struct tm* gTime = gmtime(&time);
gpsSol.time.year = (uint16_t)(gTime->tm_year + 1900); gpsSolDRV.time.year = (uint16_t)(gTime->tm_year + 1900);
gpsSol.time.month = (uint16_t)(gTime->tm_mon + 1); gpsSolDRV.time.month = (uint16_t)(gTime->tm_mon + 1);
gpsSol.time.day = (uint8_t)gTime->tm_mday; gpsSolDRV.time.day = (uint8_t)gTime->tm_mday;
gpsSol.time.hours = (uint8_t)gTime->tm_hour; gpsSolDRV.time.hours = (uint8_t)gTime->tm_hour;
gpsSol.time.minutes = (uint8_t)gTime->tm_min; gpsSolDRV.time.minutes = (uint8_t)gTime->tm_min;
gpsSol.time.seconds = (uint8_t)gTime->tm_sec; gpsSolDRV.time.seconds = (uint8_t)gTime->tm_sec;
gpsSol.time.millis = 0; gpsSolDRV.time.millis = 0;
gpsSol.flags.validTime = gpsSol.fixType >= 3; gpsSolDRV.flags.validTime = gpsSol.fixType >= 3;
} }
gpsProcessNewDriverData();
} }
#endif #endif

View file

@ -81,33 +81,34 @@ void mspGPSReceiveNewData(const uint8_t * bufferPtr)
{ {
const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr; const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
gpsSol.fixType = gpsMapFixType(pkt->fixType); gpsSolDRV.fixType = gpsMapFixType(pkt->fixType);
gpsSol.numSat = pkt->satellitesInView; gpsSolDRV.numSat = pkt->satellitesInView;
gpsSol.llh.lon = pkt->longitude; gpsSolDRV.llh.lon = pkt->longitude;
gpsSol.llh.lat = pkt->latitude; gpsSolDRV.llh.lat = pkt->latitude;
gpsSol.llh.alt = pkt->mslAltitude; gpsSolDRV.llh.alt = pkt->mslAltitude;
gpsSol.velNED[X] = pkt->nedVelNorth; gpsSolDRV.velNED[X] = pkt->nedVelNorth;
gpsSol.velNED[Y] = pkt->nedVelEast; gpsSolDRV.velNED[Y] = pkt->nedVelEast;
gpsSol.velNED[Z] = pkt->nedVelDown; gpsSolDRV.velNED[Z] = pkt->nedVelDown;
gpsSol.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast); gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pkt->nedVelNorth, (float)pkt->nedVelEast);
gpsSol.groundCourse = pkt->groundCourse / 10; // in deg * 10 gpsSolDRV.groundCourse = pkt->groundCourse / 10; // in deg * 10
gpsSol.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10); gpsSolDRV.eph = gpsConstrainEPE(pkt->horizontalPosAccuracy / 10);
gpsSol.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10); gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
gpsSol.hdop = gpsConstrainHDOP(pkt->hdop); gpsSolDRV.hdop = gpsConstrainHDOP(pkt->hdop);
gpsSol.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSol.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
gpsSol.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
gpsSol.time.year = pkt->year; gpsSolDRV.time.year = pkt->year;
gpsSol.time.month = pkt->month; gpsSolDRV.time.month = pkt->month;
gpsSol.time.day = pkt->day; gpsSolDRV.time.day = pkt->day;
gpsSol.time.hours = pkt->hour; gpsSolDRV.time.hours = pkt->hour;
gpsSol.time.minutes = pkt->min; gpsSolDRV.time.minutes = pkt->min;
gpsSol.time.seconds = pkt->sec; gpsSolDRV.time.seconds = pkt->sec;
gpsSol.time.millis = 0; gpsSolDRV.time.millis = 0;
gpsSol.flags.validTime = (pkt->fixType >= 3); gpsSolDRV.flags.validTime = (pkt->fixType >= 3);
gpsProcessNewDriverData();
newDataReady = true; newDataReady = true;
} }
#endif #endif

View file

@ -200,45 +200,45 @@ static bool gpsNewFrameNMEA(char c)
switch (gps_frame) { switch (gps_frame) {
case FRAME_GGA: case FRAME_GGA:
frameOK = 1; frameOK = 1;
gpsSol.numSat = gps_Msg.numSat; gpsSolDRV.numSat = gps_Msg.numSat;
if (gps_Msg.fix) { if (gps_Msg.fix) {
gpsSol.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D gpsSolDRV.fixType = GPS_FIX_3D; // NMEA doesn't report fix type, assume 3D
gpsSol.llh.lat = gps_Msg.latitude; gpsSolDRV.llh.lat = gps_Msg.latitude;
gpsSol.llh.lon = gps_Msg.longitude; gpsSolDRV.llh.lon = gps_Msg.longitude;
gpsSol.llh.alt = gps_Msg.altitude; gpsSolDRV.llh.alt = gps_Msg.altitude;
// EPH/EPV are unreliable for NMEA as they are not real accuracy // EPH/EPV are unreliable for NMEA as they are not real accuracy
gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop); gpsSolDRV.hdop = gpsConstrainHDOP(gps_Msg.hdop);
gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); gpsSolDRV.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER); gpsSolDRV.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSol.flags.validEPE = false; gpsSolDRV.flags.validEPE = false;
} }
else { else {
gpsSol.fixType = GPS_NO_FIX; gpsSolDRV.fixType = GPS_NO_FIX;
} }
// NMEA does not report VELNED // NMEA does not report VELNED
gpsSol.flags.validVelNE = false; gpsSolDRV.flags.validVelNE = false;
gpsSol.flags.validVelD = false; gpsSolDRV.flags.validVelD = false;
break; break;
case FRAME_RMC: case FRAME_RMC:
gpsSol.groundSpeed = gps_Msg.speed; gpsSolDRV.groundSpeed = gps_Msg.speed;
gpsSol.groundCourse = gps_Msg.ground_course; gpsSolDRV.groundCourse = gps_Msg.ground_course;
// This check will miss 00:00:00.00, but we shouldn't care - next report will be valid // This check will miss 00:00:00.00, but we shouldn't care - next report will be valid
if (gps_Msg.date != 0 && gps_Msg.time != 0) { if (gps_Msg.date != 0 && gps_Msg.time != 0) {
gpsSol.time.year = (gps_Msg.date % 100) + 2000; gpsSolDRV.time.year = (gps_Msg.date % 100) + 2000;
gpsSol.time.month = (gps_Msg.date / 100) % 100; gpsSolDRV.time.month = (gps_Msg.date / 100) % 100;
gpsSol.time.day = (gps_Msg.date / 10000) % 100; gpsSolDRV.time.day = (gps_Msg.date / 10000) % 100;
gpsSol.time.hours = (gps_Msg.time / 1000000) % 100; gpsSolDRV.time.hours = (gps_Msg.time / 1000000) % 100;
gpsSol.time.minutes = (gps_Msg.time / 10000) % 100; gpsSolDRV.time.minutes = (gps_Msg.time / 10000) % 100;
gpsSol.time.seconds = (gps_Msg.time / 100) % 100; gpsSolDRV.time.seconds = (gps_Msg.time / 100) % 100;
gpsSol.time.millis = (gps_Msg.time & 100) * 10; gpsSolDRV.time.millis = (gps_Msg.time & 100) * 10;
gpsSol.flags.validTime = true; gpsSolDRV.flags.validTime = true;
} }
else { else {
gpsSol.flags.validTime = false; gpsSolDRV.flags.validTime = false;
} }
break; break;
@ -276,8 +276,9 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
while (serialRxBytesWaiting(gpsState.gpsPort)) { while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort); uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameNMEA(newChar)) { if (gpsNewFrameNMEA(newChar)) {
gpsSol.flags.validVelNE = false; gpsSolDRV.flags.validVelNE = false;
gpsSol.flags.validVelD = false; gpsSolDRV.flags.validVelD = false;
gpsProcessNewDriverData();
ptSemaphoreSignal(semNewDataReady); ptSemaphoreSignal(semNewDataReady);
break; break;
} }

View file

@ -57,6 +57,7 @@ typedef struct {
} gpsReceiverData_t; } gpsReceiverData_t;
extern gpsReceiverData_t gpsState; extern gpsReceiverData_t gpsState;
extern gpsSolutionData_t gpsSolDRV;
extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT]; extern baudRate_e gpsToSerialBaudRate[GPS_BAUDRATE_COUNT];
@ -66,6 +67,7 @@ extern void gpsFinalizeChangeBaud(void);
extern uint16_t gpsConstrainEPE(uint32_t epe); extern uint16_t gpsConstrainEPE(uint32_t epe);
extern uint16_t gpsConstrainHDOP(uint32_t hdop); extern uint16_t gpsConstrainHDOP(uint32_t hdop);
void gpsProcessNewDriverData(void);
void gpsProcessNewSolutionData(void); void gpsProcessNewSolutionData(void);
void gpsSetProtocolTimeout(timeMs_t timeoutMs); void gpsSetProtocolTimeout(timeMs_t timeoutMs);

View file

@ -604,84 +604,84 @@ static bool gpsParceFrameUBLOX(void)
{ {
switch (_msg_id) { switch (_msg_id) {
case MSG_POSLLH: case MSG_POSLLH:
gpsSol.llh.lon = _buffer.posllh.longitude; gpsSolDRV.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude; gpsSolDRV.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm gpsSolDRV.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
gpsSol.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10); gpsSolDRV.eph = gpsConstrainEPE(_buffer.posllh.horizontal_accuracy / 10);
gpsSol.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10); gpsSolDRV.epv = gpsConstrainEPE(_buffer.posllh.vertical_accuracy / 10);
gpsSol.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
if (next_fix_type != GPS_NO_FIX) if (next_fix_type != GPS_NO_FIX)
gpsSol.fixType = next_fix_type; gpsSolDRV.fixType = next_fix_type;
_new_position = true; _new_position = true;
break; break;
case MSG_STATUS: case MSG_STATUS:
next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type); next_fix_type = gpsMapFixType(_buffer.status.fix_status & NAV_STATUS_FIX_VALID, _buffer.status.fix_type);
if (next_fix_type == GPS_NO_FIX) if (next_fix_type == GPS_NO_FIX)
gpsSol.fixType = GPS_NO_FIX; gpsSolDRV.fixType = GPS_NO_FIX;
break; break;
case MSG_SOL: case MSG_SOL:
next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type); next_fix_type = gpsMapFixType(_buffer.solution.fix_status & NAV_STATUS_FIX_VALID, _buffer.solution.fix_type);
if (next_fix_type == GPS_NO_FIX) if (next_fix_type == GPS_NO_FIX)
gpsSol.fixType = GPS_NO_FIX; gpsSolDRV.fixType = GPS_NO_FIX;
gpsSol.numSat = _buffer.solution.satellites; gpsSolDRV.numSat = _buffer.solution.satellites;
gpsSol.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP); gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.solution.position_DOP);
break; break;
case MSG_VELNED: case MSG_VELNED:
gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s gpsSolDRV.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 gpsSolDRV.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.velNED[X] = _buffer.velned.ned_north; gpsSolDRV.velNED[X] = _buffer.velned.ned_north;
gpsSol.velNED[Y] = _buffer.velned.ned_east; gpsSolDRV.velNED[Y] = _buffer.velned.ned_east;
gpsSol.velNED[Z] = _buffer.velned.ned_down; gpsSolDRV.velNED[Z] = _buffer.velned.ned_down;
gpsSol.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSol.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
_new_speed = true; _new_speed = true;
break; break;
case MSG_TIMEUTC: case MSG_TIMEUTC:
if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) { if (UBX_VALID_GPS_DATE_TIME(_buffer.timeutc.valid)) {
gpsSol.time.year = _buffer.timeutc.year; gpsSolDRV.time.year = _buffer.timeutc.year;
gpsSol.time.month = _buffer.timeutc.month; gpsSolDRV.time.month = _buffer.timeutc.month;
gpsSol.time.day = _buffer.timeutc.day; gpsSolDRV.time.day = _buffer.timeutc.day;
gpsSol.time.hours = _buffer.timeutc.hour; gpsSolDRV.time.hours = _buffer.timeutc.hour;
gpsSol.time.minutes = _buffer.timeutc.min; gpsSolDRV.time.minutes = _buffer.timeutc.min;
gpsSol.time.seconds = _buffer.timeutc.sec; gpsSolDRV.time.seconds = _buffer.timeutc.sec;
gpsSol.time.millis = _buffer.timeutc.nano / (1000*1000); gpsSolDRV.time.millis = _buffer.timeutc.nano / (1000*1000);
gpsSol.flags.validTime = true; gpsSolDRV.flags.validTime = true;
} else { } else {
gpsSol.flags.validTime = false; gpsSolDRV.flags.validTime = false;
} }
break; break;
case MSG_PVT: case MSG_PVT:
next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type); next_fix_type = gpsMapFixType(_buffer.pvt.fix_status & NAV_STATUS_FIX_VALID, _buffer.pvt.fix_type);
gpsSol.fixType = next_fix_type; gpsSolDRV.fixType = next_fix_type;
gpsSol.llh.lon = _buffer.pvt.longitude; gpsSolDRV.llh.lon = _buffer.pvt.longitude;
gpsSol.llh.lat = _buffer.pvt.latitude; gpsSolDRV.llh.lat = _buffer.pvt.latitude;
gpsSol.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm gpsSolDRV.llh.alt = _buffer.pvt.altitude_msl / 10; //alt in cm
gpsSol.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s gpsSolDRV.velNED[X]=_buffer.pvt.ned_north / 10; // to cm/s
gpsSol.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s gpsSolDRV.velNED[Y]=_buffer.pvt.ned_east / 10; // to cm/s
gpsSol.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s gpsSolDRV.velNED[Z]=_buffer.pvt.ned_down / 10; // to cm/s
gpsSol.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s gpsSolDRV.groundSpeed = _buffer.pvt.speed_2d / 10; // to cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 gpsSolDRV.groundCourse = (uint16_t) (_buffer.pvt.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.numSat = _buffer.pvt.satellites; gpsSolDRV.numSat = _buffer.pvt.satellites;
gpsSol.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10); gpsSolDRV.eph = gpsConstrainEPE(_buffer.pvt.horizontal_accuracy / 10);
gpsSol.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10); gpsSolDRV.epv = gpsConstrainEPE(_buffer.pvt.vertical_accuracy / 10);
gpsSol.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP); gpsSolDRV.hdop = gpsConstrainHDOP(_buffer.pvt.position_DOP);
gpsSol.flags.validVelNE = true; gpsSolDRV.flags.validVelNE = true;
gpsSol.flags.validVelD = true; gpsSolDRV.flags.validVelD = true;
gpsSol.flags.validEPE = true; gpsSolDRV.flags.validEPE = true;
if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) { if (UBX_VALID_GPS_DATE_TIME(_buffer.pvt.valid)) {
gpsSol.time.year = _buffer.pvt.year; gpsSolDRV.time.year = _buffer.pvt.year;
gpsSol.time.month = _buffer.pvt.month; gpsSolDRV.time.month = _buffer.pvt.month;
gpsSol.time.day = _buffer.pvt.day; gpsSolDRV.time.day = _buffer.pvt.day;
gpsSol.time.hours = _buffer.pvt.hour; gpsSolDRV.time.hours = _buffer.pvt.hour;
gpsSol.time.minutes = _buffer.pvt.min; gpsSolDRV.time.minutes = _buffer.pvt.min;
gpsSol.time.seconds = _buffer.pvt.sec; gpsSolDRV.time.seconds = _buffer.pvt.sec;
gpsSol.time.millis = _buffer.pvt.nano / (1000*1000); gpsSolDRV.time.millis = _buffer.pvt.nano / (1000*1000);
gpsSol.flags.validTime = true; gpsSolDRV.flags.validTime = true;
} else { } else {
gpsSol.flags.validTime = false; gpsSolDRV.flags.validTime = false;
} }
_new_position = true; _new_position = true;
@ -985,6 +985,7 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
while (serialRxBytesWaiting(gpsState.gpsPort)) { while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort); uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameUBLOX(newChar)) { if (gpsNewFrameUBLOX(newChar)) {
gpsProcessNewDriverData();
ptSemaphoreSignal(semNewDataReady); ptSemaphoreSignal(semNewDataReady);
break; break;
} }