From bf16d6d86a6bff1aa30d252a3d124276b975a012 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 16:36:17 +0100 Subject: [PATCH 1/3] more improvements for gps handler --- linux/devices/GpsHyperionLinuxController.cpp | 142 +++++++++++-------- mission/system/objects/ComSubsystem.h | 4 +- 2 files changed, 82 insertions(+), 64 deletions(-) diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index a6e0a1a3..a00e21ac 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 << "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; + } + 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,80 @@ 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.setValidity(false, true); - } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) { - gpsSet.setValidity(true, true); } + gpsSet.fixMode.setValid(modeIsSet); - gpsSet.satInUse.value = gps.satellites_used; - gpsSet.satInView.value = gps.satellites_visible; + bool satInfoValid = false; + if (SATELLITE_SET == (SATELLITE_SET & gps.set)) { + satInfoValid = true; + gpsSet.satInUse.value = gps.satellites_used; + gpsSet.satInView.value = gps.satellites_visible; + } + gpsSet.satInUse.setValid(satInfoValid); + gpsSet.satInView.setValid(satInfoValid); 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); - - 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); + gpsSet.longitude.setValid(longValid); 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)) { + 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); + 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 +313,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/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h index 50f34b0d..ca09a434 100644 --- a/mission/system/objects/ComSubsystem.h +++ b/mission/system/objects/ComSubsystem.h @@ -17,8 +17,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF { * @param maxNumberOfSequences * @param maxNumberOfTables * @param transmitterTimeout Maximum time the transmitter of the syrlinks - * will be - * enabled + * will + * be enabled */ ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables, uint32_t transmitterTimeout); From 8810ad3d602d96f77ad9c57ccdca8ced0b75405a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 17:03:10 +0100 Subject: [PATCH 2/3] fsfw update --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index 893b4347..174a6aa8 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 893b43472872cdc90b4d602670c8fa9750ca7407 +Subproject commit 174a6aa862ed3fb30ab66f6ce11392abd4e9ff59 From 9ae43d83f949826305e200659308885fda13b786 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 18:21:05 +0100 Subject: [PATCH 3/3] this is okay for now --- CHANGELOG.md | 5 +++++ linux/devices/GpsHyperionLinuxController.cpp | 23 +++++++++++++++----- linux/devices/GpsHyperionLinuxController.h | 2 ++ tmtc | 2 +- 4 files changed, 26 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8b1b2ed5..b078c4d7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## 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 a00e21ac..3b396504 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -195,7 +195,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { 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 " + 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; @@ -237,15 +237,25 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } gpsSet.fixMode.setValid(modeIsSet); - bool satInfoValid = false; + // 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)) { - satInfoValid = true; 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.satInUse.setValid(satInfoValid); - gpsSet.satInView.setValid(satInfoValid); + // LATLON is set for every message, no need for a counter bool latValid = false; bool longValid = false; if (LATLON_SET == (LATLON_SET & gps.set)) { @@ -270,6 +280,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.latitude.setValid(latValid); gpsSet.longitude.setValid(longValid); + // ALTITUDE is set for every message, no need for a counter bool altitudeValid = false; if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) { gpsSet.altitude.value = gps.fix.altitude; @@ -280,6 +291,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } gpsSet.altitude.setValid(altitudeValid); + // 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; @@ -287,6 +299,7 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } 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; 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 diff --git a/tmtc b/tmtc index 13014eb2..9720fcdd 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 13014eb25053368f4fb9a445788aba71ff98de19 +Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca