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

Merge remote-tracking branch 'origin/develop' into meier/ploc-commands
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-03-13 11:04:09 +01:00
parent 6004233c67 fde644cdbf
commit f115e6ca33
218 changed files with 8621 additions and 4789 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
mission/acsDefs.cpp
View File

@ -11,10 +11,6 @@ const char* acs::getModeStr(AcsMode mode) {
modeStr = "SAFE";
break;
}
case (acs::AcsMode::DETUMBLE): {
modeStr = "DETUBMLE";
break;
}
case (acs::AcsMode::PTG_NADIR): {
modeStr = "POITNING NADIR";
break;

15
mission/acsDefs.h
View File

@ -10,14 +10,17 @@ namespace acs {
enum AcsMode : Mode_t {
OFF = HasModesIF::MODE_OFF,
SAFE = 10,
DETUMBLE = 11,
PTG_IDLE = 12,
PTG_NADIR = 13,
PTG_TARGET = 14,
PTG_TARGET_GS = 15,
PTG_INERTIAL = 16,
PTG_IDLE = 11,
PTG_NADIR = 12,
PTG_TARGET = 13,
PTG_TARGET_GS = 14,
PTG_INERTIAL = 15,
};
enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
// static constexpr uint8_t ACS_SYSTEM_DETUMBLE_SUBMODE = 1;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//!< The limits for the rotation in safe mode were violated.
static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);

4
mission/config/comCfg.cpp
View File

@ -9,12 +9,12 @@ MutexIF* DATARATE_LOCK = nullptr;
MutexIF* lazyLock();
com::Datarate com::getCurrentDatarate() {
MutexGuard mg(lazyLock());
MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com");
return DATARATE_CFG_RAW;
}
void com::setCurrentDatarate(com::Datarate newRate) {
MutexGuard mg(lazyLock());
MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com");
DATARATE_CFG_RAW = newRate;
}

3
mission/config/torquer.h
View File

@ -9,6 +9,9 @@ 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;
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
static constexpr uint32_t LOCK_TIMEOUT = 20;
static constexpr char LOCK_CTX[] = "torquer";
MutexIF* lazyLock();
extern bool TORQUEING;

160
mission/controller/AcsController.cpp
View File

@ -6,12 +6,8 @@
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId),
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
parameterHelper(this),
mgmDataRaw(this),
@ -112,12 +108,16 @@ void AcsController::performControlOperation() {
}
case InternalState::READY: {
if (mode != MODE_OFF) {
switch (submode) {
switch (mode) {
case acs::SAFE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
switch (submode) {
case SUBMODE_NONE:
performSafe();
break;
case acs::DETUMBLE:
performDetumble();
break;
}
break;
case acs::PTG_IDLE:
case acs::PTG_TARGET:
@ -142,7 +142,7 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -157,23 +157,28 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
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);
result = 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);
} else {
safeCtrl.safeNoMekf(
result = 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);
sunTargetDir, satRateSafe, &errAng, magMomMtq);
}
if (result == returnvalue::FAILED) {
// ToDo: this should never ever happen or we are dead. prob add an event at least
}
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
// detumble check and switch
if (mekfData.satRotRateMekf.isValid() &&
@ -184,20 +189,21 @@ void AcsController::performSafe() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else {
detumbleCounter = 0;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE);
}
updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::performDetumble() {
@ -207,7 +213,7 @@ void AcsController::performDetumble() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -220,8 +226,11 @@ void AcsController::performDetumble() {
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
acsParameters.detumbleParameter.gainD);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -231,13 +240,14 @@ void AcsController::performDetumble() {
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else {
detumbleCounter = 0;
} else if (detumbleCounter > 0) {
detumbleCounter -= 1;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
detumbleCounter = 0;
// Triggers safe mode transition in subsystem
triggerEvent(acs::SAFE_RATE_RECOVERY);
startTransition(mode, acs::SafeSubmode::DEFAULT);
}
disableCtrlValData();
@ -254,15 +264,16 @@ void AcsController::performPointingCtrl() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData);
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
triggerEvent(acs::MEKF_INVALID_INFO);
mekfInvalidFlag = true;
}
if (mekfInvalidCounter > 4) {
triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
if (mekfInvalidCounter == 5) {
// Trigger this so STR FDIR can set the device faulty.
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
}
mekfInvalidCounter++;
// commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration,
@ -278,7 +289,7 @@ void AcsController::performPointingCtrl() {
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
if (multipleRwUnavailableCounter > 4) {
if (multipleRwUnavailableCounter == 5) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
}
multipleRwUnavailableCounter++;
@ -286,24 +297,26 @@ void AcsController::performPointingCtrl() {
} else {
multipleRwUnavailableCounter = 0;
}
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
double mgtDpDes[3] = {0, 0, 0};
// Variables required for guidance
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
switch (submode) {
// Variables required for setting actuators
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
mgtDpDes[3] = {0, 0, 0};
switch (mode) {
case acs::PTG_IDLE:
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -327,7 +340,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -341,20 +354,20 @@ void AcsController::performPointingCtrl() {
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
&acsParameters.gsTargetModeControllerParameters, 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);
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
break;
case acs::PTG_NADIR:
@ -372,7 +385,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -395,7 +408,7 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
@ -403,20 +416,26 @@ void AcsController::performPointingCtrl() {
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
break;
default:
sif::error << "AcsController: Invalid mode for performPointingCtrl";
break;
}
actuatorCmd.cmdSpeedToRws(
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
acsParameters.rwHandlingParameters.maxRwSpeed,
acsParameters.rwHandlingParameters.inertiaWheel);
if (enableAntiStiction) {
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
}
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax);
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -429,7 +448,8 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
uint16_t rampTime) {
{
PoolReadGuard pg(&dipoleSet);
MutexGuard mg(torquer::lazyLock());
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
torquer::LOCK_CTX);
torquer::NEW_ACTUATION_FLAG = true;
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
}
@ -573,6 +593,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
// GPS Processed
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
@ -631,8 +652,14 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
} else {
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) {
} else if (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) {
if (mode == acs::AcsMode::SAFE) {
if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
}
} else if (not(submode == SUBMODE_NONE)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
@ -649,19 +676,25 @@ void AcsController::announceMode(bool recursive) {
const char *modeStr = "UNKNOWN";
if (mode == HasModesIF::MODE_OFF) {
modeStr = "OFF";
} else if (mode == HasModesIF::MODE_ON) {
modeStr = "ON";
} else if (mode == DeviceHandlerIF::MODE_NORMAL) {
modeStr = "NORMAL";
} else {
modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
}
const char *submodeStr = "UNKNOWN";
if (submode == HasModesIF::SUBMODE_NONE) {
submodeStr = "NONE";
}
if (mode == acs::AcsMode::SAFE) {
acs::SafeSubmode safeSubmode = static_cast<acs::SafeSubmode>(this->submode);
if (safeSubmode == acs::SafeSubmode::DETUMBLE) {
submodeStr = "DETUMBLE";
}
}
const char *submodeStr = acs::getModeStr(static_cast<acs::AcsMode>(submode));
sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode"
<< std::endl;
return ExtendedControllerBase::announceMode(recursive);
}
void AcsController::copyMgmData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
if (pg.getReadResult() == returnvalue::OK) {
@ -806,7 +839,6 @@ void AcsController::copySusData() {
}
void AcsController::copyGyrData() {
ACS::SensorValues sensorValues;
{
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
if (pg.getReadResult() == returnvalue::OK) {

4
mission/controller/AcsController.h
View File

@ -4,13 +4,14 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
#include <mission/devices/devicedefinitions/SusDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
#include <mission/trace.h>
#include "acs/ActuatorCmd.h"
@ -189,6 +190,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
acsctrl::GpsDataProcessed gpsDataProcessed;
PoolEntry<double> gcLatitude = PoolEntry<double>();
PoolEntry<double> gdLongitude = PoolEntry<double>();
PoolEntry<double> altitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);

64
mission/controller/ThermalController.cpp
View File

@ -3,13 +3,13 @@
#include <bsp_q7s/core/CoreDefinitions.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/thermal/ThermalComponentIF.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
#include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
@ -474,8 +474,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg0(&susSet0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg0.getReadResult() == returnvalue::OK) {
susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.temperatureCelcius.value;
susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.temperatureCelcius.isValid());
susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.tempC.value;
susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.tempC.isValid());
if (not susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.isValid()) {
susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = INVALID_TEMPERATURE;
}
@ -485,8 +485,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg1(&susSet1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg1.getReadResult() == returnvalue::OK) {
susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.temperatureCelcius.value;
susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.temperatureCelcius.isValid());
susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.tempC.value;
susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.tempC.isValid());
if (not susTemperatures.sus_6_r_loc_xfybzm_pt_xf.isValid()) {
susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = INVALID_TEMPERATURE;
}
@ -496,8 +496,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg2(&susSet2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg2.getReadResult() == returnvalue::OK) {
susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.temperatureCelcius.value;
susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.temperatureCelcius.isValid());
susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.tempC.value;
susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.tempC.isValid());
if (not susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.isValid()) {
susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = INVALID_TEMPERATURE;
}
@ -507,8 +507,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg3(&susSet3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg3.getReadResult() == returnvalue::OK) {
susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.temperatureCelcius.value;
susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.temperatureCelcius.isValid());
susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.tempC.value;
susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.tempC.isValid());
if (not susTemperatures.sus_7_r_loc_xbybzm_pt_xb.isValid()) {
susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = INVALID_TEMPERATURE;
}
@ -518,8 +518,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg4(&susSet4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg4.getReadResult() == returnvalue::OK) {
susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.temperatureCelcius.value;
susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.temperatureCelcius.isValid());
susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.tempC.value;
susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.tempC.isValid());
if (not susTemperatures.sus_2_n_loc_xfybzb_pt_yb.isValid()) {
susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = INVALID_TEMPERATURE;
}
@ -529,8 +529,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg5(&susSet5, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg5.getReadResult() == returnvalue::OK) {
susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.temperatureCelcius.value;
susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.temperatureCelcius.isValid());
susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.tempC.value;
susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.tempC.isValid());
if (not susTemperatures.sus_8_r_loc_xbybzb_pt_yb.isValid()) {
susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = INVALID_TEMPERATURE;
}
@ -540,8 +540,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg6(&susSet6, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg6.getReadResult() == returnvalue::OK) {
susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.temperatureCelcius.value;
susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.temperatureCelcius.isValid());
susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.tempC.value;
susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.tempC.isValid());
if (not susTemperatures.sus_3_n_loc_xfybzf_pt_yf.isValid()) {
susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = INVALID_TEMPERATURE;
}
@ -551,8 +551,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg7(&susSet7, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg7.getReadResult() == returnvalue::OK) {
susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.temperatureCelcius.value;
susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.temperatureCelcius.isValid());
susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.tempC.value;
susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.tempC.isValid());
if (not susTemperatures.sus_9_r_loc_xbybzb_pt_yf.isValid()) {
susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = INVALID_TEMPERATURE;
}
@ -562,8 +562,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg8(&susSet8, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg8.getReadResult() == returnvalue::OK) {
susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.temperatureCelcius.value;
susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.temperatureCelcius.isValid());
susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.tempC.value;
susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.tempC.isValid());
if (not susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.isValid()) {
susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = INVALID_TEMPERATURE;
}
@ -573,8 +573,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg9(&susSet9, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg9.getReadResult() == returnvalue::OK) {
susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.temperatureCelcius.value;
susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.temperatureCelcius.isValid());
susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.tempC.value;
susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.tempC.isValid());
if (not susTemperatures.sus_10_n_loc_xmybzf_pt_zf.isValid()) {
susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = INVALID_TEMPERATURE;
}
@ -584,8 +584,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg10(&susSet10, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg10.getReadResult() == returnvalue::OK) {
susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.temperatureCelcius.value;
susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.temperatureCelcius.isValid());
susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.tempC.value;
susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.tempC.isValid());
if (not susTemperatures.sus_5_n_loc_xfymzb_pt_zb.isValid()) {
susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = INVALID_TEMPERATURE;
}
@ -595,8 +595,8 @@ void ThermalController::copySus() {
{
PoolReadGuard pg11(&susSet11, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg11.getReadResult() == returnvalue::OK) {
susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.temperatureCelcius.value;
susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.temperatureCelcius.isValid());
susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.tempC.value;
susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.tempC.isValid());
if (not susTemperatures.sus_11_r_loc_xbymzb_pt_zb.isValid()) {
susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = INVALID_TEMPERATURE;
}
@ -854,7 +854,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempGyro0 =
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
@ -867,7 +867,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE);
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
@ -881,7 +881,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempGyro2 =
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
@ -894,7 +894,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE);
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
@ -908,7 +908,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempMgm0 =
lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl;
@ -922,7 +922,7 @@ void ThermalController::copyDevices() {
{
lp_var_t<float> tempMgm2 =
lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl;

26
mission/controller/ThermalController.h
View File

@ -6,8 +6,8 @@
#include <fsfw/timemanager/Countdown.h>
#include <mission/controller/controllerdefinitions/ThermalControllerDefinitions.h>
#include <mission/devices/devicedefinitions/Max31865Definitions.h>
#include <mission/devices/devicedefinitions/SusDefinitions.h>
#include <mission/devices/devicedefinitions/Tmp1075Definitions.h>
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
#include <list>
@ -112,18 +112,18 @@ class ThermalController : public ExtendedControllerBase {
TMP1075::Tmp1075Dataset tmp1075SetIfBoard;
// SUS
SUS::SusDataset susSet0;
SUS::SusDataset susSet1;
SUS::SusDataset susSet2;
SUS::SusDataset susSet3;
SUS::SusDataset susSet4;
SUS::SusDataset susSet5;
SUS::SusDataset susSet6;
SUS::SusDataset susSet7;
SUS::SusDataset susSet8;
SUS::SusDataset susSet9;
SUS::SusDataset susSet10;
SUS::SusDataset susSet11;
susMax1227::SusDataset susSet0;
susMax1227::SusDataset susSet1;
susMax1227::SusDataset susSet2;
susMax1227::SusDataset susSet3;
susMax1227::SusDataset susSet4;
susMax1227::SusDataset susSet5;
susMax1227::SusDataset susSet6;
susMax1227::SusDataset susSet7;
susMax1227::SusDataset susSet8;
susMax1227::SusDataset susSet9;
susMax1227::SusDataset susSet10;
susMax1227::SusDataset susSet11;
// TempLimits
TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);

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

@ -30,19 +30,19 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x2: // InertiaEIVE
switch (parameterId) {
case 0x0:
parameterWrapper->set(inertiaEIVE.inertiaMatrix);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix);
break;
case 0x1:
parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed);
break;
case 0x2:
parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed);
break;
case 0x3:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1);
break;
case 0x4:
parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -51,58 +51,58 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x3: // MgmHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm0hardIronOffset);
break;
case 0x6:
parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm1hardIronOffset);
break;
case 0x7:
parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm2hardIronOffset);
break;
case 0x8:
parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm3hardIronOffset);
break;
case 0x9:
parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
parameterWrapper->setVector(mgmHandlingParameters.mgm4hardIronOffset);
break;
case 0xA:
parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm0softIronInverse);
break;
case 0xB:
parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm1softIronInverse);
break;
case 0xC:
parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm2softIronInverse);
break;
case 0xD:
parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm3softIronInverse);
break;
case 0xE:
parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
parameterWrapper->setMatrix(mgmHandlingParameters.mgm4softIronInverse);
break;
case 0xF:
parameterWrapper->set(mgmHandlingParameters.mgm02variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm02variance);
break;
case 0x10:
parameterWrapper->set(mgmHandlingParameters.mgm13variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm13variance);
break;
case 0x11:
parameterWrapper->set(mgmHandlingParameters.mgm4variance);
parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -111,112 +111,112 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x4: // SusHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus4orientationMatrix);
break;
case 0x5:
parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus5orientationMatrix);
break;
case 0x6:
parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus6orientationMatrix);
break;
case 0x7:
parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus7orientationMatrix);
break;
case 0x8:
parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus8orientationMatrix);
break;
case 0x9:
parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus9orientationMatrix);
break;
case 0xA:
parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus10orientationMatrix);
break;
case 0xB:
parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
parameterWrapper->setMatrix(susHandlingParameters.sus11orientationMatrix);
break;
case 0xC:
parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus0coeffAlpha);
break;
case 0xD:
parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus0coeffBeta);
break;
case 0xE:
parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus1coeffAlpha);
break;
case 0xF:
parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus1coeffBeta);
break;
case 0x10:
parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus2coeffAlpha);
break;
case 0x11:
parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus2coeffBeta);
break;
case 0x12:
parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus3coeffAlpha);
break;
case 0x13:
parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus3coeffBeta);
break;
case 0x14:
parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus4coeffAlpha);
break;
case 0x15:
parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus4coeffBeta);
break;
case 0x16:
parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus5coeffAlpha);
break;
case 0x17:
parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus5coeffBeta);
break;
case 0x18:
parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus6coeffAlpha);
break;
case 0x19:
parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus6coeffBeta);
break;
case 0x1A:
parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus7coeffAlpha);
break;
case 0x1B:
parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus7coeffBeta);
break;
case 0x1C:
parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus8coeffAlpha);
break;
case 0x1D:
parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus8coeffBeta);
break;
case 0x1E:
parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus9coeffAlpha);
break;
case 0x1F:
parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus9coeffBeta);
break;
case 0x20:
parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus10coeffAlpha);
break;
case 0x21:
parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus10coeffBeta);
break;
case 0x22:
parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffAlpha);
break;
case 0x23:
parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -225,34 +225,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x5): // GyrHandlingParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
parameterWrapper->setMatrix(gyrHandlingParameters.gyr3orientationMatrix);
break;
case 0x4:
parameterWrapper->set(gyrHandlingParameters.gyr0bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr0bias);
break;
case 0x5:
parameterWrapper->set(gyrHandlingParameters.gyr1bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr1bias);
break;
case 0x6:
parameterWrapper->set(gyrHandlingParameters.gyr2bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr2bias);
break;
case 0x7:
parameterWrapper->set(gyrHandlingParameters.gyr3bias);
parameterWrapper->setVector(gyrHandlingParameters.gyr3bias);
break;
case 0x8:
parameterWrapper->set(gyrHandlingParameters.gyr02variance);
parameterWrapper->setVector(gyrHandlingParameters.gyr02variance);
break;
case 0x9:
parameterWrapper->set(gyrHandlingParameters.gyr13variance);
parameterWrapper->setVector(gyrHandlingParameters.gyr13variance);
break;
case 0xA:
parameterWrapper->set(gyrHandlingParameters.preferAdis);
@ -270,15 +270,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(rwHandlingParameters.maxTrq);
break;
case 0x2:
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
parameterWrapper->set(rwHandlingParameters.maxRwSpeed);
break;
case 0x3:
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
parameterWrapper->set(rwHandlingParameters.stictionSpeed);
break;
case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
case 0x6:
parameterWrapper->set(rwHandlingParameters.rampTime);
break;
default:
@ -288,25 +291,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x7): // RwMatrices
switch (parameterId) {
case 0x0:
parameterWrapper->set(rwMatrices.alignmentMatrix);
parameterWrapper->setMatrix(rwMatrices.alignmentMatrix);
break;
case 0x1:
parameterWrapper->set(rwMatrices.pseudoInverse);
parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
break;
case 0x2:
parameterWrapper->set(rwMatrices.without1);
parameterWrapper->setMatrix(rwMatrices.without1);
break;
case 0x3:
parameterWrapper->set(rwMatrices.without2);
parameterWrapper->setMatrix(rwMatrices.without2);
break;
case 0x4:
parameterWrapper->set(rwMatrices.without3);
parameterWrapper->setMatrix(rwMatrices.without3);
break;
case 0x5:
parameterWrapper->set(rwMatrices.without4);
parameterWrapper->setMatrix(rwMatrices.without4);
break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
parameterWrapper->setVector(rwMatrices.nullspace);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -330,13 +333,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
break;
case 0x5:
parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop);
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
break;
case 0x6:
parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
break;
case 0x7:
parameterWrapper->set(safeModeControllerParameters.satRateRef);
parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -345,31 +348,31 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x9): // IdleModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
parameterWrapper->set(idleModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
parameterWrapper->set(idleModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
parameterWrapper->set(idleModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
parameterWrapper->set(idleModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
parameterWrapper->set(idleModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
parameterWrapper->set(idleModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
break;
default:
@ -394,7 +397,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
@ -406,13 +409,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
@ -445,52 +448,46 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0xB): // GsTargetModeControllerParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(targetModeControllerParameters.zeta);
parameterWrapper->set(gsTargetModeControllerParameters.zeta);
break;
case 0x1:
parameterWrapper->set(targetModeControllerParameters.om);
parameterWrapper->set(gsTargetModeControllerParameters.om);
break;
case 0x2:
parameterWrapper->set(targetModeControllerParameters.omMax);
parameterWrapper->set(gsTargetModeControllerParameters.omMax);
break;
case 0x3:
parameterWrapper->set(targetModeControllerParameters.qiMin);
parameterWrapper->set(gsTargetModeControllerParameters.qiMin);
break;
case 0x4:
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
break;
case 0x7:
parameterWrapper->set(targetModeControllerParameters.desatOn);
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
break;
case 0x8:
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(targetModeControllerParameters.refDirection);
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(targetModeControllerParameters.refRotRate);
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
break;
case 0xB:
parameterWrapper->set(targetModeControllerParameters.quatRef);
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
break;
case 0xC:
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
break;
case 0xD:
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
break;
case 0xE:
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
break;
case 0xF:
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -514,7 +511,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
@ -526,10 +523,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(nadirModeControllerParameters.refDirection);
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
break;
case 0xA:
parameterWrapper->set(nadirModeControllerParameters.quatRef);
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
break;
case 0xC:
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
@ -556,7 +553,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
break;
case 0x5:
parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef);
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
break;
case 0x6:
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
@ -568,13 +565,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
break;
case 0x9:
parameterWrapper->set(inertialModeControllerParameters.tgtQuat);
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
break;
case 0xA:
parameterWrapper->set(inertialModeControllerParameters.refRotRate);
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
break;
case 0xC:
parameterWrapper->set(inertialModeControllerParameters.quatRef);
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -586,7 +583,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(strParameters.exclusionAngle);
break;
case 0x1:
parameterWrapper->set(strParameters.boresightAxis);
parameterWrapper->setVector(strParameters.boresightAxis);
break;
default:
return INVALID_IDENTIFIER_ID;
@ -597,6 +594,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x0:
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
break;
case 0x1:
parameterWrapper->set(gpsParameters.minimumFdirAltitude);
break;
case 0x2:
parameterWrapper->set(gpsParameters.maximumFdirAltitude);
break;
case 0x3:
parameterWrapper->set(gpsParameters.fdirAltitude);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -658,25 +664,25 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x12): // MagnetorquesParameter
switch (parameterId) {
case 0x0:
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq0orientationMatrix);
break;
case 0x1:
parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq1orientationMatrix);
break;
case 0x2:
parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
parameterWrapper->setMatrix(magnetorquerParameter.mtq2orientationMatrix);
break;
case 0x3:
parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
parameterWrapper->setMatrix(magnetorquerParameter.alignmentMatrixMtq);
break;
case 0x4:
parameterWrapper->set(magnetorquesParameter.inverseAlignment);
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
break;
case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax);
parameterWrapper->set(magnetorquerParameter.dipolMax);
break;
case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration);
parameterWrapper->set(magnetorquerParameter.torqueDuration);
break;
default:
return INVALID_IDENTIFIER_ID;

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

