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

Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source 
# Conflicts:
#	mission/controller/AcsController.cpp
#	mission/controller/AcsController.h
#	mission/controller/acs/AcsParameters.h
#	mission/controller/acs/ActuatorCmd.h
#	mission/controller/acs/Guidance.cpp
#	mission/controller/acs/Guidance.h
#	mission/controller/acs/MultiplicativeKalmanFilter.cpp
#	mission/controller/acs/OutputValues.h
#	mission/controller/acs/SensorProcessing.cpp
#	mission/controller/acs/SensorProcessing.h
#	mission/controller/acs/control/Detumble.cpp
#	mission/controller/acs/control/Detumble.h
#	mission/controller/acs/control/PtgCtrl.cpp
#	mission/controller/acs/util/MathOperations.h
This commit is contained in:
Marius Eggert
2022-12-13 11:26:23 +01:00
parent 2b445369fd cf1a1185fc
commit d33ae9ede7
322 changed files with 17249 additions and 9124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
mission/CMakeLists.txt
View File

@ -6,3 +6,4 @@ add_subdirectory(memory)
add_subdirectory(tmtc)
add_subdirectory(system)
add_subdirectory(csp)
add_subdirectory(cfdp)

1
mission/cfdp/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@

57
mission/cfdp/Config.h Normal file
View File

@ -0,0 +1,57 @@
#ifndef MISSION_CFDP_CONFIG_H_
#define MISSION_CFDP_CONFIG_H_
#include "fsfw/cfdp.h"
namespace cfdp {
class EiveUserHandler : public cfdp::UserBase {
public:
explicit EiveUserHandler(HasFileSystemIF& vfs) : cfdp::UserBase(vfs) {}
virtual ~EiveUserHandler() = default;
void transactionIndication(const cfdp::TransactionId& id) override {}
void eofSentIndication(const cfdp::TransactionId& id) override {}
void transactionFinishedIndication(const cfdp::TransactionFinishedParams& params) override {
sif::info << "File transaction finished for transaction with " << params.id << std::endl;
}
void metadataRecvdIndication(const cfdp::MetadataRecvdParams& params) override {
sif::info << "Metadata received for transaction with " << params.id << std::endl;
}
void fileSegmentRecvdIndication(const cfdp::FileSegmentRecvdParams& params) override {}
void reportIndication(const cfdp::TransactionId& id, cfdp::StatusReportIF& report) override {}
void suspendedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code) override {}
void resumedIndication(const cfdp::TransactionId& id, size_t progress) override {}
void faultIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override {}
void abandonedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override {}
void eofRecvIndication(const cfdp::TransactionId& id) override {
sif::info << "EOF PDU received for transaction with " << id << std::endl;
}
};
class EiveFaultHandler : public cfdp::FaultHandlerBase {
public:
void noticeOfSuspensionCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
void noticeOfCancellationCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
void abandonCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Transaction " << id
<< " was abandoned, condition code : " << cfdp::getConditionCodeString(code)
<< std::endl;
}
void ignoreCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Fault ignored for transaction " << id
<< ", condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
};
} // namespace cfdp
#endif /* MISSION_CFDP_CONFIG_H_ */

542
mission/controller/AcsController.cpp
View File

@ -2,18 +2,28 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "mission/devices/torquer.h"
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
: ExtendedControllerBase(objectId),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
mgmData(this),
susData(this) {}
mgmDataRaw(this),
mgmDataProcessed(this),
susDataRaw(this),
susDataProcessed(this),
gyrDataRaw(this),
gyrDataProcessed(this),
gpsDataProcessed(this),
mekfData(this),
ctrlValData(this),
actuatorCmdData(this) {}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
return returnvalue::OK;
@ -38,21 +48,14 @@ void AcsController::performControlOperation() {
case SUBMODE_SAFE:
performSafe();
break;
case SUBMODE_DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_SUN:
case SUBMODE_PTG_TARGET:
performPointingCtrl();
break;
case SUBMODE_PTG_SUN:
performPointingCtrl();
break;
case SUBMODE_PTG_NADIR:
performPointingCtrl();
case SUBMODE_PTG_INERTIAL:
performPointingCtrl();
break;
}
}
@ -63,109 +66,191 @@ void AcsController::performControlOperation() {
}
{
PoolReadGuard pg(&mgmData);
PoolReadGuard pg(&mgmDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyMgmData();
}
}
{
PoolReadGuard pg(&susData);
PoolReadGuard pg(&susDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
// DEBUG : REMOVE AFTER COMPLETION
mode = MODE_ON;
submode = SUBMODE_DETUMBLE;
// DEBUG END
{
PoolReadGuard pg(&gyrDataRaw);
if (pg.getReadResult() == returnvalue::OK) {
copyGyrData();
}
}
}
void AcsController::performSafe() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
// Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
double magMomMtq[3] = {0, 0, 0};
bool magMomMtqValid = false;
if ( !validMekf ) { // Valid = 0, Failed = 1
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
}
else {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
// Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} else {
safeCtrl.safeNoMekf(
now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
}
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
double zeroQuat[4] = {0, 0, 0, 0};
std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
// Detumble check and switch
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_DETUMBLE;
detumbleCounter = 0;
triggerEvent(SAFE_RATE_VIOLATION);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
// }
}
void AcsController::performDetumble() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
&outputValues.magFieldEstValid, magMomMtq);
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else {
detumbleCounter = 0;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_SAFE;
detumbleCounter = 0;
}
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
}
void AcsController::performPointingCtrl() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
switch (submode) {
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
switch (submode) {
case SUBMODE_PTG_TARGET:
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
break;
case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
@ -176,9 +261,9 @@ void AcsController::performPointingCtrl() {
case SUBMODE_PTG_INERTIAL:
guidance.inertialQuatPtg(targetQuat, refSatRate);
break;
}
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
} double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
@ -203,45 +288,149 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
ptgCtrl.ptgDesaturation(mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
mekfData.satRotRateMekf.value, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
}
int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
// MGM
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec);
// MGM Raw
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0});
// SUS
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec);
poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0});
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 5.0});
// MGM Processed
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), false, 5.0});
// SUS Raw
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 5.0});
// SUS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), false, 5.0});
// GYR Raw
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
poolManager.subscribeForRegularPeriodicPacket({gyrDataRaw.getSid(), false, 5.0});
// GYR Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
poolManager.subscribeForRegularPeriodicPacket({gyrDataProcessed.getSid(), false, 5.0});
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
// MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
poolManager.subscribeForRegularPeriodicPacket({mekfData.getSid(), false, 5.0});
// Ctrl Values
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
// Actuator CMD
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), false, 5.0});
return returnvalue::OK;
}
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
if (sid == mgmData.getSid()) {
return &mgmData;
} else if (sid == susData.getSid()) {
return &susData;
switch (sid.ownerSetId) {
case acsctrl::MGM_SENSOR_DATA:
return &mgmDataRaw;
case acsctrl::MGM_PROCESSED_DATA:
return &mgmDataProcessed;
case acsctrl::SUS_SENSOR_DATA:
return &susDataRaw;
case acsctrl::SUS_PROCESSED_DATA:
return &susDataProcessed;
case acsctrl::GYR_SENSOR_DATA:
return &gyrDataRaw;
case acsctrl::GYR_PROCESSED_DATA:
return &gyrDataProcessed;
case acsctrl::GPS_PROCESSED_DATA:
return &gpsDataProcessed;
case acsctrl::MEKF_DATA:
return &mekfData;
case acsctrl::CTRL_VAL_DATA:
return &ctrlValData;
case acsctrl::ACTUATOR_CMD_DATA:
return &actuatorCmdData;
default:
return nullptr;
}
return nullptr;
}
@ -255,7 +444,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode > 5) || (submode < 2)) {
if ((submode > 6) || (submode < 2)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
@ -269,110 +458,195 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
void AcsController::announceMode(bool recursive) {}
void AcsController::copyMgmData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&mgm0Lis3Set);
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&mgm1Rm3100Set);
PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&mgm2Lis3Set);
PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
3 * sizeof(float));
mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&mgm3Rm3100Set);
PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
3 + sizeof(float));
mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
}
}
{
PoolReadGuard pg(&imtqMgmSet);
PoolReadGuard pg(&sensorValues.imtqMgmSet);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float));
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
3 * sizeof(float));
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
}
}
}
void AcsController::copySusData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&susSets[0]);
PoolReadGuard pg(&sensorValues.susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[1]);
PoolReadGuard pg(&sensorValues.susSets[1]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[2]);
PoolReadGuard pg(&sensorValues.susSets[2]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[3]);
PoolReadGuard pg(&sensorValues.susSets[3]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[4]);
PoolReadGuard pg(&sensorValues.susSets[4]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[5]);
PoolReadGuard pg(&sensorValues.susSets[5]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[6]);
PoolReadGuard pg(&sensorValues.susSets[6]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[7]);
PoolReadGuard pg(&sensorValues.susSets[7]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[8]);
PoolReadGuard pg(&sensorValues.susSets[8]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[9]);
PoolReadGuard pg(&sensorValues.susSets[9]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[10]);
PoolReadGuard pg(&sensorValues.susSets[10]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
}
}
{
PoolReadGuard pg(&susSets[11]);
PoolReadGuard pg(&sensorValues.susSets[11]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t));
std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value,
6 * sizeof(uint16_t));
susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
}
}
}
void AcsController::copyGyrData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value;
gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value;
gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value;
gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() &&
sensorValues.gyr0AdisSet.angVelocY.isValid() &&
sensorValues.gyr0AdisSet.angVelocZ.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.gyr1L3gSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value;
gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value;
gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value;
gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() &&
sensorValues.gyr1L3gSet.angVelocY.isValid() &&
sensorValues.gyr1L3gSet.angVelocZ.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.gyr2AdisSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value;
gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value;
gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value;
gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() &&
sensorValues.gyr2AdisSet.angVelocY.isValid() &&
sensorValues.gyr2AdisSet.angVelocZ.isValid());
}
}
{
PoolReadGuard pg(&sensorValues.gyr3L3gSet);
if (pg.getReadResult() == returnvalue::OK) {
gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value;
gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value;
gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value;
gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() &&
sensorValues.gyr3L3gSet.angVelocY.isValid() &&
sensorValues.gyr3L3gSet.angVelocZ.isValid());
}
}
}

145
mission/controller/AcsController.h
View File

@ -1,7 +1,6 @@
#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
#define MISSION_CONTROLLER_ACSCONTROLLER_H_
#include <commonObjects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
@ -9,14 +8,15 @@
#include "acs/Guidance.h"
#include "acs/Navigation.h"
#include "acs/SensorProcessing.h"
#include "acs/control/SafeCtrl.h"
#include "acs/control/Detumble.h"
#include "acs/control/PtgCtrl.h"
#include "acs/control/SafeCtrl.h"
#include "controllerdefinitions/AcsCtrlDefinitions.h"
#include "eive/objects.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
class AcsController : public ExtendedControllerBase {
public:
@ -31,8 +31,11 @@ class AcsController : public ExtendedControllerBase {
static const Submode_t SUBMODE_PTG_SUN = 6;
static const Submode_t SUBMODE_PTG_INERTIAL = 7;
protected:
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
static const Event SAFE_RATE_VIOLATION =
MAKE_EVENT(0, severity::MEDIUM); //!< The limits for the rotation in safe mode were violated.
protected:
void performSafe();
void performDetumble();
void performPointingCtrl();
@ -67,62 +70,98 @@ class AcsController : public ExtendedControllerBase {
void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive);
/* ACS Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
// MGMs
acsctrl::MgmDataRaw mgmData;
MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm1Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm2PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm3PoolVec = PoolEntry<float>(3);
PoolEntry<float> imtqMgmPoolVec = PoolEntry<float>(3);
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);
PoolEntry<float> mgm1VecRaw = PoolEntry<float>(3);
PoolEntry<float> mgm2VecRaw = PoolEntry<float>(3);
PoolEntry<float> mgm3VecRaw = PoolEntry<float>(3);
PoolEntry<float> imtqMgmVecRaw = PoolEntry<float>(3);
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
void copyMgmData();
// Sun Sensors
acsctrl::SusDataRaw susData;
std::array<SUS::SusDataset, 12> susSets{
SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
};
PoolEntry<uint16_t> sus0PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus1PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus2PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus3PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus4PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus5PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus6PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus7PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus8PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus9PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus10PoolVec = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus11PoolVec = PoolEntry<uint16_t>(6);
acsctrl::MgmDataProcessed mgmDataProcessed;
PoolEntry<float> mgm0VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm1VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm2VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
PoolEntry<double> mgmVecTotDer = PoolEntry<double>(3);
PoolEntry<double> magIgrf = PoolEntry<double>(3);
// SUSs
acsctrl::SusDataRaw susDataRaw;
PoolEntry<uint16_t> sus0ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus1ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus2ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus3ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus4ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus5ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus6ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus7ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus8ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus9ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus10ValRaw = PoolEntry<uint16_t>(6);
PoolEntry<uint16_t> sus11ValRaw = PoolEntry<uint16_t>(6);
void copySusData();
acsctrl::SusDataProcessed susDataProcessed;
PoolEntry<float> sus0VecProc = PoolEntry<float>(3);
PoolEntry<float> sus1VecProc = PoolEntry<float>(3);
PoolEntry<float> sus2VecProc = PoolEntry<float>(3);
PoolEntry<float> sus3VecProc = PoolEntry<float>(3);
PoolEntry<float> sus4VecProc = PoolEntry<float>(3);
PoolEntry<float> sus5VecProc = PoolEntry<float>(3);
PoolEntry<float> sus6VecProc = PoolEntry<float>(3);
PoolEntry<float> sus7VecProc = PoolEntry<float>(3);
PoolEntry<float> sus8VecProc = PoolEntry<float>(3);
PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
PoolEntry<double> susVecTot = PoolEntry<double>(3);
PoolEntry<double> susVecTotDer = PoolEntry<double>(3);
PoolEntry<double> sunIjk = PoolEntry<double>(3);
// GYRs
acsctrl::GyrDataRaw gyrDataRaw;
PoolEntry<double> gyr0VecRaw = PoolEntry<double>(3);
PoolEntry<float> gyr1VecRaw = PoolEntry<float>(3);
PoolEntry<double> gyr2VecRaw = PoolEntry<double>(3);
PoolEntry<float> gyr3VecRaw = PoolEntry<float>(3);
void copyGyrData();
acsctrl::GyrDataProcessed gyrDataProcessed;
PoolEntry<double> gyr0VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr1VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr2VecProc = PoolEntry<double>(3);
PoolEntry<double> gyr3VecProc = PoolEntry<double>(3);
PoolEntry<double> gyrVecTot = PoolEntry<double>(3);
// GPS
acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>();
// MEKF
acsctrl::MekfData mekfData;
PoolEntry<double> quatMekf = PoolEntry<double>(4);
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
// Ctrl Values
acsctrl::CtrlValData ctrlValData;
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
PoolEntry<double> errQuat = PoolEntry<double>(4);
PoolEntry<double> errAng = PoolEntry<double>();
// Actuator CMD
acsctrl::ActuatorCmdData actuatorCmdData;
PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};

2
mission/controller/CMakeLists.txt
View File

@ -3,4 +3,4 @@ if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
AcsController.cpp)
endif()
add_subdirectory(acs)
add_subdirectory(acs)

1134
mission/controller/ThermalController.cpp
View File

File diff suppressed because it is too large Load Diff

16
mission/controller/ThermalController.h
View File

@ -12,7 +12,7 @@ class ThermalController : public ExtendedControllerBase {
public:
static const uint16_t INVALID_TEMPERATURE = 999;
ThermalController(object_id_t objectId, object_id_t parentId);
ThermalController(object_id_t objectId);
ReturnValue_t initialize() override;
@ -55,8 +55,11 @@ class ThermalController : public ExtendedControllerBase {
MAX31865::Max31865Set max31865Set13;
MAX31865::Max31865Set max31865Set14;
MAX31865::Max31865Set max31865Set15;
TMP1075::Tmp1075Dataset tmp1075Set1;
TMP1075::Tmp1075Dataset tmp1075Set2;
TMP1075::Tmp1075Dataset tmp1075SetTcs0;
TMP1075::Tmp1075Dataset tmp1075SetTcs1;
TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0;
TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1;
TMP1075::Tmp1075Dataset tmp1075SetIfBoard;
// SUS
SUS::SusDataset susSet0;
@ -75,6 +78,13 @@ class ThermalController : public ExtendedControllerBase {
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(DELAY);
PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075IfBrd = PoolEntry<float>(10.0);
static constexpr dur_millis_t MUTEX_TIMEOUT = 50;
void copySensors();
void copySus();
void copyDevices();

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

@ -3,15 +3,542 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <stddef.h>
#include <cmath>
AcsParameters::AcsParameters() {}
AcsParameters::AcsParameters(){}; //(uint8_t parameterModuleId) :
// parameterModuleId(parameterModuleId) {}
AcsParameters::~AcsParameters() {}
/*
ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
return returnvalue::OK;
if (domainId == parameterModuleId) {
switch (parameterId >> 8) {
case 0x0: // direct members
switch (parameterId & 0xFF) {
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x1: // OnBoardParams
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(onBoardParams.sampleTime);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x2: // InertiaEIVE
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(inertiaEIVE.inertiaMatrix);
break;
case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x3: // MgmHandlingParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
break;
case 0x6:
parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
break;
case 0x7:
parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
break;
case 0x8:
parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
break;
case 0x9:
parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
break;
case 0xA:
parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
break;
case 0xB:
parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
break;
case 0xC:
parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
break;
case 0xD:
parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
break;
case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case 0x4: // SusHandlingParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
break;
case 0x6:
parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
break;
case 0x7:
parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
break;
case 0x8:
parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
break;
case 0x9:
parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
break;
case 0xA:
parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
break;
case 0xB:
parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
break;
case 0xC:
parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
break;
case 0xD:
parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
break;
case 0xE:
parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
break;
case 0xF:
parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
break;
case 0x10:
parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
break;
case 0x11:
parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
break;
case 0x12:
parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
break;
case 0x13:
parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
break;
case 0x14:
parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
break;
case 0x15:
parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
break;
case 0x16:
parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
break;
case 0x17:
parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
break;
case 0x18:
parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
break;
case 0x19:
parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
break;
case 0x1A:
parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
break;
case 0x1B:
parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
break;
case 0x1C:
parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
break;
case 0x1D:
parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
break;
case 0x1E:
parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
break;
case 0x1F:
parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
break;
case 0x20:
parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
break;
case 0x21:
parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
break;
case 0x22:
parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
break;
case 0x23:
parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
break;
case 0x24:
parameterWrapper->set(susHandlingParameters.filterAlpha);
break;
case 0x25:
parameterWrapper->set(susHandlingParameters.sunThresh);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x5): // GyrHandlingParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x6): // RwHandlingParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(rwHandlingParameters.inertiaWheel);
break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.maxTrq);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x7): // RwMatrices
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(rwMatrices.alignmentMatrix);
break;
case 0x1:
parameterWrapper->set(rwMatrices.pseudoInverse);
break;
case 0x2:
parameterWrapper->set(rwMatrices.without0);
break;
case 0x3:
parameterWrapper->set(rwMatrices.without1);
break;
case 0x4:
parameterWrapper->set(rwMatrices.without2);
break;
case 0x5:
parameterWrapper->set(rwMatrices.without3);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x8): // SafeModeControllerParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
break;
case 0x1:
parameterWrapper->set(safeModeControllerParameters.k_align_mekf);
break;
case 0x2:
parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf);
break;
case 0x3:
parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
break;
case 0x4:
parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
break;
case 0x5:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
break;
case 0x6:
parameterWrapper->set(safeModeControllerParameters.satRateRef);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x9): // DetumbleCtrlParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(detumbleCtrlParameters.gainD);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xA): // PointingModeControllerParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.updtFlag);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.A_rw);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.refDirection);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.quatRef);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.zeta);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.zetaLow);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.om);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.omLow);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.omMax);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.qiMin);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x10:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
break;
case 0x11:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
break;
case 0x12:
parameterWrapper->set(targetModeControllerParameters.desatOn);
break;
case 0x13:
parameterWrapper->set(targetModeControllerParameters.omegaEarth);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xB): // StrParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(strParameters.exclusionAngle);
break;
case 0x1:
parameterWrapper->set(strParameters.boresightAxis);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xC): // GpsParameters
switch (parameterId & 0xFF) {
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xD): // GroundStationParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(groundStationParameters.latitudeGs);
break;
case 0x1:
parameterWrapper->set(groundStationParameters.longitudeGs);
break;
case 0x2:
parameterWrapper->set(groundStationParameters.altitudeGs);
break;
case 0x3:
parameterWrapper->set(groundStationParameters.earthRadiusEquat);
break;
case 0x4:
parameterWrapper->set(groundStationParameters.earthRadiusPolar);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xE): // SunModelParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(sunModelParameters.useSunModel);
break;
case 0x1:
parameterWrapper->set(sunModelParameters.domega);
break;
case 0x2:
parameterWrapper->set(sunModelParameters.omega_0);
break;
case 0x3:
parameterWrapper->set(sunModelParameters.m_0);
break;
case 0x4:
parameterWrapper->set(sunModelParameters.dm);
break;
case 0x5:
parameterWrapper->set(sunModelParameters.e);
break;
case 0x6:
parameterWrapper->set(sunModelParameters.e1);
break;
case 0x7:
parameterWrapper->set(sunModelParameters.p1);
break;
case 0x8:
parameterWrapper->set(sunModelParameters.p2);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0xF): // KalmanFilterParameters
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter);
break;
case 0x1:
parameterWrapper->set(kalmanFilterParameters.requestResetFlag);
break;
case 0x2:
parameterWrapper->set(
kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps);
break;
case 0x3:
parameterWrapper->set(kalmanFilterParameters.processNoiseOmega);
break;
case 0x4:
parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion);
break;
case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
break;
case 0x6:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
break;
case 0x7:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
break;
case 0x8:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
break;
case 0x9:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
break;
case 0xA:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x10): // MagnetorquesParameter
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
break;
case 0x4:
parameterWrapper->set(magnetorquesParameter.inverseAlignment);
break;
case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
case (0x11): // DetumbleParameter
switch (parameterId & 0xFF) {
case 0x0:
parameterWrapper->set(detumbleParameter.detumblecounter);
break;
case 0x1:
parameterWrapper->set(detumbleParameter.omegaDetumbleStart);
break;
case 0x2:
parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
default:
return INVALID_IDENTIFIER_ID;
}
return returnvalue::OK;
} else {
return INVALID_DOMAIN_ID;
}
}*/

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

@ -45,10 +45,10 @@ class AcsParameters /*: public HasParametersIF*/ {
} inertiaEIVE;
struct MgmHandlingParameters {
float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}}; // Documentation not consistent
float mgm0orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
float mgm1orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508};
@ -68,6 +68,9 @@ class AcsParameters /*: public HasParametersIF*/ {
{15.67482e-2, -6.958760e-2, 94.50124e-2}};
float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1};
} mgmHandlingParameters;
@ -767,6 +770,14 @@ class AcsParameters /*: public HasParametersIF*/ {
double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
// var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
// assumed to be equal for the same class of sensors
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
enum PreferAdis { NO = 0, YES = 1 };
uint8_t preferAdis = PreferAdis::YES;
} gyrHandlingParameters;
struct RwHandlingParameters {
@ -821,8 +832,8 @@ class AcsParameters /*: public HasParametersIF*/ {
double k_align_no_mekf = 0.000056875;;
double sunMagAngleMin; // ???
double sunTargetDir[3] = {0, 0, 1}; // Geometry Frame
double satRateRef[3] = {0, 0, 0}; // Geometry Frame
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
} safeModeControllerParameters;

52
mission/controller/acs/ActuatorCmd.cpp
View File

@ -5,23 +5,21 @@
* Author: Robin Marquardt
*/
#include "ActuatorCmd.h"
#include "util/MathOperations.h"
#include "util/CholeskyDecomposition.h"
#include <cmath>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) {
acsParameters = *acsParameters_;
}
#include <cmath>
ActuatorCmd::~ActuatorCmd(){
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
}
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
@ -46,7 +44,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
@ -58,22 +56,20 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits) {
// Convert to Unit frame
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentUnits[i]) > maxDipol) {
maxValue = abs(dipolMomentUnits[i]);
}
}
// Convert to Unit frame
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, dipolMoment, dipolMomentUnits, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
for (int i = 0; i < 3; i++) {
if (abs(dipolMomentUnits[i]) > maxDipol) {
maxValue = abs(dipolMomentUnits[i]);
}
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentUnits, scalingFactor, dipolMomentUnits, 3);
}
}

18
mission/controller/acs/ActuatorCmd.h
View File

@ -1,19 +1,10 @@
/*
* ActuatorCmd.h
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_
#include "AcsParameters.h"
#include "SensorProcessing.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
#include "OutputValues.h"
class ActuatorCmd{
public:
@ -48,10 +39,9 @@ public:
*/
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentUnits);
protected:
private:
AcsParameters acsParameters;
protected:
private:
AcsParameters acsParameters;
};
#endif /* ACTUATORCMD_H_ */

19
mission/controller/acs/CMakeLists.txt
View File

@ -1,14 +1,13 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE AcsParameters.cpp
ActuatorCmd.cpp
Guidance.cpp
Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp
Navigation.cpp
OutputValues.cpp
SensorProcessing.cpp
SensorValues.cpp
SusConverter.cpp)
ActuatorCmd.cpp
Guidance.cpp
Igrf13Model.cpp
MultiplicativeKalmanFilter.cpp
Navigation.cpp
SensorProcessing.cpp
SensorValues.cpp
SusConverter.cpp)
add_subdirectory(control)

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

@ -7,6 +7,7 @@
#include "Guidance.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
@ -29,8 +30,9 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
// memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
}
void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
timeval now, double targetQuat[4], double refSatRate[3]) {
void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
//-------------------------------------------------------------------------------------
@ -68,10 +70,8 @@ void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::Out
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
@ -129,22 +129,15 @@ void Guidance::targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, ACS::Out
if (outputValues->sunDirModelValid) {
double sunDirJ[3] = {0, 0, 0};
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
}
else {
sunDirB[0] = outputValues->sunDirEst[0];
sunDirB[1] = outputValues->sunDirEst[1];
sunDirB[2] = outputValues->sunDirEst[2];
} else {
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
}
double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
@ -588,8 +581,8 @@ void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
}
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
@ -597,9 +590,7 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
double satRate[3] = {0, 0, 0};
satRate[0] = outputValues->satRateMekf[0];
satRate[1] = outputValues->satRateMekf[1];
satRate[2] = outputValues->satRateMekf[2];
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
// valid checks ?
double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
@ -623,8 +614,8 @@ void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
}
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() && sensorValues->rw3Set.isValid() &&
sensorValues->rw4Set.isValid()) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];

50
mission/controller/acs/Guidance.h
View File

@ -8,25 +8,26 @@
#ifndef GUIDANCE_H_
#define GUIDANCE_H_
#include "AcsParameters.h"
#include "SensorValues.h"
#include "OutputValues.h"
#include <time.h>
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorValues.h"
class Guidance {
public:
Guidance(AcsParameters *acsParameters_);
virtual ~Guidance();
public:
Guidance(AcsParameters *acsParameters_);
virtual ~Guidance();
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
// Function to get the target quaternion and refence rotation rate from gps position and position of the ground station
void targetQuatPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]);
void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]);
void targetQuatPtgOldVersion(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground station
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
@ -42,20 +43,21 @@ public:
// Function to get the target quaternion and refence rotation rate from parameters for inertial pointing
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and
// desired
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel
// maybe can be done in "commanding.h"
void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv);
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
AcsParameters acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternionNadir;
double savedQuaternionNadir[4] = {0, 0, 0, 0};
double omegaRefSavedNadir[3] = {0, 0, 0};
private:
AcsParameters acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternionNadir;
double savedQuaternionNadir[4] = {0, 0, 0, 0};
double omegaRefSavedNadir[3] = {0, 0, 0};
};
#endif /* ACS_GUIDANCE_H_ */

