diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index 6bbbcfb7..ec9cb61a 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/linux/devices/GpsHyperionLinuxController.cpp @@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ uint32_t *msToReachTheMode) { if (not modeCommanded) { if (mode == MODE_ON or mode == MODE_OFF) { - gpsNotOpenSwitch = true; // 5h time to reach fix *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX; maxTimeToReachFix.resetTimer(); @@ -44,7 +43,12 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ } } if (mode == MODE_OFF) { + PoolReadGuard pg(&gpsSet); gpsSet.setValidity(false, true); + // There can't be a fix with a device that is off. + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0); + oneShotSwitches.reset(); + modeCommanded = false; } return returnvalue::OK; } @@ -101,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) { if (not callAgainImmediately) { handleQueue(); poolManager.performHkOperation(); + TaskFactory::delayTask(250); } } // Should never be reached. @@ -113,25 +118,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() { return result; } auto openError = [&](const char *type, int error) { - if (gpsNotOpenSwitch) { - // Opening failed + // Opening failed #if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type - << " failed | Error " << error << " | " << gps_errstr(error) << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type + << " failed | Error " << error << " | " << gps_errstr(error) << std::endl; #endif - gpsNotOpenSwitch = false; - } }; if (readMode == ReadModes::SOCKET) { int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps); if (retval != 0) { openError("Socket", retval); + return ObjectManager::CHILD_INIT_FAILED; } gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr); } else if (readMode == ReadModes::SHM) { int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps); if (retval != 0) { openError("SHM", retval); + return ObjectManager::CHILD_INIT_FAILED; } } return result; @@ -145,32 +149,33 @@ void GpsHyperionLinuxController::performControlOperation() {} bool GpsHyperionLinuxController::readGpsDataFromGpsd() { auto readError = [&]() { - if (gpsReadFailedSwitch) { - gpsReadFailedSwitch = false; + if (oneShotSwitches.gpsReadFailedSwitch) { + oneShotSwitches.gpsReadFailedSwitch = false; sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | " "Error " << errno << " | " << gps_errstr(errno) << std::endl; } }; + // GPS is off, no point in reading data from GPSD. + if(mode == MODE_OFF) { + return false; + } if (readMode == ReadModes::SOCKET) { - // Perform other necessary handling if not data seen for 0.2 seconds. - if (gps_waiting(&gps, 200000)) { + // Poll the GPS. + if (gps_waiting(&gps, 0)) { if (-1 == gps_read(&gps)) { readError(); return false; } + oneShotSwitches.gpsReadFailedSwitch = true; if (MODE_SET != (MODE_SET & gps.set)) { - if (mode == MODE_ON) { - if (noModeSetCntr >= 0) { - noModeSetCntr++; - } - if (noModeSetCntr == 10) { - // TODO: Trigger event here - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " - "read for 10 consecutive reads" - << std::endl; - noModeSetCntr = -1; - } + if (mode != MODE_OFF and 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; // did not event get mode, nothing to see. return false; } @@ -198,51 +203,58 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } bool validFix = false; - static_cast(validFix); // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - int newFixMode = gps.fix.mode; - if (newFixMode == 2 or newFixMode == 3) { + if (gps.fix.mode == 2 or gps.fix.mode == 3) { validFix = true; } - if (gpsSet.fixMode.value != newFixMode) { - triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode); + if (gpsSet.fixMode.value != gps.fix.mode) { + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode); } - gpsSet.fixMode.value = newFixMode; + 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 not fix was found + // 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) { + } 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; + bool latValid = false; if (std::isfinite(gps.fix.latitude)) { // Negative latitude -> South direction gpsSet.latitude.value = gps.fix.latitude; - } else { - gpsSet.latitude.setValid(false); + if (gps.fix.mode >= 2) { + latValid = true; + } } + gpsSet.latitude.setValid(latValid); + bool longValid = false; if (std::isfinite(gps.fix.longitude)) { // Negative longitude -> West direction gpsSet.longitude.value = gps.fix.longitude; - } else { - gpsSet.longitude.setValid(false); + if (gps.fix.mode >= 2) { + longValid = true; + } } + gpsSet.latitude.setValid(longValid); + bool altitudeValid = false; if (std::isfinite(gps.fix.altitude)) { gpsSet.altitude.value = gps.fix.altitude; - } else { - gpsSet.altitude.setValid(false); + if (gps.fix.mode == 3) { + altitudeValid = true; + } } + gpsSet.altitude.setValid(altitudeValid); if (std::isfinite(gps.fix.speed)) { gpsSet.speed.value = gps.fix.speed; @@ -250,59 +262,44 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { gpsSet.speed.setValid(false); } + if (TIME_SET == (TIME_SET & gps.set)) { + timeval time = {}; #if LIBGPS_VERSION_MINOR <= 17 - gpsSet.unixSeconds.value = gps.fix.time; + gpsSet.unixSeconds.value = std::floor(gps.fix.time); + double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value; + time.tv_usec = fractionalPart * 1000.0 * 1000.0; #else - gpsSet.unixSeconds.value = gps.fix.time.tv_sec; + gpsSet.unixSeconds.value = gps.fix.time.tv_sec; + time.tv_usec = gps.fix.time.tv_nsec / 1000; #endif - timeval time = {}; - time.tv_sec = gpsSet.unixSeconds.value; -#if LIBGPS_VERSION_MINOR <= 17 - double fractionalPart = gps.fix.time - std::floor(gps.fix.time); - time.tv_usec = fractionalPart * 1000.0 * 1000.0; -#else - time.tv_usec = gps.fix.time.tv_nsec / 1000; -#endif - std::time_t t = std::time(nullptr); - if (time.tv_sec == t) { - timeIsConstantCounter++; + time.tv_sec = gpsSet.unixSeconds.value; + // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC + // and no time file available) we set it with the roughly valid time from the GPS. + // NTP might only work if the time difference between sys time and current time is not too + // large. + overwriteTimeIfNotSane(time, validFix); + Clock::TimeOfDay_t timeOfDay = {}; + Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); + gpsSet.year = timeOfDay.year; + gpsSet.month = timeOfDay.month; + gpsSet.day = timeOfDay.day; + gpsSet.hours = timeOfDay.hour; + gpsSet.minutes = timeOfDay.minute; + gpsSet.seconds = timeOfDay.second; } else { - timeIsConstantCounter = 0; - } - if (timeInit and validFix) { - if (not utility::timeSanityCheck()) { -#if OBSW_VERBOSE_LEVEL >= 1 - time_t timeRaw = time.tv_sec; - std::tm *timeTm = std::gmtime(&timeRaw); - sif::info << "Setting invalid system time from GPS data directly: " - << std::put_time(timeTm, "%c %Z") << std::endl; -#endif - // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb.. - Clock::setClock(&time); - } - timeInit = false; - } - // If the received time does not change anymore for whatever reason, do not set it here - // to avoid stale times. Also, don't do it too often often to avoid jumping times - if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) { - // Update the system time here for now. NTP seems to be unable to do so for whatever reason. - // Further tests have shown that the time seems to be set by NTPD after some time.. - // Clock::setClock(&time); - timeUpdateCd.resetTimer(); + 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); } - Clock::TimeOfDay_t timeOfDay = {}; - Clock::convertTimevalToTimeOfDay(&time, &timeOfDay); - gpsSet.year = timeOfDay.year; - gpsSet.month = timeOfDay.month; - gpsSet.day = timeOfDay.day; - gpsSet.hours = timeOfDay.hour; - gpsSet.minutes = timeOfDay.minute; - gpsSet.seconds = timeOfDay.second; if (debugHyperionGps) { sif::info << "-- Hyperion GPS Data --" << std::endl; #if LIBGPS_VERSION_MINOR <= 17 - time_t timeRaw = gps.fix.time; + time_t timeRaw = gpsSet.unixSeconds.value; #else time_t timeRaw = gps.fix.time.tv_sec; #endif @@ -325,3 +322,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { } return returnvalue::OK; } + +void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) { + if (not timeInit and validFix) { + if (not utility::timeSanityCheck()) { +#if OBSW_VERBOSE_LEVEL >= 1 + time_t timeRaw = time.tv_sec; + std::tm *timeTm = std::gmtime(&timeRaw); + sif::info << "Overwriting invalid system time from GPS data directly: " + << std::put_time(timeTm, "%c %Z") << std::endl; +#endif + // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb.. + Clock::setClock(&time); + } + timeInit = true; + } +} diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index d2dc91b6..5d4a35ff 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/linux/devices/GpsHyperionLinuxController.h @@ -58,18 +58,30 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { const char* currentClientBuf = nullptr; ReadModes readMode = ReadModes::SOCKET; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); - bool modeCommanded = true; - bool timeInit = true; - bool gpsNotOpenSwitch = true; - bool gpsReadFailedSwitch = true; + bool modeCommanded = false; + bool timeInit = false; + + struct OneShotSwitches { + void reset() { + gpsReadFailedSwitch = true; + cantGetFixSwitch = true; + } + bool gpsReadFailedSwitch = true; + bool cantGetFixSwitch = true; + + } oneShotSwitches; + bool debugHyperionGps = false; int32_t noModeSetCntr = 0; - uint32_t timeIsConstantCounter = 0; Countdown timeUpdateCd = Countdown(60); // Returns true if the function should be called again or false if other // controller handling can be done. bool readGpsDataFromGpsd(); + // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC) + // we set it with the roughly valid time from the GPS. For some reason, NTP might only work + // if the time difference between sys time and current time is not too large + void overwriteTimeIfNotSane(timeval time, bool validFix); }; #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index 0b88cfa5..2ac7781d 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER; //! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix //! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO); +//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time +//! to get a fix after the GPS was switched on. +static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW); static constexpr DeviceCommandId_t GPS_REPLY = 0; static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;