@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF {
double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
/* var = sigma^2, 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, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
@ -783,9 +783,10 @@ class AcsParameters : public HasParametersIF {
struct RwHandlingParameters {
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t stictionSpeed = 100; // RPM
int32_t stictionReleaseSpeed = 120; // RPM
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t maxRwSpeed = 65000; // 0.1 RPM
int32_t stictionSpeed = 1000; // 0.1 RPM
int32_t stictionReleaseSpeed = 1000; // 0.1 RPM
double stictionTorque = 0.0006;
uint16_t rampTime = 10;
@ -817,7 +818,7 @@ class AcsParameters : public HasParametersIF {
double sunMagAngleMin = 5 * M_PI / 180;
double sunTargetDirLeop[3] = {0, .5, .5};
double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
double sunTargetDir[3] = {0, 0, 1};
double satRateRef[3] = {0, 0, 0};
@ -843,7 +844,7 @@ class AcsParameters : public HasParametersIF {
double refDirection[3] = {-1, 0, 0}; // Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
@ -859,7 +860,7 @@ class AcsParameters : public HasParametersIF {
struct GsTargetModeControllerParameters : PointingLawParameters {
double refDirection[3] = {-1, 0, 0}; // Antenna
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
@ -871,7 +872,7 @@ class AcsParameters : public HasParametersIF {
double refDirection[3] = {-1, 0, 0}; // Antenna
double quatRef[4] = {0, 0, 0, 1};
double refRotRate[3] = {0, 0, 0};
int8_t timeElapsedMax = 10; // rot rate calculations
uint8_t timeElapsedMax = 10; // rot rate calculations
} nadirModeControllerParameters;
struct InertialModeControllerParameters : PointingLawParameters {
@ -886,7 +887,10 @@ class AcsParameters : public HasParametersIF {
} strParameters;
struct GpsParameters {
double timeDiffVelocityMax = 30; //[s]
double timeDiffVelocityMax = 30; // [s]
double minimumFdirAltitude = 475 * 1e3; // [m]
double maximumFdirAltitude = 575 * 1e3; // [m]
double fdirAltitude = 525 * 1e3; // [m]
} gpsParameters;
struct SunModelParameters {
@ -912,16 +916,16 @@ class AcsParameters : public HasParametersIF {
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters;
struct MagnetorquesParameter {
struct MagnetorquerParameter {
double mtq0orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};
double mtq1orientationMatrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double DipolMax = 0.2; // [Am^2]
double dipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter;
} magnetorquerParameter;
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s

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

@ -10,40 +10,32 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
ActuatorCmd::ActuatorCmd() {}
ActuatorCmd::~ActuatorCmd() {}
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(rwTrq, 4, &maxIdx);
double maxValue = rwTrq[maxIdx];
double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(rwTrq[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
if (maxValue > maxTorque) {
double scalingFactor = maxTorque / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrq, 4);
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed) {
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
double deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm;
double factor = sampleTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
for (int i = 0; i < 4; i++) {
@ -51,23 +43,27 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
}
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
for (uint8_t i = 0; i < 4; i++) {
if (rwCmdSpeed[i] > maxRwSpeed) {
rwCmdSpeed[i] = maxRwSpeed;
} else if (rwCmdSpeed[i] < -maxRwSpeed) {
rwCmdSpeed[i] = -maxRwSpeed;
}
}
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
const double *inverseAlignment, double maxDipol) {
// Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 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(dipolMomentActuator[i]) > maxDipol) {
maxValue = abs(dipolMomentActuator[i]);
}
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
uint8_t maxIdx = 0;
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]);
if (maxAbsValue > maxDipol) {
double scalingFactor = maxDipol / maxAbsValue;
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3);
}

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

@ -1,23 +1,22 @@
#ifndef ACTUATORCMD_H_
#define ACTUATORCMD_H_
#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
class ActuatorCmd {
public:
ActuatorCmd(AcsParameters *acsParameters_); // Input mode ?
ActuatorCmd();
virtual ~ActuatorCmd();
/*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is
* higher then the maximum torque
* @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque
* @param: rwTrq given torque for reaction wheels which will be
* scaled if needed to be
*/
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
void scalingTorqueRws(double *rwTrq, double maxTorque);
/*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
@ -28,8 +27,9 @@ class ActuatorCmd {
* rwCmdSpeed output revolutions per minute for every
* reaction wheel
*/
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
int32_t maxRwSpeed, double inertiaWheel);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,11 +37,11 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame
*/
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
const double *inverseAlignment, double maxDipol);
protected:
private:
AcsParameters acsParameters;
};
#endif /* ACTUATORCMD_H_ */

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

@ -12,7 +12,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
Guidance::~Guidance() {}
@ -26,9 +26,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
// target direction in the ECEF frame
double targetDirE[3] = {0, 0, 0};
@ -57,9 +57,9 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
@ -96,15 +96,15 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
double sunDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
double exclAngle = acsParameters.strParameters.exclusionAngle,
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;
double exclAngle = acsParameters->strParameters.exclusionAngle,
blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
double sightAngleSun =
VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);
VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
if (!(strBlindAvoidFlag)) {
double critSightAngle = blindStart * exclAngle;
@ -113,7 +113,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
}
} else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};
if (sunDirB[1] < 0) {
blindRefRate[0] = normBlindRefRate;
@ -144,9 +144,9 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.targetModeControllerParameters.latitudeTgt,
acsParameters.targetModeControllerParameters.longitudeTgt,
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
@ -198,7 +198,7 @@ void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double vel
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
@ -211,9 +211,9 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
double groundStationE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
@ -262,7 +262,7 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
}
@ -332,9 +332,9 @@ void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], doub
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
@ -406,7 +406,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
{xAxis[2], yAxis[2], zAxis[2]}};
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
}
@ -516,19 +516,19 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
if (rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
return returnvalue::OK;
} else {
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
@ -540,28 +540,30 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
3 * sizeof(double));
}
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef,
3 * sizeof(double));
}
ReturnValue_t Guidance::solarArrayDeploymentComplete() {
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
std::error_code e;
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
std::remove(SD_0_SKEWED_PTG_FILE);
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
return returnvalue::FAILED;
}
}
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
std::remove(SD_1_SKEWED_PTG_FILE);
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
return returnvalue::FAILED;
}
}

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

@ -55,14 +55,15 @@ class Guidance {
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
AcsParameters acsParameters;
const AcsParameters *acsParameters;
bool strBlindAvoidFlag = false;
timeval timeSavedQuaternion;
double savedQuaternion[4] = {0, 0, 0, 0};
double omegaRefSaved[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/deployment";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
};
#endif /* ACS_GUIDANCE_H_ */

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

@ -12,33 +12,22 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
/*Initialisation of values for parameters in constructor*/
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
: initialQuaternion{0, 0, 0, 1},
initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
loadAcsParameters(acsParameters_);
}
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
}
ReturnValue_t MultiplicativeKalmanFilter::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, acsctrl::MekfData *mekfData) { // valids for "model measurements"?
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters) { // valids for "model measurements"?
// check for valid mag/sun
if (validMagField_ && validSS && validSSModel && validMagModel) {
validInit = true;
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -192,21 +181,18 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
return MEKF_INITIALIZED;
} else {
// no initialisation possible, no valid measurements
validInit = false;
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
return MEKF_UNINITIALIZED;
}
}
// --------------- MEKF DISCRETE TIME STEP -------------------------------
ReturnValue_t MultiplicativeKalmanFilter::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, double sampleTime,
acsctrl::MekfData *mekfData) {
ReturnValue_t MultiplicativeKalmanFilter::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,
AcsParameters *acsParameters) {
// Check for GYR Measurements
int MDF = 0; // Matrix Dimension Factor
if (!validGYRs_) {
@ -248,9 +234,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaStr = kalmanFilterParameters->sensorNoiseSTR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -912,8 +898,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -931,27 +917,31 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if (normRotEst * sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
if (normRotEst * acsParameters->onBoardParams.sampleTime < M_PI / 10) {
double fact11 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime +
1. / 3. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(acsParameters->onBoardParams.sampleTime, 2));
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
} else {
double fact22 = pow(sigmaU, 2) * sampleTime;
double fact22 = pow(sigmaU, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
double fact12_0 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
double fact12_1 = 1. / 2. * pow(sampleTime, 2);
double fact12_1 = 1. / 2. * pow(acsParameters->onBoardParams.sampleTime, 2);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
double fact12_2 =
(1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
(1. / 2. * pow(normRotEst, 2) * pow(acsParameters->onBoardParams.sampleTime, 2) +
cos(normRotEst * acsParameters->onBoardParams.sampleTime) - 1) /
pow(normRotEst, 4);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
@ -961,13 +951,15 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
double fact11_0 = pow(sigmaV, 2) * sampleTime;
double fact11_0 = pow(sigmaV, 2) * acsParameters->onBoardParams.sampleTime;
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
double fact11_1 = 1. / 3. * pow(sampleTime, 3);
double fact11_1 = 1. / 3. * pow(acsParameters->onBoardParams.sampleTime, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
pow(normRotEst, 5);
double fact11_2 =
(2 * normRotEst * acsParameters->onBoardParams.sampleTime -
2 * sin(normRotEst * acsParameters->onBoardParams.sampleTime) -
1. / 3. * pow(normRotEst, 3) * pow(acsParameters->onBoardParams.sampleTime, 3)) /
pow(normRotEst, 5);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
@ -1017,9 +1009,10 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
double fact11_1 = sin(normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
double fact11_2 =
(1 - cos(normRotEst * acsParameters->onBoardParams.sampleTime)) / pow(normRotEst, 2);
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
@ -1028,8 +1021,11 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
double fact12_0 = fact11_2;
MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiplyScalar(*identityMatrix3,
acsParameters->onBoardParams.sampleTime, *phi12_1, 3, 3);
double fact12_2 =
(normRotEst * acsParameters->onBoardParams.sampleTime -
sin(normRotEst * acsParameters->onBoardParams.sampleTime) / pow(normRotEst, 3));
MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
@ -1056,8 +1052,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
// Propagated Quaternion
double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double rotCos = cos(0.5 * normRotEst * sampleTime);
double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
double rotCos = cos(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime);
double sinFac = sin(0.5 * normRotEst * acsParameters->onBoardParams.sampleTime) / normRotEst;
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

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

@ -17,9 +17,8 @@ class MultiplicativeKalmanFilter {
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
MultiplicativeKalmanFilter();
virtual ~MultiplicativeKalmanFilter();
ReturnValue_t reset(acsctrl::MekfData *mekfData);
@ -33,8 +32,8 @@ class MultiplicativeKalmanFilter {
*/
ReturnValue_t 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,
acsctrl::MekfData *mekfData);
const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
* for the current step after the initalization
@ -54,7 +53,8 @@ class MultiplicativeKalmanFilter {
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, double sampleTime, acsctrl::MekfData *mekfData);
const bool validMagModel, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
enum MekfStatus : uint8_t {
UNINITIALIZED = 0,
@ -79,23 +79,21 @@ class MultiplicativeKalmanFilter {
private:
/*Parameters*/
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
double quaternion_STR_SB[4];
bool validInit;
/*States*/
double initialQuaternion[4]; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
uint8_t sensorsAvail;
double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
uint8_t sensorsAvail = 0;
/*Outputs*/
double quatBJ[4]; /* Output Quaternion */
double biasGYR[3]; /*Between measured and estimated sat Rate*/
/*Parameter INIT*/
/*Functions*/
void loadAcsParameters(AcsParameters *acsParameters_);
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
double satRotRate[3]);

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

@ -8,9 +8,7 @@
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
acsParameters = *acsParameters_;
}
Navigation::Navigation() {}
Navigation::~Navigation() {}
@ -18,7 +16,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed,
acsctrl::MekfData *mekfData) {
acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
@ -30,7 +28,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
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);
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
acsParameters);
return mekfStatus;
} else {
mekfStatus = multiplicativeKalmanFilter.mekfEst(
@ -39,7 +38,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
return mekfStatus;
}
}

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

@ -9,19 +9,19 @@
class Navigation {
public:
Navigation(AcsParameters *acsParameters_);
Navigation();
virtual ~Navigation();
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
AcsParameters acsParameters;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
};

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

@ -14,7 +14,7 @@
using namespace Math;
SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
SensorProcessing::SensorProcessing() {}
SensorProcessing::~SensorProcessing() {}
@ -26,7 +26,8 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here ------------------------------------------------
// ---------------- IGRF- 13 Implementation here
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
// Should be existing class object which will be called and modified here.
@ -469,10 +470,10 @@ void SensorProcessing::processGyr(
float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
if (gyr0valid) {
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
VectorOperations<double>::subtract(gyr0Value, gyrParameters->gyr0bias, gyr0Value, 3);
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
@ -480,10 +481,10 @@ void SensorProcessing::processGyr(
}
}
if (gyr1valid) {
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
VectorOperations<double>::subtract(gyr1Value, gyrParameters->gyr1bias, gyr1Value, 3);
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
@ -491,10 +492,10 @@ void SensorProcessing::processGyr(
}
}
if (gyr2valid) {
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
VectorOperations<double>::subtract(gyr2Value, gyrParameters->gyr2bias, gyr2Value, 3);
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
@ -502,10 +503,10 @@ void SensorProcessing::processGyr(
}
}
if (gyr3valid) {
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
VectorOperations<double>::subtract(gyr3Value, gyrParameters->gyr3bias, gyr3Value, 3);
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
@ -549,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
// name to convert not process
double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gpsLongitude * PI / 180.;
@ -559,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
double factor = 1 - pow(eccentricityWgs84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR
if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
gpsAltitude < gpsParameters->maximumFdirAltitude) {
altitude = gpsParameters->fdirAltitude;
} else {
altitude = gpsAltitude;
}
// Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE &&
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
@ -580,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->altitude.value = altitude;
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);

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

@ -15,7 +15,7 @@ class SensorProcessing {
public:
void reset();
SensorProcessing(AcsParameters *acsParameters_);
SensorProcessing();
virtual ~SensorProcessing();
void process(timeval now, ACS::SensorValues *sensorValues,
@ -77,7 +77,6 @@ class SensorProcessing {
bool validSavedPosSatE = false;
SusConverter susConverter;
AcsParameters acsParameters;
};
#endif /*SENSORPROCESSING_H_*/

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

@ -1,16 +1,15 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_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/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.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/SusDefinitions.h"
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
namespace ACS {
@ -27,30 +26,28 @@ class SensorValues {
ReturnValue_t updateStr();
ReturnValue_t updateRw();
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);
mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set =
mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set =
mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
imtq::RawMtmMeasurementNoTorque imtqMgmSet =
imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);
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),
std::array<susMax1227::SusDataset, 12> susSets{
susMax1227::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
susMax1227::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
susMax1227::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
susMax1227::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
susMax1227::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
susMax1227::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
susMax1227::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
susMax1227::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
susMax1227::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
susMax1227::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
susMax1227::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
susMax1227::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
};
AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER);

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

@ -1,11 +1,3 @@
/*
* Detumble.cpp
*
* Created on: 17 Aug 2022
* Author: Robin Marquardt
*/
#include "Detumble.h"
#include <fsfw/globalfunctions/constants.h>
@ -17,33 +9,32 @@
#include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
Detumble::Detumble() {}
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) {
const double *magField, const bool magFieldValid, double *magMom,
double gain) {
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
// convert uT to T
double magFieldT[3], magRateT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
// control law
double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
VectorOperations<double>::mulScalar(magRateT, factor, magMom, 3);
return returnvalue::OK;
}
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
double *magMom) {
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double dipolMax) {
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
double dipolMax = magnetorquesParameter->DipolMax;
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
@ -51,14 +42,20 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK;
}
ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid,
const double *magField, const bool *magFieldValid,
double *magMom) {
double *magMom, double gain) {
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
// convert uT to T
double magFieldT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
// control law
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
return returnvalue::OK;
}

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

@ -12,30 +12,22 @@
class Detumble {
public:
Detumble(AcsParameters *acsParameters_);
Detumble();
virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters for 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);
const bool magFieldValid, double *magMom, double gain);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double dipolMax);
ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom, double gain);
private:
AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquesParameter *magnetorquesParameter;
};
#endif /*ACS_CONTROL_DETUMBLE_H_*/

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

@ -1,10 +1,3 @@
/*
* PtgCtrl.cpp
*
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*/
#include "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h>
@ -16,16 +9,10 @@
#include "../util/MathOperations.h"
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {}
void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
inertiaEIVE = &(acsParameters_->inertiaEIVE);
rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
rwMatrices = &(acsParameters_->rwMatrices);
}
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
double *torqueRws) {
@ -62,8 +49,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
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);
MatrixOperations<double>::multiply(
*gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -72,8 +59,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
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>::multiply(
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);
//------------------------------------------------------------------------------------------------
@ -91,18 +78,19 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
pErrorSign[i] = pError[i];
}
}
// Torque for quaternion error
// torque for quaternion error
double torqueQuat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);
// Torque for rate error
// torque for rate error
double torqueRate[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
torqueRate, 3, 3, 1);
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
// Final commanded Torque for every reaction wheel
// final commanded Torque for every reaction wheel
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
@ -120,20 +108,22 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
return;
}
// calculating momentum of satellite and momentum of reaction wheels
// calculating momentum of satellite and momentum of reaction wheels
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,
1);
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
momentumRwU, 4);
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
momentumRw, 3, 4, 1);
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
momentumSat, 3, 3, 1);
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
// calculating momentum error
// calculating momentum error
double deltaMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
deltaMomentum, 3);
// resulting magnetic dipole command
// resulting magnetic dipole command
double crossMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
@ -147,53 +137,50 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
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
// conversion to [rad/s] for further calculations
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
double diffRwSpeed[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
wheelMomentum, 4);
double gainNs = pointingLawParameters->gainNullspace;
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
acsParameters->rwMatrices.nullspace,
*nullSpaceMatrix, 4);
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
bool rwAvailable[4] = {
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
int32_t currRwSpeed[4] = {
sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
(omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
if (rwCmdSpeeds[i] != 0) {
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
if (currRwSpeed[i] == 0) {
if (rwCmdSpeeds[i] > 0) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (rwCmdSpeeds[i] < 0) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
} else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
}
torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}

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

@ -1,16 +1,3 @@
/*
* PtgCtrl.h
*
* Created on: 17 Jul 2022
* Author: Robin Marquardt
*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
*/
#ifndef PTGCTRL_H_
#define PTGCTRL_H_
@ -23,6 +10,13 @@
#include "eive/resultClassIds.h"
class PtgCtrl {
/*
* @brief: This class handles the pointing control mechanism. Calculation of an commanded
* torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
*
* @note: A description of the used algorithms can be found in
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
*/
public:
/* @brief: Constructor
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
@ -33,13 +27,7 @@ class PtgCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);
/* @brief: Calculates the needed torque for the pointing control mechanism
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
@ -54,18 +42,12 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: sensorValues class containing all RW related values
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
private:
AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4] = {0, 0, 0, 0};
double omegaMemory[4] = {0, 0, 0, 0};
const AcsParameters *acsParameters;
};
#endif /* ACS_CONTROL_PTGCTRL_H_ */

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

@ -1,10 +1,3 @@
/*
* SafeCtrl.cpp
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#include "SafeCtrl.h"
#include <fsfw/globalfunctions/constants.h>
@ -15,32 +8,21 @@
#include "../util/MathOperations.h"
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) {
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
3);
}
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
SafeCtrl::~SafeCtrl() {}
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) {
double *outputAngle, double *outputMagMomB) {
if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
*outputValid = false;
return SAFECTRL_MEKF_INPUT_INVALID;
}
double kRate = 0, kAlign = 0;
kRate = safeModeControllerParameters->k_rate_mekf;
kAlign = safeModeControllerParameters->k_align_mekf;
double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -49,22 +31,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
double crossSun[3] = {0, 0, 0};
// change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
double crossSun[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
// calc angle alpha between sunDirRef and sunDIr
double alpha = 0, dotSun = 0;
dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
alpha = acos(dotSun);
double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
double alpha = acos(dotSun);
// 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 scalarFac = 0;
scalarFac = kAlign * alpha / normCrossSun;
double scalarFac = kAlign * alpha / normCrossSun;
VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
double rateSafeMode[3] = {0, 0, 0};
@ -73,6 +55,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
*gainMatrixInertia, 3,
3); // why only for mekf one and not for no mekf
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque)
@ -82,39 +67,38 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
*outputAngle = alpha;
*outputValid = true;
return returnvalue::OK;
}
// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
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) {
ReturnValue_t 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) {
// Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) {
*outputValid = false;
return;
return returnvalue::FAILED;
}
// change unit from uT to T
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// normalize sunDir and magDir
double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3);
VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3);
// 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;
double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
double rateFactor = 1 - pow(cosAngleSunMag, 2);
double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
// Full rate or estimate
double estSatRate[3] = {0, 0, 0};
@ -129,8 +113,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
/* Only valid if angle between sun direction and magnetic field direction
* is sufficiently large */
double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
return;
if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
return returnvalue::FAILED;
}
// Rate for Torque Calculation
@ -138,9 +122,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
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;
double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0};
@ -159,17 +142,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
// 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);
MatrixOperations<double>::multiply(*(acsParameters->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>::cross(magFieldBT, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
*outputAngle = angleAlignErr;
*outputValid = true;
return returnvalue::OK;
}

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

@ -17,25 +17,20 @@ class SafeCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
void loadAcsParameters(AcsParameters *acsParameters_);
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);
double *outputAngle, double *outputMagMomB);
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);
void idleSunPointing(); // with reaction wheels
ReturnValue_t 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);
protected:
private:
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters *acsParameters;
double gainMatrixInertia[3][3];
double magFieldBState[3];

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

@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
// GPS Processed
GC_LATITUDE,
GD_LONGITUDE,
ALTITUDE,
GPS_POSITION,
GPS_VELOCITY,
// MEKF
@ -109,7 +110,7 @@ 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 = 4;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
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);
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);

122
mission/core/GenericFactory.cpp
View File

