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

Merge remote-tracking branch 'origin/main' into relax-sus-fdir
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-09-07 15:41:59 +02:00
parent 05fbff2e6d 1b295139a0
commit f9f6ac27e8
42 changed files with 696 additions and 227 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
mission/acs/archive/GPSDefinitions.h
View File

@ -20,32 +20,47 @@ static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity
static constexpr DeviceCommandId_t GPS_REPLY = 0;
static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
static constexpr uint32_t DATASET_ID = 0;
enum SetIds : uint32_t {
CORE_DATASET,
SKYVIEW_DATASET,
};
enum GpsPoolIds : lp_id_t {
LATITUDE = 0,
LONGITUDE = 1,
ALTITUDE = 2,
SPEED = 3,
FIX_MODE = 4,
SATS_IN_USE = 5,
SATS_IN_VIEW = 6,
UNIX_SECONDS = 7,
YEAR = 8,
MONTH = 9,
DAY = 10,
HOURS = 11,
MINUTES = 12,
SECONDS = 13
LATITUDE,
LONGITUDE,
ALTITUDE,
SPEED,
FIX_MODE,
SATS_IN_USE,
SATS_IN_VIEW,
UNIX_SECONDS,
YEAR,
MONTH,
DAY,
HOURS,
MINUTES,
SECONDS,
SKYVIEW_UNIX_SECONDS,
PRN_ID,
AZIMUTH,
ELEVATION,
SIGNAL2NOISE,
USED,
};
static constexpr uint8_t CORE_DATASET_ENTRIES = 14;
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<18> {
class GpsPrimaryDataset : public StaticLocalDataSet<GpsHyperion::CORE_DATASET_ENTRIES> {
public:
GpsPrimaryDataset(object_id_t gpsId) : StaticLocalDataSet(sid_t(gpsId, GpsHyperion::DATASET_ID)) {
GpsPrimaryDataset(object_id_t gpsId)
: StaticLocalDataSet(sid_t(gpsId, GpsHyperion::CORE_DATASET)) {
setAllVariablesReadOnly();
}
@ -69,7 +84,34 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> {
friend class GpsHyperionLinuxController;
friend class GpsCtrlDummy;
GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {}
: StaticLocalDataSet(hkOwner, GpsHyperion::CORE_DATASET) {}
};
class SkyviewDataset : public StaticLocalDataSet<GpsHyperion::SKYVIEW_ENTRIES> {
public:
SkyviewDataset(object_id_t gpsId)
: StaticLocalDataSet(sid_t(gpsId, GpsHyperion::SKYVIEW_DATASET)) {
setAllVariablesReadOnly();
}
lp_var_t<double> unixSeconds =
lp_var_t<double>(sid.objectId, GpsHyperion::SKYVIEW_UNIX_SECONDS, this);
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> prn_id =
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::PRN_ID, this);
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> azimuth =
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::AZIMUTH, this);
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> elevation =
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::ELEVATION, this);
lp_vec_t<double, GpsHyperion::MAX_SATELLITES> signal2noise =
lp_vec_t<double, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::SIGNAL2NOISE, this);
lp_vec_t<uint8_t, GpsHyperion::MAX_SATELLITES> used =
lp_vec_t<uint8_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::USED, this);
private:
friend class GpsHyperionLinuxController;
friend class GpsCtrlDummy;
SkyviewDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, GpsHyperion::SKYVIEW_DATASET) {}
};
#endif /* MISSION_ACS_ARCHIVE_GPSDEFINITIONS_H_ */

9
mission/acs/defs.h
View File

@ -42,6 +42,13 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_DETUMBLE_DETERIORATED = 21,
};
enum GpsSource : uint8_t {
NONE,
GPS,
GPS_EXTRAPOLATED,
SPG4,
};
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
@ -64,6 +71,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG
//! failed.
//! P1: Missing information about magnetic field, P2: Missing information about rotational rate
static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old.
static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
extern const char* getModeStr(AcsMode mode);

66
mission/controller/AcsController.cpp
View File

