diff --git a/linux/acs/GpsHyperionLinuxController.cpp b/linux/acs/GpsHyperionLinuxController.cpp index 5fa2d344..058c68ed 100644 --- a/linux/acs/GpsHyperionLinuxController.cpp +++ b/linux/acs/GpsHyperionLinuxController.cpp @@ -54,7 +54,7 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_ gainedNewFix.timeOut(); PoolReadGuard pg(&gpsSet); gpsSet.setValidity(false, true); - // There can't be a fix with a device that is off. + // The ctrl is off, so it cannot detect the data from the devices. handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN); gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN; oneShotSwitches.reset(); @@ -215,15 +215,9 @@ 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.getTimeoutMs() / 1000 << " seconds" << std::endl; - triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs()); - oneShotSwitches.cantGetFixSwitch = false; - } modeIsSet = false; } else { - // GPS device is off anyway, so do other handling + // GPS ctrl is off anyway, so do other handling return returnvalue::FAILED; } } @@ -253,21 +247,28 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) { validFix = true; } newFix = gps.fix.mode; - if (newFix == GpsHyperion::FixMode::NOT_SEEN or newFix == GpsHyperion::FixMode::NO_FIX) { + if (not validFix) { if (maxTimeToReachFix.hasTimedOut()) { // We are supposed to be on and functioning, but no fix was found // Set HK entries invalid gpsSet.setValidity(false, true); - if (resetCallback != nullptr) { - uint8_t chip = GpsHyperion::GnssChip::A_SIDE; - ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs); - if (result != returnvalue::OK) { - triggerEvent(GpsHyperion::RESET_FAIL, chip); - } - chip = GpsHyperion::GnssChip::B_SIDE; - result = resetCallback(&chip, 1, resetCallbackArgs); - if (result != returnvalue::OK) { - triggerEvent(GpsHyperion::RESET_FAIL, chip); + if (oneShotSwitches.cantGetFixSwitch) { + sif::warning << "GpsHyperionLinuxController: No fix detected in allowed " + << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl; + triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs()); + oneShotSwitches.cantGetFixSwitch = false; + // Try resetting the devices + if (resetCallback != nullptr) { + uint8_t chip = GpsHyperion::GnssChip::A_SIDE; + ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs); + if (result != returnvalue::OK) { + triggerEvent(GpsHyperion::RESET_FAIL, chip); + } + chip = GpsHyperion::GnssChip::B_SIDE; + result = resetCallback(&chip, 1, resetCallbackArgs); + if (result != returnvalue::OK) { + triggerEvent(GpsHyperion::RESET_FAIL, chip); + } } } } @@ -290,9 +291,12 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) { } satNotSetCounter = 0; } else { - satNotSetCounter++; - if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) { + if (satNotSetCounter < 10) { + satNotSetCounter++; + } else { + gpsSet.satInUse.value = 0; gpsSet.satInUse.setValid(false); + gpsSet.satInView.value = 0; gpsSet.satInView.setValid(false); } } @@ -300,22 +304,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) { // LATLON is set for every message, no need for a counter bool latValid = false; 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 >= GpsHyperion::FixMode::FIX_2D) { - latValid = true; + if (modeIsSet) { + 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 >= GpsHyperion::FixMode::FIX_2D) { + 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 >= GpsHyperion::FixMode::FIX_2D) { - longValid = 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 >= GpsHyperion::FixMode::FIX_2D) { + longValid = true; + } } } } @@ -324,20 +330,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) { // 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; - // As specified in gps.h: Only valid if mode == 3 - if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) { - altitudeValid = true; + if (modeIsSet) { + 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 == GpsHyperion::FixMode::FIX_3D) { + altitudeValid = true; + } } } 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; - speedValid = true; + if (modeIsSet) { + if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) { + gpsSet.speed.value = gps.fix.speed; + speedValid = true; + } } gpsSet.speed.setValid(speedValid);