@ -28,12 +28,17 @@
#include <mission/controller/ThermalController.h>
#include <mission/devices/HeaterHandler.h>
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/persistentTmStoreDefs.h>
#include <mission/system/objects/AcsBoardAssembly.h>
#include <mission/system/objects/RwAssembly.h>
#include <mission/system/objects/SusAssembly.h>
#include <mission/system/objects/TcsBoardAssembly.h>
#include <mission/tmtc/CfdpTmFunnel.h>
#include <mission/tmtc/PersistentTmStore.h>
#include <mission/tmtc/PersistentTmStoreWithTmQueue.h>
#include <mission/tmtc/PusPacketFilter.h>
#include <mission/tmtc/PusTmFunnel.h>
#include <mission/tmtc/PusTmRouteByFilterHelper.h>
#include <mission/tmtc/TmFunnelHandler.h>
#include "OBSWConfig.h"
@ -44,9 +49,12 @@
#include "mission/system/objects/RwAssembly.h"
#include "mission/system/tree/acsModeTree.h"
#include "mission/system/tree/tcsModeTree.h"
#include "mission/tmtc/tmFilters.h"
#include "objects/systemObjectList.h"
#include "tmtc/pusIds.h"
using persTmStore::PersistentTmStores;
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
// UDP server includes
@ -84,9 +92,11 @@ EiveFaultHandler EIVE_FAULT_HANDLER;
} // namespace cfdp
void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan) {
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores) {
// Framework objects
new EventManager(objects::EVENT_MANAGER);
new EventManager(objects::EVENT_MANAGER, 120);
auto healthTable = new HealthTable(objects::HEALTH_TABLE);
if (healthTable_ != nullptr) {
*healthTable_ = healthTable;
@ -95,8 +105,6 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
new VerificationReporter();
auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER);
StorageManagerIF* tcStore;
StorageManagerIF* tmStore;
StorageManagerIF* ipcStore;
{
PoolManager::LocalPoolConfig poolCfg = {{250, 16}, {250, 32}, {250, 64},
{150, 128}, {120, 1024}, {120, 2048}};
@ -104,27 +112,32 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
}
{
PoolManager::LocalPoolConfig poolCfg = {{400, 32}, {400, 64}, {250, 128},
{150, 512}, {150, 1024}, {150, 2048}};
tmStore = new PoolManager(objects::TM_STORE, poolCfg);
PoolManager::LocalPoolConfig poolCfg = {{300, 32}, {300, 32}, {400, 64}, {250, 128},
{150, 512}, {150, 1024}, {150, 1024}, {150, 2048}};
*tmStore = new PoolManager(objects::TM_STORE, poolCfg);
}
{
PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {200, 32}, {150, 64}, {150, 128},
{100, 256}, {50, 512}, {50, 1024}, {10, 2048}};
ipcStore = new PoolManager(objects::IPC_STORE, poolCfg);
*ipcStore = new PoolManager(objects::IPC_STORE, poolCfg);
}
PoolManager::LocalPoolConfig poolCfg = {{300, 32}, {400, 64}, {250, 128},
{150, 512}, {150, 1024}, {150, 2048}};
auto* ramToFileStore = new PoolManager(objects::DOWNLINK_RAM_STORE, poolCfg);
#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);
auto udpBridge =
new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50);
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;
udpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_UDP);
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
auto tcpBridge = new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR);
auto tcpBridge =
new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50);
TcpTmTcServer::TcpConfig cfg(true, true);
auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER, cfg);
// TCP is stream based. Use packet ID as start marker when parsing for space packets
@ -139,34 +152,93 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50);
*cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore,
PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", **tmStore, **ipcStore,
config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
*pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
// The PUS funnel routes all live TM to the live destinations and to the TM stores.
*pusFunnel = new PusTmFunnel(pusFunnelCfg, *ramToFileStore, *timeStamper, sdcMan);
// MISC store and PUS funnel to MISC store routing
{
PersistentTmStoreArgs storeArgs(objects::MISC_TM_STORE, "tm", "misc",
RolloverInterval::HOURLY, 2, *ramToFileStore, sdcMan);
stores.miscStore =
new PersistentTmStoreWithTmQueue(storeArgs, "MISC STORE", config::MISC_STORE_QUEUE_SIZE);
(*pusFunnel)
->addPersistentTmStoreRouting(filters::miscFilter(),
stores.miscStore->getReportReceptionQueue(0));
}
// OK store and PUS Funnel to OK store routing
{
PersistentTmStoreArgs storeArgs(objects::OK_TM_STORE, "tm", "ok", RolloverInterval::MINUTELY,
30, *ramToFileStore, sdcMan);
stores.okStore =
new PersistentTmStoreWithTmQueue(storeArgs, "OK STORE", config::OK_STORE_QUEUE_SIZE);
(*pusFunnel)
->addPersistentTmStoreRouting(filters::okFilter(),
stores.okStore->getReportReceptionQueue(0));
}
// NOT OK store and PUS funnel to NOT OK store routing
{
PersistentTmStoreArgs storeArgs(objects::NOT_OK_TM_STORE, "tm", "nok",
RolloverInterval::MINUTELY, 30, *ramToFileStore, sdcMan);
stores.notOkStore =
new PersistentTmStoreWithTmQueue(storeArgs, "NOT OK STORE", config::NOK_STORE_QUEUE_SIZE);
(*pusFunnel)
->addPersistentTmStoreRouting(filters::notOkFilter(),
stores.notOkStore->getReportReceptionQueue(0));
}
// HK store and PUS funnel to HK store routing
{
PersistentTmStoreArgs storeArgs(objects::HK_TM_STORE, "tm", "hk", RolloverInterval::MINUTELY,
15, *ramToFileStore, sdcMan);
stores.hkStore =
new PersistentTmStoreWithTmQueue(storeArgs, "HK STORE", config::HK_STORE_QUEUE_SIZE);
(*pusFunnel)
->addPersistentTmStoreRouting(filters::hkFilter(),
stores.hkStore->getReportReceptionQueue(0));
}
// CFDP store and PUS funnel to CFDP store routing
{
PersistentTmStoreArgs storeArgs(objects::CFDP_TM_STORE, "tm", "cfdp",
RolloverInterval::MINUTELY, 30, *ramToFileStore, sdcMan);
stores.cfdpStore =
new PersistentTmStoreWithTmQueue(storeArgs, "CFDP STORE", config::CFDP_STORE_QUEUE_SIZE);
(*pusFunnel)
->addPersistentTmStoreRouting(filters::cfdpFilter(),
stores.cfdpStore->getReportReceptionQueue(0));
}
PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", **tmStore,
**ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH);
*cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0),
*ramToFileStore, config::EIVE_CFDP_APID);
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
(*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0);
(*pusFunnel)->addDestination("UDP Server", *udpBridge, 0);
(*cfdpFunnel)->addLiveDestination("UDP Server", *udpBridge, 0);
(*pusFunnel)->addLiveDestination("UDP Server", *udpBridge, 0);
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
(*cfdpFunnel)->addDestination("TCP Server", *tcpBridge, 0);
(*pusFunnel)->addDestination("TCP Server", *tcpBridge, 0);
(*cfdpFunnel)->addLiveDestination("TCP Server", *tcpBridge, 0);
(*pusFunnel)->addLiveDestination("TCP Server", *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);
pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40);
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);
40, 120);
new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
pus::PUS_SERVICE_8, 16, 60);
new Service9TimeManagement(
@ -194,7 +266,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
new CfdpDistributor(distribCfg);
auto* msgQueue = QueueFactory::instance()->createMessageQueue(32);
FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, **cfdpFunnel, *tcStore, *tmStore,
FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, **cfdpFunnel, *tcStore, **tmStore,
*msgQueue);
cfdp::IndicationCfg indicationCfg;
UnsignedByteField<uint16_t> apid(config::EIVE_LOCAL_CFDP_ENTITY_ID);
@ -221,7 +293,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF&
{new HealthDevice(objects::HEATER_4_CAMERA, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_4},
{new HealthDevice(objects::HEATER_5_STR, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_5},
{new HealthDevice(objects::HEATER_6_DRO, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_6},
{new HealthDevice(objects::HEATER_7_HPA, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_7},
{new HealthDevice(objects::HEATER_7_SYRLINKS, MessageQueueIF::NO_QUEUE), gpioIds::HEATER_7},
}});
heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher,
pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V);
@ -236,7 +308,7 @@ void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t the
std::array<DeviceHandlerBase*, 4> rws,
std::array<object_id_t, 4> rwIds) {
RwHelper rwHelper(rwIds);
auto* rwAss = new RwAssembly(objects::RW_ASS, &pwrSwitcher, theSwitch, rwHelper);
auto* rwAss = new RwAssembly(objects::RW_ASSY, &pwrSwitcher, theSwitch, rwHelper);
for (size_t idx = 0; idx < rwIds.size(); idx++) {
ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss);
if (result != returnvalue::OK) {

8
mission/core/GenericFactory.h
View File

@ -3,12 +3,16 @@
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/memory/SdCardMountedIF.h>
#include <mission/persistentTmStoreDefs.h>
#include <mission/tmtc/PersistentTmStoreWithTmQueue.h>
#include "fsfw/objectmanager/SystemObjectIF.h"
#include "fsfw/power/PowerSwitchIF.h"
#include "fsfw_hal/common/gpio/GpioIF.h"
#include "mission/devices/devicedefinitions/Max31865Definitions.h"
using persTmStore::PersistentTmStores;
class HeaterHandler;
class HealthTableIF;
class PusTmFunnel;
@ -38,7 +42,9 @@ const std::array<std::pair<object_id_t, std::string>, EiveMax31855::NUM_RTDS> RT
namespace ObjectFactory {
void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan);
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores);
void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
HeaterHandler*& heaterHandler);

770
mission/core/pollingSeqTables.cpp
View File

@ -30,22 +30,6 @@ ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) {
#endif
static_cast<void>(length);
#if OBSW_ADD_PL_PCDU == 1
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_RAD_SENSORS == 1
/* Radiation sensor */
thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
return thisSequence->checkSequence();
}
@ -189,390 +173,155 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
/* Length of a communication cycle */
uint32_t length = thisSequence->getPeriodMs();
bool enableAside = true;
bool enableBside = true;
if (cfg.scheduleAcsBoard) {
if (enableAside) {
// A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
}
}
// SUS: 16 ms
bool addSus0 = true;
bool addSus1 = true;
bool addSus2 = true;
bool addSus3 = true;
bool addSus4 = true;
bool addSus5 = true;
bool addSus6 = true;
bool addSus7 = true;
bool addSus8 = true;
bool addSus9 = true;
bool addSus10 = true;
bool addSus11 = true;
if (cfg.scheduleSus) {
if (addSus0) {
/* Write setup */
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_READ);
/* Write setup */
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus1) {
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus2) {
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus3) {
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus4) {
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus5) {
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus6) {
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus7) {
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus8) {
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus9) {
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus10) {
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus11) {
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::spiSched::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (cfg.scheduleStr) {
@ -583,83 +332,250 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
}
bool enableAside = true;
bool enableBside = true;
if (cfg.scheduleAcsBoard) {
if (enableAside) {
// A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableAside) {
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableBside) {
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
length * config::spiSched::SCHED_BLOCK_3_PERIOD,
DeviceHandlerIF::GET_READ);
}
}
if (cfg.scheduleImtq) {
// This is the MTM measurement cycle
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::DHB_OP);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::START_MEASURE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::START_MEASURE_GET);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
imtq::ComStep::READ_MEASURE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
imtq::ComStep::READ_MEASURE_GET);
}
thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0);
thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD,
0);
if (cfg.scheduleImtq) {
// This is the torquing cycle.
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
imtq::ComStep::START_ACTUATE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
imtq::ComStep::START_ACTUATE_GET);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD,
imtq::ComStep::READ_ACTUATE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD,
imtq::ComStep::READ_ACTUATE_GET);
}
if (cfg.scheduleRws) {
// this is the torquing cycle
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
}
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0);
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD,
0);
#if OBSW_ADD_PL_PCDU == 1
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_RAD_SENSORS == 1
/* Radiation sensor */
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::GET_READ);
#endif
return returnvalue::OK;
}

5
mission/devices/CMakeLists.txt
View File

@ -14,7 +14,10 @@ target_sources(
ImtqHandler.cpp
HeaterHandler.cpp
RadiationSensorHandler.cpp
GyroADIS1650XHandler.cpp
GyrAdis1650XHandler.cpp
GyrL3gCustomHandler.cpp
MgmRm3100CustomHandler.cpp
MgmLis3CustomHandler.cpp
RwHandler.cpp
max1227.cpp
SusHandler.cpp

226
mission/devices/GyrAdis1650XHandler.cpp Normal file
View File

@ -0,0 +1,226 @@
#include "mission/devices/GyrAdis1650XHandler.h"
#include "fsfw/action/HasActionsIF.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, adis1650x::Type type)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
adisType(type),
primaryDataset(this),
configDataset(this),
breakCountdown() {
adisRequest.type = adisType;
}
void GyrAdis1650XHandler::doStartUp() {
if (internalState != InternalState::STARTUP) {
internalState = InternalState::STARTUP;
commandExecuted = false;
}
// Initial 310 ms start up time after power-up
if (internalState == InternalState::STARTUP) {
if (not commandExecuted) {
warningSwitch = true;
breakCountdown.setTimeout(adis1650x::START_UP_TIME);
commandExecuted = true;
}
if (breakCountdown.hasTimedOut()) {
updatePeriodicReply(true, adis1650x::REPLY);
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(MODE_ON);
}
internalState = InternalState::NONE;
}
}
}
void GyrAdis1650XHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
commandExecuted = false;
primaryDataset.setValidity(false, true);
internalState = InternalState::SHUTDOWN;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, adis1650x::REPLY);
internalState = InternalState::NONE;
commandExecuted = false;
setMode(MODE_OFF);
}
}
ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = adis1650x::REQUEST;
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch (internalState) {
case (InternalState::STARTUP): {
*id = adis1650x::REQUEST;
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
case (InternalState::SHUTDOWN): {
*id = adis1650x::REQUEST;
acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
request->mode = acs::SimpleSensorMode::OFF;
request->type = adisType;
return returnvalue::OK;
}
default: {
return NOTHING_TO_SEND;
}
}
return returnvalue::OK;
}
ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
return NOTHING_TO_SEND;
}
void GyrAdis1650XHandler::fillCommandAndReplyMap() {
insertInCommandMap(adis1650x::REQUEST);
insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
}
ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or
getMode() == _MODE_POWER_DOWN) {
return IGNORE_FULL_PACKET;
}
if (remainingSize != sizeof(acs::Adis1650XReply)) {
*foundLen = remainingSize;
return returnvalue::FAILED;
}
*foundId = adis1650x::REPLY;
*foundLen = remainingSize;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
return returnvalue::OK;
}
ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
using namespace adis1650x;
const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
switch (adisType) {
case (adis1650x::Type::ADIS16507): {
if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
<< reply->cfg.prodId << "for ADIS type 16507" << std::endl;
return returnvalue::FAILED;
}
break;
}
case (adis1650x::Type::ADIS16505): {
if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
<< reply->cfg.prodId << "for ADIS type 16505" << std::endl;
return returnvalue::FAILED;
}
break;
}
}
PoolReadGuard rg(&configDataset);
configDataset.setValidity(true, true);
configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
configDataset.rangMdl = reply->cfg.rangMdl;
configDataset.diagStatReg = reply->cfg.diagStat;
configDataset.filterSetting = reply->cfg.filterSetting;
configDataset.decRateReg = reply->cfg.decRateReg;
commandExecuted = true;
}
if (reply->dataWasSet) {
{
PoolReadGuard rg(&configDataset);
configDataset.diagStatReg = reply->cfg.diagStat;
}
auto sensitivity = reply->data.sensitivity;
auto accelSensitivity = reply->data.accelScaling;
PoolReadGuard pg(&primaryDataset);
primaryDataset.setValidity(true, true);
primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
// TODO: Check whether we need to divide by INT16_MAX
primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
}
return returnvalue::OK;
}
LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
return &primaryDataset;
} else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
return &configDataset;
}
return nullptr;
}
uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
uint8_t valueTwo) {
uint8_t secondReg = startReg + 1;
startReg |= adis1650x::WRITE_MASK;
secondReg |= adis1650x::WRITE_MASK;
cmdBuf[0] = startReg;
cmdBuf[1] = valueOne;
cmdBuf[2] = secondReg;
cmdBuf[3] = valueTwo;
this->rawPacketLen = 4;
this->rawPacket = cmdBuf.data();
}
ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl);
localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
return returnvalue::OK;
}
adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
using namespace adis1650x;
configDataset.mscCtrlReg.read();
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
configDataset.mscCtrlReg.commit();
return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
}
void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
adisRequest.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
rawPacketLen = sizeof(acs::Adis1650XRequest);
return returnvalue::OK;
}

42
mission/devices/GyroADIS1650XHandler.h → mission/devices/GyrAdis1650XHandler.h
View File

@ -1,27 +1,24 @@
#ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
#define MISSION_DEVICES_GYROADIS16507HANDLER_H_
#include <mission/devices/devicedefinitions/acsPolling.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include "FSFWConfig.h"
#include "OBSWConfig.h"
#include "devicedefinitions/GyroADIS1650XDefinitions.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
#ifdef FSFW_OSAL_LINUX
class SpiComIF;
class SpiCookie;
#endif
/**
* @brief Device handle for the ADIS16507 Gyroscope by Analog Devices
* @details
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
*/
class GyroADIS1650XHandler : public DeviceHandlerBase {
class GyrAdis1650XHandler : public DeviceHandlerBase {
public:
GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
ADIS1650X::Type type);
GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
adis1650x::Type type);
void enablePeriodicPrintouts(bool enable, uint8_t divider);
void setToGoToNormalModeImmediately();
@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
private:
std::array<uint8_t, 32> commandBuffer;
ADIS1650X::Type adisType;
std::array<uint8_t, 32> cmdBuf;
acs::Adis1650XRequest adisRequest;
adis1650x::Type adisType;
AdisGyroPrimaryDataset primaryDataset;
AdisGyroConfigDataset configDataset;
double sensitivity = ADIS1650X::SENSITIVITY_UNSET;
double sensitivity = adis1650x::SENSITIVITY_UNSET;
bool goToNormalMode = false;
bool warningSwitch = true;
enum class InternalState { STARTUP, CONFIG, IDLE };
enum class BurstModes {
BURST_16_BURST_SEL_0,
BURST_16_BURST_SEL_1,
BURST_32_BURST_SEL_0,
BURST_32_BURST_SEL_1
};
enum class InternalState { NONE, STARTUP, SHUTDOWN };
InternalState internalState = InternalState::STARTUP;
bool commandExecuted = false;
PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
void prepareReadCommand(uint8_t *regList, size_t len);
BurstModes getBurstMode();
#ifdef FSFW_OSAL_LINUX
static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData,
size_t sendLen, void *args);
#endif
adis1650x::BurstModes getBurstMode();
Countdown breakCountdown;
void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode);
ReturnValue_t handleSensorData(const uint8_t *packet);

191
mission/devices/GyrL3gCustomHandler.cpp Normal file
View File

@ -0,0 +1,191 @@
#include <mission/devices/GyrL3gCustomHandler.h>
#include <cmath>
#include "fsfw/datapool/PoolReadGuard.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, uint32_t transitionDelayMs)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
transitionDelayMs(transitionDelayMs),
dataset(this) {}
GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
void GyrL3gCustomHandler::doStartUp() {
if (internalState != InternalState::STARTUP) {
internalState = InternalState::STARTUP;
updatePeriodicReply(true, l3gd20h::REPLY);
commandExecuted = false;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
if (goNormalModeImmediately) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
internalState = InternalState::NONE;
commandExecuted = false;
}
}
}
void GyrL3gCustomHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
internalState = InternalState::SHUTDOWN;
dataset.setValidity(false, true);
commandExecuted = false;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
internalState = InternalState::NONE;
updatePeriodicReply(false, l3gd20h::REPLY);
commandExecuted = false;
setMode(MODE_OFF);
}
}
ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch (internalState) {
case (InternalState::NONE):
case (InternalState::NORMAL): {
return NOTHING_TO_SEND;
}
case (InternalState::STARTUP): {
*id = l3gd20h::REQUEST;
gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
case (InternalState::SHUTDOWN): {
*id = l3gd20h::REQUEST;
return doPeriodicRequest(acs::SimpleSensorMode::OFF);
}
default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
/* Might be a configuration error. */
sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
"Unknown internal state!"
<< std::endl;
#else
sif::printDebug(
"GyroL3GD20Handler::buildTransitionDeviceCommand: "
"Unknown internal state!\n");
#endif
return returnvalue::OK;
}
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = l3gd20h::REQUEST;
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
switch (deviceCommand) {
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
return IGNORE_FULL_PACKET;
}
if (len != sizeof(acs::GyroL3gReply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = l3gd20h::REPLY;
*foundLen = len;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
return returnvalue::OK;
}
ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
if (reply->cfgWasSet) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&dataset);
dataset.setValidity(true, true);
dataset.angVelocX = static_cast<float>(reply->angVelocities[0]) * reply->sensitivity;
dataset.angVelocY = static_cast<float>(reply->angVelocities[1]) * reply->sensitivity;
dataset.angVelocZ = static_cast<float>(reply->angVelocities[2]) * reply->sensitivity;
if (std::abs(dataset.angVelocX.value) > absLimitX) {
dataset.angVelocX.setValid(false);
}
if (std::abs(dataset.angVelocY.value) > absLimitY) {
dataset.angVelocY.setValid(false);
}
if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
dataset.angVelocZ.setValid(false);
}
dataset.temperature = 25.0 - reply->tempOffsetRaw;
}
return returnvalue::OK;
}
uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
return this->transitionDelayMs;
}
void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; }
ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
return returnvalue::OK;
}
void GyrL3gCustomHandler::fillCommandAndReplyMap() {
insertInCommandMap(l3gd20h::REQUEST);
insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true);
}
void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }
void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
this->absLimitX = limitX;
this->absLimitY = limitY;
this->absLimitZ = limitZ;
}
void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
if (sid == dataset.getSid()) {
return &dataset;
}
return nullptr;
}
ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
gyrRequest.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
rawPacketLen = sizeof(acs::GyroL3gRequest);
return returnvalue::OK;
}

91
mission/devices/GyrL3gCustomHandler.h Normal file
View File

@ -0,0 +1,91 @@
#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_
#define MISSION_DEVICES_GYROL3GD20HANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
#include <mission/devices/devicedefinitions/acsPolling.h>
/**
* @brief Device Handler for the L3GD20H gyroscope sensor
* (https://www.st.com/en/mems-and-sensors/l3gd20h.html)
* @details
* Advanced documentation:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro
*
* Data is read big endian with the smallest possible range of 245 degrees per second.
*/
class GyrL3gCustomHandler : public DeviceHandlerBase {
public:
GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
uint32_t transitionDelayMs);
virtual ~GyrL3gCustomHandler();
void enablePeriodicPrintouts(bool enable, uint8_t divider);
/**
* Set the absolute limit for the values on the axis in degrees per second.
* The dataset values will be marked as invalid if that limit is exceeded
* @param xLimit
* @param yLimit
* @param zLimit
*/
void setAbsoluteLimits(float limitX, float limitY, float limitZ);
/**
* @brief Configure device handler to go to normal mode immediately
*/
void setToGoToNormalMode(bool enable);
protected:
/* DeviceHandlerBase overrides */
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) override;
virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
void fillCommandAndReplyMap() override;
void modeChanged() override;
virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
private:
uint32_t transitionDelayMs = 0;
GyroPrimaryDataset dataset;
float absLimitX = l3gd20h::RANGE_DPS_00;
float absLimitY = l3gd20h::RANGE_DPS_00;
float absLimitZ = l3gd20h::RANGE_DPS_00;
enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL };
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
uint8_t statusReg = 0;
bool goNormalModeImmediately = false;
uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL;
uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL;
uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL;
uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL;
uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL;
acs::GyroL3gRequest gyrRequest{};
// Set default value
float sensitivity = l3gd20h::SENSITIVITY_00;
bool periodicPrintout = false;
PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode);
};
#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */

524
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -1,524 +0,0 @@
#include "GyroADIS1650XHandler.h"
#include <fsfw/action/HasActionsIF.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include "fsfw/FSFW.h"
#ifdef FSFW_OSAL_LINUX
#include <sys/ioctl.h>
#include <unistd.h>
#include "fsfw_hal/linux/UnixFileGuard.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/utility.h"
#endif
GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, ADIS1650X::Type type)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
adisType(type),
primaryDataset(this),
configDataset(this),
breakCountdown() {
#ifdef FSFW_OSAL_LINUX
SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
if (cookie != nullptr) {
cookie->setCallbackMode(&spiSendCallback, this);
}
#endif
}
void GyroADIS1650XHandler::doStartUp() {
// Initial 310 ms start up time after power-up
if (internalState == InternalState::STARTUP) {
if (not commandExecuted) {
warningSwitch = true;
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
commandExecuted = true;
}
if (breakCountdown.hasTimedOut()) {
internalState = InternalState::CONFIG;
commandExecuted = false;
}
}
// Read all configuration registers first
if (internalState == InternalState::CONFIG) {
if (commandExecuted) {
commandExecuted = false;
internalState = InternalState::IDLE;
}
}
if (internalState == InternalState::IDLE) {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(MODE_ON);
}
}
}
void GyroADIS1650XHandler::doShutDown() {
commandExecuted = false;
internalState = InternalState::STARTUP;
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = ADIS1650X::READ_SENSOR_DATA;
return buildCommandFromCommand(*id, nullptr, 0);
}
ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch (internalState) {
case (InternalState::CONFIG): {
*id = ADIS1650X::READ_OUT_CONFIG;
buildCommandFromCommand(*id, nullptr, 0);
break;
}
case (InternalState::STARTUP): {
return NOTHING_TO_SEND;
break;
}
default: {
// Might be a configuration error
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
"Unknown internal state!"
<< std::endl;
return returnvalue::OK;
}
}
return returnvalue::OK;
}
ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
switch (deviceCommand) {
case (ADIS1650X::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
uint8_t regList[6] = {};
regList[0] = ADIS1650X::DIAG_STAT_REG;
regList[1] = ADIS1650X::FILTER_CTRL_REG;
regList[2] = ADIS1650X::RANG_MDL_REG;
regList[3] = ADIS1650X::MSC_CTRL_REG;
regList[4] = ADIS1650X::DEC_RATE_REG;
regList[5] = ADIS1650X::PROD_ID_REG;
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
}
case (ADIS1650X::READ_SENSOR_DATA): {
if (breakCountdown.isBusy()) {
// A glob command is pending and sensor data can't be read currently
return NO_REPLY_EXPECTED;
}
std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
ADIS1650X::BURST_READ_ENABLE.size());
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
this->rawPacket = commandBuffer.data();
break;
}
case (ADIS1650X::SELF_TEST_SENSORS): {
if (breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
break;
}
case (ADIS1650X::SELF_TEST_MEMORY): {
if (breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
break;
}
case (ADIS1650X::UPDATE_NV_CONFIGURATION): {
if (breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
break;
}
case (ADIS1650X::RESET_SENSOR_CONFIGURATION): {
if (breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
break;
}
case (ADIS1650X::SW_RESET): {
if (breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
break;
}
case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
#if OBSW_VERBOSE_LEVEL >= 1
PoolReadGuard pg(&configDataset);
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
<< std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
<< configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
<< configDataset.filterSetting.value << " | DEC_RATE: 0x"
<< configDataset.decRateReg.value << std::setfill(' ') << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
}
}
return returnvalue::OK;
}
void GyroADIS1650XHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
}
ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
// For SPI, the ID will always be the one of the last sent command
*foundId = this->getPendingCommand();
*foundLen = this->rawPacketLen;
return returnvalue::OK;
}
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
using namespace ADIS1650X;
switch (id) {
case (ADIS1650X::READ_OUT_CONFIG): {
uint16_t readProdId = packet[12] << 8 | packet[13];
if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
#if OBSW_VERBOSE_LEVEL >= 1
if (warningSwitch) {
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
<< readProdId << std::endl;
}
warningSwitch = false;
#endif
return returnvalue::FAILED;
}
PoolReadGuard rg(&configDataset);
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
configDataset.filterSetting.value = packet[4] << 8 | packet[5];
uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
ADIS1650X::RangMdlBitfield bitfield =
static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
switch (bitfield) {
case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
sensitivity = SENSITIVITY_1BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
sensitivity = SENSITIVITY_2BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
sensitivity = SENSITIVITY_3BMLZ;
break;
}
case (RangMdlBitfield::RESERVED): {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
#endif
break;
}
}
configDataset.rangMdl.value = rangMdlRaw;
configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
configDataset.decRateReg.value = packet[10] << 8 | packet[11];
configDataset.setValidity(true, true);
if (internalState == InternalState::CONFIG) {
commandExecuted = true;
}
break;
}
case (ADIS1650X::READ_SENSOR_DATA): {
return handleSensorData(packet);
}
}
return returnvalue::OK;
}
ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
BurstModes burstMode = getBurstMode();
switch (burstMode) {
case (BurstModes::BURST_16_BURST_SEL_1):
case (BurstModes::BURST_32_BURST_SEL_1): {
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
" not implemented!"
<< std::endl;
return returnvalue::OK;
}
case (BurstModes::BURST_16_BURST_SEL_0): {
uint16_t checksum = packet[20] << 8 | packet[21];
// Now verify the read checksum with the expected checksum according to datasheet p. 20
uint16_t calcChecksum = 0;
for (size_t idx = 2; idx < 20; idx++) {
calcChecksum += packet[idx];
}
if (checksum != calcChecksum) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
"Invalid checksum detected!"
<< std::endl;
#endif
return returnvalue::FAILED;
}
ReturnValue_t result = configDataset.diagStatReg.read();
if (result == returnvalue::OK) {
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
configDataset.diagStatReg.setValid(true);
}
configDataset.diagStatReg.commit();
{
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
float accelScaling = 0;
if (adisType == ADIS1650X::Type::ADIS16507) {
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
} else if (adisType == ADIS1650X::Type::ADIS16505) {
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
} else {
sif::warning << "GyroADIS1650XHandler::handleSensorData: "
"Unknown ADIS type"
<< std::endl;
}
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
int16_t temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
// Ignore data counter for now
primaryDataset.setValidity(true, true);
}
if (periodicPrintout) {
if (debugDivider.checkAndIncrement()) {
sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
sif::info << "X: " << primaryDataset.accelX.value << std::endl;
sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
}
}
break;
}
case (BurstModes::BURST_32_BURST_SEL_0): {
break;
}
}
return returnvalue::OK;
}
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
uint8_t valueTwo) {
uint8_t secondReg = startReg + 1;
startReg |= ADIS1650X::WRITE_MASK;
secondReg |= ADIS1650X::WRITE_MASK;
commandBuffer[0] = startReg;
commandBuffer[1] = valueOne;
commandBuffer[2] = secondReg;
commandBuffer[3] = valueTwo;
this->rawPacketLen = 4;
this->rawPacket = commandBuffer.data();
}
void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
for (size_t idx = 0; idx < len; idx++) {
commandBuffer[idx * 2] = regList[idx];
commandBuffer[idx * 2 + 1] = 0x00;
}
commandBuffer[len * 2] = 0x00;
commandBuffer[len * 2 + 1] = 0x00;
}
ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl);
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
return returnvalue::OK;
}
GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
configDataset.mscCtrlReg.read();
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
configDataset.mscCtrlReg.commit();
if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
return BurstModes::BURST_32_BURST_SEL_1;
} else {
return BurstModes::BURST_32_BURST_SEL_0;
}
} else {
if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
return BurstModes::BURST_16_BURST_SEL_1;
} else {
return BurstModes::BURST_16_BURST_SEL_0;
}
}
}
#ifdef FSFW_OSAL_LINUX
ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
const uint8_t *sendData, size_t sendLen,
void *args) {
GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
if (handler == nullptr) {
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
<< std::endl;
return returnvalue::FAILED;
}
DeviceCommandId_t currentCommand = handler->getPendingCommand();
switch (currentCommand) {
case (ADIS1650X::READ_SENSOR_DATA): {
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
}
case (ADIS1650X::READ_OUT_CONFIG):
default: {
ReturnValue_t result = returnvalue::OK;
int retval = 0;
// Prepare transfer
int fileDescriptor = 0;
std::string device = comIf->getSpiDev();
UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
if (fileHelper.getOpenResult() != returnvalue::OK) {
return SpiComIF::OPENING_FILE_FAILED;
}
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
uint32_t spiSpeed = 0;
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
cookie->assignWriteBuffer(sendData);
cookie->setTransferSize(2);
gpioId_t gpioId = cookie->getChipSelectPin();
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) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid"
<< std::endl;
return returnvalue::FAILED;
#endif
}
if (gpioId != gpio::NO_GPIO) {
result = mutex->lockMutex(timeoutType, timeoutMs);
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
return result;
}
}
size_t idx = 0;
spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
uint64_t origTx = transferStruct->tx_buf;
uint64_t origRx = transferStruct->rx_buf;
while (idx < sendLen) {
// Pull SPI CS low. For now, no support for active high given
if (gpioId != gpio::NO_GPIO) {
gpioIF.pullLow(gpioId);
}
// Execute transfer
// Initiate a full duplex SPI transfer.
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
if (retval < 0) {
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
}
#if FSFW_HAL_SPI_WIRETAPPING == 1
comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if (gpioId != gpio::NO_GPIO) {
gpioIF.pullHigh(gpioId);
}
idx += 2;
if (idx < sendLen) {
usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
}
transferStruct->tx_buf += 2;
transferStruct->rx_buf += 2;
}
transferStruct->tx_buf = origTx;
transferStruct->rx_buf = origRx;
if (gpioId != gpio::NO_GPIO) {
mutex->unlockMutex();
}
}
}
return returnvalue::OK;
}
void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */

18
mission/devices/HeaterHandler.cpp
View File

@ -30,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
if (mainLineSwitcher == nullptr) {
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
}
heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
if (heaterHealthAndStateMutex == nullptr) {
handlerLock = MutexFactory::instance()->createMutex();
if (handlerLock == nullptr) {
throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
}
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@ -144,7 +144,7 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
if (action == SwitchAction::SET_SWITCH_ON) {
HasHealthIF::HealthState health;
{
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
health = heater.healthDevice->getHealth();
}
if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
@ -270,7 +270,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
} else {
triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
{
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
heater.switchState = ON;
}
}
@ -320,7 +320,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
triggerEvent(GPIO_PULL_LOW_FAILED, result);
} else {
{
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
heater.switchState = OFF;
}
triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
@ -346,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
}
HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
return heaterVec.at(switchNr).switchState;
}
@ -396,7 +396,7 @@ object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectI
ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
{
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
if (mg.getLockResult() != returnvalue::OK) {
return returnvalue::FAILED;
}
@ -409,7 +409,7 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& stat
bool HeaterHandler::allSwitchesOff() {
bool allSwitchesOrd = false;
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
/* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@ -442,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
auto* healthDev = heaterVec.at(heater).healthDevice;
if (healthDev != nullptr) {
MutexGuard mg(heaterHealthAndStateMutex);
MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
return healthDev->getHealth();
}
return HasHealthIF::HealthState::FAULTY;

5
mission/devices/HeaterHandler.h
View File

@ -136,7 +136,10 @@ class HeaterHandler : public ExecutableObjectIF,
HeaterMap heaterVec = {};
MutexIF* heaterHealthAndStateMutex = nullptr;
MutexIF* handlerLock = nullptr;
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
static constexpr uint32_t LOCK_TIMEOUT = 20;
static constexpr char LOCK_CTX[] = "HeaterHandler";
HeaterHelper helper;
ModeHelper modeHelper;

280
mission/devices/ImtqHandler.cpp
View File

@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION;
auto actuateStep = [&]() {
if (ignoreActForRestOfComSteps) {
requestStep = imtq::RequestType::DO_NOTHING;
} else {
requestStep = imtq::RequestType::ACTUATE;
}
};
switch (static_cast<imtq::ComStep>(opCode)) {
case (imtq::ComStep::DHB_OP): {
break;
@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
break;
}
case (imtq::ComStep::START_ACTUATE_SEND): {
requestStep = imtq::RequestType::ACTUATE;
if (manualTorqueCmdActive) {
if (manuallyCommandedTorqueDuration.isBusy()) {
ignoreActForRestOfComSteps = true;
requestStep = imtq::RequestType::DO_NOTHING;
} else {
manualTorqueCmdActive = false;
PoolReadGuard pg(&dipoleSet);
dipoleSet.dipoles[0] = 0;
dipoleSet.dipoles[1] = 0;
dipoleSet.dipoles[2] = 0;
dipoleSet.currentTorqueDurationMs = 0;
requestStep = imtq::RequestType::ACTUATE;
ignoreActForRestOfComSteps = false;
}
} else {
requestStep = imtq::RequestType::ACTUATE;
}
dhbOpCode = DeviceHandlerIF::SEND_WRITE;
break;
}
case (imtq::ComStep::START_ACTUATE_GET): {
requestStep = imtq::RequestType::ACTUATE;
actuateStep();
dhbOpCode = DeviceHandlerIF::GET_WRITE;
break;
}
case (imtq::ComStep::READ_ACTUATE_SEND): {
requestStep = imtq::RequestType::ACTUATE;
actuateStep();
dhbOpCode = DeviceHandlerIF::SEND_READ;
break;
}
case (imtq::ComStep::READ_ACTUATE_GET): {
requestStep = imtq::RequestType::ACTUATE;
actuateStep();
dhbOpCode = DeviceHandlerIF::GET_READ;
break;
}
@ -108,18 +131,37 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
ImtqHandler::~ImtqHandler() = default;
void ImtqHandler::doStartUp() {
updatePeriodicReply(true, imtq::cmdIds::REPLY);
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
if (internalState != InternalState::STARTUP) {
commandExecuted = false;
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
internalState = InternalState::STARTUP;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
if (goToNormalMode) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
commandExecuted = false;
}
}
}
void ImtqHandler::doShutDown() {
updatePeriodicReply(false, imtq::cmdIds::REPLY);
updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
specialRequestActive = false;
firstReplyCycle = true;
internalState = InternalState::NONE;
commandExecuted = false;
statusSet.setValidity(false, true);
rawMtmNoTorque.setValidity(false, true);
rawMtmWithTorque.setValidity(false, true);
hkDatasetNoTorque.setValidity(false, true);
hkDatasetWithTorque.setValidity(false, true);
calMtmMeasurementSet.setValidity(false, true);
setMode(_MODE_POWER_DOWN);
}
@ -133,11 +175,24 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
*id = imtq::cmdIds::START_ACTUATION_DIPOLE;
return buildCommandFromCommand(*id, nullptr, 0);
}
default: {
*id = imtq::cmdIds::REQUEST;
request.request = imtq::RequestType::DO_NOTHING;
request.specialRequest = imtq::SpecialRequest::NONE;
expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(imtq::Request);
return returnvalue::OK;
}
}
return NOTHING_TO_SEND;
}
ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
if (internalState == InternalState::STARTUP) {
*id = imtq::cmdIds::REQUEST;
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
@ -145,11 +200,12 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
const uint8_t* commandData,
size_t commandDataLen) {
auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(specialRequest);
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.specialRequest = specialRequest;
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
specialRequestActive = true;
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(imtq::Request);
};
switch (deviceCommand) {
case (imtq::cmdIds::POS_X_SELF_TEST): {
@ -181,44 +237,50 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return returnvalue::OK;
}
case (imtq::cmdIds::REQUEST): {
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
request.setMeasureRequest(imtq::SpecialRequest::NONE);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.specialRequest = imtq::SpecialRequest::NONE;
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(imtq::Request);
return returnvalue::OK;
}
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
// commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
if (commandData != nullptr && commandDataLen < 8) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
// Commands override anything which was set in the software
if (commandData != nullptr) {
// Read set dipole values from local pool
{
// Do this in any case to read values which might be commanded by the ACS controller.
PoolReadGuard pg(&dipoleSet);
int16_t xDipole = 0, yDipole = 0, zDipole = 0;
uint16_t torqueDuration = 0;
dipoleSet.xDipole = xDipole;
dipoleSet.yDipole = yDipole;
dipoleSet.zDipole = zDipole;
dipoleSet.currentTorqueDurationMs = torqueDuration;
// Commands override anything which was set in the software
if (commandData != nullptr) {
dipoleSet.setValidityBufferGeneration(false);
ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::NETWORK);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
manualTorqueCmdActive = true;
manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value);
}
}
request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value,
dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value);
expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
request.request = imtq::RequestType::ACTUATE;
request.specialRequest = imtq::SpecialRequest::NONE;
std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles));
request.torqueDuration = dipoleSet.currentTorqueDurationMs.value;
if (ACTUATION_WIRETAPPING) {
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
<< ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0]
<< ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2]
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
}
MutexGuard mg(torquer::lazyLock());
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
torquer::LOCK_CTX);
torquer::TORQUEING = true;
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
rawPacket = commandBuffer;
rawPacketLen = ImtqRequest::REQUEST_LEN;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(imtq::Request);
return returnvalue::OK;
}
default:
@ -230,7 +292,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
void ImtqHandler::fillCommandAndReplyMap() {
insertInCommandMap(imtq::cmdIds::REQUEST);
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true);
insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true);
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
@ -242,12 +305,12 @@ void ImtqHandler::fillCommandAndReplyMap() {
ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
return IGNORE_FULL_PACKET;
}
if (remainingSize > 0) {
*foundLen = remainingSize;
*foundId = imtq::cmdIds::REPLY;
*foundId = expectedReply;
return returnvalue::OK;
}
return returnvalue::FAILED;
@ -257,14 +320,20 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
ReturnValue_t result;
ReturnValue_t status = returnvalue::OK;
if (getMode() != MODE_NORMAL) {
// Ignore replies during transitions.
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
ImtqRepliesDefault replies(packet);
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
commandExecuted = true;
}
}
return returnvalue::OK;
}
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
requestStep = imtq::RequestType::ACTUATE;
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
// sif::debug << "handle measure" << std::endl;
ImtqRepliesDefault replies(packet);
if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
commandExecuted = true;
}
if (specialRequestActive) {
if (replies.wasSpecialRequestRead()) {
uint8_t* specialRequest = replies.getSpecialRequest();
@ -307,29 +376,29 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
}
if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl;
}
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
fillRawMtmDataset(rawMtmNoTorque, rawMgmMeasurement);
} else {
status = result;
}
if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read"
<< std::endl;
}
uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(calibMgmMeasurement);
fillCalibratedMtmDataset(calibMgmMeasurement);
} else {
status = result;
}
} else {
} else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
// sif::debug << "handle measure with torque" << std::endl;
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) {
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
@ -345,7 +414,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
if (result == returnvalue::OK) {
fillRawMtmDataset(rawMgmMeasurement);
fillRawMtmDataset(rawMtmWithTorque, rawMgmMeasurement);
} else {
status = result;
}
@ -361,7 +430,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else {
status = result;
}
fillEngHkDataset(hkDatasetNoTorque, engHkReply);
fillEngHkDataset(hkDatasetWithTorque, engHkReply);
if (firstReplyCycle) {
firstReplyCycle = false;
}
@ -374,6 +443,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
return &hkDatasetNoTorque;
} else if (sid == dipoleSet.getSid()) {
return &dipoleSet;
} else if (sid == statusSet.getSid()) {
return &statusSet;
} else if (sid == hkDatasetWithTorque.getSid()) {
return &hkDatasetWithTorque;
} else if (sid == rawMtmWithTorque.getSid()) {
@ -395,7 +466,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
} else if (sid == negZselfTestDataset.getSid()) {
return &negZselfTestDataset;
} else {
sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl;
return nullptr;
}
}
@ -409,21 +480,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
// ENG HK No Torque
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
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);
// ENG HK With Torque
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry<float>({0}));
localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque);
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque);
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry);
localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
/** Entries of calibrated MTM measurement dataset */
@ -431,8 +507,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
/** Entries of raw MTM measurement dataset */
localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>(3));
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque);
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque);
localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque);
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque);
/** INIT measurements for positive X axis test */
localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
@ -708,6 +787,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0));
return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
}
@ -731,7 +814,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
}
ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
uint8_t cmdErrorField = packet[1] & 0xF;
if (cmdErrorField == 0) {
return returnvalue::OK;
}
@ -754,16 +837,20 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
<< " has invalid parameter" << std::endl;
return imtq::PARAMETER_INVALID;
case 5:
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl;
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable"
<< std::endl;
return imtq::CC_UNAVAILABLE;
case 7:
sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl;
sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x"
<< std::setw(2) << command << std::endl;
return imtq::INTERNAL_PROCESSING_ERROR;
default:
sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x"
<< static_cast<int>(cmdErrorField) << std::endl;
sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
<< command << " contains unknown error code 0x" << static_cast<int>(cmdErrorField)
<< std::endl;
return imtq::CMD_ERR_UNKNOWN;
}
sif::error << std::dec;
}
void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
@ -777,20 +864,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
offset += 2;
hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
hkDataset.coilXCurrentmA =
hkDataset.coilCurrentsMilliamps[0] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
hkDataset.coilYCurrentmA =
hkDataset.coilCurrentsMilliamps[1] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
hkDataset.coilZCurrentmA =
hkDataset.coilCurrentsMilliamps[2] =
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
offset += 2;
hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset));
offset += 2;
size_t dummy = 2;
SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
@ -804,12 +891,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl;
sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl;
sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl;
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl;
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl;
sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA"
<< std::endl;
sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA"
<< std::endl;
sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA"
<< std::endl;
sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl;
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl;
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl;
sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl;
#endif
}
@ -847,8 +937,11 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
}
}
void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
PoolReadGuard rg(&rawMtmNoTorque);
void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
PoolReadGuard rg(&set);
if (rg.getReadResult() != returnvalue::OK) {
sif::error << "ImtqHandler::fillRawMtmDataset: Read failure" << std::endl;
}
unsigned int offset = 2;
size_t deSerLen = 16;
const uint8_t* dataStart = packet + offset;
@ -876,18 +969,19 @@ void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
if (res != returnvalue::OK) {
return;
}
rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5;
rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5;
rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5;
rawMtmNoTorque.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
rawMtmNoTorque.setValidity(true, true);
set.mtmRawNt[0] = static_cast<float>(xRaw) * 7.5;
set.mtmRawNt[1] = static_cast<float>(yRaw) * 7.5;
set.mtmRawNt[2] = static_cast<float>(zRaw) * 7.5;
set.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
set.setValidity(true, true);
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl;
sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl;
sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;
sif::info << "IMTQ coil actuation status during MTM measurement: "
<< (unsigned int)rawMtmNoTorque.coilActuationStatus.value << std::endl;
<< (unsigned int)set.coilActuationStatus.value << std::endl;
#endif
}
}

27
mission/devices/ImtqHandler.h
View File

@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase {
//! link between IMTQ and OBC.
static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE;
bool commandExecuted = false;
imtq::Request request{};
imtq::StatusDataset statusSet;
imtq::DipoleActuationSet dipoleSet;
imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
@ -98,6 +103,9 @@ class ImtqHandler : public DeviceHandlerBase {
imtq::NegYSelfTestSet negYselfTestDataset;
imtq::PosZSelfTestSet posZselfTestDataset;
imtq::NegZSelfTestSet negZselfTestDataset;
bool manualTorqueCmdActive = false;
bool ignoreActForRestOfComSteps = false;
Countdown manuallyCommandedTorqueDuration = Countdown();
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
@ -107,13 +115,20 @@ class ImtqHandler : public DeviceHandlerBase {
PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
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);
PoolEntry<int16_t> dipolesPoolEntry = PoolEntry<int16_t>({0, 0, 0}, false);
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>({0}, false);
PoolEntry<float> coilCurrentsMilliampsNoTorque = PoolEntry<float>(3);
PoolEntry<float> coilCurrentsMilliampsWithTorque = PoolEntry<float>(3);
PoolEntry<int16_t> coilTempsNoTorque = PoolEntry<int16_t>(3);
PoolEntry<int16_t> coilTempsWithTorque = PoolEntry<int16_t>(3);
PoolEntry<float> mtmRawNoTorque = PoolEntry<float>(3);
PoolEntry<uint8_t> actStatusNoTorque = PoolEntry<uint8_t>(1);
PoolEntry<float> mtmRawWithTorque = PoolEntry<float>(3);
PoolEntry<uint8_t> actStatusWithTorque = PoolEntry<uint8_t>(1);
power::Switch_t switcher = power::NO_SWITCH;
uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
bool goToNormalMode = false;
bool debugMode = false;
bool specialRequestActive = false;
@ -159,7 +174,7 @@ class ImtqHandler : public DeviceHandlerBase {
* @param packet Pointer to the reply data requested with the GET_RAW_MTM_MEASUREMENTS
* command.
*/
void fillRawMtmDataset(const uint8_t* packet);
void fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet);
/**
* @brief This function handles all self test results. This comprises parsing the error byte

233
mission/devices/LegacySusHandler.cpp Normal file
View File

@ -0,0 +1,233 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <mission/devices/LegacySusHandler.h>
#include "OBSWConfig.h"
LegacySusHandler::LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF,
CookieIF *comCookie)
: DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {}
LegacySusHandler::~LegacySusHandler() {}
void LegacySusHandler::doStartUp() {
if (comState == ComStates::IDLE) {
comState = ComStates::WRITE_SETUP;
commandExecuted = false;
}
if (comState == ComStates::WRITE_SETUP) {
if (commandExecuted) {
if (goToNormalModeImmediately) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
commandExecuted = false;
if (clkMode == ClkModes::INT_CLOCKED) {
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
} else {
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
}
}
}
}
void LegacySusHandler::doShutDown() {
setMode(_MODE_POWER_DOWN);
comState = ComStates::IDLE;
}
ReturnValue_t LegacySusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
switch (comState) {
case (ComStates::IDLE): {
break;
}
case (ComStates::WRITE_SETUP): {
*id = susMax1227::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_CONVERSIONS): {
*id = susMax1227::READ_EXT_TIMED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::START_INT_CLOCKED_CONVERSIONS): {
*id = susMax1227::START_INT_TIMED_CONVERSIONS;
comState = ComStates::READ_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::READ_INT_CLOCKED_CONVERSIONS): {
*id = susMax1227::READ_INT_TIMED_CONVERSIONS;
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_TEMP): {
*id = susMax1227::READ_EXT_TIMED_TEMPS;
return buildCommandFromCommand(*id, nullptr, 0);
}
}
return NOTHING_TO_SEND;
}
ReturnValue_t LegacySusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
if (comState == ComStates::WRITE_SETUP) {
*id = susMax1227::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
ReturnValue_t LegacySusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
using namespace max1227;
switch (deviceCommand) {
case (susMax1227::WRITE_SETUP): {
if (clkMode == ClkModes::INT_CLOCKED) {
cmdBuffer[0] = susMax1227::SETUP_INT_CLOKED;
} else {
cmdBuffer[0] = susMax1227::SETUP_EXT_CLOCKED;
}
rawPacket = cmdBuffer;
rawPacketLen = 1;
break;
}
case (susMax1227::START_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
cmdBuffer[0] = max1227::buildResetByte(true);
cmdBuffer[1] = susMax1227::CONVERSION;
rawPacket = cmdBuffer;
rawPacketLen = 2;
break;
}
case (susMax1227::READ_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
rawPacketLen = susMax1227::SIZE_READ_INT_CONVERSIONS;
break;
}
case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
for (uint8_t idx = 0; idx < 6; idx++) {
cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false);
cmdBuffer[idx * 2 + 1] = 0;
}
cmdBuffer[12] = 0x00;
rawPacketLen = susMax1227::SIZE_READ_EXT_CONVERSIONS;
break;
}
case (susMax1227::READ_EXT_TIMED_TEMPS): {
cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true);
std::memset(cmdBuffer + 1, 0, 24);
rawPacket = cmdBuffer;
rawPacketLen = 25;
break;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return returnvalue::OK;
}
void LegacySusHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(susMax1227::WRITE_SETUP, 1);
insertInCommandAndReplyMap(susMax1227::START_INT_TIMED_CONVERSIONS, 1);
insertInCommandAndReplyMap(susMax1227::READ_INT_TIMED_CONVERSIONS, 1, &dataset,
susMax1227::SIZE_READ_INT_CONVERSIONS);
insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_CONVERSIONS, 1, &dataset,
susMax1227::SIZE_READ_EXT_CONVERSIONS);
insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_TEMPS, 1);
}
ReturnValue_t LegacySusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
*foundId = this->getPendingCommand();
*foundLen = remainingSize;
return returnvalue::OK;
}
ReturnValue_t LegacySusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
switch (id) {
case susMax1227::WRITE_SETUP: {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
}
return returnvalue::OK;
}
case susMax1227::START_INT_TIMED_CONVERSIONS: {
return returnvalue::OK;
}
case susMax1227::READ_INT_TIMED_CONVERSIONS: {
PoolReadGuard readSet(&dataset);
dataset.tempC = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3];
}
dataset.setValidity(true, true);
printDataset();
break;
}
case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
PoolReadGuard readSet(&dataset);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2];
}
dataset.channels.setValid(true);
// Read temperature in next read cycle
if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) {
comState = ComStates::EXT_CLOCKED_TEMP;
}
printDataset();
break;
}
case (susMax1227::READ_EXT_TIMED_TEMPS): {
PoolReadGuard readSet(&dataset);
dataset.tempC = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]);
dataset.tempC.setValid(true);
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
break;
}
default: {
sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return returnvalue::OK;
}
uint32_t LegacySusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; }
ReturnValue_t LegacySusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
return returnvalue::OK;
}
void LegacySusHandler::setToGoToNormalMode(bool enable) {
this->goToNormalModeImmediately = enable;
}
void LegacySusHandler::printDataset() {
if (periodicPrintout) {
if (divider.checkAndIncrement()) {
sif::info << "SUS ADC " << static_cast<int>(susIdx) << " hex [" << std::setfill('0')
<< std::hex;
for (uint8_t idx = 0; idx < 6; idx++) {
sif::info << std::setw(3) << dataset.channels[idx];
if (idx < 6 - 1) {
sif::info << ",";
}
}
sif::info << "] | T[C] " << std::dec << dataset.tempC.value << std::endl;
}
}
}
void LegacySusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
this->periodicPrintout = enable;
this->divider.setDivider(divider);
}

92
mission/devices/LegacySusHandler.h Normal file
View File

@ -0,0 +1,92 @@
#ifndef MISSION_DEVICES_LEGACYSUSHANDLER_H_
#define MISSION_DEVICES_LEGACYSUSHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
#include "events/subsystemIdRanges.h"
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
#include "mission/devices/max1227.h"
#include "returnvalues/classIds.h"
/**
* @brief This is the device handler class for the SUS sensor based on the MAX1227 ADC.
*
* @details
* Datasheet of MAX1227: https://datasheets.maximintegrated.com/en/ds/MAX1227-MAX1231.pdf
* Details about the SUS electronic can be found at
* https://egit.irs.uni-stuttgart.de/eive/eive_dokumente/src/branch/master/400_Raumsegment/443_SunSensorDocumentation/release
*
* @note When adding a SusHandler to the polling sequence table make sure to add a slot with
* the executionStep FIRST_WRITE. Otherwise the communication sequence will never be
* started.
*
* @author J. Meier
*/
class LegacySusHandler : public DeviceHandlerBase {
public:
enum ClkModes { INT_CLOCKED, EXT_CLOCKED, EXT_CLOCKED_WITH_TEMP };
static const uint8_t FIRST_WRITE = 7;
LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF* comCookie);
virtual ~LegacySusHandler();
void enablePeriodicPrintout(bool enable, uint8_t divider);
void setToGoToNormalMode(bool enable);
protected:
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
void fillCommandAndReplyMap() override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData,
size_t commandDataLen) 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;
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
private:
static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER;
static const ReturnValue_t ERROR_UNLOCK_MUTEX = MAKE_RETURN_CODE(0xA0);
static const ReturnValue_t ERROR_LOCK_MUTEX = MAKE_RETURN_CODE(0xA1);
enum class ComStates {
IDLE,
WRITE_SETUP,
EXT_CLOCKED_CONVERSIONS,
EXT_CLOCKED_TEMP,
START_INT_CLOCKED_CONVERSIONS,
READ_INT_CLOCKED_CONVERSIONS
};
bool periodicPrintout = false;
PeriodicOperationDivider divider;
bool goToNormalModeImmediately = false;
bool commandExecuted = false;
susMax1227::SusDataset dataset;
// Read temperature in each alternating communication step when using
// externally clocked mode
ClkModes clkMode = ClkModes::INT_CLOCKED;
PoolEntry<float> tempC = PoolEntry<float>({0.0});
PoolEntry<uint16_t> channelVec = PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0});
uint8_t susIdx = 0;
uint8_t cmdBuffer[susMax1227::MAX_CMD_SIZE];
ComStates comState = ComStates::IDLE;
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 20;
void printDataset();
MutexIF* spiMutex = nullptr;
};
#endif /* MISSION_DEVICES_LEGACYSUSHANDLER_H_ */

150
mission/devices/MgmLis3CustomHandler.cpp Normal file
View File

@ -0,0 +1,150 @@
#include <mission/devices/MgmLis3CustomHandler.h>
#include <cmath>
#include "fsfw/datapool/PoolReadGuard.h"
MgmLis3CustomHandler::MgmLis3CustomHandler(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie, uint32_t transitionDelay)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
dataset(this),
transitionDelay(transitionDelay) {}
MgmLis3CustomHandler::~MgmLis3CustomHandler() = default;
void MgmLis3CustomHandler::doStartUp() {
if (internalState != InternalState::STARTUP) {
commandExecuted = false;
updatePeriodicReply(true, REPLY);
internalState = InternalState::STARTUP;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
setMode(MODE_NORMAL);
internalState = InternalState::NONE;
commandExecuted = false;
}
}
}
void MgmLis3CustomHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
dataset.setValidity(false, true);
internalState = InternalState::SHUTDOWN;
commandExecuted = false;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, REPLY);
commandExecuted = false;
internalState = InternalState::NONE;
setMode(MODE_OFF);
}
}
ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
if (internalState == InternalState::STARTUP) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
} else if (internalState == InternalState::SHUTDOWN) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::OFF);
}
return NOTHING_TO_SEND;
}
ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t MgmLis3CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
return NOTHING_TO_SEND;
}
ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (len != sizeof(acs::MgmLis3Reply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = REPLY;
*foundLen = len;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
return returnvalue::OK;
}
ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
const auto *reply = reinterpret_cast<const acs::MgmLis3Reply *>(packet);
if (reply->dataWasSet) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&dataset);
for (uint8_t idx = 0; idx < 3; idx++) {
dataset.fieldStrengths[idx] =
reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
}
dataset.setValidity(true, true);
if (std::abs(dataset.fieldStrengths[0]) > absLimitX or
std::abs(dataset.fieldStrengths[1]) > absLimitY or
std::abs(dataset.fieldStrengths[2]) > absLimitZ) {
dataset.fieldStrengths.setValid(false);
}
dataset.temperature = 25.0 + ((static_cast<float>(reply->temperatureRaw)) / 8.0);
}
return returnvalue::OK;
}
void MgmLis3CustomHandler::fillCommandAndReplyMap() {
insertInCommandMap(REQUEST);
insertInReplyMap(REPLY, 5, nullptr, 0, true);
}
void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }
uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
return transitionDelay;
}
void MgmLis3CustomHandler::modeChanged(void) { internalState = InternalState::NONE; }
ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ);
localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature);
poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0});
return returnvalue::OK;
}
void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) {
this->absLimitX = xLimit;
this->absLimitY = yLimit;
this->absLimitZ = zLimit;
}
void MgmLis3CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
request.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&request);
rawPacketLen = sizeof(acs::MgmLis3Request);
return returnvalue::OK;
}
LocalPoolDataSetBase *MgmLis3CustomHandler::getDataSetHandle(sid_t sid) {
if (sid == dataset.getSid()) {
return &dataset;
}
return nullptr;
}

103
mission/devices/MgmLis3CustomHandler.h Normal file
View File

@ -0,0 +1,103 @@
#ifndef MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_
#define MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
class PeriodicOperationDivider;
/**
* @brief Device handler object for the LIS3MDL 3-axis magnetometer
* by STMicroeletronics
* @details
* Datasheet can be found online by googling LIS3MDL.
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM
* @author L. Loidold, R. Mueller
*/
class MgmLis3CustomHandler : public DeviceHandlerBase {
public:
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x77;
static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL;
MgmLis3CustomHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
uint32_t transitionDelay);
virtual ~MgmLis3CustomHandler();
void enablePeriodicPrintouts(bool enable, uint8_t divider);
/**
* Set the absolute limit for the values on the axis in microtesla. The dataset values will
* be marked as invalid if that limit is exceeded
* @param xLimit
* @param yLimit
* @param zLimit
*/
void setAbsoluteLimits(float xLimit, float yLimit, float zLimit);
void setToGoToNormalMode(bool enable);
protected:
/** DeviceHandlerBase overrides */
void doShutDown() override;
void doStartUp() override;
virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) override;
/**
* This implementation is tailored towards space applications and will flag values larger
* than 100 microtesla on X,Y and 150 microtesla on Z as invalid
* @param id
* @param packet
* @return
*/
virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
void fillCommandAndReplyMap() override;
void modeChanged(void) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
private:
mgmLis3::MgmPrimaryDataset dataset;
acs::MgmLis3Request request{};
uint32_t transitionDelay;
float absLimitX = 100;
float absLimitY = 100;
float absLimitZ = 150;
uint8_t statusRegister = 0;
bool goToNormalMode = false;
enum class InternalState {
NONE,
STARTUP,
SHUTDOWN,
};
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
PoolEntry<float> mgmXYZ = PoolEntry<float>(3);
PoolEntry<float> temperature = PoolEntry<float>();
/*------------------------------------------------------------------------*/
/* Device specific commands and variables */
/*------------------------------------------------------------------------*/
bool periodicPrintout = false;
PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
ReturnValue_t prepareRequest(acs::SimpleSensorMode mode);
};
#endif /* MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ */

139
mission/devices/MgmRm3100CustomHandler.cpp Normal file
View File

@ -0,0 +1,139 @@
#include <mission/devices/MgmRm3100CustomHandler.h>
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
#include "fsfw/globalfunctions/bitutility.h"
#include "fsfw/objectmanager/SystemObjectIF.h"
#include "fsfw/returnvalues/returnvalue.h"
MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF *comCookie,
uint32_t transitionDelay)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
primaryDataset(this),
transitionDelay(transitionDelay) {}
MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default;
void MgmRm3100CustomHandler::doStartUp() {
if (internalState != InternalState::STARTUP) {
commandExecuted = false;
updatePeriodicReply(true, REPLY);
internalState = InternalState::STARTUP;
}
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
commandExecuted = false;
internalState = InternalState::NONE;
setMode(MODE_NORMAL);
}
}
}
void MgmRm3100CustomHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
commandExecuted = false;
primaryDataset.setValidity(false, true);
internalState = InternalState::SHUTDOWN;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, REPLY);
setMode(MODE_OFF);
commandExecuted = false;
}
}
ReturnValue_t MgmRm3100CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
if (internalState == InternalState::STARTUP) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
} else if (internalState == InternalState::SHUTDOWN) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::OFF);
}
return NOTHING_TO_SEND;
}
ReturnValue_t MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
return NOTHING_TO_SEND;
}
ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (len != sizeof(acs::MgmRm3100Reply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = REPLY;
*foundLen = len;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
return returnvalue::OK;
}
ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
const auto *reply = reinterpret_cast<const acs::MgmRm3100Reply *>(packet);
if (reply->dataWasRead) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
PoolReadGuard pg(&primaryDataset);
primaryDataset.setValidity(true, true);
for (uint8_t idx = 0; idx < 3; idx++) {
primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx];
}
}
return returnvalue::OK;
}
void MgmRm3100CustomHandler::fillCommandAndReplyMap() {
insertInCommandMap(REQUEST);
insertInReplyMap(REPLY, 5, nullptr, 0, true);
}
void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; }
ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ);
poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0});
return returnvalue::OK;
}
uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
return this->transitionDelay;
}
void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; }
void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
periodicPrintout = enable;
debugDivider.setDivider(divider);
}
ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
request.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&request);
rawPacketLen = sizeof(acs::MgmRm3100Request);
return returnvalue::OK;
}
LocalPoolDataSetBase *MgmRm3100CustomHandler::getDataSetHandle(sid_t sid) {
if (sid == primaryDataset.getSid()) {
return &primaryDataset;
}
return nullptr;
}

97
mission/devices/MgmRm3100CustomHandler.h Normal file
View File

@ -0,0 +1,97 @@
#ifndef MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_
#define MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
/**
* @brief Device Handler for the RM3100 geomagnetic magnetometer sensor
* (https://www.pnicorp.com/rm3100/)
* @details
* Flight manual:
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM
*/
class MgmRm3100CustomHandler : public DeviceHandlerBase {
public:
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x77;
static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
//! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0
static constexpr Event TMRC_SET =
event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x00, severity::INFO);
//! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X
//! P1: Second two bytes new Cycle Count Y
//! P2: New cycle count Z
static constexpr Event cycleCountersSet =
event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO);
MgmRm3100CustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
uint32_t transitionDelay);
virtual ~MgmRm3100CustomHandler();
void enablePeriodicPrintouts(bool enable, uint8_t divider);
/**
* Configure device handler to go to normal mode after startup immediately
* @param enable
*/
void setToGoToNormalMode(bool enable);
protected:
/* DeviceHandlerBase overrides */
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
void fillCommandAndReplyMap() override;
void modeChanged(void) override;
virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
private:
enum class InternalState { NONE, STARTUP, SHUTDOWN };
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
mgmRm3100::Rm3100PrimaryDataset primaryDataset;
acs::MgmRm3100Request request{};
// uint8_t cmmRegValue = mgmRm3100::CMM_VALUE;
// uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE;
// uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE;
// uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE;
// uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE;
float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN;
float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN;
float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN;
bool goToNormalModeAtStartup = false;
uint32_t transitionDelay;
PoolEntry<float> mgmXYZ = PoolEntry<float>(3);
bool periodicPrintout = false;
ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData, size_t commandDataLen);
ReturnValue_t handleCycleCommand(bool oneCycleValue, const uint8_t *commandData,
size_t commandDataLen);
ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen);
ReturnValue_t prepareRequest(acs::SimpleSensorMode mode);
PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
};
#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */

16
mission/devices/PcduHandler.cpp
View File

@ -18,7 +18,7 @@ PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize)
auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
commandQueue = QueueFactory::instance()->createMessageQueue(
cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
pwrMutex = MutexFactory::instance()->createMutex();
pwrLock = MutexFactory::instance()->createMutex();
}
PCDUHandler::~PCDUHandler() {}
@ -41,7 +41,7 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
if (pg.getReadResult() == returnvalue::OK) {
if (switcherSet.p60Dock5VStack.value != switchState) {
triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
MutexGuard mg(pwrMutex);
MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
switchStates[pcdu::P60_DOCK_5V_STACK] = switchState;
}
switcherSet.p60Dock5VStack.setValid(true);
@ -179,7 +179,7 @@ void PCDUHandler::updatePdu2SwitchStates() {
switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx];
}
switcherSet.pdu2Switches.setValid(true);
MutexGuard mg(pwrMutex);
MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]);
checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
@ -216,7 +216,7 @@ void PCDUHandler::updatePdu1SwitchStates() {
switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx];
}
switcherSet.pdu1Switches.setValid(true);
MutexGuard mg(pwrMutex);
MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3,
pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]);
checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V,
@ -402,9 +402,11 @@ ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const {
sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl;
return returnvalue::FAILED;
}
pwrMutex->lockMutex();
uint8_t currentState = switchStates[switchNr];
pwrMutex->unlockMutex();
uint8_t currentState = 0;
{
MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
currentState = switchStates[switchNr];
}
if (currentState == 1) {
return PowerSwitchIF::SWITCH_ON;
} else {

5
mission/devices/PcduHandler.h
View File

@ -51,7 +51,10 @@ class PCDUHandler : public PowerSwitchIF,
private:
uint32_t pstIntervalMs = 0;
MutexIF* pwrMutex = nullptr;
MutexIF* pwrLock = nullptr;
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
static constexpr uint32_t LOCK_TIMEOUT = 20;
static constexpr char LOCK_CTX[] = "PcduHandler";
/** Housekeeping manager. Handles updates of local pool variables. */
LocalDataPoolManager poolManager;

5
mission/devices/RwHandler.cpp
View File

@ -366,9 +366,8 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
statusSet.setValidity(true, true);
if (statusSet.state == rws::STATE_ERROR) {
// This requires the commanding of the init reaction wheel controller command to recover
// from error state which must be handled by the FDIR instance.
triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0);
// Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue.
triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0);
sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl;
}

9
mission/devices/ScexDeviceHandler.cpp
View File

@ -319,11 +319,14 @@ void ScexDeviceHandler::performOperationHook() {
auto mntPrefix = sdcMan.getCurrentMountPrefix();
if (mntPrefix != nullptr) {
std::filesystem::path fullFilePath = mntPrefix;
std::error_code e;
fullFilePath /= "scex";
bool fileExists = std::filesystem::exists(fullFilePath);
bool fileExists = std::filesystem::exists(fullFilePath, e);
if (not fileExists) {
std::filesystem::create_directory(fullFilePath);
bool created = std::filesystem::create_directory(fullFilePath, e);
if (not created) {
sif::error << "Could not create SCEX directory: " << e << std::endl;
}
}
}
uint32_t remainingMillis = finishCountdown.getRemainingMillis();

10
mission/devices/SolarArrayDeploymentHandler.cpp
View File

@ -43,15 +43,16 @@ ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCod
#endif
if (opDivider.checkAndIncrement()) {
auto activeSdc = sdcMan.getActiveSdCard();
std::error_code e;
if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
sdcMan.isSdCardUsable(activeSdc.value())) {
if (exists(SD_0_DEPL_FILE)) {
if (exists(SD_0_DEPL_FILE, e)) {
// 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)) {
if (exists(SD_1_DEPL_FILE, e)) {
// perform autonomous deployment handling
performAutonomousDepl(sd::SdCard::SLOT_1, dryRunStringInFile(SD_1_DEPL_FILE));
}
@ -137,15 +138,16 @@ ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCa
of << "phase: init\n";
of << "secs_since_start: 0\n";
};
std::error_code e;
if (sdCard == sd::SdCard::SLOT_0) {
if (not exists(SD_0_DEPLY_INFO)) {
if (not exists(SD_0_DEPLY_INFO, e)) {
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)) {
if (not exists(SD_1_DEPLY_INFO, e)) {
initFile(SD_1_DEPLY_INFO);
}
if (not autonomousDeplForFile(sd::SdCard::SLOT_1, SD_1_DEPLY_INFO, dryRun)) {

261
mission/devices/SusHandler.cpp
View File

@ -1,231 +1,128 @@
#include "SusHandler.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <mission/devices/max1227.h>
#include "OBSWConfig.h"
#include <cmath>
SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF *comCookie)
: DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {}
#include "fsfw/datapool/PoolReadGuard.h"
SusHandler::~SusHandler() {}
SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t deviceCommunication,
CookieIF *comCookie)
: DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), susIdx(susIdx) {}
SusHandler::~SusHandler() = default;
void SusHandler::doStartUp() {
if (comState == ComStates::IDLE) {
comState = ComStates::WRITE_SETUP;
if (internalState != InternalState::STARTUP) {
commandExecuted = false;
updatePeriodicReply(true, REPLY);
internalState = InternalState::STARTUP;
}
if (comState == ComStates::WRITE_SETUP) {
if (internalState == InternalState::STARTUP) {
if (commandExecuted) {
if (goToNormalModeImmediately) {
setMode(MODE_NORMAL);
} else {
setMode(_MODE_TO_ON);
}
setMode(MODE_NORMAL);
internalState = InternalState::NONE;
commandExecuted = false;
if (clkMode == ClkModes::INT_CLOCKED) {
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
} else {
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
}
}
}
}
void SusHandler::doShutDown() {
setMode(_MODE_POWER_DOWN);
comState = ComStates::IDLE;
}
ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
switch (comState) {
case (ComStates::IDLE): {
break;
}
case (ComStates::WRITE_SETUP): {
*id = SUS::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_CONVERSIONS): {
*id = SUS::READ_EXT_TIMED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::START_INT_CLOCKED_CONVERSIONS): {
*id = SUS::START_INT_TIMED_CONVERSIONS;
comState = ComStates::READ_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::READ_INT_CLOCKED_CONVERSIONS): {
*id = SUS::READ_INT_TIMED_CONVERSIONS;
comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
return buildCommandFromCommand(*id, nullptr, 0);
}
case (ComStates::EXT_CLOCKED_TEMP): {
*id = SUS::READ_EXT_TIMED_TEMPS;
return buildCommandFromCommand(*id, nullptr, 0);
}
if (internalState != InternalState::SHUTDOWN) {
dataset.setValidity(false, true);
internalState = InternalState::SHUTDOWN;
commandExecuted = false;
}
if (internalState == InternalState::SHUTDOWN and commandExecuted) {
updatePeriodicReply(false, REPLY);
commandExecuted = false;
internalState = InternalState::NONE;
setMode(MODE_OFF);
}
return NOTHING_TO_SEND;
}
ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
if (comState == ComStates::WRITE_SETUP) {
*id = SUS::WRITE_SETUP;
return buildCommandFromCommand(*id, nullptr, 0);
if (internalState == InternalState::STARTUP) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
} else if (internalState == InternalState::SHUTDOWN) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::OFF);
}
return NOTHING_TO_SEND;
}
ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = REQUEST;
return prepareRequest(acs::SimpleSensorMode::NORMAL);
}
ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t *commandData,
size_t commandDataLen) {
using namespace max1227;
switch (deviceCommand) {
case (SUS::WRITE_SETUP): {
if (clkMode == ClkModes::INT_CLOCKED) {
cmdBuffer[0] = SUS::SETUP_INT_CLOKED;
} else {
cmdBuffer[0] = SUS::SETUP_EXT_CLOCKED;
}
return NOTHING_TO_SEND;
}
rawPacket = cmdBuffer;
rawPacketLen = 1;
break;
ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) {
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (len != sizeof(acs::SusReply)) {
*foundLen = len;
return returnvalue::FAILED;
}
*foundId = REPLY;
*foundLen = len;
if (internalState == InternalState::SHUTDOWN) {
commandExecuted = true;
}
return returnvalue::OK;
}
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
const auto *reply = reinterpret_cast<const acs::SusReply *>(packet);
if (reply->dataWasSet) {
if (internalState == InternalState::STARTUP) {
commandExecuted = true;
}
case (SUS::START_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
cmdBuffer[0] = max1227::buildResetByte(true);
cmdBuffer[1] = SUS::CONVERSION;
rawPacket = cmdBuffer;
rawPacketLen = 2;
break;
}
case (SUS::READ_INT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
rawPacketLen = SUS::SIZE_READ_INT_CONVERSIONS;
break;
}
case (SUS::READ_EXT_TIMED_CONVERSIONS): {
std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
rawPacket = cmdBuffer;
for (uint8_t idx = 0; idx < 6; idx++) {
cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false);
cmdBuffer[idx * 2 + 1] = 0;
}
cmdBuffer[12] = 0x00;
rawPacketLen = SUS::SIZE_READ_EXT_CONVERSIONS;
break;
}
case (SUS::READ_EXT_TIMED_TEMPS): {
cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true);
std::memset(cmdBuffer + 1, 0, 24);
rawPacket = cmdBuffer;
rawPacketLen = 25;
break;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
PoolReadGuard pg(&dataset);
dataset.setValidity(true, true);
dataset.tempC = max1227::getTemperature(reply->tempRaw);
std::memcpy(dataset.channels.value, reply->channelsRaw, sizeof(reply->channelsRaw));
}
return returnvalue::OK;
}
void SusHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(SUS::WRITE_SETUP, 1);
insertInCommandAndReplyMap(SUS::START_INT_TIMED_CONVERSIONS, 1);
insertInCommandAndReplyMap(SUS::READ_INT_TIMED_CONVERSIONS, 1, &dataset,
SUS::SIZE_READ_INT_CONVERSIONS);
insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_CONVERSIONS, 1, &dataset,
SUS::SIZE_READ_EXT_CONVERSIONS);
insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_TEMPS, 1);
insertInCommandMap(REQUEST);
insertInReplyMap(REPLY, 5, nullptr, 0, true);
}
ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
*foundId = this->getPendingCommand();
*foundLen = remainingSize;
return returnvalue::OK;
}
void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }
ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
switch (id) {
case SUS::WRITE_SETUP: {
if (getMode() == _MODE_START_UP) {
commandExecuted = true;
}
return returnvalue::OK;
}
case SUS::START_INT_TIMED_CONVERSIONS: {
return returnvalue::OK;
}
case SUS::READ_INT_TIMED_CONVERSIONS: {
PoolReadGuard readSet(&dataset);
dataset.temperatureCelcius = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3];
}
dataset.setValidity(true, true);
printDataset();
break;
}
case (SUS::READ_EXT_TIMED_CONVERSIONS): {
PoolReadGuard readSet(&dataset);
for (uint8_t idx = 0; idx < 6; idx++) {
dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2];
}
dataset.channels.setValid(true);
// Read temperature in next read cycle
if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) {
comState = ComStates::EXT_CLOCKED_TEMP;
}
printDataset();
break;
}
case (SUS::READ_EXT_TIMED_TEMPS): {
PoolReadGuard readSet(&dataset);
dataset.temperatureCelcius = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]);
dataset.temperatureCelcius.setValid(true);
comState = ComStates::EXT_CLOCKED_CONVERSIONS;
break;
}
default: {
sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return returnvalue::OK;
}
uint32_t SusHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return transitionDelay; }
uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; }
void SusHandler::modeChanged(void) { internalState = InternalState::NONE; }
ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(SUS::TEMPERATURE_C, &tempC);
localDataPoolMap.emplace(SUS::CHANNEL_VEC, &channelVec);
localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
return returnvalue::OK;
}
void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; }
ReturnValue_t SusHandler::prepareRequest(acs::SimpleSensorMode mode) {
request.mode = mode;
rawPacket = reinterpret_cast<uint8_t *>(&request);
rawPacketLen = sizeof(acs::SusRequest);
return returnvalue::OK;
}
void SusHandler::printDataset() {
if (periodicPrintout) {
if (divider.checkAndIncrement()) {
sif::info << "SUS ADC " << static_cast<int>(susIdx) << " hex [" << std::setfill('0')
<< std::hex;
for (uint8_t idx = 0; idx < 6; idx++) {
sif::info << std::setw(3) << dataset.channels[idx];
if (idx < 6 - 1) {
sif::info << ",";
}
}
sif::info << "] | T[C] " << std::dec << dataset.temperatureCelcius.value << std::endl;
}
LocalPoolDataSetBase *SusHandler::getDataSetHandle(sid_t sid) {
if (sid == dataset.getSid()) {
return &dataset;
}
}
void SusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
this->periodicPrintout = enable;
this->divider.setDivider(divider);
return nullptr;
}

108
mission/devices/SusHandler.h
View File

@ -1,92 +1,68 @@
#ifndef MISSION_DEVICES_SUSHANDLER_H_
#define MISSION_DEVICES_SUSHANDLER_H_
#ifndef MISSION_DEVICES_SusHandler_H_
#define MISSION_DEVICES_SusHandler_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <eive/eventSubsystemIds.h>
#include <eive/resultClassIds.h>
#include "devicedefinitions/SusDefinitions.h"
#include "events/subsystemIdRanges.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
#include "mission/devices/max1227.h"
#include "returnvalues/classIds.h"
#include "mission/devices/devicedefinitions/acsPolling.h"
#include "mission/devices/devicedefinitions/susMax1227Helpers.h"
/**
* @brief This is the device handler class for the SUS sensor based on the MAX1227 ADC.
*
* @details
* Datasheet of MAX1227: https://datasheets.maximintegrated.com/en/ds/MAX1227-MAX1231.pdf
* Details about the SUS electronic can be found at
* https://egit.irs.uni-stuttgart.de/eive/eive_dokumente/src/branch/master/400_Raumsegment/443_SunSensorDocumentation/release
*
* @note When adding a SusHandler to the polling sequence table make sure to add a slot with
* the executionStep FIRST_WRITE. Otherwise the communication sequence will never be
* started.
*
* @author J. Meier
*/
class SusHandler : public DeviceHandlerBase {
public:
enum ClkModes { INT_CLOCKED, EXT_CLOCKED, EXT_CLOCKED_WITH_TEMP };
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x77;
static const uint8_t FIRST_WRITE = 7;
static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SUS_BOARD_ASS;
SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF* comCookie);
SusHandler(uint32_t objectId, uint8_t susIdx, object_id_t deviceCommunication,
CookieIF *comCookie);
virtual ~SusHandler();
void enablePeriodicPrintout(bool enable, uint8_t divider);
void enablePeriodicPrintouts(bool enable, uint8_t divider);
void setToGoToNormalMode(bool enable);
protected:
void doStartUp() override;
/** DeviceHandlerBase overrides */
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
void fillCommandAndReplyMap() override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData,
void doStartUp() override;
uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) 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;
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
void fillCommandAndReplyMap() override;
void modeChanged(void) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override;
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
private:
static const uint8_t INTERFACE_ID = CLASS_ID::SUS_HANDLER;
susMax1227::SusDataset dataset;
acs::SusRequest request{};
uint8_t susIdx;
static const ReturnValue_t ERROR_UNLOCK_MUTEX = MAKE_RETURN_CODE(0xA0);
static const ReturnValue_t ERROR_LOCK_MUTEX = MAKE_RETURN_CODE(0xA1);
uint32_t transitionDelay = 1000;
bool goToNormalMode = false;
enum class ComStates {
IDLE,
WRITE_SETUP,
EXT_CLOCKED_CONVERSIONS,
EXT_CLOCKED_TEMP,
START_INT_CLOCKED_CONVERSIONS,
READ_INT_CLOCKED_CONVERSIONS
};
bool periodicPrintout = false;
PeriodicOperationDivider divider;
bool goToNormalModeImmediately = false;
bool commandExecuted = false;
SUS::SusDataset dataset;
// Read temperature in each alternating communication step when using
// externally clocked mode
ClkModes clkMode = ClkModes::INT_CLOCKED;
PoolEntry<float> tempC = PoolEntry<float>({0.0});
PoolEntry<uint16_t> channelVec = PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0});
uint8_t susIdx = 0;
uint8_t cmdBuffer[SUS::MAX_CMD_SIZE];
ComStates comState = ComStates::IDLE;
enum class InternalState {
NONE,
STARTUP,
SHUTDOWN,
};
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 20;
void printDataset();
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
MutexIF* spiMutex = nullptr;
ReturnValue_t prepareRequest(acs::SimpleSensorMode mode);
};
#endif /* MISSION_DEVICES_SUSHANDLER_H_ */
#endif /* MISSION_DEVICES_SusHandler_H_ */

2
mission/devices/devicedefinitions/CMakeLists.txt
View File

@ -1,2 +1,2 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
imtqHelpers.cpp)
imtqHelpers.cpp gyroAdisHelpers.cpp)

16
mission/devices/devicedefinitions/GyroL3GD20Definitions.h
View File

@ -6,7 +6,7 @@
#include <cstdint>
namespace L3GD20H {
namespace l3gd20h {
/* Actual size is 15 but we round up a bit */
static constexpr size_t MAX_BUFFER_SIZE = 16;
@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS;
enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE };
} // namespace L3GD20H
} // namespace l3gd20h
class GyroPrimaryDataset : public StaticLocalDataSet<5> {
public:
/** Constructor for data users like controllers */
GyroPrimaryDataset(object_id_t mgmId)
: StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) {
: StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) {
setAllVariablesReadOnly();
}
/* Angular velocities in degrees per second (DPS) */
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_X, this);
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Y, this);
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, L3GD20H::TEMPERATURE, this);
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_X, this);
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Y, this);
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, l3gd20h::TEMPERATURE, this);
private:
friend class GyroHandlerL3GD20H;
/** Constructor for the data creator */
GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {}
: StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {}
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */

92
mission/devices/devicedefinitions/acsPolling.h Normal file
View File

@ -0,0 +1,92 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
#include "fsfw/thermal/tcsDefinitions.h"
#include "gyroAdisHelpers.h"
namespace acs {
enum SimpleSensorMode { NORMAL = 0, OFF = 1 };
struct Adis1650XRequest {
SimpleSensorMode mode;
adis1650x::Type type;
};
struct Adis1650XConfig {
uint16_t diagStat;
uint16_t filterSetting;
uint16_t rangMdl;
uint16_t mscCtrlReg;
uint16_t decRateReg;
uint16_t prodId;
};
struct Adis1650XData {
double sensitivity = 0.0;
// Angular velocities in all axes (X, Y and Z)
int16_t angVelocities[3]{};
double accelScaling = 0.0;
// Accelerations in all axes
int16_t accelerations[3]{};
int16_t temperatureRaw = thermal::INVALID_TEMPERATURE;
};
struct Adis1650XReply {
bool cfgWasSet = false;
Adis1650XConfig cfg;
bool dataWasSet = false;
Adis1650XData data;
};
struct GyroL3gRequest {
SimpleSensorMode mode = SimpleSensorMode::OFF;
uint8_t ctrlRegs[5]{};
};
struct GyroL3gReply {
bool cfgWasSet = false;
uint8_t statusReg;
// Angular velocities in all axes (X, Y and Z)
int16_t angVelocities[3]{};
int8_t tempOffsetRaw = 0;
uint8_t ctrlRegs[5]{};
float sensitivity = 0.0;
};
struct MgmRm3100Request {
SimpleSensorMode mode = SimpleSensorMode::OFF;
};
struct MgmRm3100Reply {
bool dataWasRead = false;
float scaleFactors[3]{};
int32_t mgmValuesRaw[3]{};
};
struct MgmLis3Request {
SimpleSensorMode mode = SimpleSensorMode::OFF;
};
struct MgmLis3Reply {
bool dataWasSet = false;
float sensitivity = 0.0;
int16_t mgmValuesRaw[3]{};
bool temperatureWasSet = false;
int16_t temperatureRaw = thermal::INVALID_TEMPERATURE;
};
struct SusRequest {
SimpleSensorMode mode = SimpleSensorMode::OFF;
};
struct SusReply {
bool cfgWasSet = false;
bool dataWasSet = false;
uint16_t tempRaw = 0;
uint16_t channelsRaw[6]{};
};
} // namespace acs
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */

54
mission/devices/devicedefinitions/gyroAdisHelpers.cpp Normal file
View File

@ -0,0 +1,54 @@
#include "gyroAdisHelpers.h"
size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf,
size_t maxLen) {
if (len * 2 + 2 > maxLen) {
return 0;
}
for (size_t idx = 0; idx < len; idx++) {
outBuf[idx * 2] = regList[idx];
outBuf[idx * 2 + 1] = 0x00;
}
outBuf[len * 2] = 0x00;
outBuf[len * 2 + 1] = 0x00;
return len * 2 + 2;
}
adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) {
if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) {
if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
return BurstModes::BURST_32_BURST_SEL_1;
} else {
return BurstModes::BURST_32_BURST_SEL_0;
}
} else {
if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
return BurstModes::BURST_16_BURST_SEL_1;
} else {
return BurstModes::BURST_16_BURST_SEL_0;
}
}
}
double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
adis1650x::RangMdlBitfield bitfield =
static_cast<adis1650x::RangMdlBitfield>((rangMdl >> 2) & 0b11);
switch (bitfield) {
case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): {
return SENSITIVITY_1BMLZ;
}
case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): {
return SENSITIVITY_2BMLZ;
}
case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): {
return SENSITIVITY_3BMLZ;
}
case (RangMdlBitfield::RESERVED):
default: {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
#endif
return 0.0;
}
}
}

56
mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h → mission/devices/devicedefinitions/gyroAdisHelpers.h
View File

@ -6,9 +6,20 @@
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
namespace ADIS1650X {
namespace adis1650x {
enum class Type { ADIS16505, ADIS16507 };
enum class BurstModes {
BURST_16_BURST_SEL_0,
BURST_16_BURST_SEL_1,
BURST_32_BURST_SEL_0,
BURST_32_BURST_SEL_1
};
size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
double rangMdlToSensitivity(uint16_t rangMdl);
enum class Type : uint8_t { ADIS16505, ADIS16507 };
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
static constexpr DeviceCommandId_t SW_RESET = 31;
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x77;
static constexpr uint16_t BURST_32_BIT = 1 << 9;
static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
@ -111,57 +125,57 @@ enum FilterSettings : uint8_t {
SIXTYFOUR_TAPS = 6
};
} // namespace ADIS1650X
} // namespace adis1650x
class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> {
public:
/** Constructor for data users like controllers */
AdisGyroPrimaryDataset(object_id_t adisId)
: StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
: StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) {
setAllVariablesReadOnly();
}
/* Angular velocities in degrees per second (DPS) */
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_X, this);
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Y, this);
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Z, this);
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_X, this);
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Y, this);
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, ADIS1650X::TEMPERATURE, this);
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_X, this);
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Y, this);
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Z, this);
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_X, this);
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Y, this);
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, adis1650x::TEMPERATURE, this);
private:
friend class GyroADIS1650XHandler;
friend class GyrAdis1650XHandler;
friend class GyroAdisDummy;
/** Constructor for the data creator */
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
: StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {}
};
class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
public:
/** Constructor for data users like controllers */
AdisGyroConfigDataset(object_id_t adisId)
: StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
: StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) {
setAllVariablesReadOnly();
}
lp_var_t<uint16_t> diagStatReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this);
lp_var_t<uint16_t>(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this);
lp_var_t<uint8_t> filterSetting =
lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS, this);
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL, this);
lp_var_t<uint8_t>(sid.objectId, adis1650x::FILTER_SETTINGS, this);
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, adis1650x::RANG_MDL, this);
lp_var_t<uint16_t> mscCtrlReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this);
lp_var_t<uint16_t>(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this);
lp_var_t<uint16_t> decRateReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this);
lp_var_t<uint16_t>(sid.objectId, adis1650x::DEC_RATE_REGISTER, this);
private:
friend class GyroADIS1650XHandler;
friend class GyrAdis1650XHandler;
/** Constructor for the data creator */
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
: StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {}
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */

238
mission/devices/devicedefinitions/imtqHelpers.h
View File

@ -10,19 +10,7 @@ class ImtqHandler;
namespace imtq {
enum ComStep : uint8_t {
DHB_OP = 0,
START_MEASURE_SEND = 1,
START_MEASURE_GET = 2,
READ_MEASURE_SEND = 3,
READ_MEASURE_GET = 4,
START_ACTUATE_SEND = 5,
START_ACTUATE_GET = 6,
READ_ACTUATE_SEND = 7,
READ_ACTUATE_GET = 8,
};
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE };
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
enum class SpecialRequest : uint8_t {
NONE = 0,
@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t {
GET_SELF_TEST_RESULT = 7
};
struct Request {
imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
uint8_t integrationTimeSel = 3;
int16_t dipoles[3]{};
uint16_t torqueDuration = 0;
};
enum ComStep : uint8_t {
DHB_OP = 0,
START_MEASURE_SEND = 1,
START_MEASURE_GET = 2,
READ_MEASURE_SEND = 3,
READ_MEASURE_GET = 4,
START_ACTUATE_SEND = 5,
START_ACTUATE_GET = 6,
READ_ACTUATE_SEND = 7,
READ_ACTUATE_GET = 8,
};
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0);
@ -53,7 +61,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
namespace cmdIds {
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x71;
static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71;
static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72;
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;
@ -195,20 +204,28 @@ enum PoolIds : lp_id_t {
ANALOG_VOLTAGE_MV,
DIGITAL_CURRENT,
ANALOG_CURRENT,
COIL_X_CURRENT,
COIL_Y_CURRENT,
COIL_Z_CURRENT,
COIL_X_TEMPERATURE,
COIL_Y_TEMPERATURE,
COIL_Z_TEMPERATURE,
COIL_CURRENTS,
COIL_TEMPERATURES,
MCU_TEMPERATURE,
DIGITAL_VOLTAGE_MV_WT,
ANALOG_VOLTAGE_MV_WT,
DIGITAL_CURRENT_WT,
ANALOG_CURRENT_WT,
COIL_CURRENTS_WT,
COIL_TEMPERATURES_WT,
MCU_TEMPERATURE_WT,
MGM_CAL_NT,
ACTUATION_CAL_STATUS,
MTM_RAW,
ACTUATION_RAW_STATUS,
DIPOLES_X,
DIPOLES_Y,
DIPOLES_Z,
MTM_RAW_WT,
ACTUATION_RAW_STATUS_WT,
DIPOLES_ID,
CURRENT_TORQUE_DURATION,
INIT_POS_X_ERR,
@ -476,34 +493,56 @@ class StatusDataset : public StaticLocalDataSet<4> {
class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
public:
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {}
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 7> pids)
: StaticLocalDataSet(owner, setId),
digitalVoltageMv(sid.objectId, pids[0], this),
analogVoltageMv(sid.objectId, pids[1], this),
digitalCurrentmA(sid.objectId, pids[2], this),
analogCurrentmA(sid.objectId, pids[3], this),
coilCurrentsMilliamps(sid.objectId, pids[4], this),
/** All temperatures in [C] for X, Y, Z */
coilTemperatures(sid.objectId, pids[5], this),
mcuTemperature(sid.objectId, pids[6], this) {}
HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {}
HkDataset(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 7> pids)
: StaticLocalDataSet(sid_t(objectId, setId)),
digitalVoltageMv(sid.objectId, pids[0], this),
analogVoltageMv(sid.objectId, pids[1], this),
digitalCurrentmA(sid.objectId, pids[2], this),
analogCurrentmA(sid.objectId, pids[3], this),
coilCurrentsMilliamps(sid.objectId, pids[4], this),
/** All temperatures in [C] for X, Y, Z */
coilTemperatures(sid.objectId, pids[5], this),
mcuTemperature(sid.objectId, pids[6], this) {}
// Engineering HK variables
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);
lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId, DIGITAL_CURRENT, this);
lp_var_t<float> analogCurrentmA = lp_var_t<float>(sid.objectId, ANALOG_CURRENT, this);
lp_var_t<float> coilXCurrentmA = lp_var_t<float>(sid.objectId, COIL_X_CURRENT, this);
lp_var_t<float> coilYCurrentmA = lp_var_t<float>(sid.objectId, COIL_Y_CURRENT, this);
lp_var_t<float> coilZCurrentmA = lp_var_t<float>(sid.objectId, COIL_Z_CURRENT, this);
/** All temperatures in [<5B>C] */
lp_var_t<int16_t> coilXTemperature = lp_var_t<int16_t>(sid.objectId, COIL_X_TEMPERATURE, this);
lp_var_t<int16_t> coilYTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Y_TEMPERATURE, this);
lp_var_t<int16_t> coilZTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Z_TEMPERATURE, this);
lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
lp_var_t<uint16_t> digitalVoltageMv;
lp_var_t<uint16_t> analogVoltageMv;
lp_var_t<float> digitalCurrentmA;
lp_var_t<float> analogCurrentmA;
lp_vec_t<float, 3> coilCurrentsMilliamps;
/** All temperatures in [C] for X, Y, Z */
lp_vec_t<int16_t, 3> coilTemperatures;
lp_var_t<int16_t> mcuTemperature;
private:
};
class HkDatasetNoTorque : public HkDataset {
public:
HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {}
HkDatasetNoTorque(HasLocalDataPoolIF* owner)
: HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE,
{DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT,
COIL_CURRENTS, COIL_TEMPERATURES, MCU_TEMPERATURE}) {}
};
class HkDatasetWithTorque : public HkDataset {
public:
HkDatasetWithTorque(HasLocalDataPoolIF* owner)
: HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {}
: HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE,
{DIGITAL_VOLTAGE_MV_WT, ANALOG_VOLTAGE_MV_WT, DIGITAL_CURRENT_WT,
ANALOG_CURRENT_WT, COIL_CURRENTS_WT, COIL_TEMPERATURES_WT, MCU_TEMPERATURE_WT}) {
}
};
/**
*
@ -529,32 +568,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
*/
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
: StaticLocalDataSet(owner, setId) {}
RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 2> pids)
: StaticLocalDataSet(sid_t(objectId, setId)),
mtmRawNt(sid.objectId, pids.at(0), this),
coilActuationStatus(sid.objectId, pids.at(1), this) {}
RawMtmMeasurementSet(object_id_t objectId, uint32_t setId)
: StaticLocalDataSet(sid_t(objectId, setId)) {}
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 2> pids)
: StaticLocalDataSet(owner, setId),
mtmRawNt(sid.objectId, pids.at(0), this),
coilActuationStatus(sid.objectId, pids.at(1), this) {}
/** The unit of all measurements is nT */
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
lp_vec_t<float, 3> mtmRawNt;
/** 1 if coils were actuating during measurement otherwise 0 */
lp_var_t<uint8_t> coilActuationStatus =
lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
lp_var_t<uint8_t> coilActuationStatus;
};
class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
public:
RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE,
{PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
RawMtmMeasurementNoTorque(object_id_t objectId)
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE,
{PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
};
class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
public:
RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner)
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE,
{PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
RawMtmMeasurementWithTorque(object_id_t objectId)
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE,
{PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
};
/**
@ -608,28 +654,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
uint16_t currentTorqueDurationMs_) {
if (xDipole.value != xDipole_) {
xDipole = xDipole_;
}
if (yDipole.value != yDipole_) {
yDipole = yDipole_;
}
if (zDipole.value != zDipole_) {
zDipole = zDipole_;
}
dipoles[0] = xDipole_;
dipoles[1] = yDipole_;
dipoles[2] = zDipole_;
currentTorqueDurationMs = currentTorqueDurationMs_;
}
void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) {
xDipole_ = xDipole.value;
yDipole_ = yDipole.value;
zDipole_ = zDipole.value;
}
const int16_t* getDipoles() const { return dipoles.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_vec_t<int16_t, 3> dipoles = lp_vec_t<int16_t, 3>(sid.objectId, DIPOLES_ID, this);
lp_var_t<uint16_t> currentTorqueDurationMs =
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
};
@ -1098,80 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
} // namespace imtq
struct ImtqRequest {
friend class ImtqHandler;
public:
static constexpr size_t REQUEST_LEN = 10;
ImtqRequest(const uint8_t* data, size_t maxSize)
: rawData(const_cast<uint8_t*>(data)), maxSize(maxSize) {}
imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
void setMeasureRequest(imtq::SpecialRequest specialRequest) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
rawData[1] = static_cast<uint8_t>(specialRequest);
}
void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ,
uint16_t torqueDuration) {
size_t dummy = 0;
rawData[0] = static_cast<uint8_t>(imtq::RequestType::ACTUATE);
rawData[1] = static_cast<uint8_t>(imtq::SpecialRequest::NONE);
uint8_t* serPtr = rawData + 2;
SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
}
uint8_t* startOfActuateDataPtr() { return rawData + 2; }
int16_t* getDipoles() { return reinterpret_cast<int16_t*>(rawData + 2); }
uint16_t getTorqueDuration() {
uint8_t* data = rawData + 2 + 6;
uint16_t value = 0;
size_t dummy = 0;
SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE);
return value;
}
void setSpecialRequest(imtq::SpecialRequest specialRequest) {
rawData[1] = static_cast<uint8_t>(specialRequest);
}
imtq::SpecialRequest getSpecialRequest() const {
return static_cast<imtq::SpecialRequest>(rawData[1]);
}
private:
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
if (rawData != nullptr) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
}
}
uint8_t* rawData;
size_t maxSize = 0;
};
struct ImtqRepliesDefault {
friend class ImtqPollingTask;
public:
static constexpr size_t BASE_LEN =
imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
+imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 +
imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
initPointers();
}
void setConfigured() { rawData[0] = true; }
bool devWasConfigured() const { return rawData[0]; }
uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; }
bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; };
@ -1193,7 +1168,7 @@ struct ImtqRepliesDefault {
private:
void initPointers() {
swReset = rawData;
swReset = rawData + 1;
systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1;
startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1;
rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
@ -1201,6 +1176,7 @@ struct ImtqRepliesDefault {
calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
}
uint8_t* rawData;
uint8_t* swReset;
uint8_t* systemState;

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

@ -3,6 +3,7 @@
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <mission/memory/NvmParameterBase.h>
#include <cstddef>
#include <filesystem>
@ -10,7 +11,6 @@
#include "OBSWConfig.h"
#include "mission/devices/max1227.h"
#include "mission/memory/NVMParameterBase.h"
namespace plpcdu {

6
mission/devices/devicedefinitions/SusDefinitions.h → mission/devices/devicedefinitions/susMax1227Helpers.h
View File

@ -6,7 +6,7 @@
#include <cstdint>
namespace SUS {
namespace susMax1227 {
static const DeviceCommandId_t NONE = 0x0; // Set when no command is pending
@ -69,9 +69,9 @@ class SusDataset : public StaticLocalDataSet<POOL_ENTRIES> {
SusDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, SUS_DATA_SET_ID)) {}
lp_var_t<float> temperatureCelcius = lp_var_t<float>(sid.objectId, TEMPERATURE_C, this);
lp_var_t<float> tempC = lp_var_t<float>(sid.objectId, TEMPERATURE_C, this);
lp_vec_t<uint16_t, 6> channels = lp_vec_t<uint16_t, 6>(sid.objectId, CHANNEL_VEC, this);
};
} // namespace SUS
} // namespace susMax1227
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_SUS_H_ */

2
mission/memory/CMakeLists.txt
View File

@ -1 +1 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE NVMParameterBase.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE NvmParameterBase.cpp)

14
mission/memory/NVMParameterBase.cpp → mission/memory/NvmParameterBase.cpp
View File

@ -1,4 +1,4 @@
#include "NVMParameterBase.h"
#include <mission/memory/NvmParameterBase.h>
#include <fstream>
@ -10,13 +10,14 @@ NVMParameterBase::NVMParameterBase(std::string fullName) : fullName(fullName) {}
NVMParameterBase::NVMParameterBase() {}
ReturnValue_t NVMParameterBase::readJsonFile() {
if (std::filesystem::exists(fullName)) {
std::error_code e;
if (std::filesystem::exists(fullName, e)) {
// Read JSON file content into object
std::ifstream i(fullName);
try {
i >> json;
} catch (nlohmann::json::exception& e) {
sif::warning << "Reading JSON file failed with error " << e.what() << std::endl;
} catch (nlohmann::json::exception& nlohmannE) {
sif::warning << "Reading JSON file failed with error " << nlohmannE.what() << std::endl;
return returnvalue::FAILED;
}
return returnvalue::OK;
@ -39,7 +40,10 @@ void NVMParameterBase::setFullName(std::string fullName) { this->fullName = full
std::string NVMParameterBase::getFullName() const { return fullName; }
bool NVMParameterBase::getJsonFileExists() { return std::filesystem::exists(fullName); }
bool NVMParameterBase::getJsonFileExists() {
std::error_code e;
return std::filesystem::exists(fullName, e);
}
void NVMParameterBase::printKeys() const {
sif::info << "Printing keys for JSON file " << fullName << std::endl;

0
mission/memory/NVMParameterBase.h → mission/memory/NvmParameterBase.h
View File

43
mission/persistentTmStoreDefs.h Normal file
View File

@ -0,0 +1,43 @@
#ifndef MISSION_PERSISTENTTMSTOREDEFS_H_
#define MISSION_PERSISTENTTMSTOREDEFS_H_
#include <mission/tmtc/PersistentTmStoreWithTmQueue.h>
#include "eive/eventSubsystemIds.h"
namespace persTmStore {
struct PersistentTmStores {
PersistentTmStoreWithTmQueue* okStore;
PersistentTmStoreWithTmQueue* notOkStore;
PersistentTmStoreWithTmQueue* miscStore;
PersistentTmStoreWithTmQueue* hkStore;
PersistentTmStoreWithTmQueue* cfdpStore;
};
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PERSISTENT_TM_STORE;
//! [EXPORT] : [COMMENT]
//! P1: Result code of TM packet parser.
//! P2: Timestamp of possibly corrupt file as a unix timestamp.
static constexpr Event POSSIBLE_FILE_CORRUPTION = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW);
//! [EXPORT] : [COMMENT] File in store too large. P1: Detected file size
//! P2: Allowed file size
static constexpr Event FILE_TOO_LARGE = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
static constexpr Event BUSY_DUMPING_EVENT = event::makeEvent(SUBSYSTEM_ID, 2, severity::INFO);
//! [EXPORT] : [COMMENT] Dump was cancelled. P1: Object ID of store.
static constexpr Event DUMP_WAS_CANCELLED = event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW);
//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes.
static constexpr Event DUMP_OK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes.
static constexpr Event DUMP_NOK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes.
static constexpr Event DUMP_MISC_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 7, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes.
static constexpr Event DUMP_HK_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO);
//! [EXPORT] : [COMMENT] P1: Number of dumped packets. P2: Total dumped bytes.
static constexpr Event DUMP_CFDP_STORE_DONE = event::makeEvent(SUBSYSTEM_ID, 9, severity::INFO);
}; // namespace persTmStore
#endif /* MISSION_PERSISTENTTMSTOREDEFS_H_ */

5
mission/system/fdir/CMakeLists.txt
View File

@ -1,3 +1,4 @@
target_sources(
${LIB_EIVE_MISSION} PRIVATE AcsBoardFdir.cpp RtdFdir.cpp SusFdir.cpp
SyrlinksFdir.cpp GomspacePowerFdir.cpp)
${LIB_EIVE_MISSION}
PRIVATE AcsBoardFdir.cpp RtdFdir.cpp StrFdir.cpp SusFdir.cpp SyrlinksFdir.cpp
GomspacePowerFdir.cpp)

14
mission/system/fdir/StrFdir.cpp Normal file
View File

@ -0,0 +1,14 @@
#include "StrFdir.h"
#include "mission/acsDefs.h"
StrFdir::StrFdir(object_id_t strObject)
: DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
setFaulty(event->getEvent());
return returnvalue::OK;
}
return DeviceHandlerFailureIsolation::eventReceived(event);
}

12
mission/system/fdir/StrFdir.h Normal file
View File

@ -0,0 +1,12 @@
#ifndef MISSION_SYSTEM_FDIR_STRFDIR_H_
#define MISSION_SYSTEM_FDIR_STRFDIR_H_
#include <fsfw/devicehandlers/DeviceHandlerFailureIsolation.h>
class StrFdir : public DeviceHandlerFailureIsolation {
public:
StrFdir(object_id_t strObject);
ReturnValue_t eventReceived(EventMessage* event) override;
};
#endif /* MISSION_SYSTEM_FDIR_STRFDIR_H_ */

6
mission/system/fdir/SyrlinksFdir.cpp
View File

@ -47,8 +47,8 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
}
// else
if (missedReplyCount.incrementAndCheck()) {
// handleRecovery(event->getEvent());
triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
handleRecovery(event->getEvent());
// triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
}
break;
case StorageManagerIF::GET_DATA_FAILED:
@ -80,7 +80,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
break;
case Fuse::POWER_BELOW_LOW_LIMIT:
// Device might got stuck during boot, retry.
// handleRecovery(event->getEvent());
handleRecovery(event->getEvent());
triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
break;
//****Thermal*****

40
mission/system/objects/AcsBoardAssembly.cpp
View File

@ -55,6 +55,12 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
@ -274,4 +280,36 @@ void AcsBoardAssembly::refreshHelperModes() {
}
}
ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); }
ReturnValue_t AcsBoardAssembly::initialize() {
for (const auto& child : childrenMap) {
updateChildModeByObjId(child.first, MODE_OFF, 0);
}
return AssemblyBase::initialize();
}
ReturnValue_t AcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
Submode_t deviceSubmode) {
using namespace returnvalue;
ReturnValue_t status = returnvalue::OK;
auto overwriteHealthForOneDev = [&](object_id_t dev) {
HealthState health = healthHelper.healthTable->getHealth(dev);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(dev, health);
status = NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
};
if (deviceSubmode == duallane::DUAL_MODE) {
overwriteHealthForOneDev(helper.mgm0Lis3IdSideA);
overwriteHealthForOneDev(helper.mgm1Rm3100IdSideA);
overwriteHealthForOneDev(helper.mgm2Lis3IdSideB);
overwriteHealthForOneDev(helper.mgm3Rm3100IdSideB);
overwriteHealthForOneDev(helper.gyro0AdisIdSideA);
overwriteHealthForOneDev(helper.gyro1L3gIdSideA);
overwriteHealthForOneDev(helper.gyro2AdisIdSideB);
overwriteHealthForOneDev(helper.gyro3L3gIdSideB);
overwriteHealthForOneDev(helper.gpsId);
}
return status;
}

1
mission/system/objects/AcsBoardAssembly.h
View File

@ -121,6 +121,7 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
void refreshHelperModes();
};

87
mission/system/objects/AcsSubsystem.cpp
View File

@ -8,92 +8,7 @@
AcsSubsystem::AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {
auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}
ReturnValue_t AcsSubsystem::initialize() {
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
ReturnValue_t result = manager->registerListener(eventQueue->getId());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "AcsSubsystem::registerListener: Failed to register as "
"listener"
<< std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
;
}
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_VIOLATION));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_VIOLATION failed" << std::endl;
}
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::SAFE_RATE_RECOVERY));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl;
}
result =
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::MULTIPLE_RW_INVALID));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
}
result = manager->subscribeToEvent(eventQueue->getId(),
event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION));
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
}
return Subsystem::initialize();
}
void AcsSubsystem::performChildOperation() {
handleEventMessages();
return Subsystem::performChildOperation();
}
void AcsSubsystem::handleEventMessages() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
ReturnValue_t status;
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
if (event.getEvent() == acs::SAFE_RATE_VIOLATION) {
CommandMessage msg;
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0);
status = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
}
}
if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
event.getEvent() == acs::MULTIPLE_RW_INVALID ||
event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
CommandMessage msg;
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
status = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (status != returnvalue::OK) {
sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl;
}
}
break;
default:
sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
"to this event message"
<< std::endl;
break;
}
}
}
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {}
void AcsSubsystem::announceMode(bool recursive) {
const char* modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));

6
mission/system/objects/AcsSubsystem.h
View File

@ -8,13 +8,7 @@ class AcsSubsystem : public Subsystem {
AcsSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
private:
ReturnValue_t initialize() override;
void performChildOperation() override;
void announceMode(bool recursive) override;
void handleEventMessages();
MessageQueueIF* eventQueue = nullptr;
};
#endif /* MISSION_SYSTEM_ACSSUBSYSTEM_H_ */

3
mission/system/objects/CMakeLists.txt
View File

@ -7,10 +7,13 @@ target_sources(
TcsSubsystem.cpp
PayloadSubsystem.cpp
AcsBoardAssembly.cpp
ImtqAssembly.cpp
SyrlinksAssembly.cpp
Stack5VHandler.cpp
SusAssembly.cpp
RwAssembly.cpp
DualLanePowerStateMachine.cpp
StrAssembly.cpp
PowerStateMachineBase.cpp
DualLaneAssemblyBase.cpp
TcsBoardAssembly.cpp)

3
mission/system/objects/ComSubsystem.cpp
View File

@ -30,6 +30,7 @@ void ComSubsystem::performChildOperation() {
if (countdownActive) {
checkTransmitterCountdown();
}
Subsystem::performChildOperation();
}
@ -184,7 +185,7 @@ void ComSubsystem::startRxAndTxLowRateSeq() {
void ComSubsystem::checkTransmitterCountdown() {
if (transmitterCountdown.hasTimedOut()) {
triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout);
triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout);
startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
countdownActive = false;
}

18
mission/system/objects/ComSubsystem.h
View File

@ -1,24 +1,23 @@
#ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_
#define MISSION_SYSTEM_COMSUBSYSTEM_H_
#include <common/config/eive/eventSubsystemIds.h>
#include <fsfw/events/EventMessage.h>
#include <fsfw/parameters/HasParametersIF.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/subsystem/Subsystem.h>
#include <common/config/eive/eventSubsystemIds.h>
#include "mission/comDefs.h"
class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
public:
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM;
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired
//! P1: The current timer value
static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO);
//! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock
static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO);
//! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired
//! P1: The current timer value
static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO);
//! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock
static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO);
/**
* @brief Constructor
@ -27,7 +26,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
* @param maxNumberOfSequences
* @param maxNumberOfTables
* @param transmitterTimeout Maximum time the transmitter of the syrlinks
* will be enabled
* will
* be enabled
*/
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
uint32_t transmitterTimeout);

2
mission/system/objects/DualLaneAssemblyBase.h
View File

@ -70,6 +70,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
virtual void handleChildrenLostMode(ReturnValue_t result) override;
virtual void handleModeTransitionFailed(ReturnValue_t result) override;
virtual void handleModeReached() override;
ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
MessageQueueId_t getEventReceptionQueue() override;
@ -95,7 +96,6 @@ inline void DualLaneAssemblyBase::initModeTableEntry(
entry.setObject(id);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
entry.setInheritSubmode(false);
modeTable.insert(entry);
}

54
mission/system/objects/EiveSystem.cpp
View File

@ -1,10 +1,17 @@
#include "EiveSystem.h"
#include <eive/objects.h>
#include <fsfw/events/EventManager.h>
#include <fsfw/ipc/QueueFactory.h>
#include <mission/acsDefs.h>
EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {}
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {
auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}
void EiveSystem::announceMode(bool recursive) {
const char* modeStr = "UNKNOWN";
@ -17,10 +24,6 @@ void EiveSystem::announceMode(bool recursive) {
modeStr = "SAFE";
break;
}
case (acs::AcsMode::DETUMBLE): {
modeStr = "DETUBMLE";
break;
}
case (acs::AcsMode::PTG_IDLE): {
modeStr = "POINTING IDLE";
break;
@ -41,3 +44,44 @@ void EiveSystem::announceMode(bool recursive) {
sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
return Subsystem::announceMode(recursive);
}
void EiveSystem::performChildOperation() {
handleEventMessages();
return Subsystem::performChildOperation();
}
ReturnValue_t EiveSystem::initialize() {
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "AcsSubsystem::initialize: Invalid event manager" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
ReturnValue_t result = manager->registerListener(eventQueue->getId());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "AcsSubsystem::registerListener: Failed to register as "
"listener"
<< std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
return Subsystem::initialize();
}
void EiveSystem::handleEventMessages() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
break;
default:
sif::debug << "AcsSubsystem::performChildOperation: Did not subscribe "
"to this event message"
<< std::endl;
break;
}
}
}

5
mission/system/objects/EiveSystem.h
View File

@ -8,7 +8,12 @@ class EiveSystem : public Subsystem {
EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
private:
ReturnValue_t initialize() override;
void performChildOperation() override;
void announceMode(bool recursive) override;
void handleEventMessages();
MessageQueueIF* eventQueue = nullptr;
};
#endif /* MISSION_SYSTEM_EIVESYSTEM_H_ */

54
mission/system/objects/ImtqAssembly.cpp Normal file
View File

@ -0,0 +1,54 @@
#include "ImtqAssembly.h"
#include <eive/objects.h>
using namespace returnvalue;
ImtqAssembly::ImtqAssembly(object_id_t objectId) : AssemblyBase(objectId) {
ModeListEntry entry;
entry.setObject(objects::IMTQ_HANDLER);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
commandTable.insert(entry);
}
ReturnValue_t ImtqAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(submode);
if (recoveryState == RECOVERY_IDLE) {
ReturnValue_t result = checkAndHandleHealthState(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return OK;
}
}
HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
executeTable(iter);
return returnvalue::OK;
}
ReturnValue_t ImtqAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
if (childrenMap[objects::IMTQ_HANDLER].mode != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return returnvalue::OK;
}
ReturnValue_t ImtqAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
return returnvalue::OK;
}
return returnvalue::FAILED;
}
ReturnValue_t ImtqAssembly::checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode) {
HealthState health = healthHelper.healthTable->getHealth(objects::IMTQ_HANDLER);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(objects::IMTQ_HANDLER, health);
return NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
return OK;
}
void ImtqAssembly::handleChildrenLostMode(ReturnValue_t result) { startTransition(mode, submode); }

18
mission/system/objects/ImtqAssembly.h Normal file
View File

@ -0,0 +1,18 @@
#pragma once
#include <fsfw/devicehandlers/AssemblyBase.h>
class ImtqAssembly : public AssemblyBase {
public:
ImtqAssembly(object_id_t objectId);
private:
FixedArrayList<ModeListEntry, 1> commandTable;
ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
void handleChildrenLostMode(ReturnValue_t result) override;
ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
};

1
mission/system/objects/RwAssembly.cpp
View File

@ -8,7 +8,6 @@ RwAssembly::RwAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::
entry.setObject(helper.rwIds[idx]);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
entry.setInheritSubmode(false);
modeTable.insert(entry);
}
}

8
mission/system/objects/Stack5VHandler.cpp
View File

@ -5,7 +5,7 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
}
ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateStates) {
MutexGuard mg(stackLock);
MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
if (updateStates) {
updateInternalStates();
}
@ -27,7 +27,7 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateSt
}
ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateStates) {
MutexGuard mg(stackLock);
MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
if (updateStates) {
updateInternalStates();
}
@ -55,12 +55,12 @@ ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateS
}
bool Stack5VHandler::isSwitchOn() {
MutexGuard mg(stackLock);
MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
return updateInternalStates();
}
void Stack5VHandler::update() {
MutexGuard mg(stackLock);
MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
updateInternalStates();
}

4
mission/system/objects/Stack5VHandler.h
View File

@ -21,7 +21,11 @@ class Stack5VHandler {
void update();
private:
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
static constexpr uint32_t LOCK_TIMEOUT = 20;
MutexIF* stackLock;
static constexpr char LOCK_CTX[] = "Stack5VHandler";
PowerSwitchIF& switcher;
bool switchIsOn = false;
bool targetState = false;

30
mission/system/objects/StrAssembly.cpp Normal file
View File

@ -0,0 +1,30 @@
#include "StrAssembly.h"
#include <eive/objects.h>
StrAssembly::StrAssembly(object_id_t objectId) : AssemblyBase(objectId) {
ModeListEntry entry;
entry.setObject(objects::STAR_TRACKER);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
commandTable.insert(entry);
}
ReturnValue_t StrAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(submode);
HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
executeTable(iter);
return returnvalue::OK;
}
ReturnValue_t StrAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
if (childrenMap[objects::STAR_TRACKER].mode != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return returnvalue::OK;
}
ReturnValue_t StrAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
return returnvalue::OK;
}

18
mission/system/objects/StrAssembly.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_
#define MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_
#include "fsfw/devicehandlers/AssemblyBase.h"
class StrAssembly : public AssemblyBase {
public:
StrAssembly(object_id_t objectId);
private:
FixedArrayList<ModeListEntry, 1> commandTable;
ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
};
#endif /* MISSION_SYSTEM_OBJECTS_STRASSEMBLY_H_ */

33
mission/system/objects/SusAssembly.cpp
View File

@ -24,6 +24,12 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
@ -120,7 +126,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan
return returnvalue::OK;
}
ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); }
ReturnValue_t SusAssembly::initialize() {
for (const auto& child : childrenMap) {
updateChildModeByObjId(child.first, MODE_OFF, 0);
}
return AssemblyBase::initialize();
}
bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
if (healthHelper.healthTable->isFaulty(object)) {
@ -143,3 +154,23 @@ void SusAssembly::refreshHelperModes() {
helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
}
}
ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
using namespace returnvalue;
ReturnValue_t status = returnvalue::OK;
auto overwriteHealthForOneDev = [&](object_id_t dev) {
HealthState health = healthHelper.healthTable->getHealth(dev);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(dev, health);
status = NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
};
if (deviceSubmode == duallane::DUAL_MODE) {
for (uint8_t idx = 0; idx < 12; idx++) {
overwriteHealthForOneDev(helper.susIds[idx]);
}
}
return status;
}

1
mission/system/objects/SusAssembly.h
View File

@ -66,6 +66,7 @@ class SusAssembly : public DualLaneAssemblyBase {
void powerStateMachine(Mode_t mode, Submode_t submode);
ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
void refreshHelperModes();
ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
};
#endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */

57
mission/system/objects/SyrlinksAssembly.cpp Normal file
View File

@ -0,0 +1,57 @@
#include "SyrlinksAssembly.h"
#include <eive/objects.h>
using namespace returnvalue;
SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) {
ModeListEntry entry;
entry.setObject(objects::SYRLINKS_HANDLER);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
commandTable.insert(entry);
}
ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode) {
commandTable[0].setMode(mode);
commandTable[0].setSubmode(submode);
HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
if (recoveryState == RECOVERY_IDLE) {
ReturnValue_t result = checkAndHandleHealthState(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return OK;
}
}
executeTable(iter);
return returnvalue::OK;
}
ReturnValue_t SyrlinksAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
if (childrenMap[objects::SYRLINKS_HANDLER].mode != wantedMode) {
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
}
return returnvalue::OK;
}
ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
return returnvalue::OK;
}
return returnvalue::FAILED;
}
ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode,
Submode_t deviceSubmode) {
HealthState health = healthHelper.healthTable->getHealth(objects::SYRLINKS_HANDLER);
if (health == FAULTY or health == PERMANENT_FAULTY) {
overwriteDeviceHealth(objects::SYRLINKS_HANDLER, health);
return NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
return OK;
}
void SyrlinksAssembly::handleChildrenLostMode(ReturnValue_t result) {
startTransition(mode, submode);
}

20
mission/system/objects/SyrlinksAssembly.h Normal file
View File

@ -0,0 +1,20 @@
#ifndef MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
#define MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
#include <fsfw/devicehandlers/AssemblyBase.h>
class SyrlinksAssembly : public AssemblyBase {
public:
SyrlinksAssembly(object_id_t objectId);
private:
FixedArrayList<ModeListEntry, 1> commandTable;
ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
void handleChildrenLostMode(ReturnValue_t result) override;
ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
};
#endif /* MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ */

37
mission/system/objects/TcsBoardAssembly.cpp
View File

@ -12,7 +12,6 @@ TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitc
entry.setObject(helper.rtdInfos[idx].first);
entry.setMode(MODE_OFF);
entry.setSubmode(SUBMODE_NONE);
entry.setInheritSubmode(false);
modeTable.insert(entry);
}
}
@ -41,6 +40,12 @@ ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
modeTable[idx].setMode(MODE_OFF);
modeTable[idx].setSubmode(SUBMODE_NONE);
}
if (recoveryState == RecoveryState::RECOVERY_IDLE) {
result = checkAndHandleHealthStates(mode, submode);
if (result == NEED_TO_CHANGE_HEALTH) {
return returnvalue::OK;
}
}
if (recoveryState != RecoveryState::RECOVERY_STARTED) {
if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
result = handleNormalOrOnModeCmd(mode, submode);
@ -62,10 +67,10 @@ ReturnValue_t TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
} catch (const std::out_of_range& e) {
sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
}
if (devsInWrongMode >= 3) {
if (devsInWrongMode == NUMBER_RTDS) {
if (warningSwitch) {
sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in"
<< " wrong mode" << std::endl;
sif::warning << "TcsBoardAssembly::checkChildrenStateOn: All devices in wrong mode"
<< std::endl;
warningSwitch = false;
}
return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
@ -180,9 +185,29 @@ void TcsBoardAssembly::handleModeReached() {
}
void TcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
// TODO: Maybe try a reboot once here?
triggerEvent(CHILDREN_LOST_MODE, result);
return;
startTransition(mode, submode);
}
ReturnValue_t TcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
Submode_t deviceSubmode) {
ReturnValue_t status = returnvalue::OK;
for (const auto& dev : helper.rtdInfos) {
HealthState health = healthHelper.healthTable->getHealth(dev.first);
if (health == HealthState::HEALTHY) {
return returnvalue::OK;
}
}
for (const auto& dev : helper.rtdInfos) {
HealthState health = healthHelper.healthTable->getHealth(dev.first);
if (health == FAULTY or health == PERMANENT_FAULTY) {
status = NEED_TO_CHANGE_HEALTH;
} else if (health == EXTERNAL_CONTROL) {
modeHelper.setForced(true);
}
}
return status;
}
void TcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {

1
mission/system/objects/TcsBoardAssembly.h
View File

@ -52,6 +52,7 @@ class TcsBoardAssembly : public AssemblyBase, public ConfirmsFailuresIF {
ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
void startTransition(Mode_t mode, Submode_t submode) override;
void handleModeReached() override;
ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
// These two overrides prevent a transition of the whole assembly back to off just because
// some devices are not working

233
mission/system/tree/acsModeTree.cpp
View File

@ -6,8 +6,11 @@
#include <fsfw/subsystem/Subsystem.h>
#include <fsfw/subsystem/modes/ModeDefinitions.h>
#include <optional>
#include "eive/objects.h"
#include "mission/acsDefs.h"
#include "mission/system/objects/definitions.h"
#include "util.h"
AcsSubsystem satsystem::acs::ACS_SUBSYSTEM(objects::ACS_SUBSYSTEM, 12, 24);
@ -17,7 +20,6 @@ namespace {
const auto check = subsystem::checkInsert;
void buildOffSequence(Subsystem& ss, ModeListEntry& eh);
void buildDetumbleSequence(Subsystem& ss, ModeListEntry& entryHelper);
void buildSafeSequence(Subsystem& ss, ModeListEntry& entryHelper);
void buildIdleSequence(Subsystem& ss, ModeListEntry& entryHelper);
void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh);
@ -40,15 +42,6 @@ auto ACS_TABLE_OFF_TRANS_0 =
auto ACS_TABLE_OFF_TRANS_1 =
std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>());
auto ACS_SEQUENCE_DETUMBLE =
std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList<ModeListEntry, 4>());
auto ACS_TABLE_DETUMBLE_TGT =
std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
auto ACS_TABLE_DETUMBLE_TRANS_0 =
std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
auto ACS_TABLE_DETUMBLE_TRANS_1 =
std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 5>());
auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 4>());
auto ACS_TABLE_SAFE_TGT =
std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
@ -100,30 +93,33 @@ Subsystem& satsystem::acs::init() {
ModeListEntry entry;
const char* ctxc = "satsystem::acs::init: generic target";
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table,
bool allowAllSubmodes = false) {
entry.setObject(obj);
entry.setMode(mode);
entry.setSubmode(submode);
if (allowAllSubmodes) {
entry.allowAllSubmodes();
}
check(table.insert(entry), "satsystem::acs::init: generic target");
};
// Build TARGET PT transition 0
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
check(ACS_SUBSYSTEM.addTable(
TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)),
ctxc);
// Build SUS board transition
iht(objects::SUS_BOARD_ASS, NML, 0, SUS_BOARD_NML_TRANS.second);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, SUS_BOARD_NML_TRANS.second, true);
check(ACS_SUBSYSTEM.addTable(TableEntry(SUS_BOARD_NML_TRANS.first, &SUS_BOARD_NML_TRANS.second)),
ctxc);
buildOffSequence(ACS_SUBSYSTEM, entry);
buildSafeSequence(ACS_SUBSYSTEM, entry);
buildDetumbleSequence(ACS_SUBSYSTEM, entry);
buildIdleSequence(ACS_SUBSYSTEM, entry);
buildTargetPtSequence(ACS_SUBSYSTEM, entry);
buildTargetPtGsSequence(ACS_SUBSYSTEM, entry);
@ -162,10 +158,10 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_0.first, &ACS_TABLE_OFF_TRANS_0.second)), ctxc);
// Build OFF transition 1
iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc);
// Build OFF sequence
@ -182,10 +178,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -196,31 +195,33 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// Build SAFE target
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
// Build SAFE target. Allow detumble submode.
iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT,
ACS_TABLE_SAFE_TGT.second, true);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
check(ss.addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc);
// Build SAFE transition 0
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
check(ss.addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true),
ctxc);
// SUS board transition table is defined above
// Build SAFE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT,
ACS_TABLE_SAFE_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true),
ctxc);
// Build SAFE sequence
ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TGT.first, 0, true);
ihs(ACS_SEQUENCE_SAFE.second, SUS_BOARD_NML_TRANS.first, 0, false);
ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_0.first, 0, false);
ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_1.first, 0, false);
check(ss.addSequence(&ACS_SEQUENCE_SAFE.second, ACS_SEQUENCE_SAFE.first, ACS_SEQUENCE_SAFE.first,
@ -228,70 +229,18 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
}
void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::acs::buildDetumbleSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// Build DETUMBLE target
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
check(ss.addTable(&ACS_TABLE_DETUMBLE_TGT.second, ACS_TABLE_DETUMBLE_TGT.first, false, true),
ctxc);
// SUS board transition table is defined above
// Build DETUMBLE transition 0
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
iht(objects::RW_ASS, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false,
true),
ctxc);
// Build DETUMBLE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false,
true),
ctxc);
// Build DETUMBLE sequence
ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TGT.first, 0, true);
ihs(ACS_SEQUENCE_DETUMBLE.second, SUS_BOARD_NML_TRANS.first, 0, false);
ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_0.first, 0, false);
ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_1.first, 0, false);
check(ss.addSequence(&ACS_SEQUENCE_DETUMBLE.second, ACS_SEQUENCE_DETUMBLE.first,
ACS_SEQUENCE_SAFE.first, false, true),
ctxc);
}
void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::acs::buildIdleSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -303,30 +252,28 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc);
};
// Build IDLE target
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_IDLE, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);
// SUS board transition table is built above
// Build IDLE transition 0
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
// Build IDLE transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_IDLE, 0, ACS_TABLE_IDLE_TRANS_1.second);
ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true);
// Build IDLE sequence
ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TGT.first, 0, true);
ihs(ACS_SEQUENCE_IDLE.second, SUS_BOARD_NML_TRANS.first, 0, true);
ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_0.first, 0, true);
ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_1.first, 0, true);
ss.addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, ACS_SEQUENCE_SAFE.first, false,
@ -338,10 +285,13 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -354,26 +304,24 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
ctxc);
// SUS board transition table is built above
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TRANS_1.second);
check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false,
true),
ctxc);
// Build IDLE sequence
ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TGT.first, 0, true);
ihs(ACS_SEQUENCE_PTG_TARGET.second, SUS_BOARD_NML_TRANS.first, 0, true);
ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TRANS_0.first, 0, true);
ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, 0, true);
check(ss.addSequence(&ACS_SEQUENCE_PTG_TARGET.second, ACS_SEQUENCE_PTG_TARGET.first,
@ -386,10 +334,13 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -402,20 +353,19 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET,
ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first,
&ACS_TABLE_PTG_TARGET_NADIR_TGT.second)),
ctxc);
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR,
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0,
ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)),
@ -437,10 +387,13 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -453,20 +406,19 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
check(ss.addTable(
TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)),
ctxc);
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0,
ACS_TABLE_PTG_TARGET_GS_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)),
@ -487,10 +439,13 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode,
ArrayList<ModeListEntry>& sequence) {
ArrayList<ModeListEntry>& sequence, bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(sequence.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -503,20 +458,20 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build TARGET PT table
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0,
ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)),
ctxc);
// Transition 0 already built
// Build TARGET PT transition 1
iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0,
ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second);
check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first,
&ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),

16
mission/system/tree/comModeTree.cpp
View File

@ -105,11 +105,11 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
// Build RX Only table. We could track the state of the CCSDS IP core handler
// as well but I do not think this is necessary because enabling that should
// not interfere with the Syrlinks Handler.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);
// Build RX Only transition 0
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)),
ctxc);
@ -147,7 +147,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build RX and TX low datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW),
COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
@ -163,7 +163,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build TX and RX low transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)),
@ -199,7 +199,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build RX and TX high datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH),
COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
@ -215,7 +215,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build TX and RX high transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)),
@ -253,7 +253,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
};
// Build RX and TX default datarate table.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT),
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
@ -269,7 +269,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build TX and RX default transition 1
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first,
&COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)),

5
mission/system/tree/comModeTree.h
View File

@ -11,8 +11,9 @@ extern ComSubsystem SUBSYSTEM;
// The syrlinks must not transmitting longer then 15 minutes otherwise the
// transceiver might be damaged due to overheating
// 15 minutes in milliseconds
static const uint32_t TRANSMITTER_TIMEOUT = 900000;
// This is the initial timeout of 2 minutes. The timeout needs to be incremented
// before each overpass
static const uint32_t TRANSMITTER_TIMEOUT = 120000;
Subsystem& init();
} // namespace com

34
mission/system/tree/system.cpp
View File

@ -35,6 +35,7 @@ void satsystem::init() {
ModeListEntry entry;
buildSafeSequence(EIVE_SYSTEM, entry);
buildIdleSequence(EIVE_SYSTEM, entry);
EIVE_SYSTEM.setInitialMode(HasModesIF::MODE_OFF, 0);
}
EiveSystem satsystem::EIVE_SYSTEM = EiveSystem(objects::EIVE_SYSTEM, 12, 24);
@ -62,10 +63,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::buildSafeSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table,
bool allowAllSubmodes = false) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
if (allowAllSubmodes) {
eh.allowAllSubmodes();
}
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
@ -77,9 +82,8 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc);
};
// Do no track ACS for now because it might jump to detumble mode and back to safe as part of
// normal operations.
// iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second);
// Do no track submode to allow transitions to DETUMBLE submode.
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second, true);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
@ -87,19 +91,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
// consecutive commanding of TCS and ACS can lead to SPI issues.
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
ctxc);
// Build SAFE transition 1
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_1.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_1.second);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &EIVE_TABLE_SAFE_TRANS_1.second)),
ctxc);
// Build Safe sequence
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_1.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);
@ -127,21 +126,16 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
// Build SAFE transition 0
// Build IDLE transition 0
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
ctxc);
// Build SAFE transition 1
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_1.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_1.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)),
ctxc);
// Build Safe sequence
// Build IDLE sequence
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false);
ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_1.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second,
EIVE_SEQUENCE_SAFE.first)),
ctxc);

28
mission/system/tree/tcsModeTree.cpp
View File

@ -19,8 +19,8 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL;
auto TCS_SEQUENCE_OFF = std::make_pair(OFF, FixedArrayList<ModeListEntry, 3>());
auto TCS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 2>());
auto TCS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 7>());
auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList<ModeListEntry, 2>());
auto TCS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 2>());
auto TCS_TABLE_OFF_TRANS_1 = std::make_pair((OFF << 24) | 3, FixedArrayList<ModeListEntry, 7>());
auto TCS_SEQUENCE_NORMAL = std::make_pair(NML, FixedArrayList<ModeListEntry, 3>());
auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 2>());
@ -59,16 +59,16 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
// OFF target table is empty
check(ss.addTable(TableEntry(TCS_TABLE_OFF_TGT.first, &TCS_TABLE_OFF_TGT.second)), ctxc);
iht(objects::TCS_BOARD_ASS, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
iht(objects::TMP1075_HANDLER_TCS_0, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
iht(objects::TMP1075_HANDLER_TCS_1, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
iht(objects::TMP1075_HANDLER_PLPCDU_0, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
// damaged
// iht(objects::TMP1075_HANDLER_PLPCDU_1, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
iht(objects::TMP1075_HANDLER_IF_BOARD, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
// Transition 1
iht(objects::THERMAL_CONTROLLER, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
check(ss.addTable(TableEntry(TCS_TABLE_OFF_TRANS_0.first, &TCS_TABLE_OFF_TRANS_0.second)), ctxc);
iht(objects::THERMAL_CONTROLLER, OFF, 0, TCS_TABLE_OFF_TRANS_0.second);
iht(objects::TCS_BOARD_ASS, OFF, 0, TCS_TABLE_OFF_TRANS_1.second);
iht(objects::TMP1075_HANDLER_TCS_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second);
iht(objects::TMP1075_HANDLER_TCS_1, OFF, 0, TCS_TABLE_OFF_TRANS_1.second);
iht(objects::TMP1075_HANDLER_PLPCDU_0, OFF, 0, TCS_TABLE_OFF_TRANS_1.second);
// TMP PL PCDU 1 is damaged
iht(objects::TMP1075_HANDLER_IF_BOARD, OFF, 0, TCS_TABLE_OFF_TRANS_1.second);
check(ss.addTable(TableEntry(TCS_TABLE_OFF_TRANS_1.first, &TCS_TABLE_OFF_TRANS_1.second)), ctxc);
ihs(TCS_SEQUENCE_OFF.second, TCS_TABLE_OFF_TGT.first, 0, false);
@ -98,20 +98,20 @@ void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) {
check(sequence.insert(eh), ctxc);
};
// OFF target table is empty
// Normal target table is empty
check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TGT.first, &TCS_TABLE_NORMAL_TGT.second)), ctxc);
iht(objects::TCS_BOARD_ASS, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
iht(objects::TMP1075_HANDLER_TCS_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
iht(objects::TMP1075_HANDLER_TCS_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
iht(objects::TMP1075_HANDLER_PLPCDU_0, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
// damaged
// iht(objects::TMP1075_HANDLER_PLPCDU_1, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
// TMP PL PCDU 1 is damaged
iht(objects::TMP1075_HANDLER_IF_BOARD, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_0.first, &TCS_TABLE_NORMAL_TRANS_0.second)),
ctxc);
iht(objects::THERMAL_CONTROLLER, NML, 0, TCS_TABLE_NORMAL_TRANS_0.second);
// Transition 1
iht(objects::THERMAL_CONTROLLER, NML, 0, TCS_TABLE_NORMAL_TRANS_1.second);
check(ss.addTable(TableEntry(TCS_TABLE_NORMAL_TRANS_1.first, &TCS_TABLE_NORMAL_TRANS_1.second)),
ctxc);

10
mission/tmtc/CMakeLists.txt
View File

@ -1,10 +1,20 @@
target_sources(
${LIB_EIVE_MISSION}
PRIVATE CcsdsIpCoreHandler.cpp
VirtualChannelWithQueue.cpp
PersistentTmStoreWithTmQueue.cpp
LiveTmTask.cpp
VirtualChannel.cpp
TmFunnelHandler.cpp
TmFunnelBase.cpp
CfdpTmFunnel.cpp
tmFilters.cpp
PusLiveDemux.cpp
PersistentSingleTmStoreTask.cpp
PersistentLogTmStoreTask.cpp
TmStoreTaskBase.cpp
PusPacketFilter.cpp
PusTmRouteByFilterHelper.cpp
Service15TmStorage.cpp
PersistentTmStore.cpp
PusTmFunnel.cpp)

112
mission/tmtc/CcsdsIpCoreHandler.cpp
View File

@ -12,11 +12,11 @@
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
#include "mission/devices/devicedefinitions/SyrlinksDefinitions.h"
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
object_id_t tcDestination, PtmeConfig* ptmeConfig,
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t tcDestination,
PtmeConfig& ptmeConfig, std::atomic_bool& linkState,
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData)
: SystemObject(objectId),
ptmeId(ptmeId),
linkState(linkState),
tcDestination(tcDestination),
parameterHelper(this),
actionHelper(this, nullptr),
@ -31,32 +31,15 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
}
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {}
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() = default;
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
readCommandQueue();
handleTelemetry();
handleTelecommands();
return returnvalue::OK;
}
void CcsdsIpCoreHandler::handleTelemetry() {
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
iter->second->performOperation();
}
}
void CcsdsIpCoreHandler::handleTelecommands() {}
ReturnValue_t CcsdsIpCoreHandler::initialize() {
ReturnValue_t result = returnvalue::OK;
PtmeIF* ptme = ObjectManager::instance()->get<PtmeIF>(ptmeId);
if (ptme == nullptr) {
sif::warning << "Invalid PTME object" << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
AcceptsTelecommandsIF* tcDistributor =
ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
if (tcDistributor == nullptr) {
@ -83,25 +66,15 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() {
return result;
}
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
result = iter->second->initialize();
if (result != returnvalue::OK) {
return result;
}
iter->second->setPtmeObject(ptme);
}
result = ptmeConfig->initialize();
result = ptmeConfig.initialize();
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
#if OBSW_SYRLINKS_SIMULATED == 1
// Update data on rising edge
ptmeConfig->invertTxClock(false);
linkState = UP;
forwardLinkstate();
ptmeConfig.invertTxClock(false);
linkState = LINK_UP;
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/
return result;
@ -134,44 +107,6 @@ void CcsdsIpCoreHandler::readCommandQueue(void) {
MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); }
void CcsdsIpCoreHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
if (vcId > config::NUMBER_OF_VIRTUAL_CHANNELS) {
sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl;
return;
}
if (virtualChannel == nullptr) {
sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel interface"
<< std::endl;
return;
}
auto status = virtualChannelMap.emplace(vcId, virtualChannel);
if (status.second == false) {
sif::warning << "CcsdsHandler::addVirtualChannel: Failed to add virtual channel to "
"virtual channel map"
<< std::endl;
return;
}
}
MessageQueueId_t CcsdsIpCoreHandler::getReportReceptionQueue(uint8_t virtualChannel) const {
if (virtualChannel < config::NUMBER_OF_VIRTUAL_CHANNELS) {
auto iter = virtualChannelMap.find(virtualChannel);
if (iter != virtualChannelMap.end()) {
return iter->second->getReportReceptionQueue();
} else {
sif::warning << "CcsdsHandler::getReportReceptionQueue: Virtual channel with ID "
<< static_cast<unsigned int>(virtualChannel) << " not in virtual channel map"
<< std::endl;
return MessageQueueIF::NO_QUEUE;
}
} else {
sif::debug << "CcsdsHandler::getReportReceptionQueue: Invalid virtual channel requested";
}
return MessageQueueIF::NO_QUEUE;
}
ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
@ -182,7 +117,7 @@ ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueI
uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; }
MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const {
// Forward packets directly to TC distributor
// Forward packets directly to the CCSDS TC distributor
return tcDistributorQueueId;
}
@ -192,18 +127,18 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu
switch (actionId) {
case SET_LOW_RATE: {
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW);
result = ptmeConfig->setRate(RATE_100KBPS);
result = ptmeConfig.setRate(RATE_100KBPS);
break;
}
case SET_HIGH_RATE: {
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH);
result = ptmeConfig->setRate(RATE_500KBPS);
result = ptmeConfig.setRate(RATE_500KBPS);
break;
}
case ARBITRARY_RATE: {
uint32_t bitrate = 0;
SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
result = ptmeConfig->setRate(bitrate);
result = ptmeConfig.setRate(bitrate);
break;
}
case EN_TRANSMITTER: {
@ -221,19 +156,19 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu
return EXECUTION_FINISHED;
}
case ENABLE_TX_CLK_MANIPULATOR: {
result = ptmeConfig->configTxManipulator(true);
result = ptmeConfig.configTxManipulator(true);
break;
}
case DISABLE_TX_CLK_MANIPULATOR: {
result = ptmeConfig->configTxManipulator(false);
result = ptmeConfig.configTxManipulator(false);
break;
}
case UPDATE_ON_RISING_EDGE: {
result = ptmeConfig->invertTxClock(false);
result = ptmeConfig.invertTxClock(false);
break;
}
case UPDATE_ON_FALLING_EDGE: {
result = ptmeConfig->invertTxClock(true);
result = ptmeConfig.invertTxClock(true);
break;
}
default:
@ -245,20 +180,14 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu
return EXECUTION_FINISHED;
}
void CcsdsIpCoreHandler::forwardLinkstate() {
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
iter->second->setLinkState(linkState);
}
}
void CcsdsIpCoreHandler::updateLinkState() { linkState = LINK_UP; }
void CcsdsIpCoreHandler::enableTransmit() {
#ifndef TE0720_1CFA
gpioIF->pullHigh(enTxClock);
gpioIF->pullHigh(enTxData);
#endif
linkState = UP;
forwardLinkstate();
linkState = LINK_UP;
}
void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
@ -283,13 +212,13 @@ ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submod
void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
auto rateHigh = [&]() {
ReturnValue_t result = ptmeConfig->setRate(RATE_500KBPS);
ReturnValue_t result = ptmeConfig.setRate(RATE_500KBPS);
if (result == returnvalue::OK) {
this->mode = HasModesIF::MODE_ON;
}
};
auto rateLow = [&]() {
ReturnValue_t result = ptmeConfig->setRate(RATE_100KBPS);
ReturnValue_t result = ptmeConfig.setRate(RATE_100KBPS);
if (result == returnvalue::OK) {
this->mode = HasModesIF::MODE_ON;
}
@ -325,8 +254,7 @@ void CcsdsIpCoreHandler::disableTransmit() {
gpioIF->pullLow(enTxClock);
gpioIF->pullLow(enTxData);
#endif
linkState = DOWN;
forwardLinkstate();
linkState = LINK_DOWN;
}
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }

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