@ -22,7 +22,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
mekfData(this),
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this) {}
fusedRotRateData(this),
tleData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -62,6 +63,26 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
mekfLost = false;
return HasActionsIF::EXECUTION_FINISHED;
}
case UPDATE_TLE: {
if (size != 69 * 2) {
return INVALID_PARAMETERS;
}
ReturnValue_t result = navigation.updateTle(data, data + 69);
if (result != returnvalue::OK) {
PoolReadGuard pg(&tleData);
navigation.updateTle(tleData.line1.value, tleData.line2.value);
return result;
}
{
PoolReadGuard pg(&tleData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(tleData.line1.value, data, 69);
std::memcpy(tleData.line2.value, data + 69, 69);
tleData.setValidity(true, true);
}
}
return HasActionsIF::EXECUTION_FINISHED;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
@ -146,12 +167,19 @@ void AcsController::performSafe() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -176,8 +204,7 @@ void AcsController::performSafe() {
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(),
acsParameters.safeModeControllerParameters.useMekf,
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
@ -195,7 +222,8 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false;
@ -271,12 +299,19 @@ void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -357,10 +392,17 @@ void AcsController::performPointingCtrl() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
@ -735,6 +777,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@ -758,6 +801,9 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// TLE Data
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
return returnvalue::OK;
}

9
mission/controller/AcsController.h
View File

@ -3,6 +3,7 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
@ -61,6 +62,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ParameterHelper parameterHelper;
bool tleTooOldFlag = false;
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;
@ -84,6 +86,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1;
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
static const DeviceCommandId_t UPDATE_TLE = 0x3;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
@ -207,6 +210,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> altitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
// MEKF
acsctrl::MekfData mekfData;
@ -234,6 +238,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
// TLE Dataset
acsctrl::TleData tleData;
PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};

9
mission/controller/ThermalController.cpp
View File

@ -1372,12 +1372,13 @@ void ThermalController::ctrlPlPcduBoard() {
tooHotHandler(objects::PLPCDU_HANDLER, tooHotFlags.eBandTooHotFlag);
}
// ToDo: remove one of the following 2
void ThermalController::ctrlPlocMissionBoard() {
ctrlCtx.thermalComponent = tcsCtrl::PLOCMISSION_BOARD;
sensors[0].first = sensorTemperatures.plocHeatspreader.isValid();
sensors[0].second = sensorTemperatures.plocHeatspreader.value;
sensors[1].first = sensorTemperatures.plocMissionboard.isValid();
sensors[1].second = sensorTemperatures.plocMissionboard.value;
sensors[0].first = sensorTemperatures.plocMissionboard.isValid();
sensors[0].second = sensorTemperatures.plocMissionboard.value;
sensors[1].first = sensorTemperatures.plocHeatspreader.isValid();
sensors[1].second = sensorTemperatures.plocHeatspreader.value;
sensors[2].first = sensorTemperatures.dacHeatspreader.isValid();
sensors[2].second = sensorTemperatures.dacHeatspreader.value;
numSensors = 3;

6
mission/controller/acs/AcsParameters.cpp
View File

@ -113,6 +113,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x13:
parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight);
break;
case 0x14:
parameterWrapper->set(mgmHandlingParameters.useMgm4);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -665,6 +668,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x3:
parameterWrapper->set(gpsParameters.fdirAltitude);
break;
case 0x4:
parameterWrapper->set(gpsParameters.useSpg4);
break;
default:
return INVALID_IDENTIFIER_ID;
}

14
mission/controller/acs/AcsParameters.h
View File

@ -79,7 +79,8 @@ class AcsParameters : public HasParametersIF {
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
float mgmVectorFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.85;
float mgmDerivativeFilterWeight = 0.99;
uint8_t useMgm4 = false;
} mgmHandlingParameters;
struct SusHandlingParameters {
@ -770,7 +771,7 @@ class AcsParameters : public HasParametersIF {
-0.000889232196185857, -0.00168429567131815}};
float susBrightnessThreshold = 0.7;
float susVectorFilterWeight = .85;
float susRateFilterWeight = .85;
float susRateFilterWeight = .99;
} susHandlingParameters;
struct GyrHandlingParameters {
@ -833,15 +834,15 @@ class AcsParameters : public HasParametersIF {
double k_alignGyr = 4.0e-5;
double k_parallelGyr = 3.75e-4;
double k_orthoSusMgm = 1.1e-2;
double k_alignSusMgm = 2.0e-5;
double k_parallelSusMgm = 4.4e-4;
double k_orthoSusMgm = 4.4e-3;
double k_alignSusMgm = 4.0e-5;
double k_parallelSusMgm = 3.75e-4;
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
uint8_t useMekf = false;
uint8_t useGyr = true;
uint8_t useGyr = false;
uint8_t dampingDuringEclipse = true;
float sineLimitSunRotRate = 0.24;
@ -916,6 +917,7 @@ class AcsParameters : public HasParametersIF {
double minimumFdirAltitude = 475 * 1e3; // [m]
double maximumFdirAltitude = 575 * 1e3; // [m]
double fdirAltitude = 525 * 1e3; // [m]
uint8_t useSpg4 = true;
} gpsParameters;
struct SunModelParameters {

37
mission/controller/acs/Navigation.cpp
View File

@ -44,3 +44,40 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
double position[3] = {0, 0, 0};
double velocity[3] = {0, 0, 0};
ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0);
if (result == returnvalue::OK) {
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::SPG4;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(true);
std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
gpsDataProcessed->gpsVelocity.setValid(true);
}
}
} else {
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::NONE;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(false);
std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
gpsDataProcessed->gpsVelocity.setValid(false);
}
}
}
return result;
}
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
return sgp4Propagator.initialize(line1, line2);
}

16
mission/controller/acs/Navigation.h
View File

@ -1,11 +1,13 @@
#ifndef NAVIGATION_H_
#define NAVIGATION_H_
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
#include <mission/controller/acs/SensorProcessing.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class Navigation {
public:
@ -19,10 +21,14 @@ class Navigation {
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
Sgp4Propagator sgp4Propagator;
};
#endif /* ACS_NAVIGATION_H_ */

97
mission/controller/acs/SensorProcessing.cpp
View File

@ -1,19 +1,5 @@
#include "SensorProcessing.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timevalOperations.h>
#include <math.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "Igrf13Model.h"
#include "util/MathOperations.h"
using namespace Math;
SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {}
@ -24,21 +10,23 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
bool gpsValid = false;
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
Igrf13Model igrf13;
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
gpsValid = true;
}
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
(not mgm4valid or not mgmParameters->useMgm4)) {
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
@ -54,6 +42,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
}
}
std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot));
return;
}
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
@ -113,7 +102,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm4valid) {
if (mgm4valid and mgmParameters->useMgm4) {
float mgm4ValueUT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3); // nT to uT
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
@ -141,14 +130,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i];
mgmVecTotDerivativeValid = true;
}
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
mgmVecTotDerivativeValid = true;
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
mgmDataProcessed->mgmVecTotDerivative.isValid()) {
@ -199,8 +188,8 @@ void SensorProcessing::processSus(
double JC2000 = JD2000 / 36525.;
double meanLongitude =
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * M_PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * M_PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
@ -277,6 +266,7 @@ void SensorProcessing::processSus(
susDataProcessed->sunIjkModel.setValid(true);
}
}
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
return;
}
@ -365,13 +355,13 @@ void SensorProcessing::processSus(
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) {
for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i];
susVecTotDerivativeValid = true;
}
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
susVecTotDerivativeValid = true;
}
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
susDataProcessed->susVecTotDerivative.isValid()) {
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
@ -535,14 +525,31 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
// init variables
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2);
uint8_t gpsSource = acs::GpsSource::NONE;
// We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(gdLatitude));
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->altitude.value = altitude;
gpsDataProcessed->setValidity(true, true);
}
}
return;
} else if (validGps) {
// Transforming from Degree to Radians and calculation geocentric latitude from geodetic
gdLongitude = gpsLongitude * M_PI / 180.;
double latitudeRad = gpsLatitude * M_PI / 180.;
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR
@ -569,6 +576,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS;
}
{
PoolReadGuard pg(gpsDataProcessed);
@ -579,6 +588,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->source.value = gpsSource;
gpsDataProcessed->source.setValid(true);
}
}
}
@ -606,11 +617,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
sensorValues->gpsSet.altitude.value,
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
mgmDataProcessed);
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),

27
mission/controller/acs/SensorProcessing.h
View File

@ -1,15 +1,23 @@
#ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timevalOperations.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/Igrf13Model.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/SusConverter.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "eive/resultClassIds.h"
#include <cmath>
class SensorProcessing {
public:
@ -25,6 +33,7 @@ class SensorProcessing {
private:
static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
protected:
// short description needed for every function
@ -32,8 +41,8 @@ class SensorProcessing {
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed);
void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
bool sus1valid, const uint16_t *sus2Value, bool sus2valid,

29
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,10 +9,12 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateTotalValid,
const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) {
@ -20,7 +22,7 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
} else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateSplitValid) {
} else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
@ -95,9 +97,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle) {
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -105,8 +108,14 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun);
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
if (VectorOperations<double>::norm(rotRateParallelB, 3) != 0 and
VectorOperations<double>::norm(rotRateOrthogonalB, 3) != 0) {
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
} else {
splitRotationalRate(rotRateTotalB, sunDirB);
}
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB,

8
mission/controller/acs/control/SafeCtrl.h
View File

@ -14,7 +14,6 @@ class SafeCtrl {
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateSplitValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled);
@ -25,9 +24,10 @@ class SafeCtrl {
void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle);
void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle);

51
mission/controller/acs/util/MathOperations.h
View File

@ -3,14 +3,15 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <math.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <stdint.h>
#include <string.h>
#include <sys/time.h>
#include <cmath>
#include <iostream>
using namespace Math;
#include "fsfw/serviceinterface.h"
template <typename T1, typename T2 = T1>
class MathOperations {
@ -114,6 +115,44 @@ class MathOperations {
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
}
static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
* cartesian coordinates in ECEF
* @param: x x-value of position vector [m]
* y y-value of position vector [m]
* z z-value of position vector [m]
* latitude geodetic latitude [rad]
* longitude longitude [rad]
* altitude altitude [m]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
* Landis Markley and John L. Crassidis*/
// From World Geodetic System the Earth Radii
double a = 6378137.0; // semimajor axis [m]
double b = 6356752.3142; // semiminor axis [m]
// Calculation
double e2 = 1 - pow(b, 2) / pow(a, 2);
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
double p = std::abs(vector[2]) / epsilon2;
double s = pow(rho, 2) / (e2 * epsilon2);
double q = pow(p, 2) - pow(b, 2) + s;
double u = p / sqrt(q);
double v = pow(b, 2) * pow(u, 2) / q;
double P = 27 * v * s / q;
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
double t = (1 + Q + 1 / Q) / 6;
double c = sqrt(pow(u, 2) - 1 + 2 * t);
double w = (c - u) / 2;
double d =
sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
latitude = asin((epsilon2 + 1) * d / N);
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
longitude = atan2(vector[1], vector[0]);
}
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
@ -140,7 +179,7 @@ class MathOperations {
double FloorRest = floor(rest);
double secOfDay = rest - FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * PI / 180;
gmst = secOfDay / 240 * M_PI / 180;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
@ -191,11 +230,11 @@ class MathOperations {
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// Earth Rotation angle
double era = 0;
era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
// Greenwich Mean Sidereal Time
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
double arcsecFactor = 1 * PI / (180 * 3600);
double arcsecFactor = 1 * M_PI / (180 * 3600);
gmst2000 *= arcsecFactor;
gmst2000 += era;
@ -247,7 +286,7 @@ class MathOperations {
double de = 9.203 * arcsecFactor * cos(Om);
// % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp);

19
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -20,6 +20,7 @@ enum SetIds : uint32_t {
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
TLE_SET,
};
enum PoolIds : lp_id_t {
@ -85,6 +86,7 @@ enum PoolIds : lp_id_t {
GYR_3_VEC,
GYR_VEC_TOT,
// GPS Processed
SOURCE,
GC_LATITUDE,
GD_LONGITUDE,
ALTITUDE,
@ -108,6 +110,9 @@ enum PoolIds : lp_id_t {
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
// TLE
TLE_LINE_1,
TLE_LINE_2,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -116,11 +121,12 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
static constexpr uint8_t TLE_SET_ENTRIES = 2;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -239,6 +245,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
lp_var_t<uint8_t> source = lp_var_t<uint8_t>(sid.objectId, SOURCE, this);
private:
};
@ -292,6 +299,16 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
private:
};
class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
public:
TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
private:
};
} // namespace acsctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */

12
mission/power/gsLibDefs.h
View File

@ -8,12 +8,12 @@
// I really don't want to pull in all of those GomSpace headers just for 6 constants..
// Those are the headers which contain the defines which were just hardcoded below.
//#include "p60acu_hk.h"
//#include "p60acu_param.h"
//#include "p60dock_hk.h"
//#include "p60dock_param.h"
//#include "p60pdu_hk.h"
//#include "p60pdu_param.h"
// #include "p60acu_hk.h"
// #include "p60acu_param.h"
// #include "p60dock_hk.h"
// #include "p60dock_param.h"
// #include "p60pdu_hk.h"
// #include "p60pdu_param.h"
#endif

65
mission/system/EiveSystem.cpp
View File

@ -133,6 +133,10 @@ void EiveSystem::handleEventMessages() {
case pdec::INVALID_TC_FRAME: {
if (event.getParameter1() == pdec::FRAME_DIRTY_RETVAL) {
frameDirtyErrorCounter++;
// Check whether threshold was reached after 10 seconds.
if (frameDirtyErrorCounter == 1) {
frameDirtyCheckCd.resetTimer();
}
}
break;
}
@ -296,42 +300,39 @@ ReturnValue_t EiveSystem::sendFullRebootCommand() {
}
void EiveSystem::pdecRecoveryLogic() {
if (pdecResetWasAttempted and pdecResetWasAttemptedCd.hasTimedOut()) {
pdecResetWasAttempted = false;
// PDEC reset has happened too often in the last time. Perform reboot to same image.
if (pdecResetCounter >= PDEC_RESET_MAX_COUNT_BEFORE_REBOOT) {
if (waitingForPdecReboot) {
return;
}
triggerEvent(core::PDEC_REBOOT);
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
// Send reboot command.
ReturnValue_t result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
sif::error << "Sending a reboot command has failed" << std::endl;
// If the previous operation failed, it should be re-attempted the next task cycle.
pdecResetCounterResetCd.resetTimer();
return;
}
waitingForPdecReboot = true;
return;
}
if (frameDirtyCheckCd.hasTimedOut()) {
if (pdecResetCounterResetCd.hasTimedOut()) {
pdecResetCounter = 0;
}
if (frameDirtyCheckCd.hasTimedOut() and frameDirtyErrorCounter > 0) {
if (frameDirtyErrorCounter >= FRAME_DIRTY_COM_REBOOT_LIMIT) {
// If a PTME reset was already attempted and there is still an issue receiving TC frames,
// reboot the system.
if (pdecResetWasAttempted) {
if (waitingForPdecReboot) {
return;
}
triggerEvent(core::PDEC_REBOOT);
// Some delay to ensure that the event is stored in the persistent TM store as well.
TaskFactory::delayTask(500);
// Send reboot command.
ReturnValue_t result = sendSelfRebootCommand();
if (result != returnvalue::OK) {
sif::error << "Sending a reboot command has failed" << std::endl;
// If the previous operation failed, it should be re-attempted the next task cycle.
pdecResetWasAttemptedCd.resetTimer();
return;
}
waitingForPdecReboot = true;
return;
} else {
// Try one full PDEC reset.
CommandMessage msg;
store_address_t dummy{};
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
pdecResetWasAttemptedCd.resetTimer();
pdecResetWasAttempted = true;
}
// Try one full PDEC reset.
CommandMessage msg;
store_address_t dummy{};
ActionMessage::setCommand(&msg, pdec::RESET_PDEC_WITH_REINIITALIZATION, dummy);
commandQueue->sendMessage(pdecHandlerQueueId, &msg);
pdecResetCounterResetCd.resetTimer();
pdecResetCounter++;
}
frameDirtyErrorCounter = 0;
frameDirtyCheckCd.resetTimer();
}
}

5
mission/system/EiveSystem.h
View File

@ -10,6 +10,7 @@
class EiveSystem : public Subsystem, public HasActionsIF {
public:
static constexpr uint8_t FRAME_DIRTY_COM_REBOOT_LIMIT = 4;
static constexpr uint32_t PDEC_RESET_MAX_COUNT_BEFORE_REBOOT = 10;
static constexpr ActionId_t EXECUTE_I2C_REBOOT = 10;
@ -39,11 +40,11 @@ class EiveSystem : public Subsystem, public HasActionsIF {
Countdown frameDirtyCheckCd = Countdown(10000);
// If the PDEC reset was already attempted in the last 2 minutes, there is a high chance that
// only a full reboot will fix the issue.
Countdown pdecResetWasAttemptedCd = Countdown(120000);
bool pdecResetWasAttempted = false;
Countdown pdecResetCounterResetCd = Countdown(120000);
bool waitingForI2cReboot = false;
bool waitingForPdecReboot = false;
uint32_t pdecResetCounter = 0;
ActionHelper actionHelper;
PowerSwitchIF* powerSwitcher = nullptr;
std::atomic_uint16_t& i2cErrors;

1
mission/tmtc/PersistentTmStore.cpp
View File

@ -320,6 +320,7 @@ ReturnValue_t PersistentTmStore::loadNextDumpFile() {
}
// File will change, reset this field for correct state-keeping.
dumpParams.currentSameFileIdx = std::nullopt;
dumpParams.currentFileUnixStamp = dumpParams.dumpIter->epoch;
// Increment iterator for next cycle.
dumpParams.dumpIter++;
};