239
mission/controller/acs/Igrf13Model.cpp
View File

@ -1,125 +1,152 @@
/*
* Igrf13Model.cpp
*
* Created on: 10 Mar 2022
* Author: Robin Marquardt
*/
#include "Igrf13Model.h"
#include "util/MathOperations.h"
#include <math.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <stdint.h>
#include <string.h>
//#include <time.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 <time.h>
#include <cmath>
Igrf13Model::Igrf13Model(){
}
Igrf13Model::~Igrf13Model(){
}
#include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {}
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement, double* magFieldModelInertial) {
const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * PI / 180;
theta *= (-1);
double phi = longitude, theta = gcLatitude; //geocentric
/* Here is the co-latitude needed*/
theta -= 90*Math::PI/180;
theta *= (-1);
double rE = 6371200.0; // radius earth [m]
/* Predefine recursive associated Legendre polynomials */
double P11 = 1;
double P10 = P11; // P10 = P(n-1,m-0)
double dP11 = 0; // derivative
double dP10 = dP11; // derivative
double rE = 6371200.0; // radius earth [m]
/* Predefine recursive associated Legendre polynomials */
double P11 = 1;
double P10 = P11; //P10 = P(n-1,m-0)
double dP11 = 0; //derivative
double dP10 = dP11; //derivative
double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
for (int m = 0; m <= igrfOrder; m++) {
for (int n = 1; n <= igrfOrder; n++) {
if (m <= n) {
/* Calculation of Legendre Polynoms (normalised) */
if (n == m) {
P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2;
P10 = P11;
P20 = 0;
dP11 = dP2;
dP10 = dP11;
dP20 = 0;
} else if (n == 1) {
P2 = cos(theta) * P10;
dP2 = cos(theta) * dP10 - sin(theta) * P10;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
} else {
K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3));
P2 = cos(theta) * P10 - K * P20;
dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
}
/* gradient of scalar potential towards radius */
magFieldModel[0] +=
pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
/* gradient of scalar potential towards theta */
magFieldModel[1] +=
pow(rE / (altitude + rE), (n + 2)) *
((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
/* gradient of scalar potential towards phi */
magFieldModel[2] +=
pow(rE / (altitude + rE), (n + 2)) *
((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
}
}
}
for (int m = 0; m <= igrfOrder; m++) {
for (int n = 1; n <= igrfOrder; n++) {
if (m <= n) {
/* Calculation of Legendre Polynoms (normalised) */
if (n == m) {
P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 - cos(theta) * P11;
P11 = P2;
P10 = P11;
P20 = 0;
dP11 = dP2;
dP10 = dP11;
dP20 = 0;
} else if (n == 1) {
P2 = cos(theta) * P10;
dP2 = cos(theta) * dP10 - sin(theta) * P10;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
} else {
K = (pow((n - 1), 2) - pow(m, 2))
/ ((2 * n - 1) * (2 * n - 3));
P2 = cos(theta) * P10 - K * P20;
dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
}
/* gradient of scalar potential towards radius */
magFieldModel[0]+=pow(rE/(altitude+rE),(n+2))*(n+1)*
((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*P2);
/* gradient of scalar potential towards phi */
magFieldModel[1]+=pow(rE/(altitude+rE),(n+2))*
((updatedG[m][n-1]*cos(m*phi) + updatedH[m][n-1]*sin(m*phi))*dP2);
/* gradient of scalar potential towards theta */
magFieldModel[2]+=pow(rE/(altitude+rE),(n+2))*
((-updatedG[m][n-1]*sin(m*phi) + updatedH[m][n-1]*cos(m*phi))*P2*m);
}
}
}
magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta));
magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta));
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double UT1 = JD2000 / 36525.;
/* Next step: transform into inertial KOS (IJK)*/
//Julean Centuries
double JD2000Floor = 0;
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
JD2000Floor = floor(JD2000);
double JC2000 = JD2000Floor / 36525;
double gst =
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst = std::fmod(gst, 360.);
gst *= PI / 180.;
double lst = gst + longitude; // local sidereal time [rad]
double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000,2)
- 0.000000026 * pow(JC2000,3); //greenwich sidereal time
gst *= PI/180; //convert to radians
double sec = (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ?
double omega0 = 0.00007292115; // mean angular velocity earth [rad/s]
gst +=omega0 * sec;
magFieldModelInertial[0] =
(magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
magFieldModel[2] * sin(lst);
magFieldModelInertial[1] =
(magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
magFieldModel[2] * cos(lst);
magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double lst = gst + longitude; //local sidereal time [rad]
double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*cos(lst) - magFieldModel[1] * sin(lst);
magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + magFieldModel[1] * sin(theta)*sin(lst) + magFieldModel[1] * cos(lst);
magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
double normVecMagFieldInert[3] = {0,0,0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
}
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement){
double JD2000Igrf = (2458850.0-2451545); //Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000-JD2000Igrf);
for(int i = 0;i <= igrfOrder; i++){
for(int j = 0;j <= (igrfOrder-1); j++){
updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days/365);
updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days/365);
}
}
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000 - JD2000Igrf);
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365);
updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365);
}
}
}
void Igrf13Model::schmidtNormalization() {
double kronDelta = 0;
schmidtFactors[0][0] = 1;
for (int n = 1; n <= igrfOrder; n++) {
if (n == 1) {
schmidtFactors[0][n - 1] = 1;
} else {
schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
}
for (int m = 1; m <= igrfOrder; m++) {
if (m == 1) {
kronDelta = 1;
} else {
kronDelta = 0;
}
schmidtFactors[m][n - 1] =
schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
}
}
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];
svG[i][j] = schmidtFactors[i][j] * svG[i][j];
svH[i][j] = schmidtFactors[i][j] * svH[i][j];
}
}
}

187
mission/controller/acs/Igrf13Model.h
View File

@ -16,103 +16,124 @@
#ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_
#include <cmath>
#include <fsfw/parameters/HasParametersIF.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <fsfw/parameters/HasParametersIF.h>
#include <cmath>
// Output should be transformed to [T] instead of [nT]
// Updating Coefficients has to be implemented yet. Question, updating everyday ?
class Igrf13Model/*:public HasParametersIF*/{
class Igrf13Model /*:public HasParametersIF*/ {
public:
Igrf13Model();
virtual ~Igrf13Model();
public:
Igrf13Model();
virtual ~Igrf13Model();
// Main Function
void magFieldComp(const double longitude, const double gcLatitude, const double altitude,
timeval timeOfMagMeasurement, double* magFieldModelInertial);
// Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is
// oriented from the satellite to earth COM Difference up to 25 km, which is 5 % of the total
// flight altitude
/* Inputs:
* - longitude: geocentric longitude [rad]
* - latitude: geocentric latitude [rad]
* - altitude: [m]
* - timeOfMagMeasurement: time of actual measurement [s]
*
* Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/
// Main Function
void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial);
// Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM
// Difference up to 25 km, which is 5 % of the total flight altitude
/* Inputs:
* - longitude: geocentric longitude [rad]
* - latitude: geocentric latitude [rad]
* - altitude: [m]
* - timeOfMagMeasurement: time of actual measurement [s]
*
* Outputs:
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
void schmidtNormalization();
private:
double coeffG[14][13] = {
{-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
{-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
{0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
{0.0, 0.0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
{0.0, 0.0, 0.0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3},
{0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8},
{0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -11.9, -2.4, -0.6, -0.5, 0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -3.8, 0.2, 0.1, 0.1},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, -1.1, 0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV)
double magFieldModel[3];
double coeffH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0},
{4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
{0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
{0.0, 0.0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4},
{0.0, 0.0, 0.0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4},
{0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3},
{0.0, 0.0, 0.0, 0.0, 0.0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6, -0.1, -3.0, 0.2, 0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.8, -2.0, -0.9, 0.5},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.6, 0.0, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT
private:
const double coeffG[14][13] = {{-29404.8,-2499.6, 1363.2, 903.0,-234.3, 66.0, 80.6, 23.7, 5.0,-1.9, 3.0,-2.0, 0.1},
{ -1450.9, 2982.0,-2381.2, 809.5, 363.2, 65.5,-76.7, 9.7, 8.4,-6.2,-1.4,-0.1,-0.9},
{ 0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5},
{ 0.0, 0.0, 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
{ 0.0, 0.0, 0.0, 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3},
{ 0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8},
{ 0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-11.9,-2.4,-0.6,-0.5, 0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-3.8, 0.2, 0.1, 0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1,-1.1, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.3,-0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.4}}; // [m][n] in nT
double svG[14][13] = {
{5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, -12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, -5.1, 1.3, -1.4, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.9, 0.0, -0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.9, -0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double coeffH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 },
{4652.5,-2991.6, -82.1, 281.9, 47.7,-19.1,-51.5, 8.4,-23.4, 3.4, 0.0,-1.2,-0.9},
{ 0.0, -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6},
{ 0.0, 0.0,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4},
{ 0.0, 0.0, 0.0,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4},
{ 0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4,-3.4,-1.6, 0.6,-0.1},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6,-0.1,-3.0, 0.2, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-8.8,-2.0,-0.9, 0.5},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-2.6, 0.0,-0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5,-0.4},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.6}}; // [m][n] in nT
double svH[14][13] = {
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, -5.0, 3.0, 0.8, -0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.3, 0.0, -1.1, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1, -0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double schmidtFactors[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
;
const double svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0 ,0.0},
{7.4, -7.0, -5.9,-1.6, 0.5,-0.3,-0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.9, 0.0,-0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
const double svH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{-25.9,-30.2, 6.0,-0.1, 0.0, 0.0, 0.6,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
double updatedG[14][13];
double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation
double updatedG[14][13];
double updatedH[14][13];
static const int igrfOrder = 13; // degree of truncation
};
#endif /* IGRF13MODEL_H_ */

2277
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

File diff suppressed because it is too large Load Diff

151
mission/controller/acs/MultiplicativeKalmanFilter.h
View File

@ -7,101 +7,94 @@
* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
* means of the spacecraft attitude sensors
*
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin Marquardt
* @note: A description of the used algorithms can be found in the bachelor thesis of Robin
* Marquardt
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
*/
#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_
#include "config/classIds.h"
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
//#include <_timeval.h>
#include <stdint.h> //uint8_t
#include <time.h> /*purpose, timeval ?*/
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "config/classIds.h"
class MultiplicativeKalmanFilter{
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter();
class MultiplicativeKalmanFilter {
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
virtual ~MultiplicativeKalmanFilter();
void reset(); // NOT YET DEFINED - should only reset all mekf variables
void reset(); // NOT YET DEFINED - should only reset all mekf variables
/* @brief: init() - This function initializes the Kalman Filter and will provide the first quaternion through
* the QUEST algorithm
* @param: magneticField_ magnetic field vector in the body frame
* sunDir_ sun direction vector in the body frame
* sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame
*/
void init(const double *magneticField_, const bool *validMagField_,
const double *sunDir_, const bool *validSS,
const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ,const bool *validMagModel);
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
* quaternion through the QUEST algorithm
* @param: magneticField_ magnetic field vector in the body frame
* sunDir_ sun direction vector in the body frame
* sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame
*/
void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
const bool validSS, const double *sunDirJ, const bool validSSModel,
const double *magFieldJ, const bool validMagModel);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter for the current step
* after the initalization
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
* rateRMUs_ Estimated satellite rotation rate from the Rate Measurement Units [rad/s]
* magneticField_ magnetic field vector in the body frame
* sunDir_ sun direction vector in the body frame
* sunDirJ sun direction vector in the ECI frame
* magFieldJ magnetic field vector in the ECI frame
* outputQuat Stores the calculated quaternion
* outputSatRate Stores the adjusted satellite rate
* @return ReturnValue_t Feedback of this class, KALMAN_NO_RMU_MEAS if no satellite rate from the sensors was provided,
* KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model calculations,
* KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
* RETURN_OK else
*/
ReturnValue_t mekfEst(
const double *quaternionSTR, const bool *validSTR_,
const double *rateRMUs_, const bool *validRMUs_,
const double *magneticField_, const bool *validMagField_,
const double *sunDir_, const bool *validSS,
const double *sunDirJ, const bool *validSSModel,
const double *magFieldJ,const bool *validMagModel,
double *outputQuat, double *outputSatRate);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization
* @param: quaternionSTR Star Tracker Quaternion between from body to ECI frame
* rateGYRs_ Estimated satellite rotation rate from the
* Gyroscopes [rad/s] magneticField_ magnetic field vector in the body frame sunDir_
* sun direction vector in the body frame sunDirJ sun direction vector in the ECI
* frame magFieldJ magnetic field vector in the ECI frame
* outputQuat Stores the calculated quaternion
* outputSatRate Stores the adjusted satellite rate
* @return ReturnValue_t Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
* the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
* calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
* RETURN_OK else
*/
ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
const bool validGYRs_, const double *magneticField_,
const bool validMagField_, const double *sunDir_, const bool validSS,
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
const bool validMagModel, acsctrl::MekfData *mekfData);
// Declaration of Events (like init) and memberships
// static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
// (/config/returnvalues/classIDs.h) static const Event RESET =
// MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
// resetting Mekf
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
// Declaration of Events (like init) and memberships
//static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND (/config/returnvalues/classIDs.h)
//static const Event RESET = MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
// resetting Mekf
static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
static const ReturnValue_t KALMAN_NO_RMU_MEAS = MAKE_RETURN_CODE(0x01);
static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
private:
/*Parameters*/
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
double sampleTime = 0.1;
private:
/*Parameters*/
AcsParameters::InertiaEIVE* inertiaEIVE;
AcsParameters::KalmanFilterParameters* kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
double sampleTime = 0.1;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
bool validMekf;
uint8_t sensorsAvail;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6];/*after reset?QUEST*/
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
bool validMekf;
uint8_t sensorsAvail;
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasRMU[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
//double alpha, gamma, beta;
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
// double alpha, gamma, beta;
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
};
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

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

@ -21,31 +21,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
Navigation::~Navigation() {}
void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
void Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
ReturnValue_t *mekfValid) {
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatJBValid =
(sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid());
bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
sensorValues->strSet.caliQy.isValid() &&
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
if (kalmanInit) {
*mekfValid = multiplicativeKalmanFilter.mekfEst(
quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid,
outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst,
&outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
outputValues->satRateMekf); // VALIDS FOR QUAT AND RATE ??
}
else {
multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->sunDirEst, &outputValues->sunDirEstValid,
outputValues->sunDirModel, &outputValues->sunDirModelValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid);
quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(),
mekfData); // VALIDS FOR QUAT AND RATE ??
} else {
multiplicativeKalmanFilter.init(
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
kalmanInit = true;
*mekfValid = 0;
*mekfValid = returnvalue::OK;
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the init
//of kalman filter where does this class know from that kalman filter was not initialized ?
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the
// init of kalman filter where does this class know from that kalman filter was not
// initialized ?
}
}

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

@ -8,28 +8,28 @@
#ifndef NAVIGATION_H_
#define NAVIGATION_H_
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorProcessing.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
#include "OutputValues.h"
class Navigation{
public:
Navigation(AcsParameters *acsParameters_); //Input mode ?
virtual ~Navigation();
class Navigation {
public:
Navigation(AcsParameters *acsParameters_); // Input mode ?
virtual ~Navigation();
void useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid);
void processSensorData();
void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
ReturnValue_t *mekfValid);
void processSensorData();
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
bool kalmanInit = false;
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
bool kalmanInit = false;
};
#endif /* ACS_NAVIGATION_H_ */

19
mission/controller/acs/OutputValues.cpp
View File

@ -1,19 +0,0 @@
/*
* OutputValues.cpp
*
* Created on: 30 Mar 2022
* Author: rooob
*/
#include "OutputValues.h"
namespace ACS {
OutputValues::OutputValues(){
}
OutputValues::~OutputValues(){
}
}

52
mission/controller/acs/OutputValues.h
View File

@ -1,52 +0,0 @@
/*
* OutputValues.h
*
* Created on: 30 Mar 2022
* Author: rooob
*/
#ifndef OUTPUTVALUES_H_
#define OUTPUTVALUES_H_
namespace ACS {
class OutputValues {
public:
OutputValues();
virtual ~OutputValues();
double magFieldEst[3]; // sensor fusion (G)
bool magFieldEstValid;
double magFieldModel[3]; //igrf (IJK)
bool magFieldModelValid;
double magneticFieldVectorDerivative[3];
bool magneticFieldVectorDerivativeValid;
bool mgmUpdated;
double sunDirEst[3]; // sensor fusion (G)
bool sunDirEstValid;
double sunDirModel[3]; //sun model (IJK)
bool sunDirModelValid;
double quatMekfBJ[4]; //mekf
bool quatMekfBJValid;
double satRateEst[3];
bool satRateEstValid;
double satRateMekf[3]; // after mekf with correction of bias
bool satRateMekfValid;
double sunVectorDerivative[3];
bool sunVectorDerivativeValid;
double gcLatitude; // geocentric latitude, radian
double gdLongitude; // Radian longitude
double gpsVelocity[3]; //earth fixed frame
};
}
#endif /*OUTPUTVALUES_H_*/

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

@ -7,6 +7,7 @@
#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>
@ -20,27 +21,35 @@
using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) : savedMagFieldEst{0, 0, 0} {
validMagField = false;
validGcLatitude = false;
}
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
: savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
SensorProcessing::~SensorProcessing() {}
bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
bool mgm1valid, const float *mgm2Value, bool mgm2valid,
const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
const double gpsLatitude, const double gpsLongitude,
const double gpsAltitude, bool gpsValid, double *magFieldEst,
bool *outputValid, double *magFieldModel,
bool *magFieldModelValid, double *magneticFieldVectorDerivative,
bool *magneticFieldVectorDerivativeValid) {
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
*outputValid = false;
validMagField = false;
return false;
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
mgmDataProcessed->setValidity(false, true);
}
}
return;
}
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
@ -49,9 +58,8 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
bool validUnit[5] = {false, false, false, false, false};
uint8_t validCount = 0;
if (mgm0valid) {
VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
3);
@ -59,8 +67,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm0ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
mgm0ValueBody, 3, 3, 1);
validCount += 1;
validUnit[0] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm1valid) {
VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
@ -69,8 +79,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm1ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
mgm1ValueBody, 3, 3, 1);
validCount += 1;
validUnit[1] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm2valid) {
VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
@ -79,8 +91,10 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm2ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
mgm2ValueBody, 3, 3, 1);
validCount += 1;
validUnit[2] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
}
}
if (mgm3valid) {
VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
@ -89,81 +103,79 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
mgm3ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
mgm3ValueBody, 3, 3, 1);
validCount += 1;
validUnit[3] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
}
}
if (mgm4valid) {
VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
3);
float mgm4ValueNT[3];
VectorOperations<float>::mulScalar(mgm4Value, 1e3, mgm4ValueNT, 3); // uT to nT
VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
mgm4ValueNoBias, 3);
MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
mgm4ValueCalib, 3, 3, 1);
MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
mgm4ValueBody, 3, 3, 1);
validCount += 1;
validUnit[4] = true;
}
/* -------- MagFieldEst: Middle Value ------- */
float mgmValues[3][5] = {
{mgm0ValueBody[0], mgm1ValueBody[0], mgm2ValueBody[0], mgm3ValueBody[0], mgm4ValueBody[0]},
{mgm0ValueBody[1], mgm1ValueBody[1], mgm2ValueBody[1], mgm3ValueBody[1], mgm4ValueBody[1]},
{mgm0ValueBody[2], mgm1ValueBody[2], mgm2ValueBody[2], mgm3ValueBody[2], mgm4ValueBody[2]}};
double mgmValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
mgmValidValues[0][j] = mgmValues[0][i];
mgmValidValues[1][j] = mgmValues[1][i];
mgmValidValues[2][j] = mgmValues[2][i];
j += 1;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
}
}
// Selection Sort
double mgmValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*mgmValidValues, *mgmValidValuesSort, 3, validCount);
uint8_t n = ceil(validCount / 2);
magFieldEst[0] = mgmValidValuesSort[0][n];
magFieldEst[1] = mgmValidValuesSort[1][n];
magFieldEst[2] = mgmValidValuesSort[2][n];
validMagField = true;
//-----------------------Mag Rate Computation ---------------------------------------------------
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
double mgmVecTot[3] = {0.0, 0.0, 0.0};
for (uint8_t i = 0; i < 3; i++) {
magneticFieldVectorDerivative[i] = (magFieldEst[i] - savedMagFieldEst[i]) / timeDiff;
savedMagFieldEst[i] = magFieldEst[i];
mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
*magneticFieldVectorDerivativeValid = true;
if (timeOfSavedMagFieldEst.tv_sec == 0) {
magneticFieldVectorDerivative[0] = 0;
magneticFieldVectorDerivative[1] = 0;
magneticFieldVectorDerivative[2] = 0;
*magneticFieldVectorDerivativeValid = false;
//-----------------------Mgm Rate Computation ---------------------------------------------------
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool mgmVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
if (timeOfSavedMagFieldEst.tv_sec != 0) {
for (uint8_t i = 0; i < 3; i++) {
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
savedMgmVecTot[i] = mgmVecTot[i];
}
}
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
*outputValid = true;
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
if (!gpsValid) {
*magFieldModelValid = false;
} else {
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
Igrf13Model igrf13;
// So the line above should not be done here. Update: Can be done here as long updated coffs
// stored in acsParameters ?
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(gpsLongitude, gpsLatitude, gpsAltitude, timeOfMgmMeasurement,
magFieldModel);
*magFieldModelValid = false;
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
}
{
PoolReadGuard pg(mgmDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
mgmDataProcessed->mgm0vec.setValid(mgm0valid);
std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
mgmDataProcessed->mgm1vec.setValid(mgm1valid);
std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
mgmDataProcessed->mgm2vec.setValid(mgm2valid);
std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
mgmDataProcessed->mgm3vec.setValid(mgm3valid);
std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
mgmDataProcessed->mgm4vec.setValid(mgm4valid);
std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
mgmDataProcessed->mgmVecTot.setValid(true);
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, mgmVecTotDerivative,
3 * sizeof(double));
mgmDataProcessed->mgmVecTotDerivative.setValid(mgmVecTotDerivativeValid);
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
mgmDataProcessed->setValidity(true, false);
}
}
return true;
}
void SensorProcessing::processSus(
@ -174,9 +186,8 @@ void SensorProcessing::processSus(
const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
double *sunVectorDerivative, bool *sunVectorDerivativeValid) {
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) {
if (sus0valid) {
sus0valid = susConverter.checkSunSensorData(sus0Value);
}
@ -216,142 +227,176 @@ void SensorProcessing::processSus(
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
*sunDirEstValid = false;
return;
} else {
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
// Transformation into Geomtry Frame
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
if (sus0valid) {
MatrixOperations<float>::multiply(
susParameters->sus0orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1);
}
if (sus1valid) {
MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1);
}
if (sus2valid) {
MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
susParameters->sus2coeffBeta),
sus2VecBody, 3, 3, 1);
}
if (sus3valid) {
MatrixOperations<float>::multiply(
susParameters->sus3orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
susParameters->sus3coeffBeta),
sus3VecBody, 3, 3, 1);
}
if (sus4valid) {
MatrixOperations<float>::multiply(
susParameters->sus4orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
susParameters->sus4coeffBeta),
sus4VecBody, 3, 3, 1);
}
if (sus5valid) {
MatrixOperations<float>::multiply(
susParameters->sus5orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
susParameters->sus5coeffBeta),
sus5VecBody, 3, 3, 1);
}
if (sus6valid) {
MatrixOperations<float>::multiply(
susParameters->sus6orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
susParameters->sus6coeffBeta),
sus6VecBody, 3, 3, 1);
}
if (sus7valid) {
MatrixOperations<float>::multiply(
susParameters->sus7orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
susParameters->sus7coeffBeta),
sus7VecBody, 3, 3, 1);
}
if (sus8valid) {
MatrixOperations<float>::multiply(
susParameters->sus8orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
susParameters->sus8coeffBeta),
sus8VecBody, 3, 3, 1);
}
if (sus9valid) {
MatrixOperations<float>::multiply(
susParameters->sus9orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
susParameters->sus9coeffBeta),
sus9VecBody, 3, 3, 1);
}
if (sus10valid) {
MatrixOperations<float>::multiply(
susParameters->sus10orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
susParameters->sus10coeffBeta),
sus10VecBody, 3, 3, 1);
}
if (sus11valid) {
MatrixOperations<float>::multiply(
susParameters->sus11orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
susParameters->sus11coeffBeta),
sus11VecBody, 3, 3, 1);
}
/* ------ Mean Value: susDirEst ------ */
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
double susMeanValue[3] = {0, 0, 0};
for (uint8_t i = 0; i < 12; i++) {
if (validIds[i]) {
susMeanValue[0] += susVecBody[0][i];
susMeanValue[1] += susVecBody[1][i];
susMeanValue[2] += susVecBody[2][i];
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
susDataProcessed->setValidity(false, true);
}
}
VectorOperations<double>::normalize(susMeanValue, sunDirEst, 3);
*sunDirEstValid = true;
return;
}
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
// Transformation into Geomtry Frame
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
if (sus0valid) {
MatrixOperations<float>::multiply(
susParameters->sus0orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
susParameters->sus0coeffBeta),
sus0VecBody, 3, 3, 1);
}
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
susDataProcessed->sus0vec.setValid(sus0valid);
if (!sus0valid) {
std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus1valid) {
MatrixOperations<float>::multiply(
susParameters->sus1orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
susParameters->sus1coeffBeta),
sus1VecBody, 3, 3, 1);
}
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
susDataProcessed->sus1vec.setValid(sus1valid);
if (!sus1valid) {
std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
}
}
}
if (sus2valid) {
MatrixOperations<float>::multiply(
susParameters->sus2orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
susParameters->sus2coeffBeta),
sus2VecBody, 3, 3, 1);
}
if (sus3valid) {
MatrixOperations<float>::multiply(
susParameters->sus3orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
susParameters->sus3coeffBeta),
sus3VecBody, 3, 3, 1);
}
if (sus4valid) {
MatrixOperations<float>::multiply(
susParameters->sus4orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
susParameters->sus4coeffBeta),
sus4VecBody, 3, 3, 1);
}
if (sus5valid) {
MatrixOperations<float>::multiply(
susParameters->sus5orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
susParameters->sus5coeffBeta),
sus5VecBody, 3, 3, 1);
}
if (sus6valid) {
MatrixOperations<float>::multiply(
susParameters->sus6orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
susParameters->sus6coeffBeta),
sus6VecBody, 3, 3, 1);
}
if (sus7valid) {
MatrixOperations<float>::multiply(
susParameters->sus7orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
susParameters->sus7coeffBeta),
sus7VecBody, 3, 3, 1);
}
if (sus8valid) {
MatrixOperations<float>::multiply(
susParameters->sus8orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
susParameters->sus8coeffBeta),
sus8VecBody, 3, 3, 1);
}
if (sus9valid) {
MatrixOperations<float>::multiply(
susParameters->sus9orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
susParameters->sus9coeffBeta),
sus9VecBody, 3, 3, 1);
}
if (sus10valid) {
MatrixOperations<float>::multiply(
susParameters->sus10orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
susParameters->sus10coeffBeta),
sus10VecBody, 3, 3, 1);
}
if (sus11valid) {
MatrixOperations<float>::multiply(
susParameters->sus11orientationMatrix[0],
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
susParameters->sus11coeffBeta),
sus11VecBody, 3, 3, 1);
}
/* ------ Mean Value: susDirEst ------ */
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
double susMeanValue[3] = {0, 0, 0};
for (uint8_t i = 0; i < 12; i++) {
if (validIds[i]) {
susMeanValue[0] += susVecBody[0][i];
susMeanValue[1] += susVecBody[1][i];
susMeanValue[2] += susVecBody[2][i];
}
}
double susVecTot[3] = {0.0, 0.0, 0.0};
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
/* -------- Sun Derivatiative --------------------- */
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
bool susVecTotDerivativeValid = false;
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
for (uint8_t i = 0; i < 3; i++) {
sunVectorDerivative[i] = (sunDirEst[i] - savedSunVector[i]) / timeDiff;
savedSunVector[i] = sunDirEst[i];
if (timeOfSavedSusDirEst.tv_sec != 0) {
for (uint8_t i = 0; i < 3; i++) {
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
savedSusVecTot[i] = susVecTot[i];
}
}
*sunVectorDerivativeValid = true;
if (timeOfSavedSusDirEst.tv_sec == 0) {
sunVectorDerivative[0] = 0;
sunVectorDerivative[1] = 0;
sunVectorDerivative[2] = 0;
*sunVectorDerivativeValid = false;
}
timeOfSavedSusDirEst = timeOfSusMeasurement;
/* -------- Sun Model Direction (IJK frame) ------- */
@ -359,10 +404,11 @@ void SensorProcessing::processSus(
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
// Julean Centuries
double JC2000 = JD2000 / 36525;
double sunIjkModel[3] = {0.0, 0.0, 0.0};
double JC2000 = JD2000 / 36525.;
double meanLongitude =
(sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
@ -370,11 +416,46 @@ void SensorProcessing::processSus(
double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
sunVectorInertial[0] = cos(eclipticLongitude);
sunVectorInertial[1] = sin(eclipticLongitude) * cos(epsilon);
sunVectorInertial[2] = sin(eclipticLongitude) * sin(epsilon);
*sunVectorInertialValid = true;
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
{
PoolReadGuard pg(susDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
susDataProcessed->sus0vec.setValid(sus0valid);
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
susDataProcessed->sus1vec.setValid(sus1valid);
std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
susDataProcessed->sus2vec.setValid(sus2valid);
std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
susDataProcessed->sus3vec.setValid(sus3valid);
std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
susDataProcessed->sus4vec.setValid(sus4valid);
std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
susDataProcessed->sus5vec.setValid(sus5valid);
std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
susDataProcessed->sus6vec.setValid(sus6valid);
std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
susDataProcessed->sus7vec.setValid(sus7valid);
std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
susDataProcessed->sus8vec.setValid(sus8valid);
std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
susDataProcessed->sus9vec.setValid(sus9valid);
std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
susDataProcessed->sus10vec.setValid(sus10valid);
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
susDataProcessed->sus11vec.setValid(sus11valid);
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
susDataProcessed->susVecTot.setValid(true);
std::memcpy(susDataProcessed->susVecTotDerivative.value, susVecTotDerivative,
3 * sizeof(double));
susDataProcessed->susVecTotDerivative.setValid(susVecTotDerivativeValid);
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
susDataProcessed->sunIjkModel.setValid(true);
susDataProcessed->setValidity(true, false);
}
}
}
void SensorProcessing::processGyr(
@ -385,87 +466,110 @@ void SensorProcessing::processGyr(
const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
double *satRatEst, bool *satRateEstValid) {
if (!gyr0axXvalid && !gyr0axYvalid && !gyr0axZvalid && !gyr1axXvalid && !gyr1axYvalid &&
!gyr1axZvalid && !gyr2axXvalid && !gyr2axYvalid && !gyr2axZvalid && !gyr3axXvalid &&
!gyr3axYvalid && !gyr3axZvalid) {
*satRateEstValid = false;
acsctrl::GyrDataProcessed *gyrDataProcessed) {
bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
bool gyr2valid = (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid);
bool gyr3valid = (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid);
if (!gyr0valid && !gyr1valid && !gyr2valid && !gyr3valid) {
{
PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
gyrDataProcessed->setValidity(false, true);
}
}
return;
}
// Transforming Values to the Body Frame (actually it is the geometry frame atm)
double gyr0ValueBody[3] = {0, 0, 0}, gyr1ValueBody[3] = {0, 0, 0}, gyr2ValueBody[3] = {0, 0, 0},
gyr3ValueBody[3] = {0, 0, 0};
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
bool validUnit[4] = {false, false, false, false};
uint8_t validCount = 0;
if (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid) {
if (gyr0valid) {
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
validCount += 1;
validUnit[0] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
}
}
if (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid) {
if (gyr1valid) {
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
validCount += 1;
validUnit[1] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
}
}
if (gyr2axXvalid && gyr2axYvalid && gyr2axZvalid) {
if (gyr2valid) {
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
validCount += 1;
validUnit[2] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
}
}
if (gyr3axXvalid && gyr3axYvalid && gyr3axZvalid) {
if (gyr3valid) {
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
validCount += 1;
validUnit[3] = true;
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
}
}
/* -------- SatRateEst: Middle Value ------- */
double gyrValues[3][4] = {
{gyr0ValueBody[0], gyr1ValueBody[0], gyr2ValueBody[0], gyr3ValueBody[0]},
{gyr0ValueBody[1], gyr1ValueBody[1], gyr2ValueBody[1], gyr3ValueBody[1]},
{gyr0ValueBody[2], gyr1ValueBody[2], gyr2ValueBody[2], gyr3ValueBody[2]}};
double gyrValidValues[3][validCount];
uint8_t j = 0;
for (uint8_t i = 0; i < validCount; i++) {
if (validUnit[i]) {
gyrValidValues[0][j] = gyrValues[0][i];
gyrValidValues[1][j] = gyrValues[1][i];
gyrValidValues[2][j] = gyrValues[2][i];
j += 1;
// take ADIS measurements, if both avail
// if just one ADIS measurement avail, perform sensor fusion
double gyrVecTot[3] = {0.0, 0.0, 0.0};
if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
double gyr02ValuesSum[3];
VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
} else {
for (uint8_t i = 0; i < 3; i++) {
gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
}
}
{
PoolReadGuard pg(gyrDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(gyrDataProcessed->gyr0vec.value, gyr0ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr0vec.setValid(gyr0valid);
std::memcpy(gyrDataProcessed->gyr1vec.value, gyr1ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr1vec.setValid(gyr1valid);
std::memcpy(gyrDataProcessed->gyr2vec.value, gyr2ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr2vec.setValid(gyr2valid);
std::memcpy(gyrDataProcessed->gyr3vec.value, gyr3ValueBody, 3 * sizeof(double));
gyrDataProcessed->gyr3vec.setValid(gyr3valid);
std::memcpy(gyrDataProcessed->gyrVecTot.value, gyrVecTot, 3 * sizeof(double));
gyrDataProcessed->gyrVecTot.setValid(true);
gyrDataProcessed->setValidity(true, false);
}
}
// Selection Sort
double gyrValidValuesSort[3][validCount];
MathOperations<double>::selectionSort(*gyrValidValues, *gyrValidValuesSort, 3, validCount);
uint8_t n = ceil(validCount / 2);
satRatEst[0] = gyrValidValuesSort[0][n];
satRatEst[1] = gyrValidValuesSort[1][n];
satRatEst[2] = gyrValidValuesSort[2][n];
*satRateEstValid = true;
}
void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
const double gps0altitude, const uint32_t gps0UnixSeconds,
const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
double *gcLatitude, double *gdLongitude, double *gpsVelocityE) {
void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
const bool validGps,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process
double gdLongitude, gcLatitude;
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
*gdLongitude = gps0longitude * PI / 180;
double latitudeRad = gps0latitude * PI / 180;
gdLongitude = gpsLongitude * PI / 180;
double latitudeRad = gpsLatitude * PI / 180;
double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2);
*gcLatitude = atan(factor * tan(latitudeRad));
validGcLatitude = true;
gcLatitude = atan(factor * tan(latitudeRad));
// Calculation of the satellite velocity in earth fixed frame
double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
@ -476,6 +580,18 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
VectorOperations<double>::mulScalar(deltaDistance, 1/timeDiffGpsMeas, gpsVelocityE, 3);
}
}
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->setValidity(validGps, validGps);
if (!validGps) {
gpsDataProcessed->gdLongitude.value = 0.0;
gpsDataProcessed->gcLatitude.value = 0.0;
}
}
savedPosSatE[0] = posSatE[0];
savedPosSatE[1] = posSatE[1];
savedPosSatE[2] = posSatE[2];
@ -489,30 +605,31 @@ void SensorProcessing::processGps(const double gps0latitude, const double gps0lo
}
void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
ACS::OutputValues *outputValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters) {
sensorValues->update();
processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters,
&outputValues->gcLatitude, &outputValues->gdLongitude,
outputValues->gpsVelocity);
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
gpsDataProcessed);
outputValues->mgmUpdated = processMgm(
sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
sensorValues->mgm1Rm3100Set.fieldStrengths.value,
sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
sensorValues->mgm2Lis3Set.fieldStrengths.value,
sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(), sensorValues->imtqMgmSet.mtmRawNt.value,
sensorValues->imtqMgmSet.mtmRawNt.isValid(), now, &acsParameters->mgmHandlingParameters,
outputValues->gcLatitude, outputValues->gdLongitude, sensorValues->gpsSet.altitude.value,
sensorValues->gpsSet.isValid(), outputValues->magFieldEst, &outputValues->magFieldEstValid,
outputValues->magFieldModel, &outputValues->magFieldModelValid,
outputValues->magneticFieldVectorDerivative,
&outputValues->magneticFieldVectorDerivativeValid); // VALID outputs- PoolVariable ?
processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
sensorValues->mgm1Rm3100Set.fieldStrengths.value,
sensorValues->mgm1Rm3100Set.fieldStrengths.isValid(),
sensorValues->mgm2Lis3Set.fieldStrengths.value,
sensorValues->mgm2Lis3Set.fieldStrengths.isValid(),
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);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@ -527,10 +644,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
outputValues->sunDirEst, &outputValues->sunDirEstValid, outputValues->sunDirModel,
&outputValues->sunDirModelValid, outputValues->sunVectorDerivative,
&outputValues->sunVectorDerivativeValid);
// VALID outputs ?
susDataProcessed);
processGyr(
sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@ -545,6 +659,5 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
&acsParameters->gyrHandlingParameters, outputValues->satRateEst,
&outputValues->satRateEstValid);
&acsParameters->gyrHandlingParameters, gyrDataProcessed);
}

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

@ -11,7 +11,6 @@
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "OutputValues.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "config/classIds.h"
@ -23,19 +22,21 @@ class SensorProcessing {
SensorProcessing(AcsParameters *acsParameters_);
virtual ~SensorProcessing();
void process(timeval now, ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
void process(timeval now, ACS::SensorValues *sensorValues,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const AcsParameters *acsParameters); // Will call protected functions
private:
protected:
// short description needed for every function
bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel,
bool *magFieldModelValid, double *magneticFieldVectorDerivative,
bool *magneticFieldVectorDerivativeValid); // Output
acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
bool sus1valid, const uint16_t *sus2Value, bool sus2valid,
@ -47,9 +48,8 @@ class SensorProcessing {
bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
timeval timeOfSusMeasurement,
const AcsParameters::SusHandlingParameters *susParameters,
const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
double *sunVectorDerivative, bool *sunVectorDerivativeValid);
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed);
void processGyr(const double gyr0axXvalue, bool gyr0axXvalid, const double gyr0axYvalue,
bool gyr0axYvalid, const double gyr0axZvalue, bool gyr0axZvalid,
@ -60,19 +60,17 @@ class SensorProcessing {
const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
timeval timeOfGyrMeasurement,
const AcsParameters::GyrHandlingParameters *gyrParameters, double *satRatEst,
bool *satRateEstValid);
const AcsParameters::GyrHandlingParameters *gyrParameters,
acsctrl::GyrDataProcessed *gyrDataProcessed);
void processStr();
void processGps(const double gps0latitude, const double gps0longitude,
const double gps0altitude, const uint32_t gps0UnixSeconds,
const bool validGps, const AcsParameters::GpsParameters *gpsParameters,
double *gcLatitude, double *gdLongitude, double *gpsVelocityE);
void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
acsctrl::GpsDataProcessed *gpsDataProcessed);
double savedMagFieldEst[3];
double savedMgmVecTot[3];
timeval timeOfSavedMagFieldEst;
double savedSunVector[3];
double savedSusVecTot[3];
timeval timeOfSavedSusDirEst;
bool validMagField;
bool validGcLatitude;
@ -80,6 +78,7 @@ class SensorProcessing {
double savedPosSatE[3];
uint32_t timeOfSavedPosSatE;
bool validSavedPosSatE;
const float zeroVector[3] = {0.0, 0.0, 0.0};
SusConverter susConverter;
AcsParameters acsParameters;

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

@ -82,12 +82,14 @@ ReturnValue_t SensorValues::update() {
ReturnValue_t susUpdate = updateSus();
ReturnValue_t gyrUpdate = updateGyr();
ReturnValue_t strUpdate = updateStr();
ReturnValue_t gpsUpdate = updateGps();
ReturnValue_t rwUpdate = updateRw();
if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate) == returnvalue::FAILED) {
if ((mgmUpdate && susUpdate && gyrUpdate && strUpdate && gpsUpdate && rwUpdate) ==
returnvalue::FAILED) {
return returnvalue::FAILED;
};
return returnvalue::OK;
}
} // namespace ACS
// namespace ACS

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

@ -1,17 +1,15 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <commonObjects.h>
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
namespace ACS {

20
mission/controller/acs/SusConverter.cpp
View File

@ -107,17 +107,17 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
float* SusConverter::calculateSunVector() {
// Calculate the normalized Sun Vector
sunVectorBodyFrame[0] = (tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorBodyFrame[1] = (tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorBodyFrame[2] =
(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
sunVectorSensorFrame[2] =
-(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
return sunVectorBodyFrame;
return sunVectorSensorFrame;
}
float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],

10
mission/controller/acs/SusConverter.h
View File

@ -27,11 +27,11 @@ class SusConverter {
const float coeffBeta[9][10]);
private:
float alphaBetaRaw[2]; //[°]
// float coeffAlpha[9][10];
// float coeffBeta[9][10];
float alphaBetaCalibrated[2]; //[°]
float sunVectorBodyFrame[3]; //[-]
float alphaBetaRaw[2]; //[°]
// float coeffAlpha[9][10];
// float coeffBeta[9][10];
float alphaBetaCalibrated[2]; //[°]
float sunVectorSensorFrame[3]; //[-]
bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,

24
mission/controller/acs/config/classIds.h
View File

@ -1,26 +1,18 @@
/*
* classIds.h
*
* Created on: 1 Mar 2022
* Author: rooob
*/
#ifndef ACS_CONFIG_CLASSIDS_H_
#define ACS_CONFIG_CLASSIDS_H_
#include <common/config/commonClassIds.h>
#include <common/config/eive/resultClassIds.h>
#include <fsfw/returnvalues/FwClassIds.h>
namespace CLASS_ID {
enum eiveclassIds: uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
PTG,
DETUMBLE,
EIVE_CLASS_ID_END // [EXPORT] : [END]
enum eiveclassIds : uint8_t {
EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
KALMAN,
SAFE,
PTG,
DETUMBLE,
EIVE_CLASS_ID_END // [EXPORT] : [END]
};
}
#endif /* ACS_CONFIG_CLASSIDS_H_ */

7
mission/controller/acs/control/CMakeLists.txt
View File

@ -1,5 +1,2 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE Detumble.cpp
PtgCtrl.cpp
SafeCtrl.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE Detumble.cpp PtgCtrl.cpp
SafeCtrl.cpp)

47
mission/controller/acs/control/Detumble.cpp
View File

@ -6,33 +6,26 @@
* Author: Robin Marquardt
*/
#include "Detumble.h"
#include "../util/MathOperations.h"
#include <math.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/sign.h>
#include <math.h>
#include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
Detumble::~Detumble() {}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
Detumble::~Detumble(){
}
void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
const double *magField, const bool *magFieldValid,
double *magMom) {
@ -47,18 +40,16 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
}
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) {
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
double *magMom) {
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i<3; i++) {
magMom[i] = - dipolMax * sign(magRate[i]);
}
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
return returnvalue::OK;

40
mission/controller/acs/control/Detumble.h
View File

@ -8,33 +8,32 @@
#ifndef ACS_CONTROL_DETUMBLE_H_
#define ACS_CONTROL_DETUMBLE_H_
#include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
class Detumble{
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
public:
Detumble(AcsParameters *acsParameters_);
virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
/* @brief: Load AcsParameters für this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField,
const bool magFieldValid, double *magMom);
ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid,
const double *magField, const bool *magFieldValid,
double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
@ -48,4 +47,3 @@ private:
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

117
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -5,10 +5,8 @@
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include "../util/MathOperations.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
@ -16,77 +14,76 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
}
PtgCtrl::~PtgCtrl(){
PtgCtrl::~PtgCtrl() {}
}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices =&(acsParameters_->rwMatrices);
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
//------------------------------------------------------------------------------------------------
// Compute gain matrix K and P matrix
//------------------------------------------------------------------------------------------------
double om = pointingModeControllerParameters->om;
double zeta = pointingModeControllerParameters->zeta;
double qErrorMin = pointingModeControllerParameters->qiMin;
double omMax = pointingModeControllerParameters->omMax;
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om, 2);
double cInt = 2 * om * zeta;
double kInt = 2 * pow(om,2);
double qErrorLaw[3] = {0, 0, 0};
double qErrorLaw[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
} else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
for (int i = 0; i < 3; i++) {
if (abs(qError[i]) < qErrorMin) {
qErrorLaw[i] = qErrorMin;
}
else {
qErrorLaw[i] = abs(qError[i]);
}
}
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double gain1 = cInt * omMax / qErrorLawNorm;
double gainVector[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply(*gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix),
*gainMatrix, 3, 3, 3);
double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixDiagonal[0][0] = gainVector[0];
gainMatrixDiagonal[1][1] = gainVector[1];
gainMatrixDiagonal[2][2] = gainVector[2];
MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3,
3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------
// Torque Calculations for the reaction wheels
//------------------------------------------------------------------------------------------------
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
double pError[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
double pErrorSign[3] = {0, 0, 0};
for (int i = 0; i < 3; i++) {
if (abs(pError[i]) > 1) {
pErrorSign[i] = sign(pError[i]);
}
@ -113,7 +110,7 @@ void PtgCtrl::ptgLaw(const double mode, const double *qError, const double *delt
}
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
int32_t *speedRw3, double *mgtDpDes) {
if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
@ -124,7 +121,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
}
// calculating momentum of satellite and momentum of reaction wheels
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
@ -146,7 +143,7 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, doubl
void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
double wheelMomentum[4] = {0, 0, 0, 0};
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
// Conversion to [rad/s] for further calculations

3
mission/controller/acs/control/PtgCtrl.h
View File

@ -19,7 +19,6 @@
#include <time.h>
#include "../AcsParameters.h"
#include "../OutputValues.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
@ -45,7 +44,7 @@ class PtgCtrl {
void ptgLaw(const double mode, const double *qError, const double *deltaRate,
const double *rwPseudoInv, double *torqueRws);
void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
double *mgtDpDes);

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

@ -6,182 +6,173 @@
*/
#include "SafeCtrl.h"
#include "../util/MathOperations.h"
#include <math.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 <math.h>
#include "../util/MathOperations.h"
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, 3);
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
3);
}
SafeCtrl::~SafeCtrl(){
SafeCtrl::~SafeCtrl() {}
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_){
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
bool rateMekfValid, double *sunDirRef, double *satRatRef,
double *outputAngle, double *outputMagMomB, bool *outputValid) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
double *magFieldModel, bool *magFieldModelValid,
double *sunDirModel, bool *sunDirModelValid,
double *satRateMekf, bool *rateMekfValid,
double *sunDirRef, double *satRatRef,
double *outputMagMomB, bool *outputValid){
double kRate = 0, kAlign = 0;
kRate = safeModeControllerParameters->k_rate_mekf;
kAlign = safeModeControllerParameters->k_align_mekf;
if ( !(*quatBJValid) || !(*magFieldModelValid) || !(*sunDirModelValid) ||
!(*rateMekfValid)) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
double kRate = 0, kAlign = 0;
kRate = safeModeControllerParameters->k_rate_mekf;
kAlign = safeModeControllerParameters->k_align_mekf;
double crossSun[3] = {0, 0, 0};
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
MathOperations<double>::dcmFromQuat(quatBJ, *dcmBJ);
double sunDirB[3] = {0,0,0}, magFieldB[3] = {0,0,0};
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
double crossSun[3] = {0, 0, 0};
// calc angle alpha between sunDirRef and sunDIr
double alpha = 0, dotSun = 0;
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
alpha = acos(dotSun);
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
// Law Torque calculations
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
torqueAll[3] = {0, 0, 0};
// calc angle alpha between sunDirRef and sunDIr
double alpha = 0, dotSun = 0;
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
alpha = acos(dotSun);
double scalarFac = 0;
scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
// Law Torque calculations
double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0},
torqueRate[3] = {0, 0, 0}, torqueAll[3] = {0, 0, 0};
double rateSafeMode[3] = {0, 0, 0};
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
double scalarFac = 0;
scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
double rateSafeMode[3] = {0,0,0};
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
// MagMom B (orthogonal torque)
double torqueMgt[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque)
double torqueMgt[3] = {0,0,0};
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
double normMag = VectorOperations<double>::norm(magFieldB, 3);
VectorOperations<double>::mulScalar(torqueMgt, 1/pow(normMag,2), outputMagMomB, 3);
*outputValid = true;
return returnvalue::OK;
*outputAngle = alpha;
*outputValid = true;
return returnvalue::OK;
}
// Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
double *sunRateB, bool *sunRateBValid,
double *magFieldB, bool *magFieldBValid,
double *magRateB, bool *magRateBValid,
double *sunDirRef, double *satRateRef,
double *outputMagMomB, bool *outputValid){
void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid,
double *magRateB, bool magRateBValid, double *sunDirRef,
double *satRateRef, double *outputAngle, double *outputMagMomB,
bool *outputValid) {
// Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
}
// Check for invalid Inputs
if ( !susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
}
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Cosinus angle between sunDir and magDir
double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
// Rate parallel to sun direction and magnetic field direction
double rateParaSun = 0, rateParaMag = 0;
double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
rateFactor = 1 - pow(cosAngleSunMag, 2);
rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Rate parallel to sun direction and magnetic field direction
double rateParaSun = 0, rateParaMag = 0;
double dotSunRateMag = 0, dotmagRateSun = 0,
rateFactor = 0;
dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
rateFactor = 1 - pow(cosAngleSunMag,2);
rateParaSun = ( dotmagRateSun + cosAngleSunMag * dotSunRateMag ) / rateFactor;
rateParaMag = ( dotSunRateMag + cosAngleSunMag * dotmagRateSun ) / rateFactor;
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
/* Only valid if angle between sun direction and magnetic field direction
is sufficiently large */
double sinAngle = 0;
sinAngle = sin(acos(cos(cosAngleSunMag)));
double sinAngle = 0;
sinAngle = sin(acos(cos(cosAngleSunMag)));
if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
return;
}
if ( !(sinAngle > sin( safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
return;
}
// Rate for Torque Calculation
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Rate for Torque Calculation
double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
// Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
// Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
double torqueAlign[3] = {0, 0, 0};
double angleAlignErr = acos(cosAngleAlignErr);
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
double torqueAlign[3] = {0, 0, 0};
double angleAlignErr = acos(cosAngleAlignErr);
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
// Torque Rate Calculations
double torqueRate[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
//Torque Rate Calculations
double torqueRate[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
// Final torque
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3,
1);
//Final torque
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, 1);
//Magnetic moment
double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
double magMomFactor = pow( VectorOperations<double>::norm(magFieldB, 3), 2 );
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1/magMomFactor, magMomB, 3);
outputMagMomB[0] = magMomB[0];
outputMagMomB[1] = magMomB[1];
outputMagMomB[2] = magMomB[2];
*outputValid = true;
// Magnetic moment
double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
}

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

@ -8,57 +8,45 @@
#ifndef SAFECTRL_H_
#define SAFECTRL_H_
#include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "../config/classIds.h"
class SafeCtrl{
class SafeCtrl {
public:
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
public:
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
SafeCtrl(AcsParameters *acsParameters_);
virtual ~SafeCtrl();
void loadAcsParameters(AcsParameters *acsParameters_);
static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool rateMekfValid, double *sunDirRef,
double *satRatRef, // From Guidance (!)
double *outputAngle, double *outputMagMomB, bool *outputValid);
void loadAcsParameters(AcsParameters *acsParameters_);
void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
double *outputMagMomB, bool *outputValid);
ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
double *magFieldModel, bool *magFieldModelValid,
double *sunDirModel, bool *sunDirModelValid,
double *satRateMekf, bool *rateMekfValid,
double *sunDirRef, double *satRatRef, // From Guidance (!)
double *outputMagMomB, bool *outputValid);
void idleSunPointing(); // with reaction wheels
void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
double *sunRateB, bool *sunRateBValid,
double *magFieldB, bool *magFieldBValid,
double *magRateB, bool *magRateBValid,
double *sunDirRef, double *satRateRef,
double *outputMagMomB, bool *outputValid);
void idleSunPointing(); // with reaction wheels
protected:
private:
AcsParameters::SafeModeControllerParameters* safeModeControllerParameters;
AcsParameters::InertiaEIVE* inertiaEIVE;
double gainMatrixInertia[3][3];
double magFieldBState[3];
timeval magFieldBStateTime;
protected:
private:
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
double gainMatrixInertia[3][3];
double magFieldBState[3];
timeval magFieldBStateTime;
};
#endif /* ACS_CONTROL_SAFECTRL_H_ */

156
mission/controller/acs/util/CholeskyDecomposition.h
View File

@ -8,95 +8,91 @@
#ifndef CHOLESKYDECOMPOSITION_H_
#define CHOLESKYDECOMPOSITION_H_
#include <math.h>
//typedef unsigned int uint8_t;
// typedef unsigned int uint8_t;
template<typename T1, typename T2=T1, typename T3=T2>
template <typename T1, typename T2 = T1, typename T3 = T2>
class CholeskyDecomposition {
public:
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension)
{
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
return cholsl(matrix, result, tempMatrix, dimension);
}
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double * a, double * p, uint8_t n) {
int8_t i,j,k;
double sum;
public:
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
return cholsl(matrix, result, tempMatrix, dimension);
}
for (i = 0; i < n; i++) {
for (j = i; j < n; j++) {
sum = a[i*n+j];
for (k = i - 1; k >= 0; k--) {
sum -= a[i*n+k] * a[j*n+k];
}
if (i == j) {
if (sum <= 0) {
return 1; /* error */
}
p[i] = sqrt(sum);
}
else {
a[j*n+i] = sum / p[i];
}
}
}
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double *a, double *p, uint8_t n) {
int8_t i, j, k;
double sum;
return 0; /* success */
}
for (i = 0; i < n; i++) {
for (j = i; j < n; j++) {
sum = a[i * n + j];
for (k = i - 1; k >= 0; k--) {
sum -= a[i * n + k] * a[j * n + k];
}
if (i == j) {
if (sum <= 0) {
return 1; /* error */
}
p[i] = sqrt(sum);
} else {
a[j * n + i] = sum / p[i];
}
}
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldcsl(double * A, double * a, double * p, uint8_t n)
{
uint8_t i,j,k; double sum;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
a[i*n+j] = A[i*n+j];
if (choldc1(a, p, n)) return 1;
for (i = 0; i < n; i++) {
a[i*n+i] = 1 / p[i];
for (j = i + 1; j < n; j++) {
sum = 0;
for (k = i; k < j; k++) {
sum -= a[j*n+k] * a[k*n+i];
}
a[j*n+i] = sum / p[j];
}
}
return 0; /* success */
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
double sum;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
if (choldc1(a, p, n)) return 1;
for (i = 0; i < n; i++) {
a[i * n + i] = 1 / p[i];
for (j = i + 1; j < n; j++) {
sum = 0;
for (k = i; k < j; k++) {
sum -= a[j * n + k] * a[k * n + i];
}
a[j * n + i] = sum / p[j];
}
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t cholsl(double * A,double * a,double * p, uint8_t n)
{
uint8_t i,j,k;
if (choldcsl(A,a,p,n)) return 1;
for (i = 0; i < n; i++) {
for (j = i + 1; j < n; j++) {
a[i*n+j] = 0.0;
}
}
for (i = 0; i < n; i++) {
a[i*n+i] *= a[i*n+i];
for (k = i + 1; k < n; k++) {
a[i*n+i] += a[k*n+i] * a[k*n+i];
}
for (j = i + 1; j < n; j++) {
for (k = j; k < n; k++) {
a[i*n+j] += a[k*n+i] * a[k*n+j];
}
}
}
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
a[i*n+j] = a[j*n+i];
}
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
if (choldcsl(A, a, p, n)) return 1;
for (i = 0; i < n; i++) {
for (j = i + 1; j < n; j++) {
a[i * n + j] = 0.0;
}
}
for (i = 0; i < n; i++) {
a[i * n + i] *= a[i * n + i];
for (k = i + 1; k < n; k++) {
a[i * n + i] += a[k * n + i] * a[k * n + i];
}
for (j = i + 1; j < n; j++) {
for (k = j; k < n; k++) {
a[i * n + j] += a[k * n + i] * a[k * n + j];
}
}
}
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
a[i * n + j] = a[j * n + i];
}
}
return 0; /* success */
}
return 0; /* success */
}
};
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */

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

@ -1,107 +1,97 @@
/*
* MathOperations.h
*
* Created on: 3 Mar 2022
* Author: rooob
*/
#ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_
#include <math.h>
#include <sys/time.h>
#include <stdint.h>
#include <string.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <sys/time.h>
#include <iostream>
using namespace Math;
template<typename T1, typename T2 = T1>
template <typename T1, typename T2 = T1>
class MathOperations {
public:
static void skewMatrix(const T1 vector[], T2 *result) {
// Input Dimension [3], Output [3][3]
result[0] = 0;
result[1] = -vector[2];
result[2] = vector[1];
result[3] = vector[2];
result[4] = 0;
result[5] = -vector[0];
result[6] = -vector[1];
result[7] = vector[0];
result[8] = 0;
}
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[],
T2 *result, uint8_t size = 3) {
// Looks like MatrixOpertions::multiply is able to do the same thing
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
result[resultColumn + size * resultRow] = vector1[resultRow]
* transposeVector2[resultColumn];
public:
static void skewMatrix(const T1 vector[], T2 *result) {
// Input Dimension [3], Output [3][3]
result[0] = 0;
result[1] = -vector[2];
result[2] = vector[1];
result[3] = vector[2];
result[4] = 0;
result[5] = -vector[0];
result[6] = -vector[1];
result[7] = vector[0];
result[8] = 0;
}
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
uint8_t size = 3) {
// Looks like MatrixOpertions::multiply is able to do the same thing
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
result[resultColumn + size * resultRow] =
vector1[resultRow] * transposeVector2[resultColumn];
}
}
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
matrixMag[i][j] = magEstB[i] * magEstB[j];
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
}
}
}
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
matrixMag[i][j] = magEstB[i] * magEstB[j];
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
}
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize,
uint8_t colSize) {
int min_idx;
T1 temp;
memcpy(result, matrix, rowSize * colSize * sizeof(*result));
// One by one move boundary of unsorted subarray
for (int k = 0; k < rowSize; k++) {
for (int i = 0; i < colSize - 1; i++) {
// Find the minimum element in unsorted array
min_idx = i;
for (int j = i + 1; j < colSize; j++) {
if (result[j + k * colSize]
< result[min_idx + k * colSize]) {
min_idx = j;
}
}
// Swap the found minimum element with the first element
temp = result[i + k * colSize];
result[i + k * colSize] = result[min_idx + k * colSize];
result[min_idx + k * colSize] = temp;
}
}
}
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
int min_idx;
T1 temp;
memcpy(result, matrix, rowSize * colSize * sizeof(*result));
// One by one move boundary of unsorted subarray
for (int k = 0; k < rowSize; k++) {
for (int i = 0; i < colSize - 1; i++) {
// Find the minimum element in unsorted array
min_idx = i;
for (int j = i + 1; j < colSize; j++) {
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
min_idx = j;
}
}
// Swap the found minimum element with the first element
temp = result[i + k * colSize];
result[i + k * colSize] = result[min_idx + k * colSize];
result[min_idx + k * colSize] = temp;
}
}
}
static void convertDateToJD2000(const T1 time, T2 julianDate){
// time = { Y, M, D, h, m,s}
// time in sec and microsec -> The Epoch (unixtime)
julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12))
+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440;
}
static T1 convertUnixToJD2000(timeval time){
//time = {{s},{us}}
T1 julianDate2000;
julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545;
return julianDate2000;
}
static T1 convertUnixToJD2000(timeval time) {
// time = {{s},{us}}
T1 julianDate2000;
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
return julianDate2000;
}
static void dcmFromQuat(const T1 vector[], T1 *outputDcm){
// convention q = [qx,qy,qz, qw]
outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]);
outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]);
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
// convention q = [qx,qy,qz, qw]
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]);
outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]);
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]);
outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]);
outputDcm[8] = -pow(vector[0],2) - pow(vector[1],2) + pow(vector[2],2) + pow(vector[3],2);
}
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
}
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
@ -123,7 +113,6 @@ public:
cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat);
}
static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
@ -142,26 +131,26 @@ public:
JD2000Floor += 0.5;
}
double JC2000 = JD2000Floor / 36525;
double sec = (JD2000 - JD2000Floor) * 86400;
double gmst = 0; //greenwich mean sidereal time
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) -
0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec;
double rest = gmst / 86400;
double FloorRest = floor(rest);
double secOfDay = rest-FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * PI / 180;
double JC2000 = JD2000Floor / 36525;
double sec = (JD2000 - JD2000Floor) * 86400;
double gmst = 0; // greenwich mean sidereal time
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
double rest = gmst / 86400;
double FloorRest = floor(rest);
double secOfDay = rest - FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * PI / 180;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
outputDcmEJ[2] = 0;
outputDcmEJ[3] = -sin(gmst);
outputDcmEJ[4] = cos(gmst);
outputDcmEJ[5] = 0;
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
outputDcmEJ[2] = 0;
outputDcmEJ[3] = -sin(gmst);
outputDcmEJ[4] = cos(gmst);
outputDcmEJ[5] = 0;
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
@ -172,8 +161,8 @@ public:
double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
}
}
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
* give also the back the derivative of this matrix
@ -259,7 +248,7 @@ public:
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 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
nutation[0][0]=cos(dp);
nutation[1][0]=cos(e+de)*sin(dp);
@ -290,9 +279,7 @@ public:
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
}
static void inverseMatrixDimThree(const T1 *matrix, T1 * output){
int i,j;
double determinant;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]},
@ -310,6 +297,113 @@ public:
}
}
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
float det = 0;
T1 matrix[size][size], submatrix[size - 1][size - 1];
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
if (size == 2)
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
else {
for (uint8_t col = 0; col < size; col++) {
int subRow = 0;
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
int subCol = 0;
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
if (colIndex == col) continue;
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
subCol++;
}
subRow++;
}
det += (pow(-1, col) * matrix[0][col] *
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
}
}
return det;
}
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
if (MathOperations<T1>::matrixDeterminant(inputMatrix, size) == 0) {
return 1; // Matrix is singular and not invertible
}
T1 matrix[size][size], identity[size][size];
// reformat array to matrix
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
// init identity matrix
std::memset(identity, 0.0, sizeof(identity));
for (uint8_t diag = 0; diag < size; diag++) {
identity[diag][diag] = 1;
}
// gauss-jordan algo
// sort matrix such as no diag entry shall be 0
// should not be needed as such a matrix has a det=0
for (uint8_t row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
bool swaped = false;
uint8_t rowIndex = 0;
while ((rowIndex < size) && !swaped) {
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
swaped = true;
}
rowIndex++;
}
if (!swaped) {
return 1; // matrix not invertible
}
}
}
for (int row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
uint8_t rowIndex;
if (row == 0) {
rowIndex = size - 1;
} else {
rowIndex = row - 1;
}
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
row--;
if (row < 0) {
return 1; // Matrix is not invertible
}
}
}
// remove non diag elements in matrix (jordan)
for (int row = 0; row < size; row++) {
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
if (row != rowIndex) {
double ratio = matrix[rowIndex][row] / matrix[row][row];
for (int colIndex = 0; colIndex < size; colIndex++) {
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
}
}
}
}
// normalize rows in matrix (gauss)
for (int row = 0; row < size; row++) {
for (int col = 0; col < size; col++) {
identity[row][col] = identity[row][col] / matrix[row][row];
}
}
std::memcpy(inverse, identity, sizeof(identity));
return 0; // successful inversion
}
};
#endif /* ACS_MATH_MATHOPERATIONS_H_ */

14
mission/controller/controllerdefinitions/AcsControllerDefinitions.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_
#include <fsfw/modes/HasModesIF.h>
namespace acs {
enum CtrlModes { OFF = HasModesIF::MODE_OFF, SAFE = 1, DETUMBLE = 2, IDLE = 3, TARGET_PT = 4 };
static constexpr Submode_t IDLE_CHARGE = 1;
} // namespace acs
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCONTROLLERDEFINITIONS_H_ */

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

@ -8,16 +8,37 @@
namespace acsctrl {
enum SetIds : uint32_t { MGM_SENSOR_DATA, SUS_SENSOR_DATA };
enum SetIds : uint32_t {
MGM_SENSOR_DATA,
MGM_PROCESSED_DATA,
SUS_SENSOR_DATA,
SUS_PROCESSED_DATA,
GYR_SENSOR_DATA,
GYR_PROCESSED_DATA,
GPS_PROCESSED_DATA,
MEKF_DATA,
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA
};
enum PoolIds : lp_id_t {
// MGM Raw
MGM_0_LIS3_UT,
MGM_1_RM3100_UT,
MGM_2_LIS3_UT,
MGM_3_RM3100_UT,
MGM_IMTQ_CAL_NT,
MGM_IMTQ_CAL_ACT_STATUS,
// MGM Processed
MGM_0_VEC,
MGM_1_VEC,
MGM_2_VEC,
MGM_3_VEC,
MGM_4_VEC,
MGM_VEC_TOT,
MGM_VEC_TOT_DERIVATIVE,
MAG_IGRF_MODEL,
// SUS Raw
SUS_0_N_LOC_XFYFZM_PT_XF,
SUS_6_R_LOC_XFYBZM_PT_XF,
@ -35,15 +56,64 @@ enum PoolIds : lp_id_t {
SUS_5_N_LOC_XFYMZB_PT_ZB,
SUS_11_R_LOC_XBYMZB_PT_ZB,
// SUS Processed
SUS_0_VEC,
SUS_1_VEC,
SUS_2_VEC,
SUS_3_VEC,
SUS_4_VEC,
SUS_5_VEC,
SUS_6_VEC,
SUS_7_VEC,
SUS_8_VEC,
SUS_9_VEC,
SUS_10_VEC,
SUS_11_VEC,
SUS_VEC_TOT,
SUS_VEC_TOT_DERIVATIVE,
SUN_IJK_MODEL,
// GYR Raw
GYR_0_ADIS,
GYR_1_L3,
GYR_2_ADIS,
GYR_3_L3,
// GYR Processed
GYR_0_VEC,
GYR_1_VEC,
GYR_2_VEC,
GYR_3_VEC,
GYR_VEC_TOT,
// GPS Processed
GC_LATITUDE,
GD_LONGITUDE,
// MEKF
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
// Ctrl Values
TGT_QUAT,
ERROR_QUAT,
ERROR_ANG,
// Actuator Cmd
RW_TARGET_TORQUE,
RW_TARGET_SPEED,
MTQ_TARGET_DIPOLE,
};
static constexpr uint8_t MGM_SET_ENTRIES = 10;
static constexpr uint8_t SUS_SET_ENTRIES = 12;
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10;
static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8;
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 = 2;
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
*/
class MgmDataRaw : public StaticLocalDataSet<MGM_SET_ENTRIES> {
class MgmDataRaw : public StaticLocalDataSet<MGM_SET_RAW_ENTRIES> {
public:
MgmDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_SENSOR_DATA) {}
@ -60,7 +130,24 @@ class MgmDataRaw : public StaticLocalDataSet<MGM_SET_ENTRIES> {
private:
};
class SusDataRaw : public StaticLocalDataSet<SUS_SET_ENTRIES> {
class MgmDataProcessed : public StaticLocalDataSet<MGM_SET_PROCESSED_ENTRIES> {
public:
MgmDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MGM_PROCESSED_DATA) {}
lp_vec_t<float, 3> mgm0vec = lp_vec_t<float, 3>(sid.objectId, MGM_0_VEC, this);
lp_vec_t<float, 3> mgm1vec = lp_vec_t<float, 3>(sid.objectId, MGM_1_VEC, this);
lp_vec_t<float, 3> mgm2vec = lp_vec_t<float, 3>(sid.objectId, MGM_2_VEC, this);
lp_vec_t<float, 3> mgm3vec = lp_vec_t<float, 3>(sid.objectId, MGM_3_VEC, this);
lp_vec_t<float, 3> mgm4vec = lp_vec_t<float, 3>(sid.objectId, MGM_4_VEC, this);
lp_vec_t<double, 3> mgmVecTot = lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT, this);
lp_vec_t<double, 3> mgmVecTotDerivative =
lp_vec_t<double, 3>(sid.objectId, MGM_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> magIgrfModel = lp_vec_t<double, 3>(sid.objectId, MAG_IGRF_MODEL, this);
private:
};
class SusDataRaw : public StaticLocalDataSet<SUS_SET_RAW_ENTRIES> {
public:
SusDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_SENSOR_DATA) {}
@ -74,8 +161,102 @@ class SusDataRaw : public StaticLocalDataSet<SUS_SET_ENTRIES> {
lp_vec_t<uint16_t, 6> sus7 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_7_R_LOC_XBYBZM_PT_XB, this);
lp_vec_t<uint16_t, 6> sus8 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_8_R_LOC_XBYBZB_PT_YB, this);
lp_vec_t<uint16_t, 6> sus9 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_9_R_LOC_XBYBZB_PT_YF, this);
lp_vec_t<uint16_t, 6> sus10 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this);
lp_vec_t<uint16_t, 6> sus11 = lp_vec_t<uint16_t, 6>(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this);
lp_vec_t<uint16_t, 6> sus10 =
lp_vec_t<uint16_t, 6>(sid.objectId, SUS_10_N_LOC_XMYBZF_PT_ZF, this);
lp_vec_t<uint16_t, 6> sus11 =
lp_vec_t<uint16_t, 6>(sid.objectId, SUS_11_R_LOC_XBYMZB_PT_ZB, this);
private:
};
class SusDataProcessed : public StaticLocalDataSet<SUS_SET_PROCESSED_ENTRIES> {
public:
SusDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, SUS_PROCESSED_DATA) {}
lp_vec_t<float, 3> sus0vec = lp_vec_t<float, 3>(sid.objectId, SUS_0_VEC, this);
lp_vec_t<float, 3> sus1vec = lp_vec_t<float, 3>(sid.objectId, SUS_1_VEC, this);
lp_vec_t<float, 3> sus2vec = lp_vec_t<float, 3>(sid.objectId, SUS_2_VEC, this);
lp_vec_t<float, 3> sus3vec = lp_vec_t<float, 3>(sid.objectId, SUS_3_VEC, this);
lp_vec_t<float, 3> sus4vec = lp_vec_t<float, 3>(sid.objectId, SUS_4_VEC, this);
lp_vec_t<float, 3> sus5vec = lp_vec_t<float, 3>(sid.objectId, SUS_5_VEC, this);
lp_vec_t<float, 3> sus6vec = lp_vec_t<float, 3>(sid.objectId, SUS_6_VEC, this);
lp_vec_t<float, 3> sus7vec = lp_vec_t<float, 3>(sid.objectId, SUS_7_VEC, this);
lp_vec_t<float, 3> sus8vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> sus9vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> sus10vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<float, 3> sus11vec = lp_vec_t<float, 3>(sid.objectId, SUS_8_VEC, this);
lp_vec_t<double, 3> susVecTot = lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT, this);
lp_vec_t<double, 3> susVecTotDerivative =
lp_vec_t<double, 3>(sid.objectId, SUS_VEC_TOT_DERIVATIVE, this);
lp_vec_t<double, 3> sunIjkModel = lp_vec_t<double, 3>(sid.objectId, SUN_IJK_MODEL, this);
private:
};
class GyrDataRaw : public StaticLocalDataSet<GYR_SET_RAW_ENTRIES> {
public:
GyrDataRaw(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_SENSOR_DATA) {}
lp_vec_t<double, 3> gyr0Adis = lp_vec_t<double, 3>(sid.objectId, GYR_0_ADIS, this);
lp_vec_t<float, 3> gyr1L3 = lp_vec_t<float, 3>(sid.objectId, GYR_1_L3, this);
lp_vec_t<double, 3> gyr2Adis = lp_vec_t<double, 3>(sid.objectId, GYR_2_ADIS, this);
lp_vec_t<float, 3> gyr3L3 = lp_vec_t<float, 3>(sid.objectId, GYR_3_L3, this);
private:
};
class GyrDataProcessed : public StaticLocalDataSet<GYR_SET_PROCESSED_ENTRIES> {
public:
GyrDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GYR_PROCESSED_DATA) {}
lp_vec_t<double, 3> gyr0vec = lp_vec_t<double, 3>(sid.objectId, GYR_0_VEC, this);
lp_vec_t<double, 3> gyr1vec = lp_vec_t<double, 3>(sid.objectId, GYR_1_VEC, this);
lp_vec_t<double, 3> gyr2vec = lp_vec_t<double, 3>(sid.objectId, GYR_2_VEC, this);
lp_vec_t<double, 3> gyr3vec = lp_vec_t<double, 3>(sid.objectId, GYR_3_VEC, this);
lp_vec_t<double, 3> gyrVecTot = lp_vec_t<double, 3>(sid.objectId, GYR_VEC_TOT, this);
private:
};
class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
public:
GpsDataProcessed(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, GPS_PROCESSED_DATA) {}
lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
private:
};
class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
public:
MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
private:
};
class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
public:
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
private:
};
class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
public:
ActuatorCmdData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ACTUATOR_CMD_DATA) {}
lp_vec_t<double, 4> rwTargetTorque = lp_vec_t<double, 4>(sid.objectId, RW_TARGET_TORQUE, this);
lp_vec_t<int32_t, 4> rwTargetSpeed = lp_vec_t<int32_t, 4>(sid.objectId, RW_TARGET_SPEED, this);
lp_vec_t<int16_t, 3> mtqTargetDipole =
lp_vec_t<int16_t, 3>(sid.objectId, MTQ_TARGET_DIPOLE, this);
private:
};

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

@ -30,8 +30,11 @@ enum PoolIds : lp_id_t {
SENSOR_PLPCDU_HEATSPREADER,
SENSOR_TCS_BOARD,
SENSOR_MAGNETTORQUER,
SENSOR_TMP1075_1,
SENSOR_TMP1075_2,
SENSOR_TMP1075_TCS_0,
SENSOR_TMP1075_TCS_1,
SENSOR_TMP1075_PLPCDU_0,
SENSOR_TMP1075_PLPCDU_1,
SENSOR_TMP1075_IF_BOARD,
SUS_0_N_LOC_XFYFZM_PT_XF,
SUS_6_R_LOC_XFYBZM_PT_XF,
@ -75,7 +78,7 @@ enum PoolIds : lp_id_t {
TEMP_ADC_PAYLOAD_PCDU
};
static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 18;
static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
static const uint8_t ENTRIES_DEVICE_TEMPERATURE_SET = 25;
static const uint8_t ENTRIES_SUS_TEMPERATURE_SET = 12;
@ -111,8 +114,14 @@ class SensorTemperatures : public StaticLocalDataSet<ENTRIES_SENSOR_TEMPERATURE_
lp_var_t<float> sensor_tcs_board = lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TCS_BOARD, this);
lp_var_t<float> sensor_magnettorquer =
lp_var_t<float>(sid.objectId, PoolIds::SENSOR_MAGNETTORQUER, this);
lp_var_t<float> sensor_tmp1075_1 = lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_1, this);
lp_var_t<float> sensor_tmp1075_2 = lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_2, this);
lp_var_t<float> tmp1075Tcs0 = lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_TCS_0, this);
lp_var_t<float> tmp1075Tcs1 = lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_TCS_1, this);
lp_var_t<float> tmp1075PlPcdu0 =
lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_PLPCDU_0, this);
lp_var_t<float> tmp1075PlPcdu1 =
lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_PLPCDU_1, this);
lp_var_t<float> tmp1075IfBrd =
lp_var_t<float>(sid.objectId, PoolIds::SENSOR_TMP1075_IF_BOARD, this);
};
/**

177
mission/core/GenericFactory.cpp
View File

@ -1,8 +1,12 @@
#include "GenericFactory.h"
#include <fsfw/cfdp/CfdpDistributor.h>
#include <fsfw/cfdp/handler/CfdpHandler.h>
#include <fsfw/cfdp/handler/RemoteConfigTableIF.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/internalerror/InternalErrorReporter.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/pus/CService200ModeCommanding.h>
#include <fsfw/pus/CService201HealthCommanding.h>
#include <fsfw/pus/Service17Test.h>
@ -14,24 +18,28 @@
#include <fsfw/pus/Service8FunctionManagement.h>
#include <fsfw/pus/Service9TimeManagement.h>
#include <fsfw/storagemanager/PoolManager.h>
#include <fsfw/tcdistribution/CCSDSDistributor.h>
#include <fsfw/tcdistribution/CcsdsDistributor.h>
#include <fsfw/tcdistribution/PusDistributor.h>
#include <fsfw/timemanager/CdsShortTimeStamper.h>
#include <mission/tmtc/TmFunnel.h>
#include <fsfw_hal/host/HostFilesystem.h>
#include <mission/tmtc/CfdpTmFunnel.h>
#include <mission/tmtc/PusTmFunnel.h>
#include <mission/tmtc/TmFunnelHandler.h>
#include "OBSWConfig.h"
#include "eive/definitions.h"
#include "fsfw/pus/Service11TelecommandScheduling.h"
#include "mission/cfdp/Config.h"
#include "objects/systemObjectList.h"
#include "tmtc/apid.h"
#include "tmtc/pusIds.h"
#if OBSW_ADD_TCPIP_BRIDGE == 1
#if OBSW_USE_TMTC_TCP_BRIDGE == 0
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
// UDP server includes
#include "fsfw/osal/common/UdpTcPollingTask.h"
#include "fsfw/osal/common/UdpTmTcBridge.h"
#else
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
// TCP server includes
#include "fsfw/osal/common/TcpTmTcBridge.h"
#include "fsfw/osal/common/TcpTmTcServer.h"
@ -46,7 +54,21 @@
#define OBSW_TM_TO_PTME 0
#endif
void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) {
namespace cfdp {
PacketInfoList<64> PACKET_LIST;
LostSegmentsList<128> LOST_SEGMENTS;
EntityId REMOTE_CFDP_ID(UnsignedByteField<uint16_t>(config::EIVE_GROUND_CFDP_ENTITY_ID));
RemoteEntityCfg GROUND_REMOTE_CFG(REMOTE_CFDP_ID);
OneRemoteConfigProvider REMOTE_CFG_PROVIDER(GROUND_REMOTE_CFG);
HostFilesystem HOST_FS;
EiveUserHandler USER_HANDLER(HOST_FS);
EiveFaultHandler EIVE_FAULT_HANDLER;
} // namespace cfdp
void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel) {
// Framework objects
new EventManager(objects::EVENT_MANAGER);
auto healthTable = new HealthTable(objects::HEALTH_TABLE);
@ -56,17 +78,18 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) {
new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER);
new VerificationReporter();
auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER);
StorageManagerIF* tcStore;
StorageManagerIF* tmStore;
{
PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {300, 32}, {200, 64},
{200, 128}, {100, 1024}, {10, 2048}};
new PoolManager(objects::TC_STORE, poolCfg);
PoolManager::LocalPoolConfig poolCfg = {{250, 16}, {250, 32}, {250, 64},
{150, 128}, {120, 1024}, {120, 2048}};
tcStore = new PoolManager(objects::TC_STORE, poolCfg);
}
{
PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {300, 32}, {100, 64},
{100, 128}, {100, 1024}, {10, 2048}};
new PoolManager(objects::TM_STORE, poolCfg);
PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {350, 32}, {350, 64},
{200, 128}, {150, 1024}, {150, 2048}};
tmStore = new PoolManager(objects::TM_STORE, poolCfg);
}
{
@ -75,58 +98,90 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_) {
new PoolManager(objects::IPC_STORE, poolCfg);
}
auto* ccsdsDistrib = new CCSDSDistributor(apid::EIVE_OBSW, objects::CCSDS_PACKET_DISTRIBUTOR);
new PusDistributor(apid::EIVE_OBSW, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
uint8_t vc = 0;
#if OBSW_TM_TO_PTME == 1
vc = config::LIVE_TM;
#endif
// Every TM packet goes through this funnel
new TmFunnel(objects::TM_FUNNEL, *timeStamper, 50, vc);
// PUS service stack
new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, apid::EIVE_OBSW,
pus::PUS_SERVICE_1, objects::TM_FUNNEL, 20);
new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, apid::EIVE_OBSW,
pus::PUS_SERVICE_2, 3, 10);
new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, apid::EIVE_OBSW,
pus::PUS_SERVICE_3);
new Service5EventReporting(
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, apid::EIVE_OBSW, pus::PUS_SERVICE_5), 15,
45);
new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, apid::EIVE_OBSW,
pus::PUS_SERVICE_8, 16, 60);
new Service9TimeManagement(
PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, apid::EIVE_OBSW, pus::PUS_SERVICE_9));
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, apid::EIVE_OBSW, pus::PUS_SERVICE_11),
ccsdsDistrib);
new Service17Test(PsbParams(objects::PUS_SERVICE_17_TEST, apid::EIVE_OBSW, pus::PUS_SERVICE_17));
new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, apid::EIVE_OBSW,
pus::PUS_SERVICE_20);
new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, apid::EIVE_OBSW,
pus::PUS_SERVICE_200, 8);
new CService201HealthCommanding(objects::PUS_SERVICE_201_HEALTH, apid::EIVE_OBSW,
pus::PUS_SERVICE_201);
#if OBSW_ADD_TCPIP_BRIDGE == 1
#if OBSW_USE_TMTC_TCP_BRIDGE == 0
auto tmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR);
new UdpTcPollingTask(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE);
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
auto udpBridge = new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR);
new UdpTcPollingTask(objects::UDP_TMTC_POLLING_TASK, objects::UDP_TMTC_SERVER);
sif::info << "Created UDP server for TMTC commanding with listener port "
<< udpBridge->getUdpPort() << std::endl;
#else
auto tmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR);
auto tcpServer = new TcpTmTcServer(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE);
udpBridge->setMaxNumberOfPacketsStored(150);
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
auto tcpBridge = new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR);
auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER);
// TCP is stream based. Use packet ID as start marker when parsing for space packets
tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID});
tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID, common::CFDP_PACKET_ID});
sif::info << "Created TCP server for TMTC commanding with listener port "
<< tcpServer->getTcpPort() << std::endl;
#if OBSW_TCP_SERVER_WIRETAPPING == 1
tcpServer->enableWiretapping(true);
#endif /* OBSW_TCP_SERVER_WIRETAPPING == 1 */
tcpBridge->setMaxNumberOfPacketsStored(150);
#endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */
tmtcBridge->setMaxNumberOfPacketsStored(300);
#endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */
auto* ccsdsDistrib =
new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
*cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50);
*pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, 80);
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
(*cfdpFunnel)->addDestination(*udpBridge, 0);
(*pusFunnel)->addDestination(*udpBridge, 0);
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
(*cfdpFunnel)->addDestination(*tcpBridge, 0);
(*pusFunnel)->addDestination(*tcpBridge, 0);
#endif
#endif
// Every TM packet goes through this funnel
new TmFunnelHandler(objects::TM_FUNNEL, **pusFunnel, **cfdpFunnel);
// PUS service stack
new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID,
pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 20);
new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID,
pus::PUS_SERVICE_2, 3, 10);
new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID,
pus::PUS_SERVICE_3);
new Service5EventReporting(
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
15, 45);
new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
pus::PUS_SERVICE_8, 16, 60);
new Service9TimeManagement(
PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9));
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11),
ccsdsDistrib);
new Service17Test(
PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17));
new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID,
pus::PUS_SERVICE_20);
new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID,
pus::PUS_SERVICE_200, 8);
new CService201HealthCommanding(objects::PUS_SERVICE_201_HEALTH, config::EIVE_PUS_APID,
pus::PUS_SERVICE_201);
#if OBSW_ADD_CFDP_COMPONENTS == 1
using namespace cfdp;
MessageQueueIF* cfdpMsgQueue = QueueFactory::instance()->createMessageQueue(32);
CfdpDistribCfg distribCfg(objects::CFDP_DISTRIBUTOR, *tcStore, cfdpMsgQueue);
new CfdpDistributor(distribCfg);
auto* msgQueue = QueueFactory::instance()->createMessageQueue(32);
FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, **cfdpFunnel, *tcStore, *tmStore,
*msgQueue);
cfdp::IndicationCfg indicationCfg;
UnsignedByteField<uint16_t> apid(config::EIVE_LOCAL_CFDP_ENTITY_ID);
cfdp::EntityId localId(apid);
GROUND_REMOTE_CFG.defaultChecksum = cfdp::ChecksumType::CRC_32;
CfdpHandlerCfg cfdpCfg(localId, indicationCfg, USER_HANDLER, EIVE_FAULT_HANDLER, PACKET_LIST,
LOST_SEGMENTS, REMOTE_CFG_PROVIDER);
auto* cfdpHandler = new CfdpHandler(params, cfdpCfg);
// All CFDP packets arrive wrapped inside CCSDS space packets
CcsdsDistributorIF::DestInfo info("CFDP Destination", config::EIVE_CFDP_APID,
cfdpHandler->getRequestQueue(), true);
ccsdsDistrib->registerApplication(info);
#endif
}

5
mission/core/GenericFactory.h
View File

@ -2,10 +2,13 @@
#define MISSION_CORE_GENERICFACTORY_H_
class HealthTableIF;
class PusTmFunnel;
class CfdpTmFunnel;
namespace ObjectFactory {
void produceGenericObjects(HealthTableIF** healthTable = nullptr);
void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel);
}

2
mission/devices/BpxBatteryHandler.cpp
View File

@ -216,7 +216,7 @@ ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, cons
if (packet[2] != sentPingByte) {
return DeviceHandlerIF::INVALID_DATA;
}
if (mode == _MODE_START_UP) {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
}
break;

8
mission/devices/CMakeLists.txt
View File

@ -11,7 +11,7 @@ target_sources(
SyrlinksHkHandler.cpp
Max31865PT1000Handler.cpp
Max31865EiveHandler.cpp
IMTQHandler.cpp
ImtqHandler.cpp
HeaterHandler.cpp
RadiationSensorHandler.cpp
GyroADIS1650XHandler.cpp
@ -19,4 +19,8 @@ target_sources(
max1227.cpp
SusHandler.cpp
PayloadPcduHandler.cpp
SolarArrayDeploymentHandler.cpp)
SolarArrayDeploymentHandler.cpp
ScexDeviceHandler.cpp
torquer.cpp)
add_subdirectory(devicedefinitions)

4
mission/devices/GomspaceDeviceHandler.cpp
View File

@ -1,6 +1,5 @@
#include "GomspaceDeviceHandler.h"
#include <common/config/commonObjects.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>
@ -8,6 +7,7 @@
#include "devicedefinitions/GomSpacePackets.h"
#include "devicedefinitions/powerDefinitions.h"
#include "eive/objects.h"
using namespace GOMSPACE;
@ -570,7 +570,7 @@ ReturnValue_t GomspaceDeviceHandler::generateRequestFullCfgTableCmd(DeviceType d
uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; }
void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; }
void GomspaceDeviceHandler::setModeNormal() { setMode(_MODE_TO_NORMAL); }
ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) {
sif::info << "No printHkTable implementation given.." << std::endl;

8
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -422,12 +422,12 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
cookie->setTransferSize(2);
gpioId_t gpioId = cookie->getChipSelectPin();
GpioIF *gpioIF = comIf->getGpioInterface();
GpioIF &gpioIF = comIf->getGpioInterface();
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
MutexIF *mutex = comIf->getCsMutex();
cookie->getMutexParams(timeoutType, timeoutMs);
if (mutex == nullptr or gpioIF == nullptr) {
if (mutex == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid"
@ -453,7 +453,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
while (idx < sendLen) {
// Pull SPI CS low. For now, no support for active high given
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullLow(gpioId);
gpioIF.pullLow(gpioId);
}
// Execute transfer
@ -468,7 +468,7 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
gpioIF.pullHigh(gpioId);
}
idx += 2;

266
mission/devices/IMTQHandler.cpp → mission/devices/ImtqHandler.cpp
View File

@ -1,18 +1,41 @@
#include "IMTQHandler.h"
#include <bits/stdint-intn.h>
#include <commonConfig.h>
#include <fsfw/datapool/PoolEntry.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapoollocal/LocalDataPoolManager.h>
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/ipc/MutexFactory.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/messageQueueDefinitions.h>
#include <fsfw/modes/ModeMessage.h>
#include <fsfw/objectmanager/SystemObjectIF.h>
#include <fsfw/power/definitions.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serialize/SerializeIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw/timemanager/clockDefinitions.h>
#include <mission/devices/ImtqHandler.h>
#include <cmath>
#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
#include "OBSWConfig.h"
#include "mission/devices/torquer.h"
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
static constexpr bool ACTUATION_WIRETAPPING = false;
ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher)
: DeviceHandlerBase(objectId, comIF, comCookie),
engHkDataset(this),
calMtmMeasurementSet(this),
rawMtmMeasurementSet(this),
dipoleSet(*this),
posXselfTestDataset(this),
negXselfTestDataset(this),
posYselfTestDataset(this),
@ -20,14 +43,14 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
posZselfTestDataset(this),
negZselfTestDataset(this),
switcher(pwrSwitcher) {
if (comCookie == NULL) {
if (comCookie == nullptr) {
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
}
}
IMTQHandler::~IMTQHandler() {}
ImtqHandler::~ImtqHandler() = default;
void IMTQHandler::doStartUp() {
void ImtqHandler::doStartUp() {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
@ -35,9 +58,11 @@ void IMTQHandler::doStartUp() {
}
}
void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
bool buildCommand = true;
// Depending on the normal polling mode configuration, 3-4 communication steps are recommended
switch (communicationStep) {
case CommunicationStep::GET_ENG_HK_DATA:
*id = IMTQ::GET_ENG_HK_DATA;
@ -45,29 +70,61 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
break;
case CommunicationStep::START_MTM_MEASUREMENT:
*id = IMTQ::START_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
break;
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
if (pollingMode == NormalPollingMode::BOTH or
pollingMode == NormalPollingMode::UNCALIBRATED) {
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
} else {
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
}
break;
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
if (integrationTimeCd.getRemainingMillis() > 0) {
TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
}
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
if (pollingMode == NormalPollingMode::BOTH) {
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
} else {
communicationStep = CommunicationStep::DIPOLE_ACTUATION;
}
break;
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
if (integrationTimeCd.getRemainingMillis() > 0) {
TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
}
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
communicationStep = CommunicationStep::DIPOLE_ACTUATION;
break;
case CommunicationStep::DIPOLE_ACTUATION: {
// If the dipole is not commanded but set by the ACS control algorithm,
// the dipoles will be set by the ACS controller directly using the dipole local pool set.
// This set has a flag to determine whether the ACS controller actually set any new input.
MutexGuard mg(torquer::lazyLock());
if (torquer::NEW_ACTUATION_FLAG) {
*id = IMTQ::START_ACTUATION_DIPOLE;
torquer::NEW_ACTUATION_FLAG = false;
} else {
buildCommand = false;
}
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
break;
}
default:
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
if (buildCommand) {
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
return NOTHING_TO_SEND;
}
ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
size_t commandDataLen) {
switch (deviceCommand) {
@ -122,20 +179,36 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
case (IMTQ::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
if (commandData == nullptr) {
if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
commandBuffer[1] = commandData[1];
commandBuffer[2] = commandData[0];
commandBuffer[3] = commandData[3];
commandBuffer[4] = commandData[2];
commandBuffer[5] = commandData[5];
commandBuffer[6] = commandData[4];
commandBuffer[7] = commandData[7];
commandBuffer[8] = commandData[6];
rawPacket = commandBuffer;
rawPacketLen = 9;
return returnvalue::OK;
ReturnValue_t result;
// Commands override anything which was set in the software
if (commandData != nullptr) {
dipoleSet.setValidityBufferGeneration(false);
result =
dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
} else {
// Read set dipole values from local pool
PoolReadGuard pg(&dipoleSet);
}
if (ACTUATION_WIRETAPPING) {
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
<< ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
}
result = buildDipoleActuationCommand();
if (result != returnvalue::OK) {
return result;
}
MutexGuard mg(torquer::lazyLock());
torquer::TORQUEING = true;
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
return result;
}
case (IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
@ -151,6 +224,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
}
case (IMTQ::START_MTM_MEASUREMENT): {
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
integrationTimeCd.resetTimer();
rawPacket = commandBuffer;
rawPacketLen = 1;
return returnvalue::OK;
@ -173,30 +247,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::FAILED;
}
void IMTQHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
IMTQ::SIZE_SELF_TEST_RESULTS);
this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_DATA_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
IMTQ::SIZE_STATUS_REPLY);
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
uint8_t* serPtr = commandBuffer + 1;
size_t serSize = 1;
dipoleSet.setValidityBufferGeneration(false);
ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
SerializeIF::Endianness::LITTLE);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
rawPacket = commandBuffer;
rawPacketLen = 9;
return result;
}
ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize,
void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS);
insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
}
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
ReturnValue_t result = returnvalue::OK;
@ -233,8 +319,16 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
*foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
*foundId = IMTQ::GET_SELF_TEST_RESULT;
break;
case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): {
sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available "
"bytes. Keep 1 ms delay between I2C send and read"
<< std::endl;
result = IGNORE_REPLY_DATA;
break;
}
default:
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize
<< "contains invalid command code " << static_cast<int>(*start) << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
@ -242,7 +336,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
return result;
}
ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t result = returnvalue::OK;
result = parseStatusByte(packet);
@ -286,9 +380,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
return returnvalue::OK;
}
void IMTQHandler::setNormalDatapoolEntriesInvalid() {}
void ImtqHandler::setNormalDatapoolEntriesInvalid() {}
LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
if (sid == engHkDataset.getSid()) {
return &engHkDataset;
} else if (sid == calMtmMeasurementSet.getSid()) {
@ -313,9 +407,9 @@ LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
}
}
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
/** Entries of engineering housekeeping dataset */
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
@ -330,6 +424,11 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry);
localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry);
localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry);
localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
/** Entries of calibrated MTM measurement dataset */
localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry);
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
@ -611,7 +710,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
return returnvalue::OK;
}
ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
DeviceCommandId_t commandId = getPendingCommand();
switch (commandId) {
case IMTQ::POS_X_SELF_TEST:
@ -630,7 +729,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
return returnvalue::OK;
}
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
case 0:
@ -661,7 +760,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
}
}
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
void ImtqHandler::fillEngHkDataset(const uint8_t* packet) {
PoolReadGuard rg(&engHkDataset);
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
@ -687,7 +786,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
offset += 2;
engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
size_t dummy = 2;
SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy,
SerializeIF::Endianness::LITTLE);
engHkDataset.setValidity(true, true);
@ -708,9 +809,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
}
}
void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
if (wiretappingMode == RAW) {
/* Data already sent in doGetRead() */
return;
@ -734,7 +835,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom
}
}
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
uint8_t tmData[6];
/* Switching endianess of received dipole values */
tmData[0] = *(packet + 3);
@ -746,7 +847,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
}
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&calMtmMeasurementSet);
calMtmMeasurementSet.setValidity(true, true);
int8_t offset = 2;
@ -776,7 +877,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
}
}
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmMeasurementSet);
unsigned int offset = 2;
size_t deSerLen = 16;
@ -824,7 +925,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
}
}
void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleSelfTestReply(const uint8_t* packet) {
uint16_t offset = 2;
checkErrorByte(*(packet + offset), *(packet + offset + 1));
@ -858,7 +959,7 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1070,7 +1171,7 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1282,7 +1383,7 @@ void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1494,7 +1595,7 @@ void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1706,7 +1807,7 @@ void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -1918,7 +2019,7 @@ void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
PoolReadGuard rg(&posXselfTestDataset);
uint16_t offset = 2;
@ -2130,9 +2231,9 @@ void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
}
}
void IMTQHandler::setDebugMode(bool enable) { this->debugMode = enable; }
void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; }
void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
std::string stepString("");
if (step < 8) {
stepString = makeStepString(step);
@ -2190,7 +2291,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
}
}
std::string IMTQHandler::makeStepString(const uint8_t step) {
void ImtqHandler::doSendRead() {
TaskFactory::delayTask(1);
DeviceHandlerBase::doSendRead();
}
std::string ImtqHandler::makeStepString(const uint8_t step) {
std::string stepString("");
switch (step) {
case IMTQ::SELF_TEST_STEPS::INIT:
@ -2225,7 +2331,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) {
return stepString;
}
ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
if (switcher != power::NO_SWITCH) {
*numberOfSwitches = 1;
*switches = &switcher;

29
mission/devices/IMTQHandler.h → mission/devices/ImtqHandler.h
View File

@ -2,7 +2,7 @@
#define MISSION_DEVICES_IMTQHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <string.h>
#include "events/subsystemIdRanges.h"
@ -13,12 +13,17 @@
*
* @author J. Meier
*/
class IMTQHandler : public DeviceHandlerBase {
class ImtqHandler : public DeviceHandlerBase {
public:
IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher);
virtual ~IMTQHandler();
enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };
ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
power::Switch_t pwrSwitcher);
virtual ~ImtqHandler();
void setPollingMode(NormalPollingMode pollMode);
void doSendRead() override;
/**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
*/
@ -95,6 +100,7 @@ class IMTQHandler : public DeviceHandlerBase {
IMTQ::EngHkDataset engHkDataset;
IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
IMTQ::DipoleActuationSet dipoleSet;
IMTQ::PosXSelfTestSet posXselfTestDataset;
IMTQ::NegXSelfTestSet negXselfTestDataset;
IMTQ::PosYSelfTestSet posYselfTestDataset;
@ -102,7 +108,16 @@ class IMTQHandler : public DeviceHandlerBase {
IMTQ::PosZSelfTestSet posZselfTestDataset;
IMTQ::NegZSelfTestSet negZselfTestDataset;
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
// Hardcoded to default integration time of 10 ms.
// SHOULDDO: Support for other integration times
Countdown integrationTimeCd = Countdown(10);
power::Switch_t switcher = power::NO_SWITCH;
@ -114,7 +129,8 @@ class IMTQHandler : public DeviceHandlerBase {
GET_ENG_HK_DATA,
START_MTM_MEASUREMENT,
GET_CAL_MTM_MEASUREMENT,
GET_RAW_MTM_MEASUREMENT
GET_RAW_MTM_MEASUREMENT,
DIPOLE_ACTUATION
};
CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
@ -196,6 +212,7 @@ class IMTQHandler : public DeviceHandlerBase {
void handlePositiveZSelfTestReply(const uint8_t* packet);
void handleNegativeZSelfTestReply(const uint8_t* packet);
ReturnValue_t buildDipoleActuationCommand();
/**
* @brief This function checks the error byte of a self test measurement.
*

18
mission/devices/Max31865EiveHandler.cpp
View File

@ -30,16 +30,13 @@ void Max31865EiveHandler::doStartUp() {
}
void Max31865EiveHandler::doShutDown() {
updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID);
if (state == InternalState::NONE or state == InternalState::ACTIVE or
state == InternalState::ON) {
state = InternalState::INACTIVE;
transitionOk = false;
} else {
transitionOk = true;
}
if (state == InternalState::INACTIVE and transitionOk) {
setMode(_MODE_POWER_DOWN);
setMode(MODE_OFF);
}
}
@ -104,7 +101,7 @@ void Max31865EiveHandler::simpleCommand(EiveMax31855::RtdCommands cmd) {
rawPacketLen = 1;
}
void Max31865EiveHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
if (mode == _MODE_TO_NORMAL) {
if (getMode() == _MODE_TO_NORMAL) {
if (state != InternalState::ACTIVE) {
state = InternalState::ACTIVE;
transitionOk = false;
@ -125,7 +122,7 @@ void Max31865EiveHandler::fillCommandAndReplyMap() {
ReturnValue_t Max31865EiveHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
if (mode == _MODE_POWER_ON or mode == _MODE_WAIT_ON) {
if (getMode() == _MODE_POWER_ON or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (remainingSize != structLen) {
@ -145,12 +142,17 @@ ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id,
if (result != returnvalue::OK) {
return result;
}
if (mode == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) {
if (getMode() == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) {
transitionOk = true;
}
if (mode == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) {
if (getMode() == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) {
transitionOk = true;
}
if (getMode() == _MODE_SHUT_DOWN and not exchangeStruct.active) {
transitionOk = true;
return returnvalue::OK;
}
// Calculate resistance
float rtdValue = exchangeStruct.adcCode * EiveMax31855::RTD_RREF_PT1000 / INT16_MAX;
// calculate approximation

4
mission/devices/Max31865PT1000Handler.cpp
View File

@ -302,7 +302,7 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t r
} else if (internalState == InternalState::CLEAR_FAULT_BYTE) {
*foundId = MAX31865::CLEAR_FAULT_BYTE;
*foundLen = 2;
if (mode == _MODE_START_UP) {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
} else {
internalState = InternalState::RUNNING;
@ -524,7 +524,7 @@ void Max31865PT1000Handler::setInstantNormal(bool instantNormal) {
}
void Max31865PT1000Handler::modeChanged() {
if (mode == MODE_OFF) {
if (getMode() == MODE_OFF) {
lastFaultStatus = 0;
currentFaultStatus = 0;
sameFaultStatusCounter = 0;

2
mission/devices/P60DockHandler.h
View File

@ -4,7 +4,7 @@
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include "GomspaceDeviceHandler.h"
#include "commonSubsystemIds.h"
#include "eive/eventSubsystemIds.h"
/**
* @brief Device handler for the P60Dock. The P60Dock serves as carrier for the ACU, PDU1 and

13
mission/devices/PCDUHandler.cpp
View File

@ -52,8 +52,8 @@ ReturnValue_t PCDUHandler::initialize() {
return returnvalue::FAILED;
}
result = pdu2Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage(
static_cast<uint32_t>(P60System::SetIds::PDU_2_CORE), this->getObjectId(),
commandQueue->getId(), true);
static_cast<uint32_t>(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(),
true);
if (result != returnvalue::OK) {
sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from "
<< "PDU2Handler" << std::endl;
@ -68,8 +68,8 @@ ReturnValue_t PCDUHandler::initialize() {
return returnvalue::FAILED;
}
result = pdu1Handler->getSubscriptionInterface()->subscribeForSetUpdateMessage(
static_cast<uint32_t>(P60System::SetIds::PDU_1_CORE), this->getObjectId(),
commandQueue->getId(), true);
static_cast<uint32_t>(P60System::SetIds::CORE), this->getObjectId(), commandQueue->getId(),
true);
if (result != returnvalue::OK) {
sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from "
<< "PDU1Handler" << std::endl;
@ -110,11 +110,10 @@ void PCDUHandler::readCommandQueue() {
MessageQueueId_t PCDUHandler::getCommandQueue() const { return commandQueue->getId(); }
void PCDUHandler::handleChangedDataset(sid_t sid, store_address_t storeId, bool* clearMessage) {
if (sid == sid_t(objects::PDU2_HANDLER, static_cast<uint32_t>(P60System::SetIds::PDU_2_CORE))) {
if (sid == sid_t(objects::PDU2_HANDLER, static_cast<uint32_t>(P60System::SetIds::CORE))) {
updateHkTableDataset(storeId, &pdu2CoreHk, &timeStampPdu2HkDataset);
updatePdu2SwitchStates();
} else if (sid ==
sid_t(objects::PDU1_HANDLER, static_cast<uint32_t>(P60System::SetIds::PDU_1_CORE))) {
} else if (sid == sid_t(objects::PDU1_HANDLER, static_cast<uint32_t>(P60System::SetIds::CORE))) {
updateHkTableDataset(storeId, &pdu1CoreHk, &timeStampPdu1HkDataset);
updatePdu1SwitchStates();
} else {

51
mission/devices/PayloadPcduHandler.cpp
View File

@ -33,6 +33,7 @@ void PayloadPcduHandler::doStartUp() {
if (pwrStateMachine.getState() == power::States::IDLE) {
pwrStateMachine.start(MODE_ON, pwrSubmode);
}
clearSetOnOffFlag = true;
auto opCode = pwrStateMachine.fsm();
if (opCode == power::OpCodes::TO_NOT_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
pwrStateMachine.reset();
@ -50,6 +51,11 @@ void PayloadPcduHandler::doShutDown() {
if (pwrStateMachine.getState() == power::States::IDLE) {
pwrStateMachine.start(MODE_OFF, 0);
}
if (clearSetOnOffFlag) {
std::memset(adcSet.processed.value, 0, adcSet.processed.getSerializedSize());
clearSetOnOffFlag = false;
}
auto opCode = pwrStateMachine.fsm();
if (opCode == power::OpCodes::TO_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
pwrStateMachine.reset();
@ -60,7 +66,7 @@ void PayloadPcduHandler::doShutDown() {
}
void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
if (mode == _MODE_TO_NORMAL) {
if (getMode() == _MODE_TO_NORMAL) {
stateMachineToNormal(modeFrom, subModeFrom);
return;
}
@ -70,7 +76,7 @@ void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_t subModeFrom) {
using namespace plpcdu;
bool doFinish = true;
if (((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
if (((getSubmode() >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == 1) {
if (state == States::PL_PCDU_OFF) {
sif::error << "PayloadPcduHandler::stateMachineToNormal: Unexpected state PL_PCDU_OFF"
<< "detected" << std::endl;
@ -123,7 +129,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
auto switchHandler = [&](NormalSubmodeBits bit, gpioId_t id, std::string info) {
if (((diffMask >> bit) & 1) == 1) {
if (((submode >> bit) & 1) == 1) {
if (((getSubmode() >> bit) & 1) == 1) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "Enabling PL PCDU " << info << " module" << std::endl;
#endif
@ -144,7 +150,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
switchHandler(MPA_ON, gpioIds::PLPCDU_ENB_MPA, "MPA");
switchHandler(HPA_ON, gpioIds::PLPCDU_ENB_HPA, "HPA");
if (doFinish) {
setMode(MODE_NORMAL, submode);
setMode(MODE_NORMAL);
}
return returnvalue::OK;
}
@ -171,7 +177,7 @@ ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t
*id = plpcdu::SETUP_CMD;
return buildCommandFromCommand(*id, nullptr, 0);
}
if (mode == _MODE_TO_NORMAL) {
if (getMode() == _MODE_TO_NORMAL) {
return buildNormalDeviceCommand(id);
}
return NOTHING_TO_SEND;
@ -242,7 +248,7 @@ ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
using namespace plpcdu;
switch (id) {
case (SETUP_CMD): {
if (mode == _MODE_TO_NORMAL) {
if (getMode() == _MODE_TO_NORMAL) {
adcCmdExecuted = true;
}
break;
@ -491,8 +497,8 @@ void PayloadPcduHandler::checkAdcValues() {
void PayloadPcduHandler::checkJsonFileInit() {
if (not jsonFileInitComplete) {
sd::SdCard activeSd = sdcMan->getActiveSdCard();
if (sdcMan->isSdCardMounted(activeSd)) {
auto activeSd = sdcMan->getActiveSdCard();
if (activeSd and sdcMan->isSdCardUsable(activeSd.value())) {
params.initialize(sdcMan->getCurrentMountPrefix());
jsonFileInitComplete = true;
}
@ -534,33 +540,34 @@ bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event)
ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
using namespace plpcdu;
if (mode == MODE_NORMAL) {
diffMask = submode ^ this->submode;
uint8_t dhbSubmode = getSubmode();
diffMask = submode ^ dhbSubmode;
// Also deals with the case where the mode is MODE_ON, submode should be 0 here
if ((((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == SOLID_STATE_RELAYS_ADC_ON) and
(this->mode == MODE_NORMAL and this->submode != ALL_OFF_SUBMODE)) {
(getMode() == MODE_NORMAL and dhbSubmode != ALL_OFF_SUBMODE)) {
return TRANS_NOT_ALLOWED;
}
if (((((submode >> DRO_ON) & 1) == 1) and
((this->submode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
((dhbSubmode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
return TRANS_NOT_ALLOWED;
}
if ((((submode >> X8_ON) & 1) == 1) and
((this->submode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
((dhbSubmode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
return TRANS_NOT_ALLOWED;
}
if (((((submode >> TX_ON) & 1) == 1) and
((this->submode & 0b111) !=
((dhbSubmode & 0b111) !=
((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
return TRANS_NOT_ALLOWED;
}
if ((((submode >> MPA_ON) & 1) == 1 and
((this->submode & 0b1111) !=
((dhbSubmode & 0b1111) !=
((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
return TRANS_NOT_ALLOWED;
}
if ((((submode >> HPA_ON) & 1) == 1 and
((this->submode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) |
(1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
((dhbSubmode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) |
(1 << SOLID_STATE_RELAYS_ADC_ON))))) {
return TRANS_NOT_ALLOWED;
}
return returnvalue::OK;
@ -713,11 +720,11 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook
cookie->setTransferSize(transferLen);
gpioId_t gpioId = cookie->getChipSelectPin();
GpioIF* gpioIF = comIf->getGpioInterface();
GpioIF& gpioIF = comIf->getGpioInterface();
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
MutexIF* mutex = comIf->getCsMutex();
if (mutex == nullptr or gpioIF == nullptr) {
if (mutex == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid"
@ -747,7 +754,7 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook
transferStruct->len = transferLen;
// Pull SPI CS low. For now, no support for active high given
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullLow(gpioId);
gpioIF.pullLow(gpioId);
}
// Execute transfer
@ -762,14 +769,14 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
gpioIF.pullHigh(gpioId);
}
transferStruct->tx_buf += transferLen;
transferStruct->rx_buf += transferLen;
transferStruct->len = plpcdu::TEMP_REPLY_SIZE - 1;
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullLow(gpioId);
gpioIF.pullLow(gpioId);
}
// Execute transfer
@ -784,7 +791,7 @@ ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cook
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if (gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
gpioIF.pullHigh(gpioId);
}
transferStruct->tx_buf = origTx;

5
mission/devices/PayloadPcduHandler.h
View File

@ -10,8 +10,8 @@
#include "fsfw_hal/common/gpio/GpioIF.h"
#include "mission/devices/devicedefinitions/payloadPcduDefinitions.h"
#include "mission/memory/SdCardMountedIF.h"
#include "mission/system/DualLanePowerStateMachine.h"
#include "mission/system/definitions.h"
#include "mission/system/objects/DualLanePowerStateMachine.h"
#include "mission/system/objects/definitions.h"
#ifdef FSFW_OSAL_LINUX
class SpiComIF;
@ -124,6 +124,7 @@ class PayloadPcduHandler : public DeviceHandlerBase {
bool txToMpaInjectionRequested = false;
bool mpaToHpaInjectionRequested = false;
bool allOnInjectRequested = false;
bool clearSetOnOffFlag = true;
PeriodicOperationDivider opDivider = PeriodicOperationDivider(5);
uint8_t tempReadDivisor = 1;

4
mission/devices/RwHandler.cpp
View File

@ -304,9 +304,9 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return INVALID_SPEED;
}
uint16_t rampTime = *(commandData + 4) << 8 | *(commandData + 5);
uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
if (rampTime < 10 || rampTime > 10000) {
if (rampTime < 10 || rampTime > 20000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
return INVALID_RAMP_TIME;
}

385
mission/devices/ScexDeviceHandler.cpp Normal file
View File

@ -0,0 +1,385 @@
#include "ScexDeviceHandler.h"
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <linux/devices/ScexHelper.h>
#include <mission/memory/SdCardMountedIF.h>
#include <algorithm>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <random>
#include "fsfw/globalfunctions/CRC.h"
#include "mission/devices/devicedefinitions/ScexDefinitions.h"
using std::ofstream;
using namespace returnvalue;
ScexDeviceHandler::ScexDeviceHandler(object_id_t objectId, ScexUartReader& reader, CookieIF* cookie,
SdCardMountedIF& sdcMan)
: DeviceHandlerBase(objectId, reader.getObjectId(), cookie), sdcMan(sdcMan), reader(reader) {}
ScexDeviceHandler::~ScexDeviceHandler() {}
void ScexDeviceHandler::doStartUp() { setMode(MODE_ON); }
void ScexDeviceHandler::doShutDown() {
reader.reset();
commandActive = false;
multiFileFinishOutstanding = false;
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t ScexDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { return OK; }
ReturnValue_t ScexDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return OK; }
ReturnValue_t ScexDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
size_t commandDataLen) {
using namespace scex;
auto cmdTyped = static_cast<scex::Cmds>(deviceCommand);
if (std::find(VALID_CMDS.begin(), VALID_CMDS.end(), deviceCommand) == VALID_CMDS.end()) {
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
bool tempCheck = false;
if (commandDataLen == 1) {
tempCheck = commandData[0];
}
if (commandActive) {
return DeviceHandlerIF::BUSY;
}
switch (deviceCommand) {
case (PING): {
finishCountdown.setTimeout(SHORT_CD);
// countdown starten
finishCountdown.resetTimer();
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {nullptr, 0},
tempCheck);
break;
}
case (EXP_STATUS_CMD): {
finishCountdown.setTimeout(SHORT_CD);
// countdown starten
finishCountdown.resetTimer();
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {nullptr, 0},
tempCheck);
break;
}
case (ION_CMD): {
finishCountdown.setTimeout(SHORT_CD);
// countdown starten
finishCountdown.resetTimer();
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {nullptr, 0},
tempCheck);
break;
}
case (TEMP_CMD): {
finishCountdown.setTimeout(SHORT_CD);
// countdown starten
finishCountdown.resetTimer();
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen, {nullptr, 0},
tempCheck);
break;
}
case (FRAM): {
finishCountdown.setTimeout(LONG_CD);
// countdown starten
finishCountdown.resetTimer();
if (debugMode) {
uint32_t remainingMillis = finishCountdown.getRemainingMillis();
sif::info << "ScexDeviceHandler::buildCommandFromCommand: RemainingMillis: "
<< remainingMillis << std::endl;
}
multiFileFinishOutstanding = true;
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen,
{commandData + 1, commandDataLen - 1}, tempCheck);
updatePeriodicReply(true, deviceCommand);
break;
}
case (ONE_CELL): {
finishCountdown.setTimeout(LONG_CD);
// countdown starts
finishCountdown.resetTimer();
multiFileFinishOutstanding = true;
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen,
{commandData + 1, commandDataLen - 1}, tempCheck);
updatePeriodicReply(true, deviceCommand);
break;
}
case (ALL_CELLS_CMD): {
finishCountdown.setTimeout(LONG_CD);
// countdown starts
finishCountdown.resetTimer();
multiFileFinishOutstanding = true;
prepareScexCmd(cmdTyped, {cmdBuf.data(), cmdBuf.size()}, rawPacketLen,
{commandData + 1, commandDataLen - 1}, tempCheck);
updatePeriodicReply(true, deviceCommand);
break;
}
default: {
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
}
commandActive = true;
rawPacket = cmdBuf.data();
return OK;
}
void ScexDeviceHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(scex::Cmds::PING, 5, nullptr, 0, false, false, 0, &finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::ION_CMD, 3, nullptr, 0, false, false, 0, &finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::TEMP_CMD, 3, nullptr, 0, false, false, 0,
&finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::EXP_STATUS_CMD, 3, nullptr, 0, false, false, 0,
&finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::ALL_CELLS_CMD, 0, nullptr, 0, true, false,
scex::Cmds::ALL_CELLS_CMD, &finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::ONE_CELL, 0, nullptr, 0, true, false, scex::Cmds::ONE_CELL,
&finishCountdown);
insertInCommandAndReplyMap(scex::Cmds::FRAM, 0, nullptr, 0, true, false, scex::Cmds::FRAM,
&finishCountdown);
insertInReplyMap(scex::Cmds::ERROR_REPLY, 3);
}
ReturnValue_t ScexDeviceHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
size_t len = remainingSize;
ReturnValue_t result = helper.deSerialize(&start, &len);
if (result == ScexHelper::INVALID_CRC) {
sif::warning << "ScexDeviceHandler::scanForReply: CRC invalid" << std::endl;
*foundLen = remainingSize;
} else {
result = handleValidReply(remainingSize, foundId, foundLen);
}
return result;
}
ReturnValue_t ScexDeviceHandler::handleValidReply(size_t remSize, DeviceCommandId_t* foundId,
size_t* foundLen) {
using namespace scex;
ReturnValue_t result = OK;
switch (helper.getCmd()) {
case (FRAM): {
if (debugMode) {
uint32_t remainingMillis = finishCountdown.getRemainingMillis();
sif::info << "ScexDeviceHandler::handleValidReply: RemMillis: " << remainingMillis
<< std::endl;
}
result = APERIODIC_REPLY;
break;
}
case (ONE_CELL): {
result = APERIODIC_REPLY;
break;
}
case (ALL_CELLS_CMD): {
result = APERIODIC_REPLY;
break;
}
default: {
break;
}
}
if (result == APERIODIC_REPLY and multiFileFinishOutstanding) {
finishAction(true, helper.getCmd(), OK);
multiFileFinishOutstanding = false;
}
*foundId = helper.getCmd();
*foundLen = remSize;
return result;
}
ReturnValue_t ScexDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
using namespace scex;
ReturnValue_t status = OK;
auto oneFileHandler = [&](std::string cmdName) {
auto activeSd = sdcMan.getActiveSdCard();
if (not activeSd) {
return HasFileSystemIF::FILESYSTEM_INACTIVE;
}
fileId = date_time_string();
std::ostringstream oss;
auto prefix = sdcMan.getCurrentMountPrefix();
oss << prefix << "/scex/scex-" << cmdName << fileId << ".bin";
fileName = oss.str();
ofstream out(fileName, ofstream::binary);
if (out.bad()) {
sif::error << "ScexDeviceHandler::interpretDeviceReply: Could not open file " << fileName
<< std::endl;
return FAILED;
}
out << helper;
return OK;
};
auto multiFileHandler = [&](std::string cmdName) {
if ((helper.getPacketCounter() == 1) or (not fileNameSet)) {
auto activeSd = sdcMan.getActiveSdCard();
if (not activeSd) {
return HasFileSystemIF::FILESYSTEM_INACTIVE;
}
fileId = date_time_string();
std::ostringstream oss;
auto prefix = sdcMan.getCurrentMountPrefix();
oss << prefix << "/scex/scex-" << cmdName << fileId << ".bin";
fileName = oss.str();
fileNameSet = true;
ofstream out(fileName, ofstream::binary);
if (out.bad()) {
sif::error << "ScexDeviceHandler::handleValidReply: Could not open file " << fileName
<< std::endl;
return FAILED;
}
out << helper;
} else {
ofstream out(fileName,
ofstream::binary | ofstream::app); // append
if (out.bad()) {
sif::error << "ScexDeviceHandler::handleValidReply: Could not open file " << fileName
<< std::endl;
return FAILED;
}
out << helper;
}
return OK;
};
id = helper.getCmd();
switch (id) {
case (PING): {
status = oneFileHandler("ping_");
break;
}
case (ION_CMD): {
status = oneFileHandler("ion_");
break;
}
case (TEMP_CMD): {
status = oneFileHandler("temp_");
break;
}
case (EXP_STATUS_CMD): {
status = oneFileHandler("exp_status_");
break;
}
case (FRAM): {
status = multiFileHandler("fram_");
break;
}
case (ONE_CELL): {
status = multiFileHandler("one_cell_");
break;
}
case (ALL_CELLS_CMD): {
status = multiFileHandler("multi_cell_");
break;
}
default:
// Unknown DeviceCommand
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
if (helper.getPacketCounter() == helper.getTotalPacketCounter()) {
reader.finish();
commandActive = false;
if (id != PING) {
fileNameSet = false;
}
if (id == FRAM or id == ALL_CELLS_CMD or id == ONE_CELL) {
triggerEvent(MULTI_PACKET_COMMAND_DONE, id);
updatePeriodicReply(false, id);
}
}
if (debugMode) {
uint32_t remainingMillis = finishCountdown.getRemainingMillis();
sif::info << __FILE__ << __func__ << "(" << __LINE__ << ") RemMillis: " << remainingMillis
<< std::endl;
}
return status;
}
void ScexDeviceHandler::performOperationHook() {
uint32_t remainingMillis = finishCountdown.getRemainingMillis();
if (commandActive and finishCountdown.hasTimedOut()) {
triggerEvent(scex::EXPERIMENT_TIMEDOUT, currCmd, 0);
reader.finish();
sif::warning << "ScexDeviceHandler::scanForReply: Reader timeout; RemMillis: "
<< remainingMillis << std::endl;
fileNameSet = false;
commandActive = false;
}
}
uint32_t ScexDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return OK; }
ReturnValue_t ScexDeviceHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
if (switchId) {
*numberOfSwitches = 1;
*switches = &switchId.value();
}
return OK;
}
ReturnValue_t ScexDeviceHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
return OK;
}
std::string ScexDeviceHandler::date_time_string() {
using namespace std;
string date_time;
Clock::TimeOfDay_t tod;
Clock::getDateAndTime(&tod);
ostringstream oss(std::ostringstream::ate);
if (tod.hour < 10) {
oss << tod.year << tod.month << tod.day << "_0" << tod.hour;
} else {
oss << tod.year << tod.month << tod.day << "_" << tod.hour;
}
if (tod.minute < 10) {
oss << 0 << tod.minute;
} else {
oss << tod.minute;
}
if (tod.second < 10) {
oss << 0 << tod.second;
} else {
oss << tod.second;
}
date_time = oss.str();
return date_time;
}
void ScexDeviceHandler::modeChanged() {}
void ScexDeviceHandler::setPowerSwitcher(PowerSwitchIF& powerSwitcher, power::Switch_t switchId) {
DeviceHandlerBase::setPowerSwitcher(&powerSwitcher);
this->switchId = switchId;
}
ReturnValue_t ScexDeviceHandler::initializeAfterTaskCreation() {
auto mntPrefix = sdcMan.getCurrentMountPrefix();
std::filesystem::path fullFilePath = mntPrefix;
fullFilePath /= "scex";
bool fileExists = std::filesystem::exists(fullFilePath);
if (not fileExists) {
std::filesystem::create_directory(fullFilePath);
}
return DeviceHandlerBase::initializeAfterTaskCreation();
}

65
mission/devices/ScexDeviceHandler.h Normal file
View File

@ -0,0 +1,65 @@
#ifndef MISSION_DEVICES_SCEXDEVICEHANDLER_H_
#define MISSION_DEVICES_SCEXDEVICEHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <linux/devices/ScexHelper.h>
#include <linux/devices/ScexUartReader.h>
#include <optional>
#include "eive/eventSubsystemIds.h"
class SdCardMountedIF;
class ScexDeviceHandler : public DeviceHandlerBase {
public:
ScexDeviceHandler(object_id_t objectId, ScexUartReader &reader, CookieIF *cookie,
SdCardMountedIF &sdcMan);
void setPowerSwitcher(PowerSwitchIF &powerSwitcher, power::Switch_t switchId);
virtual ~ScexDeviceHandler();
private:
static constexpr uint32_t LONG_CD = 180 * 1000;
static constexpr uint32_t SHORT_CD = 12000;
std::array<uint8_t, 64> cmdBuf = {};
std::optional<power::Switch_t> switchId;
std::string fileId = "";
std::string fileName = "";
bool fileNameSet = false;
bool commandActive = false;
bool multiFileFinishOutstanding = false;
bool debugMode = false;
scex::Cmds currCmd = scex::Cmds::PING;
SdCardMountedIF &sdcMan;
Countdown finishCountdown = Countdown(LONG_CD);
std::string date_time_string();
// DeviceHandlerBase private function implementation
void doStartUp() override;
void doShutDown() override;
ScexHelper helper;
ScexUartReader &reader;
void performOperationHook() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override;
void fillCommandAndReplyMap() override;
ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize, DeviceCommandId_t *foundId,
size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
ReturnValue_t handleValidReply(size_t remSize, DeviceCommandId_t *foundId, size_t *foundLen);
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
ReturnValue_t initializeAfterTaskCreation() override;
void modeChanged() override;
};
#endif /* MISSION_DEVICES_SCEXDEVICEHANDLER_H_ */

589
mission/devices/SolarArrayDeploymentHandler.cpp
View File

@ -1,171 +1,471 @@
#include "SolarArrayDeploymentHandler.h"
#include <devices/gpioIds.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/tasks/TaskFactory.h>
SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(
object_id_t setObjectId_, object_id_t gpioDriverId_, CookieIF* gpioCookie_,
object_id_t mainLineSwitcherObjectId_, pcdu::Switches mainLineSwitch_, gpioId_t deplSA1,
gpioId_t deplSA2, uint32_t burnTimeMs)
#include <filesystem>
#include <fstream>
#include <iostream>
#include "devices/gpioIds.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw_hal/common/gpio/GpioCookie.h"
static constexpr bool DEBUG_MODE = true;
SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
GpioIF& gpioInterface,
PowerSwitchIF& mainLineSwitcher_,
pcdu::Switches mainLineSwitch_,
gpioId_t deplSA1, gpioId_t deplSA2,
SdCardMountedIF& sdcMountedIF)
: SystemObject(setObjectId_),
gpioDriverId(gpioDriverId_),
gpioCookie(gpioCookie_),
mainLineSwitcherObjectId(mainLineSwitcherObjectId_),
mainLineSwitch(mainLineSwitch_),
gpioInterface(gpioInterface),
deplSA1(deplSA1),
deplSA2(deplSA2),
burnTimeMs(burnTimeMs),
mainLineSwitcher(mainLineSwitcher_),
mainLineSwitch(mainLineSwitch_),
sdcMan(sdcMountedIF),
actionHelper(this, nullptr) {
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
commandQueue = QueueFactory::instance()->createMessageQueue(
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}
SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {}
SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;
ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
handleStateMachine();
return returnvalue::OK;
}
return returnvalue::OK;
}
ReturnValue_t SolarArrayDeploymentHandler::initialize() {
ReturnValue_t result = SystemObject::initialize();
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioDriverId);
if (gpioInterface == nullptr) {
sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface." << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
if (result != returnvalue::OK) {
sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
<< std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
mainLineSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(mainLineSwitcherObjectId);
if (mainLineSwitcher == nullptr) {
sif::error
<< "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object"
<< "from object ID." << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
using namespace std::filesystem;
if (opDivider.checkAndIncrement()) {
auto activeSdc = sdcMan.getActiveSdCard();
if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
sdcMan.isSdCardUsable(activeSdc.value())) {
if (exists(SD_0_DEPL_FILE)) {
// perform autonomous deployment handling
performAutonomousDepl(sd::SdCard::SLOT_0, dryRunStringInFile(SD_0_DEPL_FILE));
}
} else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and
sdcMan.isSdCardUsable(activeSdc.value())) {
if (exists(SD_1_DEPL_FILE)) {
// perform autonomous deployment handling
performAutonomousDepl(sd::SdCard::SLOT_1, dryRunStringInFile(SD_1_DEPL_FILE));
}
} else {
// TODO: This is FDIR domain. If both SD cards are not available for whatever reason,
// there is not much we can do except somehow use the scratch buffer which is
// not non-volatile. Implementation effort is considerable as well.
}
}
result = actionHelper.initialize(commandQueue);
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
readCommandQueue();
handleStateMachine();
return returnvalue::OK;
}
void SolarArrayDeploymentHandler::handleStateMachine() {
switch (stateMachine) {
case WAIT_ON_DELOYMENT_COMMAND:
readCommandQueue();
break;
case SWITCH_8V_ON:
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
stateMachine = WAIT_ON_8V_SWITCH;
break;
case WAIT_ON_8V_SWITCH:
performWaitOn8VActions();
break;
case SWITCH_DEPL_GPIOS:
switchDeploymentTransistors();
break;
case WAIT_ON_DEPLOYMENT_FINISH:
handleDeploymentFinish();
break;
case WAIT_FOR_MAIN_SWITCH_OFF:
if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) {
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
} else if (mainSwitchCountdown.hasTimedOut()) {
triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main"
<< " switch off" << std::endl;
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
if (stateMachine == MAIN_POWER_ON) {
mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
stateMachine = WAIT_MAIN_POWER_ON;
sif::info << "S/A Deployment: Deployment power line on" << std::endl;
}
if (stateMachine == MAIN_POWER_OFF) {
// These should never fail
allOff();
stateMachine = WAIT_MAIN_POWER_OFF;
sif::info << "S/A Deployment: Deployment power line off" << std::endl;
}
if (stateMachine == WAIT_MAIN_POWER_ON) {
if (checkMainPowerOn()) {
if (DEBUG_MODE) {
sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_ON done -> SWITCH_DEPL_GPIOS" << std::endl;
}
break;
default:
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" << std::endl;
break;
stateMachine = SWITCH_DEPL_GPIOS;
}
}
if (stateMachine == WAIT_MAIN_POWER_OFF) {
if (checkMainPowerOff()) {
if (DEBUG_MODE) {
sif::debug << "SA DEPL FSM: WAIT_MAIN_POWER_OFF done -> FSM DONE" << std::endl;
}
sif::info << "S/A Deployment: FSM done" << std::endl;
finishFsm(returnvalue::OK);
}
}
if (stateMachine == SWITCH_DEPL_GPIOS) {
burnCountdown.setTimeout(fsmInfo.burnCountdownMs);
// This should never fail
channelAlternationCd.resetTimer();
if (not fsmInfo.dryRun) {
sa2Off();
sa1On();
fsmInfo.alternationDummy = true;
}
sif::info << "S/A Deployment: Burning" << std::endl;
triggerEvent(BURN_PHASE_START, fsmInfo.burnCountdownMs, fsmInfo.dryRun);
stateMachine = BURNING;
}
if (stateMachine == BURNING) {
saGpioAlternation();
if (burnCountdown.hasTimedOut()) {
if (DEBUG_MODE) {
sif::debug << "SA DEPL FSM: BURNING done -> WAIT_MAIN_POWER_OFF" << std::endl;
}
allOff();
triggerEvent(BURN_PHASE_DONE, fsmInfo.burnCountdownMs, fsmInfo.dryRun);
stateMachine = WAIT_MAIN_POWER_OFF;
}
}
}
void SolarArrayDeploymentHandler::performWaitOn8VActions() {
if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
stateMachine = SWITCH_DEPL_GPIOS;
ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCard, bool dryRun) {
using namespace std::filesystem;
using namespace std;
auto initFile = [](const char* filename) {
ofstream of(filename);
of << "phase: init\n";
of << "secs_since_start: 0\n";
};
if (sdCard == sd::SdCard::SLOT_0) {
if (not exists(SD_0_DEPLY_INFO)) {
initFile(SD_0_DEPLY_INFO);
}
if (not autonomousDeplForFile(sd::SdCard::SLOT_0, SD_0_DEPLY_INFO, dryRun)) {
initFile(SD_0_DEPLY_INFO);
}
} else if (sdCard == sd::SdCard::SLOT_1) {
if (not exists(SD_1_DEPLY_INFO)) {
initFile(SD_1_DEPLY_INFO);
}
if (not autonomousDeplForFile(sd::SdCard::SLOT_1, SD_1_DEPLY_INFO, dryRun)) {
initFile(SD_1_DEPLY_INFO);
}
}
return returnvalue::OK;
}
bool SolarArrayDeploymentHandler::autonomousDeplForFile(sd::SdCard sdCard, const char* filename,
bool dryRun) {
using namespace std;
ifstream file(filename);
string line;
string word;
unsigned int lineNum = 0;
AutonomousDeplState deplState = AutonomousDeplState::INIT;
bool stateSwitch = false;
uint32_t secsSinceBoot = 0;
while (std::getline(file, line)) {
std::istringstream iss(line);
if (lineNum == 0) {
iss >> word;
if (word.find("phase:") == string::npos) {
return false;
}
iss >> word;
if (word.find(PHASE_INIT_STR) != string::npos) {
deplState = AutonomousDeplState::INIT;
} else if (word.find(PHASE_FIRST_BURN_STR) != string::npos) {
deplState = AutonomousDeplState::FIRST_BURN;
} else if (word.find(PHASE_WAIT_STR) != string::npos) {
deplState = AutonomousDeplState::WAIT;
} else if (word.find(PHASE_SECOND_BURN_STR) != string::npos) {
deplState = AutonomousDeplState::SECOND_BURN;
} else if (word.find(PHASE_DONE) != string::npos) {
deplState = AutonomousDeplState::DONE;
} else {
return false;
}
} else if (lineNum == 1) {
iss >> word;
if (iss.bad()) {
return false;
}
if (word.find("secs_since_start:") == string::npos) {
return false;
}
iss >> secsSinceBoot;
if (iss.bad()) {
return false;
}
if (not initUptime) {
initUptime = secsSinceBoot;
}
auto switchCheck = [&](AutonomousDeplState expected) {
if (deplState != expected) {
deplState = expected;
stateSwitch = true;
}
};
if ((secsSinceBoot > FIRST_BURN_START_TIME) and (secsSinceBoot < FIRST_BURN_END_TIME)) {
switchCheck(AutonomousDeplState::FIRST_BURN);
} else if ((secsSinceBoot > WAIT_START_TIME) and (secsSinceBoot < WAIT_END_TIME)) {
switchCheck(AutonomousDeplState::WAIT);
} else if ((secsSinceBoot > SECOND_BURN_START_TIME) and
(secsSinceBoot < SECOND_BURN_END_TIME)) {
switchCheck(AutonomousDeplState::SECOND_BURN);
} else if (secsSinceBoot > SECOND_BURN_END_TIME) {
switchCheck(AutonomousDeplState::DONE);
}
}
lineNum++;
}
if (initUptime) {
secsSinceBoot = initUptime.value();
}
// Uptime has increased by X seconds so we need to update the uptime count inside the file
secsSinceBoot += Clock::getUptime().tv_sec;
if (stateSwitch or firstAutonomousCycle) {
if (deplState == AutonomousDeplState::FIRST_BURN or
deplState == AutonomousDeplState::SECOND_BURN) {
startFsmOn(config::SA_DEPL_BURN_TIME_SECS, dryRun);
} else if (deplState == AutonomousDeplState::WAIT or deplState == AutonomousDeplState::DONE or
deplState == AutonomousDeplState::INIT) {
startFsmOff();
}
}
if (deplState == AutonomousDeplState::DONE) {
remove(filename);
if (sdCard == sd::SdCard::SLOT_0) {
remove(SD_0_DEPL_FILE);
} else {
remove(SD_1_DEPL_FILE);
}
triggerEvent(AUTONOMOUS_DEPLOYMENT_COMPLETED);
} else {
if (mainSwitchCountdown.hasTimedOut()) {
std::ofstream of(filename);
of << "phase: ";
if (deplState == AutonomousDeplState::INIT) {
of << PHASE_INIT_STR << "\n";
} else if (deplState == AutonomousDeplState::FIRST_BURN) {
of << PHASE_FIRST_BURN_STR << "\n";
} else if (deplState == AutonomousDeplState::WAIT) {
of << PHASE_WAIT_STR << "\n";
} else if (deplState == AutonomousDeplState::SECOND_BURN) {
of << PHASE_SECOND_BURN_STR << "\n";
}
of << "secs_since_start: " << std::to_string(secsSinceBoot) << "\n";
}
if (firstAutonomousCycle) {
firstAutonomousCycle = false;
}
return true;
}
bool SolarArrayDeploymentHandler::checkMainPowerOn() { return checkMainPower(true); }
bool SolarArrayDeploymentHandler::checkMainPowerOff() { return checkMainPower(false); }
bool SolarArrayDeploymentHandler::checkMainPower(bool onOff) {
if ((onOff and mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) or
(not onOff and
mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF)) {
return true;
}
if (mainSwitchCountdown.hasTimedOut()) {
if (onOff) {
triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
MAIN_SWITCH_TIMEOUT_FAILURE);
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
} else {
triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
}
if (retryCounter < 3) {
if (onOff) {
stateMachine = MAIN_POWER_ON;
} else {
stateMachine = MAIN_POWER_OFF;
}
retryCounter++;
} else {
finishFsm(MAIN_SWITCH_TIMEOUT_FAILURE);
}
}
return false;
}
bool SolarArrayDeploymentHandler::startFsmOn(uint32_t burnCountdownSecs, bool dryRun) {
if (stateMachine != StateMachine::IDLE) {
return false;
}
if (burnCountdownSecs > config::SA_DEPL_MAX_BURN_TIME) {
burnCountdownSecs = config::SA_DEPL_MAX_BURN_TIME;
}
fsmInfo.dryRun = dryRun;
fsmInfo.burnCountdownMs = burnCountdownSecs * 1000;
stateMachine = StateMachine::MAIN_POWER_ON;
retryCounter = 0;
return true;
}
void SolarArrayDeploymentHandler::startFsmOff() {
if (stateMachine != StateMachine::IDLE) {
// off commands override the state machine. Cancel any active action commands.
finishFsm(returnvalue::FAILED);
}
retryCounter = 0;
stateMachine = StateMachine::MAIN_POWER_OFF;
}
void SolarArrayDeploymentHandler::finishFsm(ReturnValue_t resultForActionHelper) {
retryCounter = 0;
stateMachine = StateMachine::IDLE;
fsmInfo.dryRun = false;
fsmInfo.alternationDummy = false;
if (actionActive) {
bool success = false;
if (resultForActionHelper == returnvalue::OK or
resultForActionHelper == HasActionsIF::EXECUTION_FINISHED) {
success = true;
}
actionHelper.finish(success, rememberCommanderId, activeCmd, resultForActionHelper);
}
}
void SolarArrayDeploymentHandler::switchDeploymentTransistors() {
void SolarArrayDeploymentHandler::allOff() {
deploymentTransistorsOff();
mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
}
bool SolarArrayDeploymentHandler::dryRunStringInFile(const char* filename) {
std::ifstream ifile(filename);
if (ifile.bad()) {
return false;
}
std::string line;
while (getline(ifile, line)) {
if (line.find("dryrun") != std::string::npos) {
return true;
}
}
return false;
}
ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
ReturnValue_t result = returnvalue::OK;
result = gpioInterface->pullHigh(deplSA1);
if (actionId == DEPLOY_SOLAR_ARRAYS_MANUALLY) {
ManualDeploymentCommand cmd;
if (size < cmd.getSerializedSize()) {
return HasActionsIF::INVALID_PARAMETERS;
}
result = cmd.deSerialize(&data, &size, SerializeIF::Endianness::NETWORK);
if (result != returnvalue::OK) {
return result;
}
uint32_t burnCountdown = cmd.getBurnTime();
if (not startFsmOn(burnCountdown, cmd.isDryRun())) {
return HasActionsIF::IS_BUSY;
}
actionActive = true;
rememberCommanderId = commandedBy;
return result;
} else if (actionId == SWITCH_OFF_DEPLOYMENT) {
startFsmOff();
actionActive = true;
rememberCommanderId = commandedBy;
return result;
} else {
return HasActionsIF::INVALID_ACTION_ID;
}
return result;
}
ReturnValue_t SolarArrayDeploymentHandler::saGpioAlternation() {
ReturnValue_t status = returnvalue::OK;
ReturnValue_t result;
if (channelAlternationCd.hasTimedOut() and not fsmInfo.dryRun) {
if (fsmInfo.alternationDummy) {
result = sa1Off();
if (result != returnvalue::OK) {
status = result;
}
TaskFactory::delayTask(1);
result = sa2On();
if (result != returnvalue::OK) {
status = result;
}
} else {
result = sa2Off();
if (result != returnvalue::OK) {
status = result;
}
TaskFactory::delayTask(1);
result = sa1On();
if (result != returnvalue::OK) {
status = result;
}
}
fsmInfo.alternationDummy = not fsmInfo.alternationDummy;
channelAlternationCd.resetTimer();
}
return status;
}
ReturnValue_t SolarArrayDeploymentHandler::deploymentTransistorsOff() {
ReturnValue_t status = returnvalue::OK;
ReturnValue_t result = sa1Off();
if (result != returnvalue::OK) {
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 1 high "
<< std::endl;
/* If gpio switch high failed, state machine is reset to wait for a command reinitiating
* the deployment sequence. */
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
status = result;
}
result = sa2Off();
if (result != returnvalue::OK) {
status = result;
}
return status;
}
ReturnValue_t SolarArrayDeploymentHandler::sa1On() {
ReturnValue_t result = gpioInterface.pullHigh(deplSA1);
if (result != returnvalue::OK) {
sif::warning << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 1 high"
<< std::endl;
// If gpio switch high failed, state machine is reset to wait for a command re-initiating
// the deployment sequence.
triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, SWITCHING_DEPL_SA2_FAILED);
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
}
result = gpioInterface->pullHigh(deplSA2);
if (result != returnvalue::OK) {
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 2 high "
<< std::endl;
stateMachine = WAIT_ON_DELOYMENT_COMMAND;
triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, SWITCHING_DEPL_SA2_FAILED);
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
}
deploymentCountdown.setTimeout(burnTimeMs);
stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
return result;
}
void SolarArrayDeploymentHandler::handleDeploymentFinish() {
ReturnValue_t result = returnvalue::OK;
if (deploymentCountdown.hasTimedOut()) {
actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK);
result = gpioInterface->pullLow(deplSA1);
if (result != returnvalue::OK) {
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 1 low "
ReturnValue_t SolarArrayDeploymentHandler::sa1Off() {
ReturnValue_t result = gpioInterface.pullLow(deplSA1);
if (result != returnvalue::OK) {
sif::warning << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 1 low"
<< std::endl;
}
result = gpioInterface->pullLow(deplSA2);
if (result != returnvalue::OK) {
sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 2 low "
<< std::endl;
}
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
stateMachine = WAIT_FOR_MAIN_SWITCH_OFF;
// If gpio switch high failed, state machine is reset to wait for a command re-initiating
// the deployment sequence.
triggerEvent(DEPL_SA1_GPIO_SWTICH_OFF_FAILED);
}
return result;
}
ReturnValue_t SolarArrayDeploymentHandler::sa2On() {
ReturnValue_t result = gpioInterface.pullHigh(deplSA2);
if (result != returnvalue::OK) {
sif::warning << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 2 high"
<< std::endl;
// If gpio switch high failed, state machine is reset to wait for a command re-initiating
// the deployment sequence.
triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
}
return result;
}
ReturnValue_t SolarArrayDeploymentHandler::sa2Off() {
ReturnValue_t result = gpioInterface.pullLow(deplSA2);
if (result != returnvalue::OK) {
sif::warning << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
" array deployment switch 2 low"
<< std::endl;
// If gpio switch high failed, state machine is reset to wait for a command re-initiating
// the deployment sequence.
triggerEvent(DEPL_SA2_GPIO_SWTICH_OFF_FAILED);
}
return result;
}
void SolarArrayDeploymentHandler::readCommandQueue() {
@ -181,27 +481,14 @@ void SolarArrayDeploymentHandler::readCommandQueue() {
}
}
ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
ReturnValue_t result;
if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
<< "waiting-on-command-state" << std::endl;
return DEPLOYMENT_ALREADY_EXECUTING;
}
if (actionId != DEPLOY_SOLAR_ARRAYS) {
sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command"
<< std::endl;
result = COMMAND_NOT_SUPPORTED;
} else {
stateMachine = SWITCH_8V_ON;
rememberCommanderId = commandedBy;
result = returnvalue::OK;
}
return result;
}
MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
return commandQueue->getId();
}
ReturnValue_t SolarArrayDeploymentHandler::initialize() {
ReturnValue_t result = actionHelper.initialize(commandQueue);
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
return SystemObject::initialize();
}

196
mission/devices/SolarArrayDeploymentHandler.h
View File

@ -1,22 +1,46 @@
#ifndef MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_
#define MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_
#include <devices/powerSwitcherList.h>
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/devicehandlers/CookieIF.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/objectmanager/SystemObject.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/tasks/ExecutableObjectIF.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
#include <unordered_map>
#include "devices/powerSwitcherList.h"
#include "eive/definitions.h"
#include "events/subsystemIdRanges.h"
#include "fsfw/action/HasActionsIF.h"
#include "fsfw/devicehandlers/CookieIF.h"
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
#include "fsfw/objectmanager/SystemObject.h"
#include "fsfw/power/PowerSwitchIF.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/serialize/SerialLinkedListAdapter.h"
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "fsfw/timemanager/Countdown.h"
#include "fsfw_hal/common/gpio/GpioIF.h"
#include "mission/memory/SdCardMountedIF.h"
#include "returnvalues/classIds.h"
enum DeploymentChannels : uint8_t { SA_1 = 1, SA_2 = 2 };
class ManualDeploymentCommand : public SerialLinkedListAdapter<SerializeIF> {
public:
ManualDeploymentCommand() { setLinks(); }
void setLinks() {
setStart(&burnTime);
burnTime.setNext(&dryRun);
}
uint32_t getBurnTime() const { return burnTime.entry; }
bool isDryRun() const { return dryRun.entry; }
private:
SerializeElement<uint32_t> burnTime;
SerializeElement<uint8_t> dryRun;
};
/**
* @brief This class is used to control the solar array deployment.
*
@ -26,8 +50,29 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
public SystemObject,
public HasActionsIF {
public:
static const DeviceCommandId_t DEPLOY_SOLAR_ARRAYS = 0x5;
//! Manual deployment of the solar arrays. Burn time and channels are supplied with TC parameters
static constexpr DeviceCommandId_t DEPLOY_SOLAR_ARRAYS_MANUALLY = 0x05;
static constexpr DeviceCommandId_t SWITCH_OFF_DEPLOYMENT = 0x06;
static constexpr uint32_t FIRST_BURN_START_TIME = config::SA_DEPL_INIT_BUFFER_SECS;
static constexpr uint32_t FIRST_BURN_END_TIME =
FIRST_BURN_START_TIME + config::SA_DEPL_BURN_TIME_SECS;
static constexpr uint32_t WAIT_START_TIME = FIRST_BURN_END_TIME;
static constexpr uint32_t WAIT_END_TIME = WAIT_START_TIME + config::SA_DEPL_WAIT_TIME_SECS;
static constexpr uint32_t SECOND_BURN_START_TIME = WAIT_END_TIME;
static constexpr uint32_t SECOND_BURN_END_TIME =
SECOND_BURN_START_TIME + config::SA_DEPL_WAIT_TIME_SECS;
static constexpr char SD_0_DEPL_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_DEPL_FILE[] = "/mnt/sd1/conf/deployment";
static constexpr char SD_0_DEPLY_INFO[] = "/mnt/sd0/conf/deployment_info.txt";
static constexpr char SD_1_DEPLY_INFO[] = "/mnt/sd1/conf/deployment_info.txt";
static constexpr char PHASE_INIT_STR[] = "init";
static constexpr char PHASE_FIRST_BURN_STR[] = "first_burn";
static constexpr char PHASE_WAIT_STR[] = "wait";
static constexpr char PHASE_SECOND_BURN_STR[] = "second_burn";
static constexpr char PHASE_DONE[] = "done";
/**
* @brief constructor
*
@ -43,10 +88,9 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
* @param deplSA2 gpioId of the GPIO controlling the deployment 2 transistor.
* @param burnTimeMs Time duration the power will be applied to the burn wires.
*/
SolarArrayDeploymentHandler(object_id_t setObjectId, object_id_t gpioDriverId,
CookieIF* gpioCookie, object_id_t mainLineSwitcherObjectId,
pcdu::Switches mainLineSwitch, gpioId_t deplSA1, gpioId_t deplSA2,
uint32_t burnTimeMs);
SolarArrayDeploymentHandler(object_id_t setObjectId, GpioIF& gpio,
PowerSwitchIF& mainLineSwitcher, pcdu::Switches mainLineSwitch,
gpioId_t deplSA1, gpioId_t deplSA2, SdCardMountedIF& sdcMountedIF);
virtual ~SolarArrayDeploymentHandler();
@ -58,6 +102,26 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
virtual ReturnValue_t initialize() override;
private:
enum AutonomousDeplState { INIT, FIRST_BURN, WAIT, SECOND_BURN, DONE };
enum StateMachine {
IDLE,
MAIN_POWER_ON,
MAIN_POWER_OFF,
WAIT_MAIN_POWER_ON,
WAIT_MAIN_POWER_OFF,
SWITCH_DEPL_GPIOS,
BURNING
};
struct FsmInfo {
// Not required anymore
// DeploymentChannels channel;
bool dryRun;
bool alternationDummy = false;
uint32_t burnCountdownMs = config::SA_DEPL_MAX_BURN_TIME;
};
static const uint8_t INTERFACE_ID = CLASS_ID::SA_DEPL_HANDLER;
static const ReturnValue_t COMMAND_NOT_SUPPORTED = MAKE_RETURN_CODE(0xA0);
static const ReturnValue_t DEPLOYMENT_ALREADY_EXECUTING = MAKE_RETURN_CODE(0xA1);
@ -66,23 +130,41 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
static const ReturnValue_t SWITCHING_DEPL_SA2_FAILED = MAKE_RETURN_CODE(0xA4);
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SA_DEPL_HANDLER;
static const Event MAIN_SWITCH_ON_TIMEOUT = MAKE_EVENT(0, severity::LOW);
static const Event MAIN_SWITCH_OFF_TIMEOUT = MAKE_EVENT(1, severity::LOW);
static const Event DEPLOYMENT_FAILED = MAKE_EVENT(2, severity::HIGH);
static const Event DEPL_SA1_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(3, severity::HIGH);
static const Event DEPL_SA2_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(4, severity::HIGH);
enum StateMachine {
WAIT_ON_DELOYMENT_COMMAND,
SWITCH_8V_ON,
WAIT_ON_8V_SWITCH,
SWITCH_DEPL_GPIOS,
WAIT_ON_DEPLOYMENT_FINISH,
WAIT_FOR_MAIN_SWITCH_OFF
};
//! [EXPORT] : [COMMENT] P1: Burn duration in milliseconds, P2: Dry run flag
static constexpr Event BURN_PHASE_START = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Burn duration in milliseconds, P2: Dry run flag
static constexpr Event BURN_PHASE_DONE = event::makeEvent(SUBSYSTEM_ID, 1, severity::INFO);
static constexpr Event MAIN_SWITCH_ON_TIMEOUT = event::makeEvent(SUBSYSTEM_ID, 2, severity::LOW);
static constexpr Event MAIN_SWITCH_OFF_TIMEOUT = event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW);
static constexpr Event DEPL_SA1_GPIO_SWTICH_ON_FAILED =
event::makeEvent(SUBSYSTEM_ID, 4, severity::HIGH);
static constexpr Event DEPL_SA2_GPIO_SWTICH_ON_FAILED =
event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
static constexpr Event DEPL_SA1_GPIO_SWTICH_OFF_FAILED =
event::makeEvent(SUBSYSTEM_ID, 6, severity::HIGH);
static constexpr Event DEPL_SA2_GPIO_SWTICH_OFF_FAILED =
event::makeEvent(SUBSYSTEM_ID, 7, severity::HIGH);
static constexpr Event AUTONOMOUS_DEPLOYMENT_COMPLETED =
event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO);
StateMachine stateMachine = WAIT_ON_DELOYMENT_COMMAND;
FsmInfo fsmInfo;
StateMachine stateMachine = IDLE;
bool actionActive = false;
bool firstAutonomousCycle = true;
ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID;
std::optional<uint64_t> initUptime;
PeriodicOperationDivider opDivider = PeriodicOperationDivider(5);
uint8_t retryCounter = 3;
bool startFsmOn(uint32_t burnCountdownSecs, bool dryRun);
void startFsmOff();
void finishFsm(ReturnValue_t resultForActionHelper);
ReturnValue_t performAutonomousDepl(sd::SdCard sdCard, bool dryRun);
bool dryRunStringInFile(const char* filename);
bool autonomousDeplForFile(sd::SdCard sdCard, const char* filename, bool dryRun);
/**
* This countdown is used to check if the PCDU sets the 8V line on in the intended time.
*/
@ -91,7 +173,10 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
/**
* This countdown is used to wait for the burn wire being successful cut.
*/
Countdown deploymentCountdown;
Countdown burnCountdown;
Countdown channelAlternationCd =
Countdown(config::SA_DEPL_CHANNEL_ALTERNATION_INTERVAL_SECS * 1000);
/**
* The message queue id of the component commanding an action will be stored in this variable.
@ -101,36 +186,25 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
/** Size of command queue */
size_t cmdQueueSize = 20;
/** The object ID of the GPIO driver which switches the deployment transistors */
object_id_t gpioDriverId;
CookieIF* gpioCookie;
/** Object id of the object responsible to switch the 8V power input. Typically the PCDU. */
object_id_t mainLineSwitcherObjectId;
/** Switch number of the 8V power switch */
uint8_t mainLineSwitch;
GpioIF& gpioInterface;
gpioId_t deplSA1;
gpioId_t deplSA2;
GpioIF* gpioInterface = nullptr;
/** Time duration switches are active to cut the burn wire */
uint32_t burnTimeMs;
/** Queue to receive messages from other objects. */
MessageQueueIF* commandQueue = nullptr;
/**
* After initialization this pointer will hold the reference to the main line switcher object.
*/
PowerSwitchIF* mainLineSwitcher = nullptr;
PowerSwitchIF& mainLineSwitcher;
/** Switch number of the 8V power switch */
uint8_t mainLineSwitch;
SdCardMountedIF& sdcMan;
ActionHelper actionHelper;
/** Queue to receive messages from other objects. */
MessageQueueIF* commandQueue = nullptr;
void readCommandQueue();
/**
@ -142,18 +216,18 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
* @brief This function polls the 8V switch state and changes the state machine when the
* switch has been enabled.
*/
void performWaitOn8VActions();
bool checkMainPowerOn();
bool checkMainPowerOff();
bool checkMainPower(bool onOff);
/**
* @brief This functions handles the switching of the solar array deployment transistors.
*/
void switchDeploymentTransistors();
void allOff();
/**
* @brief This function performs actions to finish the deployment. Essentially switches
* are turned of after the burn time has expired.
*/
void handleDeploymentFinish();
ReturnValue_t deploymentTransistorsOff();
ReturnValue_t saGpioAlternation();
ReturnValue_t sa1On();
ReturnValue_t sa1Off();
ReturnValue_t sa2On();
ReturnValue_t sa2Off();
};
#endif /* MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ */

2
mission/devices/SusHandler.cpp
View File

@ -150,7 +150,7 @@ ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSiz
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
switch (id) {
case SUS::WRITE_SETUP: {
if (mode == _MODE_START_UP) {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
}
return returnvalue::OK;

2
mission/devices/SyrlinksHkHandler.cpp
View File

@ -630,7 +630,7 @@ ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& lo
return returnvalue::OK;
}
void SyrlinksHkHandler::setModeNormal() { mode = MODE_NORMAL; }
void SyrlinksHkHandler::setModeNormal() { setMode(MODE_NORMAL); }
float SyrlinksHkHandler::calcTempVal(uint16_t raw) { return 0.126984 * raw - 67.87; }

6
mission/devices/Tmp1075Handler.cpp
View File

@ -12,7 +12,7 @@ Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF
Tmp1075Handler::~Tmp1075Handler() {}
void Tmp1075Handler::doStartUp() {
if (mode == _MODE_START_UP) {
if (getMode() == _MODE_START_UP) {
setMode(MODE_ON);
}
}
@ -86,7 +86,7 @@ ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const u
int16_t tempValueRaw = 0;
tempValueRaw = packet[0] << 4 | packet[1] >> 4;
float tempValue = ((static_cast<float>(tempValueRaw)) * 0.0625);
#if OBSW_VERBOSE_LEVEL >= 1
#if OBSW_DEBUG_TMP1075 == 1
sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId()
<< ": Temperature: " << tempValue << " °C" << std::endl;
#endif
@ -127,3 +127,5 @@ ReturnValue_t Tmp1075Handler::initializeLocalDataPool(localpool::DataPool &local
subdp::RegularHkPeriodicParams(dataset.getSid(), false, 30.0));
return returnvalue::OK;
}
void Tmp1075Handler::setModeNormal() { setMode(_MODE_TO_NORMAL); }

2
mission/devices/Tmp1075Handler.h
View File

@ -20,6 +20,8 @@ class Tmp1075Handler : public DeviceHandlerBase {
Tmp1075Handler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie);
virtual ~Tmp1075Handler();
void setModeNormal();
protected:
void doStartUp() override;
void doShutDown() override;

1
mission/devices/devicedefinitions/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp)

1
mission/devices/devicedefinitions/GPSDefinitions.h
View File

@ -1,6 +1,7 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
#include "eive/eventSubsystemIds.h"
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
#include "fsfw/devicehandlers/DeviceHandlerIF.h"

70
mission/devices/devicedefinitions/GomspaceDefinitions.h
View File

@ -9,12 +9,8 @@
#include <cstdint>
#include "devices/powerSwitcherList.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"
#include "fsfw/platform.h"
#include "gomspaceDefines.h"
namespace GOMSPACE {
@ -106,19 +102,7 @@ namespace P60System {
enum class BatteryModes : uint8_t { CRITICAL = 1, SAFE = 2, NORMAL = 3, FULL = 4 };
enum class SetIds : uint32_t {
PDU_1_CORE = 1,
PDU_1_AUX = 2,
PDU_2_CORE = 3,
PDU_2_AUX = 4,
P60_CORE = 5,
P60_AUX = 6,
ACU_CORE = 7,
ACU_AUX = 8,
PDU_1_CONFIG = 9,
PDU_2_CONFIG = 10
};
enum class SetIds : uint32_t { CORE = 1, AUX = 2, CONFIG = 3 };
} // namespace P60System
@ -212,8 +196,8 @@ static const uint16_t MAX_CONFIGTABLE_ADDRESS = 408;
static const uint16_t MAX_HKTABLE_ADDRESS = 187;
// Sources:
// GomSpace library lib/p60-dock_client/include/gs/p60-dock/param
static const uint16_t HK_TABLE_SIZE = P60DOCK_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = P60DOCK_PARAM_SIZE;
static const uint16_t HK_TABLE_SIZE = gsConstants::P60DOCK_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = gsConstants::P60DOCK_PARAM_SIZE;
static const size_t MAX_REPLY_SIZE = CONFIG_TABLE_SIZE;
static const uint16_t CAL_TABLE = 0xAE;
static const uint8_t HK_TABLE_ENTRIES = 100;
@ -221,10 +205,10 @@ static const uint8_t HK_TABLE_ENTRIES = 100;
class CoreHkSet : public StaticLocalDataSet<16> {
public:
CoreHkSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::P60_CORE)) {}
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
CoreHkSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::P60_CORE))) {}
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::CORE))) {}
/** Measured output currents */
lp_vec_t<int16_t, P60Dock::hk::Index::CHNLS_LEN> currents =
@ -258,10 +242,10 @@ class CoreHkSet : public StaticLocalDataSet<16> {
class HkTableDataset : public StaticLocalDataSet<32> {
public:
HkTableDataset(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::P60_AUX)) {}
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
HkTableDataset(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::P60_AUX))) {}
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::AUX))) {}
/** Number of detected latchups on each output channel */
lp_vec_t<uint16_t, P60Dock::hk::Index::CHNLS_LEN> latchups =
@ -389,8 +373,8 @@ enum Ids {
static const uint16_t MAX_CONFIGTABLE_ADDRESS = 316;
static const uint16_t MAX_HKTABLE_ADDRESS = 141;
/** The size of the csp reply containing the housekeeping table data */
static const uint16_t HK_TABLE_SIZE = P60PDU_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = P60PDU_PARAM_SIZE;
static const uint16_t HK_TABLE_SIZE = gsConstants::P60PDU_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = gsConstants::P60PDU_PARAM_SIZE;
/** When retrieving full configuration parameter table */
static const uint16_t MAX_REPLY_SIZE = CONFIG_TABLE_SIZE;
static const uint8_t HK_TABLE_ENTRIES = 73;
@ -549,25 +533,25 @@ static const uint16_t CONFIG_ADDRESS_OUT_EN_CHANNEL8 = 0x50;
class Pdu1CoreHk : public ::PDU::PduCoreHk {
public:
Pdu1CoreHk(HasLocalDataPoolIF* owner)
: PduCoreHk(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_1_CORE)) {}
: PduCoreHk(owner, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
Pdu1CoreHk(object_id_t objectId)
: PduCoreHk(objectId, static_cast<uint32_t>(::P60System::SetIds::PDU_1_CORE)) {}
: PduCoreHk(objectId, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
};
class Pdu1AuxHk : public ::PDU::PduAuxHk {
public:
Pdu1AuxHk(HasLocalDataPoolIF* owner)
: PduAuxHk(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_1_AUX)) {}
: PduAuxHk(owner, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
Pdu1AuxHk(object_id_t objectId)
: PduAuxHk(objectId, static_cast<uint32_t>(::P60System::SetIds::PDU_1_AUX)) {}
: PduAuxHk(objectId, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
};
class Pdu1Config : public ::PDU::PduConfig {
public:
Pdu1Config(HasLocalDataPoolIF* owner)
: PduConfig(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_1_CONFIG)) {}
: PduConfig(owner, static_cast<uint32_t>(::P60System::SetIds::CONFIG)) {}
};
} // namespace PDU1
@ -604,25 +588,25 @@ static const uint16_t CONFIG_ADDRESS_OUT_EN_PAYLOAD_CAMERA = 0x50;
class Pdu2CoreHk : public ::PDU::PduCoreHk {
public:
Pdu2CoreHk(HasLocalDataPoolIF* owner)
: PduCoreHk(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_2_CORE)) {}
: PduCoreHk(owner, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
Pdu2CoreHk(object_id_t objectId)
: PduCoreHk(objectId, static_cast<uint32_t>(::P60System::SetIds::PDU_2_CORE)) {}
: PduCoreHk(objectId, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
};
class Pdu2AuxHk : public ::PDU::PduAuxHk {
public:
Pdu2AuxHk(HasLocalDataPoolIF* owner)
: PduAuxHk(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_2_AUX)) {}
: PduAuxHk(owner, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
Pdu2AuxHk(object_id_t objectId)
: PduAuxHk(objectId, static_cast<uint32_t>(::P60System::SetIds::PDU_2_AUX)) {}
: PduAuxHk(objectId, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
};
class Pdu2Config : public ::PDU::PduConfig {
public:
Pdu2Config(HasLocalDataPoolIF* owner)
: PduConfig(owner, static_cast<uint32_t>(::P60System::SetIds::PDU_2_CONFIG)) {}
: PduConfig(owner, static_cast<uint32_t>(::P60System::SetIds::CONFIG)) {}
};
} // namespace PDU2
@ -658,17 +642,17 @@ enum Ids : lp_id_t {
static const uint16_t MAX_CONFIGTABLE_ADDRESS = 26;
static const uint16_t MAX_HKTABLE_ADDRESS = 120;
static const uint8_t HK_TABLE_ENTRIES = 64;
static const uint16_t HK_TABLE_SIZE = P60ACU_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = P60ACU_PARAM_SIZE;
static const uint16_t HK_TABLE_SIZE = gsConstants::P60ACU_HK_SIZE;
static const uint16_t CONFIG_TABLE_SIZE = gsConstants::P60ACU_PARAM_SIZE;
static const size_t MAX_REPLY_SIZE = HK_TABLE_SIZE;
class CoreHk : public StaticLocalDataSet<14> {
public:
CoreHk(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::ACU_CORE)) {}
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::CORE)) {}
CoreHk(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::ACU_CORE))) {}
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::CORE))) {}
lp_var_t<uint8_t> mpptMode = lp_var_t<uint8_t>(sid.objectId, pool::ACU_MPPT_MODE, this);
@ -698,10 +682,10 @@ class CoreHk : public StaticLocalDataSet<14> {
class AuxHk : public StaticLocalDataSet<12> {
public:
AuxHk(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::ACU_AUX)) {}
: StaticLocalDataSet(owner, static_cast<uint32_t>(::P60System::SetIds::AUX)) {}
AuxHk(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::ACU_AUX))) {}
: StaticLocalDataSet(sid_t(objectId, static_cast<uint32_t>(::P60System::SetIds::AUX))) {}
lp_vec_t<uint8_t, 3> dacEnables = lp_vec_t<uint8_t, 3>(sid.objectId, pool::ACU_DAC_ENABLES, this);

13
mission/devices/devicedefinitions/SCEXDefinitions.h
View File

@ -1,13 +0,0 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_
#include <cstdint>
// Definitions for the Solar Cell Experiment
namespace scex {
static constexpr uint8_t CMD_PING = 0x4e;
}
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_ */

35
mission/devices/devicedefinitions/ScexDefinitions.cpp Normal file
View File

@ -0,0 +1,35 @@
#include "ScexDefinitions.h"
#include <fsfw/globalfunctions/CRC.h>
#include <cstring>
uint8_t scex::createCmdByte(Cmds cmd, bool tempCheck) {
return (IDLE_BIT_0_DEF_STATE << 7) | (IDLE_BIT_1_DEF_STATE << 6) | (cmd << 1) | tempCheck;
}
ReturnValue_t scex::prepareScexCmd(Cmds cmd, std::pair<uint8_t*, size_t> cmdBufPair, size_t& cmdLen,
std::pair<const uint8_t*, size_t> usrDataPair, bool tempCheck) {
using namespace scex;
uint8_t* cmdBuf = cmdBufPair.first;
const uint8_t* userData = usrDataPair.first;
// Send command
if (cmdBuf == nullptr or (cmdBufPair.second < usrDataPair.second + HEADER_LEN + CRC_LEN) or
(usrDataPair.second > 0 and userData == nullptr)) {
cmdLen = 0;
return returnvalue::FAILED;
}
cmdBuf[0] = createCmdByte(cmd, tempCheck);
// These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
// telecommand so far
cmdBuf[1] = 1;
cmdBuf[2] = 1;
cmdBuf[3] = (usrDataPair.second >> 8) & 0xff;
cmdBuf[4] = usrDataPair.second & 0xff;
std::memcpy(cmdBuf + HEADER_LEN, userData, usrDataPair.second);
uint16_t crc = CRC::crc16ccitt(cmdBuf, usrDataPair.second + HEADER_LEN);
cmdBuf[usrDataPair.second + HEADER_LEN] = (crc >> 8) & 0xff;
cmdBuf[usrDataPair.second + HEADER_LEN + 1] = crc & 0xff;
cmdLen = usrDataPair.second + HEADER_LEN + CRC_LEN;
return returnvalue::OK;
}

52
mission/devices/devicedefinitions/ScexDefinitions.h Normal file
View File

@ -0,0 +1,52 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/events/Event.h>
#include <cstdint>
#include <vector>
#include "eive/eventSubsystemIds.h"
#include "eive/objects.h"
// Definitions for the Solar Cell Experiment
namespace scex {
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SCEX_HANDLER;
static constexpr Event MISSING_PACKET = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW);
static constexpr Event EXPERIMENT_TIMEDOUT = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
//! FRAM, One Cell or All cells command finished. P1: Command ID
static constexpr Event MULTI_PACKET_COMMAND_DONE =
event::makeEvent(SUBSYSTEM_ID, 2, severity::INFO);
enum Cmds : DeviceCommandId_t {
PING = 0b00111,
ALL_CELLS_CMD = 0b00101,
ONE_CELL = 0b00110,
FRAM = 0b00001,
EXP_STATUS_CMD = 0b00010,
TEMP_CMD = 0b00011,
ION_CMD = 0b00100,
ERROR_REPLY = 0b01000,
INVALID = 255
};
static const std::vector<DeviceCommandId_t> VALID_CMDS = {
PING, ALL_CELLS_CMD, ONE_CELL, FRAM, EXP_STATUS_CMD, TEMP_CMD, ION_CMD};
static constexpr uint8_t HEADER_LEN = 5;
static constexpr uint8_t CRC_LEN = 2;
static constexpr uint8_t IDLE_BIT_0_DEF_STATE = 0;
static constexpr uint8_t IDLE_BIT_1_DEF_STATE = 1;
uint8_t createCmdByte(Cmds cmd, bool tempCheck = false);
ReturnValue_t prepareScexCmd(Cmds cmd, std::pair<uint8_t*, size_t> cmdBufPair, size_t& cmdLen,
std::pair<const uint8_t*, size_t> usrDataPair, bool tempCheck = false);
} // namespace scex
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_SCEXDEFINITIONS_H_ */

36
mission/devices/devicedefinitions/SpBase.h
View File

@ -14,43 +14,48 @@ struct SpTcParams {
SpTcParams(SpacePacketCreator& creator, uint8_t* buf, size_t maxSize)
: creator(creator), buf(buf), maxSize(maxSize) {}
void setPayloadLen(size_t payloadLen_) { dataFieldLen = payloadLen_ + 2; }
void setDataFieldLen(size_t dataFieldLen_) { dataFieldLen = dataFieldLen_; }
void setFullPayloadLen(size_t fullPayloadLen_) { fullPayloadLen = fullPayloadLen_; }
SpacePacketCreator& creator;
uint8_t* buf = nullptr;
size_t maxSize = 0;
size_t dataFieldLen = 0;
size_t fullPayloadLen = 0;
};
class SpTcBase {
public:
SpTcBase(SpTcParams params) : spParams(params) {
payloadStart = spParams.buf + ccsds::HEADER_LEN;
updateSpFields();
}
SpTcBase(SpTcParams params) : SpTcBase(params, 0x00, 1, 0) {}
SpTcBase(SpTcParams params, uint16_t apid, uint16_t seqCount) : spParams(params) {
SpTcBase(SpTcParams params, uint16_t apid, size_t payloadLen)
: SpTcBase(params, apid, payloadLen, 0) {}
SpTcBase(SpTcParams params, uint16_t apid, size_t payloadLen, uint16_t seqCount)
: spParams(params) {
spParams.creator.setApid(apid);
spParams.creator.setSeqCount(seqCount);
payloadStart = spParams.buf + ccsds::HEADER_LEN;
spParams.fullPayloadLen = payloadLen;
updateSpFields();
}
void updateSpFields() {
spParams.creator.setDataLen(spParams.dataFieldLen - 1);
updateLenFromParams();
spParams.creator.setPacketType(ccsds::PacketType::TC);
}
void updateLenFromParams() { spParams.creator.setDataLenField(spParams.fullPayloadLen - 1); }
const uint8_t* getFullPacket() const { return spParams.buf; }
const uint8_t* getPacketData() const { return spParams.buf + ccsds::HEADER_LEN; }
size_t getFullPacketLen() const { return spParams.creator.getFullPacketLen(); }
uint16_t getApid() const { return spParams.creator.getApid(); }
uint16_t getSeqCount() const { return spParams.creator.getSequenceCount(); }
ReturnValue_t checkPayloadLen() {
if (ccsds::HEADER_LEN + spParams.dataFieldLen > spParams.maxSize) {
if (ccsds::HEADER_LEN + spParams.fullPayloadLen > spParams.maxSize) {
return SerializeIF::BUFFER_TOO_SHORT;
}
@ -71,7 +76,7 @@ class SpTcBase {
return serializeHeader();
}
ReturnValue_t calcCrc() {
ReturnValue_t calcAndSetCrc() {
/* Calculate crc */
uint16_t crc = CRC::crc16ccitt(spParams.buf, getFullPacketLen() - 2);
@ -101,12 +106,7 @@ class SpTmReader : public SpacePacketReader {
return setReadOnlyData(buf, maxSize);
}
/**
* @brief Returns the payload data length (data field length without CRC)
*/
uint16_t getPayloadDataLength() { return getPacketDataLen() - 2; }
ReturnValue_t checkCrc() {
ReturnValue_t checkCrc() const {
if (CRC::crc16ccitt(getFullData(), getFullPacketLen()) != 0) {
return returnvalue::FAILED;
}

3
mission/devices/devicedefinitions/SyrlinksDefinitions.h
View File

@ -1,8 +1,7 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_SYRLINKSDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_SYRLINKSDEFINITIONS_H_
#include <commonSubsystemIds.h>
#include "eive/eventSubsystemIds.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
namespace syrlinks {

33
mission/devices/devicedefinitions/gomspaceDefines.h Normal file
View File

@ -0,0 +1,33 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GOMSPACEDEFINES_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GOMSPACEDEFINES_H_
#include "fsfw/platform.h"
#ifdef PLATFORM_UNIX
// 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"
#endif
#include <cstdint>
namespace gsConstants {
static constexpr uint32_t P60DOCK_HK_SIZE = 0xBE;
static constexpr uint32_t P60DOCK_PARAM_SIZE = 0x19C;
static constexpr uint32_t P60PDU_HK_SIZE = 0x90;
static constexpr uint32_t P60PDU_PARAM_SIZE = 0x13E;
static constexpr uint32_t P60ACU_HK_SIZE = 0x7C;
static constexpr uint32_t P60ACU_PARAM_SIZE = 0x1B;
} // namespace gsConstants
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GOMSPACEDEFINES_H_ */

121
mission/devices/devicedefinitions/IMTQHandlerDefinitions.h → mission/devices/devicedefinitions/imtqHandlerDefinitions.h
View File

@ -1,8 +1,11 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
class ImtqHandler;
namespace IMTQ {
static const DeviceCommandId_t NONE = 0x0;
@ -27,15 +30,18 @@ static const uint8_t GET_TEMP_REPLY_SIZE = 2;
static const uint8_t CFGR_CMD_SIZE = 3;
static const uint8_t POINTER_REG_SIZE = 1;
static const uint32_t ENG_HK_DATA_SET_ID = 1;
static const uint32_t CAL_MTM_SET = 2;
static const uint32_t RAW_MTM_SET = 3;
static const uint32_t POS_X_TEST_DATASET = 4;
static const uint32_t NEG_X_TEST_DATASET = 5;
static const uint32_t POS_Y_TEST_DATASET = 6;
static const uint32_t NEG_Y_TEST_DATASET = 7;
static const uint32_t POS_Z_TEST_DATASET = 8;
static const uint32_t NEG_Z_TEST_DATASET = 9;
enum SetIds : uint32_t {
ENG_HK = 1,
CAL_MGM = 2,
RAW_MGM = 3,
POS_X_TEST = 4,
NEG_X_TEST = 5,
POS_Y_TEST = 6,
NEG_Y_TEST = 7,
POS_Z_TEST = 8,
NEG_Z_TEST = 9,
DIPOLES = 10
};
static const uint8_t SIZE_ENG_HK_COMMAND = 1;
static const uint8_t SIZE_STATUS_REPLY = 2;
@ -80,6 +86,7 @@ static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
static const uint8_t GET_SELF_TEST_RESULT = 0x47;
static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
}; // namespace CC
namespace SELF_TEST_AXIS {
@ -103,7 +110,7 @@ static const uint8_t Z_NEGATIVE = 0x6;
static const uint8_t FINA = 0x7;
} // namespace SELF_TEST_STEPS
enum IMTQPoolIds : lp_id_t {
enum PoolIds : lp_id_t {
DIGITAL_VOLTAGE_MV,
ANALOG_VOLTAGE_MV,
DIGITAL_CURRENT,
@ -119,6 +126,10 @@ enum IMTQPoolIds : lp_id_t {
ACTUATION_CAL_STATUS,
MTM_RAW,
ACTUATION_RAW_STATUS,
DIPOLES_X,
DIPOLES_Y,
DIPOLES_Z,
CURRENT_TORQUE_DURATION,
INIT_POS_X_ERR,
INIT_POS_X_RAW_MAG_X,
@ -375,9 +386,9 @@ enum IMTQPoolIds : lp_id_t {
class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
public:
EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) {}
EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {}
EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) {}
EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {}
lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this);
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this);
@ -398,13 +409,14 @@ class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
*/
class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CAL_MTM_SET) {}
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {}
CalibratedMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {}
/** The unit of all measurements is nT */
lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT);
lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT, this);
/** 1 if coils were actuating during measurement otherwise 0 */
lp_var_t<uint8_t> coilActuationStatus =
lp_var_t<uint8_t>(sid.objectId, ACTUATION_CAL_STATUS, this);
@ -415,9 +427,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
*/
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, RAW_MTM_SET) {}
RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}
RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {}
RawMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}
/** The unit of all measurements is nT */
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
@ -437,6 +451,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
public:
CommandDipolePacket() { setLinks(); }
SerializeElement<uint16_t> xDipole;
SerializeElement<uint16_t> yDipole;
SerializeElement<uint16_t> zDipole;
SerializeElement<uint16_t> duration;
private:
/**
* @brief Constructor
@ -456,10 +475,46 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
yDipole.setNext(&zDipole);
zDipole.setNext(&duration);
}
SerializeElement<uint16_t> xDipole;
SerializeElement<uint16_t> yDipole;
SerializeElement<uint16_t> zDipole;
SerializeElement<uint16_t> duration;
};
class DipoleActuationSet : public StaticLocalDataSet<4> {
friend class ::ImtqHandler;
public:
DipoleActuationSet(HasLocalDataPoolIF& owner)
: StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
DipoleActuationSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
// Refresh torque command without changing any of the set dipoles.
void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_,
uint16_t currentTorqueDurationMs_) {
if (xDipole.value != xDipole_) {
}
xDipole = xDipole_;
if (yDipole.value != yDipole_) {
}
yDipole = yDipole_;
if (zDipole.value != zDipole_) {
}
zDipole = zDipole_;
currentTorqueDurationMs = currentTorqueDurationMs_;
}
void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) {
xDipole_ = xDipole.value;
yDipole_ = yDipole.value;
zDipole_ = zDipole.value;
}
private:
lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this);
lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
lp_var_t<uint16_t> currentTorqueDurationMs =
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
};
/**
@ -479,10 +534,10 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {}
PosXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_X_ERR, this);
@ -556,10 +611,10 @@ class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {}
NegXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_X_ERR, this);
@ -633,10 +688,10 @@ class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {}
PosYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Y_ERR, this);
@ -710,10 +765,10 @@ class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {}
NegYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Y_ERR, this);
@ -787,10 +842,10 @@ class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {}
PosZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Z_ERR, this);
@ -864,10 +919,10 @@ class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) {}
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {}
NegZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) {}
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Z_ERR, this);

3
mission/devices/devicedefinitions/payloadPcduDefinitions.h
View File

@ -101,6 +101,9 @@ enum NormalSubmodeBits {
};
static constexpr Submode_t ALL_OFF_SUBMODE = 0;
static constexpr Submode_t ALL_ON_SUBMODE = (1 << HPA_ON) | (1 << MPA_ON) | (1 << TX_ON) |
(1 << X8_ON) | (1 << DRO_ON) |
(1 << SOLID_STATE_RELAYS_ADC_ON);
// 12 ADC values * 2 + trailing zero
static constexpr size_t ADC_REPLY_SIZE = 25;

3
mission/devices/devicedefinitions/powerDefinitions.h
View File

@ -1,9 +1,10 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_
#include <common/config/commonSubsystemIds.h>
#include <fsfw/events/Event.h>
#include "eive/eventSubsystemIds.h"
namespace power {
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_HANDLER;

27
mission/devices/torquer.cpp Normal file
View File

@ -0,0 +1,27 @@
#include "torquer.h"
#include <fsfw/ipc/MutexGuard.h>
MutexIF* TORQUE_LOCK = nullptr;
namespace torquer {
bool TORQUEING = false;
bool NEW_ACTUATION_FLAG = false;
Countdown TORQUE_COUNTDOWN = Countdown();
bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
if (TORQUEING and remainingTorqueDuration != nullptr) {
*remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
}
return TORQUEING;
}
MutexIF* lazyLock() {
if (TORQUE_LOCK == nullptr) {
TORQUE_LOCK = MutexFactory::instance()->createMutex();
}
return TORQUE_LOCK;
}
} // namespace torquer

22
mission/devices/torquer.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef MISSION_DEVICES_TOQUER_H_
#define MISSION_DEVICES_TOQUER_H_
#include <fsfw/ipc/MutexIF.h>
#include <fsfw/timemanager/Countdown.h>
namespace torquer {
// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
// time of the MGT
static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
MutexIF* lazyLock();
extern bool TORQUEING;
extern bool NEW_ACTUATION_FLAG;
extern Countdown TORQUE_COUNTDOWN;
bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
} // namespace torquer
#endif /* MISSION_DEVICES_TOQUER_H_ */

2
mission/memory/NVMParameterBase.cpp
View File

@ -2,7 +2,7 @@
#include <fstream>
#include "fsfw/memory/HasFileSystemIF.h"
#include "fsfw/filesystem/HasFileSystemIF.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
NVMParameterBase::NVMParameterBase(std::string fullName) : fullName(fullName) {}

9
mission/memory/SdCardMountedIF.h
View File

@ -1,6 +1,7 @@
#ifndef MISSION_MEMORY_SDCARDMOUNTERIF_H_
#define MISSION_MEMORY_SDCARDMOUNTERIF_H_
#include <optional>
#include <string>
#include "definitions.h"
@ -8,11 +9,11 @@
class SdCardMountedIF {
public:
virtual ~SdCardMountedIF(){};
virtual std::string getCurrentMountPrefix() const = 0;
virtual bool isSdCardMounted(sd::SdCard sdCard) = 0;
virtual sd::SdCard getPreferredSdCard() const = 0;
virtual const std::string& getCurrentMountPrefix() const = 0;
virtual bool isSdCardUsable(std::optional<sd::SdCard> sdCard) = 0;
virtual std::optional<sd::SdCard> getPreferredSdCard() const = 0;
virtual void setActiveSdCard(sd::SdCard sdCard) = 0;
virtual sd::SdCard getActiveSdCard() const = 0;
virtual std::optional<sd::SdCard> getActiveSdCard() const = 0;
private:
};

5
mission/system/AcsSubsystem.cpp
View File

@ -1,5 +0,0 @@
#include "AcsSubsystem.h"
AcsSubsystem::AcsSubsystem(object_id_t setObjectId, object_id_t parent,
uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables)
: Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {}

16
mission/system/CMakeLists.txt
View File

@ -1,15 +1,3 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE EiveSystem.cpp
AcsSubsystem.cpp
ComSubsystem.cpp
PayloadSubsystem.cpp
AcsBoardAssembly.cpp
RwAssembly.cpp
SusAssembly.cpp
DualLanePowerStateMachine.cpp
PowerStateMachineBase.cpp
DualLaneAssemblyBase.cpp
TcsBoardAssembly.cpp)
add_subdirectory(objects)
add_subdirectory(tree)
add_subdirectory(fdir)

5
mission/system/ComSubsystem.cpp
View File

@ -1,5 +0,0 @@
#include "ComSubsystem.h"
ComSubsystem::ComSubsystem(object_id_t setObjectId, object_id_t parent,
uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables)
: Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {}

5
mission/system/EiveSystem.cpp
View File

@ -1,5 +0,0 @@
#include "EiveSystem.h"
EiveSystem::EiveSystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {}

5
mission/system/PayloadSubsystem.cpp
View File

@ -1,5 +0,0 @@
#include "PayloadSubsystem.h"
PayloadSubsystem::PayloadSubsystem(object_id_t setObjectId, object_id_t parent,
uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables)
: Subsystem(setObjectId, parent, maxNumberOfSequences, maxNumberOfTables) {}

2
mission/system/fdir/AcsBoardFdir.cpp
View File

@ -1,6 +1,6 @@
#include "AcsBoardFdir.h"
#include <common/config/commonObjects.h>
#include "eive/objects.h"
AcsBoardFdir::AcsBoardFdir(object_id_t sensorId)
: DeviceHandlerFailureIsolation(sensorId, objects::ACS_BOARD_ASS) {}

2
mission/system/fdir/RtdFdir.cpp
View File

@ -1,6 +1,6 @@
#include "RtdFdir.h"
#include <common/config/commonObjects.h>
#include "eive/objects.h"
RtdFdir::RtdFdir(object_id_t sensorId)
: DeviceHandlerFailureIsolation(sensorId, objects::TCS_BOARD_ASS) {}

2
mission/system/fdir/SusFdir.cpp
View File

@ -1,6 +1,6 @@
#include "SusFdir.h"
#include <common/config/commonObjects.h>
#include "eive/objects.h"
SusFdir::SusFdir(object_id_t sensorId)
: DeviceHandlerFailureIsolation(sensorId, objects::SUS_BOARD_ASS) {}

49
mission/system/AcsBoardAssembly.cpp → mission/system/objects/AcsBoardAssembly.cpp
View File

@ -6,11 +6,10 @@
#include "OBSWConfig.h"
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
: DualLaneAssemblyBase(objectId, parentId, switcher, SWITCH_A, SWITCH_B,
POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
TRANSITION_OTHER_SIDE_FAILED),
AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, PowerSwitchIF* switcher,
AcsBoardHelper helper, GpioIF* gpioIF)
: DualLaneAssemblyBase(objectId, switcher, SWITCH_A, SWITCH_B, POWER_STATE_MACHINE_TIMEOUT,
SIDE_SWITCH_TRANSITION_NOT_ALLOWED, TRANSITION_OTHER_SIDE_FAILED),
helper(helper),
gpioIF(gpioIF) {
if (switcher == nullptr) {
@ -275,42 +274,4 @@ void AcsBoardAssembly::refreshHelperModes() {
}
}
ReturnValue_t AcsBoardAssembly::initialize() {
ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.gyro1L3gIdSideA);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.gyro2AdisIdSideB);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.gyro3L3gIdSideB);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.mgm0Lis3IdSideA);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.mgm1Rm3100IdSideA);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.mgm2Lis3IdSideB);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.mgm3Rm3100IdSideB);
if (result != returnvalue::OK) {
return result;
}
result = registerChild(helper.gpsId);
if (result != returnvalue::OK) {
return result;
}
return AssemblyBase::initialize();
}
ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); }

6
mission/system/AcsBoardAssembly.h → mission/system/objects/AcsBoardAssembly.h
View File

@ -1,13 +1,13 @@
#ifndef MISSION_SYSTEM_ACSBOARDASSEMBLY_H_
#define MISSION_SYSTEM_ACSBOARDASSEMBLY_H_
#include <common/config/commonSubsystemIds.h>
#include <devices/powerSwitcherList.h>
#include <fsfw/objectmanager/frameworkObjects.h>
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include "DualLaneAssemblyBase.h"
#include "DualLanePowerStateMachine.h"
#include "eive/eventSubsystemIds.h"
struct AcsBoardHelper {
AcsBoardHelper(object_id_t mgm0Id, object_id_t mgm1Id, object_id_t mgm2Id, object_id_t mgm3Id,
@ -94,8 +94,8 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9;
AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
AcsBoardHelper helper, GpioIF* gpioIF);
AcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, AcsBoardHelper helper,
GpioIF* gpioIF);
/**
* In dual mode, the A side or the B side GPS device can be used, but not both.

5
mission/system/objects/AcsSubsystem.cpp Normal file
View File

@ -0,0 +1,5 @@
#include "AcsSubsystem.h"
AcsSubsystem::AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {}

3
mission/system/AcsSubsystem.h → mission/system/objects/AcsSubsystem.h
View File

@ -5,8 +5,7 @@
class AcsSubsystem : public Subsystem {
public:
AcsSubsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables);
AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
private:
};

14
mission/system/objects/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE EiveSystem.cpp
CamSwitcher.cpp
AcsSubsystem.cpp
ComSubsystem.cpp
PayloadSubsystem.cpp
AcsBoardAssembly.cpp
SusAssembly.cpp
RwAssembly.cpp
DualLanePowerStateMachine.cpp
PowerStateMachineBase.cpp
DualLaneAssemblyBase.cpp
TcsBoardAssembly.cpp)

5
mission/system/objects/CamSwitcher.cpp Normal file
View File

@ -0,0 +1,5 @@
#include "CamSwitcher.h"
CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher,
power::Switch_t pwrSwitch)
: PowerSwitcherComponent(objectId, &pwrSwitcher, pwrSwitch) {}

13
mission/system/objects/CamSwitcher.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
#define MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
#include <fsfw/power/PowerSwitcherComponent.h>
class CamSwitcher : public PowerSwitcherComponent {
public:
CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher, power::Switch_t pwrSwitch);
private:
};
#endif /* MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_ */

5
mission/system/objects/ComSubsystem.cpp Normal file
View File

@ -0,0 +1,5 @@
#include "ComSubsystem.h"
ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {}

3
mission/system/ComSubsystem.h → mission/system/objects/ComSubsystem.h
View File

@ -5,8 +5,7 @@
class ComSubsystem : public Subsystem {
public:
ComSubsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables);
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
private:
};

9
mission/system/DualLaneAssemblyBase.cpp → mission/system/objects/DualLaneAssemblyBase.cpp
View File

@ -4,12 +4,11 @@
#include "OBSWConfig.h"
DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId,
PowerSwitchIF* pwrSwitcher, pcdu::Switches switch1,
pcdu::Switches switch2, Event pwrTimeoutEvent,
Event sideSwitchNotAllowedEvent,
DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
pcdu::Switches switch1, pcdu::Switches switch2,
Event pwrTimeoutEvent, Event sideSwitchNotAllowedEvent,
Event transitionOtherSideFailedEvent)
: AssemblyBase(objectId, parentId, 20),
: AssemblyBase(objectId, 20),
pwrStateMachine(switch1, switch2, pwrSwitcher),
pwrTimeoutEvent(pwrTimeoutEvent),
sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent),

6
mission/system/DualLaneAssemblyBase.h → mission/system/objects/DualLaneAssemblyBase.h
View File

@ -2,7 +2,7 @@
#define MISSION_SYSTEM_DUALLANEASSEMBLYBASE_H_
#include <fsfw/devicehandlers/AssemblyBase.h>
#include <mission/system/DualLanePowerStateMachine.h>
#include <mission/system/objects/DualLanePowerStateMachine.h>
/**
* @brief Encapsulates assemblies which are also responsible for dual lane power switching
@ -18,8 +18,8 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
static constexpr UniqueEventId_t POWER_STATE_MACHINE_TIMEOUT_ID = 2;
static constexpr UniqueEventId_t SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID = 3;
DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
pcdu::Switches switch1, pcdu::Switches switch2, Event pwrSwitchTimeoutEvent,
DualLaneAssemblyBase(object_id_t objectId, PowerSwitchIF* pwrSwitcher, pcdu::Switches switch1,
pcdu::Switches switch2, Event pwrSwitchTimeoutEvent,
Event sideSwitchNotAllowedEvent, Event transitionOtherSideFailedEvent);
protected:

Some files were not shown because too many files have changed in this diff Show More