diff --git a/CHANGELOG.md b/CHANGELOG.md index 1fdc3e1f..a3b7a3d4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,11 @@ will consitute of a breaking change warranting a new major release: - Move ACS board polling to separate worker thread. +## Fixed + +- Linux GPS handler now checks the individual `*_SET` flags when analysing the `gpsd` struct. + PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400 + # [v1.32.0] eive-tmtc: v2.16.1 diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index a6e0a1a3..3b396504 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -172,20 +172,10 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { return false; } oneShotSwitches.gpsReadFailedSwitch = true; - // did not event get mode, nothing to see. - if (MODE_SET != (MODE_SET & gps.set)) { - if (mode != MODE_OFF) { - if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) { - sif::warning - << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed " - << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl; - triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); - oneShotSwitches.cantGetFixSwitch = false; - } - // Mode is on, so do next read immediately - return true; - } - // GPS device is off anyway, so do other handling + ReturnValue_t result = handleGpsReadData(); + if (result == returnvalue::OK) { + return true; + } else { return false; } noModeSetCntr = 0; @@ -197,11 +187,26 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() { "SHM read not implemented" << std::endl; } - handleGpsReadData(); return true; } ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { + bool modeIsSet = true; + if (MODE_SET != (MODE_SET & gps.set)) { + if (mode != MODE_OFF) { + if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) { + sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed " + << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl; + triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout); + oneShotSwitches.cantGetFixSwitch = false; + } + modeIsSet = false; + } else { + // GPS device is off anyway, so do other handling + return returnvalue::FAILED; + } + } + PoolReadGuard pg(&gpsSet); if (pg.getReadResult() != returnvalue::OK) { #if FSFW_VERBOSE_LEVEL >= 1 @@ -211,66 +216,93 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } bool validFix = false; - // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - if (gps.fix.mode == 2 or gps.fix.mode == 3) { - validFix = true; - } - if (gpsSet.fixMode.value != gps.fix.mode) { - triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode); - } - gpsSet.fixMode.value = gps.fix.mode; - if (gps.fix.mode == 0 or gps.fix.mode == 1) { - if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { - // We are supposed to be on and functioning, but no fix was found - if (mode == MODE_ON or mode == MODE_NORMAL) { - mode = MODE_OFF; - } - modeCommanded = false; + if (modeIsSet) { + // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix + if (gps.fix.mode == 2 or gps.fix.mode == 3) { + validFix = true; + } + if (gpsSet.fixMode.value != gps.fix.mode) { + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode); + } + gpsSet.fixMode.value = gps.fix.mode; + if (gps.fix.mode == 0 or gps.fix.mode == 1) { + if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { + // We are supposed to be on and functioning, but no fix was found + if (mode == MODE_ON or mode == MODE_NORMAL) { + mode = MODE_OFF; + } + modeCommanded = false; + } + } + } + gpsSet.fixMode.setValid(modeIsSet); + + // Only set on specific messages, so only set a valid flag to invalid + // if not set for more than a full message set (10 messages here) + if (SATELLITE_SET == (SATELLITE_SET & gps.set)) { + gpsSet.satInUse.value = gps.satellites_used; + gpsSet.satInView.value = gps.satellites_visible; + if (not gpsSet.satInUse.isValid()) { + gpsSet.satInUse.setValid(true); + gpsSet.satInView.setValid(true); + } + satNotSetCounter = 0; + } else { + satNotSetCounter++; + if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) { + gpsSet.satInUse.setValid(false); + gpsSet.satInView.setValid(false); } - gpsSet.setValidity(false, true); - } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) { - gpsSet.setValidity(true, true); } - gpsSet.satInUse.value = gps.satellites_used; - gpsSet.satInView.value = gps.satellites_visible; - + // LATLON is set for every message, no need for a counter bool latValid = false; - if (std::isfinite(gps.fix.latitude)) { - // Negative latitude -> South direction - gpsSet.latitude.value = gps.fix.latitude; - if (gps.fix.mode >= 2) { - latValid = true; + bool longValid = false; + if (LATLON_SET == (LATLON_SET & gps.set)) { + if (std::isfinite(gps.fix.latitude)) { + // Negative latitude -> South direction + gpsSet.latitude.value = gps.fix.latitude; + // As specified in gps.h: Only valid if mode >= 2 + if (gps.fix.mode >= 2) { + latValid = true; + } + } + + if (std::isfinite(gps.fix.longitude)) { + // Negative longitude -> West direction + gpsSet.longitude.value = gps.fix.longitude; + // As specified in gps.h: Only valid if mode >= 2 + if (gps.fix.mode >= 2) { + longValid = true; + } } } gpsSet.latitude.setValid(latValid); + gpsSet.longitude.setValid(longValid); - bool longValid = false; - if (std::isfinite(gps.fix.longitude)) { - // Negative longitude -> West direction - gpsSet.longitude.value = gps.fix.longitude; - if (gps.fix.mode >= 2) { - longValid = true; - } - } - gpsSet.latitude.setValid(longValid); - + // ALTITUDE is set for every message, no need for a counter bool altitudeValid = false; - if (std::isfinite(gps.fix.altitude)) { + if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) { gpsSet.altitude.value = gps.fix.altitude; + // As specified in gps.h: Only valid if mode == 3 if (gps.fix.mode == 3) { altitudeValid = true; } } gpsSet.altitude.setValid(altitudeValid); - if (std::isfinite(gps.fix.speed)) { + // SPEED is set for every message, no need for a counter + bool speedValid = false; + if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) { gpsSet.speed.value = gps.fix.speed; - } else { - gpsSet.speed.setValid(false); + speedValid = true; } + gpsSet.speed.setValid(speedValid); + // TIME is set for every message, no need for a counter + bool timeValid = false; if (TIME_SET == (TIME_SET & gps.set)) { + timeValid = true; timeval time = {}; #if LIBGPS_VERSION_MINOR <= 17 gpsSet.unixSeconds.value = std::floor(gps.fix.time); @@ -294,15 +326,14 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.hours = timeOfDay.hour; gpsSet.minutes = timeOfDay.minute; gpsSet.seconds = timeOfDay.second; - } else { - gpsSet.unixSeconds.setValid(false); - gpsSet.year.setValid(false); - gpsSet.month.setValid(false); - gpsSet.day.setValid(false); - gpsSet.hours.setValid(false); - gpsSet.minutes.setValid(false); - gpsSet.seconds.setValid(false); } + gpsSet.unixSeconds.setValid(timeValid); + gpsSet.year.setValid(timeValid); + gpsSet.month.setValid(timeValid); + gpsSet.day.setValid(timeValid); + gpsSet.hours.setValid(timeValid); + gpsSet.minutes.setValid(timeValid); + gpsSet.seconds.setValid(timeValid); if (debugHyperionGps) { sif::info << "-- Hyperion GPS Data --" << std::endl; diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index 1e97bc29..df08d8a4 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -61,6 +61,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = false; bool timeInit = false; + uint8_t satNotSetCounter = 0; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif