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:
parent
46150ad52d
commit
4d888e438b
8 changed files with 207 additions and 183 deletions
|
@ -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);
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue