eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Merge branch 'main' into improve-dev-fdir

Browse Source
This commit is contained in:
Marius Eggert 2024-04-10 11:41:51 +02:00
commit c2d05b2045
11 changed files with 134 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
CHANGELOG.md
View File

@ -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

3
bsp_q7s/core/CoreController.cpp
View File

@ -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);

4
bsp_q7s/core/CoreController.h
View File

@ -9,7 +9,7 @@
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <fsfw_hal/linux/uio/UioMapper.h>
#include <libxiphos.h>
#include <mission/acs/archive/GPSDefinitions.h>
#include <linux/acs/GPSDefinitions.h>
#include <mission/utility/trace.h>
#include <atomic>
@ -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 {

2
bsp_q7s/objectFactory.cpp
View File

@ -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);

2
dummies/GpsCtrlDummy.h
View File

@ -2,7 +2,7 @@
#define DUMMIES_GPSCTRLDUMMY_H_
#include <fsfw/controller/ExtendedControllerBase.h>
#include <mission/acs/archive/GPSDefinitions.h>
#include <linux/acs/GPSDefinitions.h>
class GpsCtrlDummy : public ExtendedControllerBase {
public:

2
dummies/GpsDhbDummy.cpp
View File

@ -1,5 +1,5 @@
#include <dummies/GpsDhbDummy.h>
#include <mission/acs/archive/GPSDefinitions.h>
#include <linux/acs/GPSDefinitions.h>
GpsDhbDummy::GpsDhbDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
: DeviceHandlerBase(objectId, comif, comCookie) {}

16
mission/acs/archive/GPSDefinitions.h → linux/acs/GPSDefinitions.h
View File

@ -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<GpsHyperion::CORE_DATASET_ENTRIES> {

103
linux/acs/GpsHyperionLinuxController.cpp
View File

@ -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;
if (mode == MODE_ON) {
maxTimeToReachFix.resetTimer();
modeCommanded = true;
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 {
if (satNotSetCounter < 10) {
satNotSetCounter++;
if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) {
} else {
gpsSet.satInUse.value = 0;
gpsSet.satInUse.setValid(false);
gpsSet.satInView.value = 0;
gpsSet.satInView.setValid(false);
}
}
@ -292,12 +306,13 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
// LATLON is set for every message, no need for a counter
bool latValid = false;
bool longValid = false;
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 >= 2) {
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
latValid = true;
}
}
@ -306,31 +321,36 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
// 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) {
if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
longValid = true;
}
}
}
}
gpsSet.latitude.setValid(latValid);
gpsSet.longitude.setValid(longValid);
// ALTITUDE is set for every message, no need for a counter
bool altitudeValid = false;
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 == 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 (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);
// TIME is set for every message, no need for a counter
@ -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();
}

20
linux/acs/GpsHyperionLinuxController.h
View File

@ -1,14 +1,13 @@
#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_
#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_
#include <mission/acs/archive/GPSDefinitions.h>
#include <common/config/eive/eventSubsystemIds.h>
#include <fsfw/FSFW.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <linux/acs/GPSDefinitions.h>
#include <mission/utility/trace.h>
#include "eive/eventSubsystemIds.h"
#include "fsfw/FSFW.h"
#include "fsfw/controller/ExtendedControllerBase.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#ifdef FSFW_OSAL_LINUX
#include <gps.h>
#include <libgpsmm.h>
@ -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_ */

4
mission/controller/acs/Guidance.cpp
View File

@ -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<double>::mulScalar(errorQuat, -1, errorQuat, 4);

2
mission/controller/acs/SensorValues.h
View File

@ -4,7 +4,7 @@
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
#include <mission/acs/archive/GPSDefinitions.h>
#include <linux/acs/GPSDefinitions.h>
#include <mission/acs/gyroAdisHelpers.h>
#include <mission/acs/imtqHelpers.h>
#include <mission/acs/rwHelpers.h>