diff --git a/CHANGELOG.md b/CHANGELOG.md index aa609dec..f6347370 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,11 +28,26 @@ will consitute of a breaking change warranting a new major release: - All pointing laws are now allowed to use the `MEKF` per default. - Changed limits in `PWR Controller`. - PUS time service: Now dumps the time before and after relative timeshift or setting absolute time +- The `GPS Controller` does not set itself to `OFF` anymore, if it has not detected a valid fix for + some time. Instead it attempts to reset both GNSS devices once. +- The maximum time to reach a fix is shortened from 30min to 15min. +- The time the reset pin of the GNSS devices is pulled is prolonged from 5ms to 10s. +- A `GPS FIX HAS CHANGED` is now only triggered if no fix change has been detected within the past + 2min. This means, this event might be thrown with a 2min delay. It is instantly thrown, if the mode + of the controller is changed. As arguments it now displays the new fix and the numer of fix changes + missed. +- The number of satellites seen and used is reset to 0, in case they are set to invalid. +- Altitude, latitude and longitude messages are not checked anymore, in case the mode message was + already invalid. ## Added - PUS timeservice relative timeshift. +## Fixed + +- Fixed wrong order in quaternion multiplication for computation of the error quaternion. + # [v7.7.4] 2024-03-21 ## Changed diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 1f6fdec6..30eff4ec 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -2150,8 +2150,7 @@ ReturnValue_t CoreController::initClockFromTimeFile() { std::string fileName = currMntPrefix + BACKUP_TIME_FILE; std::error_code e; if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e) and - ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or - not utility::timeSanityCheck())) { + ((gpsFix == FixMode::NOT_SEEN) or not utility::timeSanityCheck())) { ifstream timeFile(fileName); string nextWord; getline(timeFile, nextWord); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index 1b1f9853..6827a392 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include @@ -211,7 +211,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t MUTEX_TIMEOUT = 20; bool enableHkSet = false; - GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::UNKNOWN; + GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::NOT_SEEN; // States for SD state machine, which is used in non-blocking mode enum class SdStates { diff --git a/bsp_q7s/objectFactory.cpp b/bsp_q7s/objectFactory.cpp index c0dc489a..0f61a51d 100644 --- a/bsp_q7s/objectFactory.cpp +++ b/bsp_q7s/objectFactory.cpp @@ -510,7 +510,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* debugGps = true; #endif RESET_ARGS_GNSS.gpioComIF = gpioComIF; - RESET_ARGS_GNSS.waitPeriodMs = 5; + RESET_ARGS_GNSS.waitPeriodMs = 10 * 1e3; auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, enableHkSets, debugGps); gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS); diff --git a/dummies/GpsCtrlDummy.h b/dummies/GpsCtrlDummy.h index 18b990fb..b64647b1 100644 --- a/dummies/GpsCtrlDummy.h +++ b/dummies/GpsCtrlDummy.h @@ -2,7 +2,7 @@ #define DUMMIES_GPSCTRLDUMMY_H_ #include -#include +#include class GpsCtrlDummy : public ExtendedControllerBase { public: diff --git a/dummies/GpsDhbDummy.cpp b/dummies/GpsDhbDummy.cpp index 4e385ec5..73addb3c 100644 --- a/dummies/GpsDhbDummy.cpp +++ b/dummies/GpsDhbDummy.cpp @@ -1,5 +1,5 @@ #include -#include +#include GpsDhbDummy::GpsDhbDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} diff --git a/mission/acs/archive/GPSDefinitions.h b/linux/acs/GPSDefinitions.h similarity index 89% rename from mission/acs/archive/GPSDefinitions.h rename to linux/acs/GPSDefinitions.h index 67169cbc..82025a22 100644 --- a/mission/acs/archive/GPSDefinitions.h +++ b/linux/acs/GPSDefinitions.h @@ -7,15 +7,19 @@ namespace GpsHyperion { -enum class FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3, UNKNOWN = 4 }; +enum FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 }; + +enum GnssChip : uint8_t { A_SIDE = 0, B_SIDE = 1 }; static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER; -//! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix +//! [EXPORT] : [COMMENT] Fix has changed. P1: New fix. P2: Missed fix changes //! 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, 1, severity::LOW); +//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. Trying to reset both GNSS +//! devices. P1: Maximum allowed time to get a fix after the GPS was switched on. +static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::MEDIUM); +//! [EXPORT] : [COMMENT] Failed to reset an GNNS Device. P1: Board-Side. +static constexpr Event RESET_FAIL = event::makeEvent(SUBSYSTEM_ID, 2, severity::HIGH); static constexpr DeviceCommandId_t GPS_REPLY = 0; static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5; @@ -53,8 +57,6 @@ static constexpr uint8_t SKYVIEW_ENTRIES = 6; static constexpr uint8_t MAX_SATELLITES = 30; -enum GpsFixModes : uint8_t { INVALID = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 }; - } // namespace GpsHyperion class GpsPrimaryDataset : public StaticLocalDataSet { diff --git a/linux/acs/GpsHyperionLinuxController.cpp b/linux/acs/GpsHyperionLinuxController.cpp index a2dbd81b..54198ca9 100644 --- a/linux/acs/GpsHyperionLinuxController.cpp +++ b/linux/acs/GpsHyperionLinuxController.cpp @@ -44,24 +44,21 @@ LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) { ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) { - if (not modeCommanded) { - if (mode == MODE_ON or mode == MODE_OFF) { - // 5h time to reach fix - *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX; - maxTimeToReachFix.resetTimer(); - modeCommanded = true; - } else if (mode == MODE_NORMAL) { - return HasModesIF::INVALID_MODE; - } + if (mode == MODE_ON) { + maxTimeToReachFix.resetTimer(); + gainedNewFix.timeOut(); + } else if (mode == MODE_NORMAL) { + return HasModesIF::INVALID_MODE; } if (mode == MODE_OFF) { + maxTimeToReachFix.timeOut(); + gainedNewFix.timeOut(); 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); - gpsSet.fixMode.value = 0; + // 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(); - modeCommanded = false; } return returnvalue::OK; } @@ -75,13 +72,16 @@ ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId, PoolReadGuard pg(&gpsSet); // Set HK entries invalid gpsSet.setValidity(false, true); - resetCallback(data, size, resetCallbackArgs); + ReturnValue_t result = resetCallback(data, size, resetCallbackArgs); + if (result != returnvalue::OK) { + return result; + } return HasActionsIF::EXECUTION_FINISHED; } return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } } - return returnvalue::OK; + return HasActionsIF::INVALID_ACTION_ID; } ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool( @@ -216,15 +216,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; } } @@ -249,27 +243,44 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) { uint8_t newFix = 0; if (modeIsSet) { // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - if (gps.fix.mode == 2 or gps.fix.mode == 3) { + if (gps.fix.mode == GpsHyperion::FixMode::FIX_2D or + gps.fix.mode == GpsHyperion::FixMode::FIX_3D) { validFix = true; + maxTimeToReachFix.resetTimer(); } newFix = gps.fix.mode; - if (newFix == 0 or newFix == 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 (gpsSet.fixMode.value != newFix) { -#if OBSW_Q7S_EM != 1 - triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix); -#endif + handleFixChangedEvent(newFix); } gpsSet.fixMode = newFix; gpsSet.fixMode.setValid(modeIsSet); + // We are supposed to be on and functioning, but no fix was found + if (not validFix) { + if (maxTimeToReachFix.hasTimedOut()) { + // Set HK entries invalid + gpsSet.setValidity(false, true); + 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); + } + } + } + } + } // 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) @@ -282,9 +293,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); } } @@ -292,22 +306,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 >= 2) { - 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 >= 2) { - 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; + } } } } @@ -316,20 +332,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 == 3) { - 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); @@ -430,3 +450,14 @@ void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool valid timeInit = true; } } + +void GpsHyperionLinuxController::handleFixChangedEvent(uint8_t newFix) { + if (gainedNewFix.hasTimedOut()) { + triggerEvent(GpsHyperion::GPS_FIX_CHANGE, newFix, fixChangeCounter); + fixChangeCounter = 0; + gainedNewFix.resetTimer(); + return; + } + fixChangeCounter++; + gainedNewFix.resetTimer(); +} diff --git a/linux/acs/GpsHyperionLinuxController.h b/linux/acs/GpsHyperionLinuxController.h index 3378ac55..b8c3ed7a 100644 --- a/linux/acs/GpsHyperionLinuxController.h +++ b/linux/acs/GpsHyperionLinuxController.h @@ -1,14 +1,13 @@ #ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_ #define MISSION_DEVICES_GPSHYPERIONHANDLER_H_ -#include +#include +#include +#include +#include +#include #include -#include "eive/eventSubsystemIds.h" -#include "fsfw/FSFW.h" -#include "fsfw/controller/ExtendedControllerBase.h" -#include "fsfw/devicehandlers/DeviceHandlerBase.h" - #ifdef FSFW_OSAL_LINUX #include #include @@ -24,8 +23,8 @@ */ class GpsHyperionLinuxController : public ExtendedControllerBase { public: - // 30 minutes - static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30; + // 15 minutes + static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 15; enum ReadModes { SHM = 0, SOCKET = 1 }; @@ -65,7 +64,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { const char* currentClientBuf = nullptr; ReadModes readMode = ReadModes::SOCKET; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); - bool modeCommanded = false; + Countdown gainedNewFix = Countdown(60 * 2 * 1000); + uint32_t fixChangeCounter = 0; bool timeInit = false; uint8_t satNotSetCounter = 0; @@ -92,6 +92,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase { // 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); + + void handleFixChangedEvent(uint8_t newFix); }; #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 2c1b3c63..07730e09 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -303,9 +303,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // First calculate error quaternion between current and target orientation without reference // quaternion double errorQuatWoRef[4] = {0, 0, 0, 0}; - QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef); + QuaternionOperations::multiply(targetQuat, currentQuat, errorQuatWoRef); // Then add rotation from reference quaternion - QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat); + QuaternionOperations::multiply(errorQuatWoRef, refQuat, errorQuat); // Keep scalar part of quaternion positive if (errorQuat[3] < 0) { VectorOperations::mulScalar(errorQuat, -1, errorQuat, 4); diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index d7da66c0..69761666 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include