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

fixed gpsSol access race conditions

Browse source
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
Download patch file Download diff file Expand all files Collapse all files

39
src/main/fc/fc_msp.c
View file

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

72
src/main/io/gps.c
View file

@ -82,7 +82,13 @@ typedef struct {
// GPS public data
gpsReceiverData_t gpsState;
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
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.lon = last_lon;
gpsSol.llh.alt = last_alt;
DISABLE_STATE(GPS_FIX);
} else {
last_lat = gpsSol.llh.lat;
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)
{
static uint32_t lastUpdateMs = 0;
@ -215,16 +220,15 @@ void updateEstimatedGPSFix(void)
int32_t dt = t - lastUpdateMs;
lastUpdateMs = t;
if (STATE(GPS_FIX) || !canEstimateGPSFix()) {
DISABLE_STATE(GPS_ESTIMATED_FIX);
bool sensorHasFix = gpsSol.fixType == GPS_FIX_3D && gpsSol.numSat >= gpsConfig()->gpsMinSats;
if (sensorHasFix || !canEstimateGPSFix()) {
estimated_lat = gpsSol.llh.lat;
estimated_lon = gpsSol.llh.lon;
estimated_alt = posControl.gpsOrigin.alt + baro.BaroAlt;
return;
}
ENABLE_STATE(GPS_ESTIMATED_FIX);
gpsSol.fixType = GPS_FIX_3D;
gpsSol.hdop = 99;
gpsSol.flags.hasNewData = true;
@ -283,9 +287,26 @@ 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)
{
if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_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);
@ -297,14 +318,11 @@ void gpsProcessNewSolutionData(void)
gpsSol.flags.validEPE = false;
DISABLE_STATE(GPS_FIX);
}
}
// Set sensor as ready and available
sensorsSet(SENSOR_GPS);
processDisableGPSFix();
updateEstimatedGPSFix();
// Pass on GPS update to NAV and IMU
onNewGPSData();
@ -322,29 +340,27 @@ void gpsProcessNewSolutionData(void)
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
}
static void gpsResetSolution(void)
static void gpsResetSolution(gpsSolutionData_t* gpsSol)
{
gpsSol.eph = 9999;
gpsSol.epv = 9999;
gpsSol.numSat = 0;
gpsSol.hdop = 9999;
gpsSol->eph = 9999;
gpsSol->epv = 9999;
gpsSol->numSat = 0;
gpsSol->hdop = 9999;
gpsSol.fixType = GPS_NO_FIX;
gpsSol->fixType = GPS_NO_FIX;
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSol.flags.validMag = false;
gpsSol.flags.validEPE = false;
gpsSol.flags.validTime = false;
gpsSol->flags.validVelNE = false;
gpsSol->flags.validVelD = false;
gpsSol->flags.validEPE = false;
gpsSol->flags.validTime = false;
}
void gpsTryEstimateOnTimeout(void)
{
gpsResetSolution();
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX);
processDisableGPSFix();
updateEstimatedGPSFix();
if (STATE(GPS_ESTIMATED_FIX)) {
@ -369,7 +385,8 @@ void gpsInit(void)
gpsStats.timeouts = 0;
// Reset solution, timeout and prepare to start
gpsResetSolution();
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
gpsSetProtocolTimeout(gpsState.baseTimeoutMs);
gpsSetState(GPS_UNKNOWN);
@ -477,7 +494,8 @@ bool gpsUpdate(void)
gpsSol.fixType = GPS_NO_FIX;
// Reset solution
gpsResetSolution();
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
// Call GPS protocol reset handler
gpsProviders[gpsState.gpsConfig->provider].restart();

2
src/main/io/gps.h
View file

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

52
src/main/io/gps_fake.c
View file

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

49
src/main/io/gps_msp.c
View file

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

51
src/main/io/gps_nmea.c
View file

@ -200,45 +200,45 @@ static bool gpsNewFrameNMEA(char c)
switch (gps_frame) {
case FRAME_GGA:
frameOK = 1;
gpsSol.numSat = gps_Msg.numSat;
gpsSolDRV.numSat = gps_Msg.numSat;
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;
gpsSol.llh.lon = gps_Msg.longitude;
gpsSol.llh.alt = gps_Msg.altitude;
gpsSolDRV.llh.lat = gps_Msg.latitude;
gpsSolDRV.llh.lon = gps_Msg.longitude;
gpsSolDRV.llh.alt = gps_Msg.altitude;
// EPH/EPV are unreliable for NMEA as they are not real accuracy
gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop);
gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSol.flags.validEPE = false;
gpsSolDRV.hdop = gpsConstrainHDOP(gps_Msg.hdop);
gpsSolDRV.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSolDRV.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
gpsSolDRV.flags.validEPE = false;
}
else {
gpsSol.fixType = GPS_NO_FIX;
gpsSolDRV.fixType = GPS_NO_FIX;
}
// NMEA does not report VELNED
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSolDRV.flags.validVelNE = false;
gpsSolDRV.flags.validVelD = false;
break;
case FRAME_RMC:
gpsSol.groundSpeed = gps_Msg.speed;
gpsSol.groundCourse = gps_Msg.ground_course;
gpsSolDRV.groundSpeed = gps_Msg.speed;
gpsSolDRV.groundCourse = gps_Msg.ground_course;
// 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) {
gpsSol.time.year = (gps_Msg.date % 100) + 2000;
gpsSol.time.month = (gps_Msg.date / 100) % 100;
gpsSol.time.day = (gps_Msg.date / 10000) % 100;
gpsSol.time.hours = (gps_Msg.time / 1000000) % 100;
gpsSol.time.minutes = (gps_Msg.time / 10000) % 100;
gpsSol.time.seconds = (gps_Msg.time / 100) % 100;
gpsSol.time.millis = (gps_Msg.time & 100) * 10;
gpsSol.flags.validTime = true;
gpsSolDRV.time.year = (gps_Msg.date % 100) + 2000;
gpsSolDRV.time.month = (gps_Msg.date / 100) % 100;
gpsSolDRV.time.day = (gps_Msg.date / 10000) % 100;
gpsSolDRV.time.hours = (gps_Msg.time / 1000000) % 100;
gpsSolDRV.time.minutes = (gps_Msg.time / 10000) % 100;
gpsSolDRV.time.seconds = (gps_Msg.time / 100) % 100;
gpsSolDRV.time.millis = (gps_Msg.time & 100) * 10;
gpsSolDRV.flags.validTime = true;
}
else {
gpsSol.flags.validTime = false;
gpsSolDRV.flags.validTime = false;
}
break;
@ -276,8 +276,9 @@ STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
while (serialRxBytesWaiting(gpsState.gpsPort)) {
uint8_t newChar = serialRead(gpsState.gpsPort);
if (gpsNewFrameNMEA(newChar)) {
gpsSol.flags.validVelNE = false;
gpsSol.flags.validVelD = false;
gpsSolDRV.flags.validVelNE = false;
gpsSolDRV.flags.validVelD = false;
gpsProcessNewDriverData();
ptSemaphoreSignal(semNewDataReady);
break;
}

2
src/main/io/gps_private.h
View file

@ -57,6 +57,7 @@ typedef struct {
} gpsReceiverData_t;
extern gpsReceiverData_t gpsState;
extern gpsSolutionData_t gpsSolDRV;
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 gpsConstrainHDOP(uint32_t hdop);
void gpsProcessNewDriverData(void);
void gpsProcessNewSolutionData(void);
void gpsSetProtocolTimeout(timeMs_t timeoutMs);

105
src/main/io/gps_ublox.c
View file

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