From bf16d6d86a6bff1aa30d252a3d124276b975a012 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Feb 2023 16:36:17 +0100 Subject: [PATCH] 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);