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

Merge remote-tracking branch 'origin/develop' into add_heater_info_set
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2023-02-17 11:21:17 +01:00
parent 0bf1df81cd d233dc9f82
commit 1e0c94246d
77 changed files with 2487 additions and 1506 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mission/CMakeLists.txt
View File

@ -9,4 +9,4 @@ add_subdirectory(csp)
add_subdirectory(cfdp)
add_subdirectory(config)
target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp trace.cpp)

2
mission/acsDefs.h
View File

@ -23,6 +23,8 @@ static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
//!< The system has recovered from a safe rate rotation violation.
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
extern const char* getModeStr(AcsMode mode);

132
mission/controller/AcsController.cpp
View File

@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
multipleRwUnavailableCounter{0},
parameterHelper(this),
mgmDataRaw(this),
mgmDataProcessed(this),
@ -50,6 +51,9 @@ ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
}
void AcsController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "ACS & TCS PST");
#endif
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
@ -116,10 +120,10 @@ void AcsController::performSafe() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf);
// Give desired satellite rate and sun direction to align
// give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
// if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) {
@ -137,8 +141,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
}
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{
PoolReadGuard pg(&ctrlValData);
@ -180,18 +184,15 @@ void AcsController::performSafe() {
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::performDetumble() {
@ -208,8 +209,8 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -228,10 +229,6 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY);
}
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
}
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
@ -239,18 +236,15 @@ void AcsController::performDetumble() {
actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}
void AcsController::performPointingCtrl() {
@ -270,10 +264,19 @@ void AcsController::performPointingCtrl() {
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
multipleRwUnavailableCounter++;
if (multipleRwUnavailableCounter > 4) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
}
return;
} 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}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
double mgtDpDes[3] = {0, 0, 0};
switch (submode) {
case acs::PTG_IDLE:
@ -392,45 +395,60 @@ void AcsController::performPointingCtrl() {
}
if (enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = {
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
&(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value),
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
}
int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
}
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
// {
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// acsParameters.rwHandlingParameters.rampTime);
}
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed,
int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
uint16_t rampTime) {
{
PoolReadGuard pg(&dipoleSet);
MutexGuard mg(torquer::lazyLock());
torquer::NEW_ACTUATION_FLAG = true;
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
}
{
PoolReadGuard pg(&rw1SpeedSet);
rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
}
{
PoolReadGuard pg(&rw2SpeedSet);
rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
}
{
PoolReadGuard pg(&rw3SpeedSet);
rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
}
{
PoolReadGuard pg(&rw4SpeedSet);
rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
}
return returnvalue::OK;
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,

18
mission/controller/AcsController.h
View File

@ -5,6 +5,7 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
@ -19,6 +20,7 @@
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
#include "mission/trace.h"
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
@ -48,9 +50,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PtgCtrl ptgCtrl;
uint8_t detumbleCounter;
uint8_t multipleRwUnavailableCounter;
ParameterHelper parameterHelper;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
InternalState internalState = InternalState::STARTUP;
@ -69,11 +76,20 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive);
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
/* ACS Sensor Values */
ACS::SensorValues sensorValues;
/* ACS Datasets */
/* ACS Actuation Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);
rws::RwSpeedActuationSet rw4SpeedSet = rws::RwSpeedActuationSet(objects::RW4);
/* ACS Datasets */
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

13
mission/controller/ThermalController.cpp
View File

@ -9,10 +9,10 @@
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/RwDefinitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <objects/systemObjectList.h>
ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater)
@ -73,6 +73,9 @@ ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) {
}
void ThermalController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "TCS Task");
#endif
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
@ -703,7 +706,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl;
@ -716,7 +719,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl;
@ -729,7 +732,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl;
@ -742,7 +745,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, RwDefinitions::TEMPERATURE_C);
lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, rws::TEMPERATURE_C);
PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl;

5
mission/controller/ThermalController.h
View File

@ -11,6 +11,7 @@
#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
#include "mission/devices/HeaterHandler.h"
#include "mission/trace.h"
#include <list>
/**
@ -155,6 +156,10 @@ class ThermalController : public ExtendedControllerBase {
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(DELAY);
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
std::array<std::pair<bool, double>, 5> sensors;
uint8_t numSensors = 0;

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

@ -278,6 +278,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque);
break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.rampTime);
break;
default:
return INVALID_IDENTIFIER_ID;
}
@ -291,17 +294,17 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
parameterWrapper->set(rwMatrices.pseudoInverse);
break;
case 0x2:
parameterWrapper->set(rwMatrices.without0);
break;
case 0x3:
parameterWrapper->set(rwMatrices.without1);
break;
case 0x4:
case 0x3:
parameterWrapper->set(rwMatrices.without2);
break;
case 0x5:
case 0x4:
parameterWrapper->set(rwMatrices.without3);
break;
case 0x5:
parameterWrapper->set(rwMatrices.without4);
break;
case 0x6:
parameterWrapper->set(rwMatrices.nullspace);
break;
@ -584,6 +587,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax);
break;
case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -73,9 +73,9 @@ class AcsParameters : public HasParametersIF {
{-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}};
float mgm02variance[3] = {1, 1, 1};
float mgm13variance[3] = {1, 1, 1};
float mgm4variance[3] = {1, 1, 1};
float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
} mgmHandlingParameters;
struct SusHandlingParameters {
@ -779,19 +779,21 @@ class AcsParameters : public HasParametersIF {
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float 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
pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = true;
} gyrHandlingParameters;
struct RwHandlingParameters {
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM
double maxTrq = 0.0032; // 3.2 [mNm]
int32_t stictionSpeed = 100; // RPM
int32_t stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006;
uint16_t rampTime = 10;
} rwHandlingParameters;
struct RwMatrices {
@ -800,13 +802,13 @@ class AcsParameters : public HasParametersIF {
{0.3907, 0.3907, 0.3907, 0.3907}};
double pseudoInverse[4][3] = {
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
double without0[4][3] = {
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without1[4][3] = {
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
double without2[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
double without3[4][3] = {
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
double without4[4][3] = {
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
} rwMatrices;
@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF {
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double DipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter;
struct DetumbleParameter {

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

@ -1,10 +1,3 @@
/*
* ActuatorCmd.cpp
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#include "ActuatorCmd.h"
#include <fsfw/globalfunctions/constants.h>
@ -38,27 +31,33 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTorque, double *rwCmdSpeed) {
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) {
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
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;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
for (int i = 0; i < 4; i++) {
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
}
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
}
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) {
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
// Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuator, 3, 3, 1);
dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0;
@ -69,8 +68,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu
}
if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue;
VectorOperations<double>::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3);
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
dipolMomentActuatorDouble, 3);
}
// scale dipole from 1 Am^2 to 1e^-4 Am^2
VectorOperations<double>::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3);
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
for (int i = 0; i < 3; i++) {
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
}
}

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

@ -28,8 +28,8 @@ 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, double *rwCmdSpeed);
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,7 +37,7 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference frame
*/
void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator);
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
protected:
private:

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

@ -610,104 +610,33 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
// under 150 arcsec ??
}
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
}
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
}
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
(sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
}
else {
if (rw1valid && rw2valid && rw3valid && rw4valid) {
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));
return returnvalue::OK;
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
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));
return returnvalue::OK;
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
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.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
return returnvalue::FAILED;
}
}

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

@ -67,7 +67,7 @@ class Guidance {
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
// reation wheel maybe can be done in "commanding.h"
void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
private:
AcsParameters acsParameters;

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

@ -20,6 +20,7 @@ Igrf13Model::~Igrf13Model() {}
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) {
double magFieldModel[3] = {0, 0, 0};
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * PI / 180;
@ -100,12 +101,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double normVecMagFieldInert[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
// convert nT to uT
VectorOperations<double>::mulScalar(magFieldModelInertial, 1e-3, magFieldModelInertial, 3);
}
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {

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

@ -47,7 +47,6 @@ class Igrf13Model /*:public HasParametersIF*/ {
// Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
void schmidtNormalization();
private:

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

@ -1,13 +1,14 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_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/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
@ -62,10 +63,10 @@ class SensorValues {
// bool mgt0valid;
RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1);
RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2);
RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3);
RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4);
rws::StatusSet rw1Set = rws::StatusSet(objects::RW1);
rws::StatusSet rw2Set = rws::StatusSet(objects::RW2);
rws::StatusSet rw3Set = rws::StatusSet(objects::RW3);
rws::StatusSet rw4Set = rws::StatusSet(objects::RW4);
};
} /* namespace ACS */

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

@ -161,8 +161,14 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
double *torqueCommand) {
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
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};
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {

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

@ -54,11 +54,10 @@ class PtgCtrl {
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: rwAvailable Boolean Flag for all reaction wheels
* omegaRw current wheel speed of reaction wheels
* @param: sensorValues class containing all RW related values
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
private:
AcsParameters::RwHandlingParameters *rwHandlingParameters;

8
mission/devices/BpxBatteryHandler.cpp
View File

@ -2,8 +2,6 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "OBSWConfig.h"
BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
: DeviceHandlerBase(objectId, comIF, comCookie), hkSet(this), cfgSet(this) {}
@ -280,3 +278,9 @@ void BpxBatteryHandler::setToGoToNormalMode(bool enable) {
}
void BpxBatteryHandler::setDebugMode(bool enable) { this->debugMode = enable; }
void BpxBatteryHandler::performOperationHook() {
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "BPX BATT");
#endif
}

6
mission/devices/BpxBatteryHandler.h
View File

@ -4,6 +4,7 @@
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include "devicedefinitions/BpxBatteryDefinitions.h"
#include "mission/trace.h"
class BpxBatteryHandler : public DeviceHandlerBase {
public:
@ -24,6 +25,10 @@ class BpxBatteryHandler : public DeviceHandlerBase {
bool debugMode = false;
bool goToNormalModeImmediately = false;
uint8_t sentPingByte = BpxBattery::DEFAULT_PING_SENT_BYTE;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
BpxBatteryHk hkSet;
DeviceCommandId_t lastCmd = DeviceHandlerIF::NO_COMMAND_ID;
BpxBatteryCfg cfgSet;
@ -47,6 +52,7 @@ class BpxBatteryHandler : public DeviceHandlerBase {
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
void fillCommandAndReplyMap() override;
void performOperationHook() 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,

22
mission/devices/ImtqHandler.cpp
View File

@ -183,18 +183,20 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
}
ReturnValue_t result;
// Commands override anything which was set in the software
if (commandData != nullptr) {
dipoleSet.setValidityBufferGeneration(false);
result =
dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
} else {
{
// Read set dipole values from local pool
PoolReadGuard pg(&dipoleSet);
// Commands override anything which was set in the software
if (commandData != nullptr) {
dipoleSet.setValidityBufferGeneration(false);
result = dipoleSet.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::NETWORK);
dipoleSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
}
}
if (ACTUATION_WIRETAPPING) {
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value

440
mission/devices/RwHandler.cpp
View File

@ -2,18 +2,21 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include "OBSWConfig.h"
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
GpioIF* gpioComIF, gpioId_t enableGpio)
GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx)
: DeviceHandlerBase(objectId, comIF, comCookie),
gpioComIF(gpioComIF),
enableGpio(enableGpio),
statusSet(this),
lastResetStatusSet(this),
tmDataset(this) {
tmDataset(this),
rwSpeedActuationSet(*this),
rwIdx(rwIdx) {
if (comCookie == nullptr) {
sif::error << "RwHandler: Invalid com cookie" << std::endl;
}
@ -25,11 +28,12 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
RwHandler::~RwHandler() {}
void RwHandler::doStartUp() {
internalState = InternalState::GET_RESET_STATUS;
internalState = InternalState::DEFAULT;
if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
}
updatePeriodicReply(true, rws::REPLY_ID);
setMode(_MODE_TO_ON);
}
@ -37,28 +41,18 @@ void RwHandler::doShutDown() {
if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
}
setMode(_MODE_POWER_DOWN);
internalState = InternalState::DEFAULT;
updatePeriodicReply(false, rws::REPLY_ID);
// The power switch is handled by the assembly, so we can go off here directly.
setMode(MODE_OFF);
}
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) {
case InternalState::GET_RESET_STATUS:
*id = RwDefinitions::GET_LAST_RESET_STATUS;
internalState = InternalState::READ_TEMPERATURE;
break;
case InternalState::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
internalState = InternalState::GET_RW_SATUS;
break;
case InternalState::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
internalState = InternalState::GET_RESET_STATUS;
break;
case InternalState::CLEAR_RESET_STATUS:
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
/** After reset status is cleared, reset status will be polled again for verification */
internalState = InternalState::GET_RESET_STATUS;
case InternalState::DEFAULT: {
*id = rws::REQUEST_ID;
break;
}
default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl;
break;
@ -76,45 +70,65 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
ReturnValue_t result = returnvalue::OK;
switch (deviceCommand) {
case (RwDefinitions::RESET_MCU): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::GET_LAST_RESET_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::GET_RW_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::INIT_RW_CONTROLLER): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) {
case (rws::SET_SPEED):
case (rws::REQUEST_ID): {
if (commandData != nullptr && commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH;
}
result = checkSpeedAndRampTime(commandData, commandDataLen);
{
PoolReadGuard pg(&rwSpeedActuationSet);
// Commands override anything which was set in the software
if (commandData != nullptr) {
rwSpeedActuationSet.setValidityBufferGeneration(false);
result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::NETWORK);
rwSpeedActuationSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
}
}
if (ACTUATION_WIRETAPPING) {
int32_t speed = 0;
uint16_t rampTime = 0;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " << speed
<< " and rampTime = " << rampTime << std::endl;
}
result = checkSpeedAndRampTime();
if (result != returnvalue::OK) {
return result;
}
prepareSetSpeedCmd(commandData, commandDataLen);
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
prepareSimpleCommand(deviceCommand);
// set speed flag.
commandBuffer[0] = true;
rawPacketLen = 1;
uint8_t* currentCmdBuf = commandBuffer + 1;
rwSpeedActuationSet.serialize(&currentCmdBuf, &rawPacketLen, sizeof(commandBuffer),
SerializeIF::Endianness::MACHINE);
commandBuffer[rawPacketLen++] = static_cast<uint8_t>(rws::SpecialRwRequest::REQUEST_NONE);
rawPacket = commandBuffer;
return returnvalue::OK;
}
case (RwDefinitions::GET_TM): {
prepareSimpleCommand(deviceCommand);
case (rws::RESET_MCU): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::RESET_MCU);
internalState = InternalState::RESET_MCU;
return returnvalue::OK;
}
case (rws::INIT_RW_CONTROLLER): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::INIT_RW_CONTROLLER);
internalState = InternalState::INIT_RW_CONTROLLER;
return returnvalue::OK;
}
case (rws::GET_TM): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::GET_TM);
internalState = InternalState::GET_TM;
return returnvalue::OK;
}
default:
@ -124,159 +138,161 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
}
void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandMap(RwDefinitions::RESET_MCU);
this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
RwDefinitions::SIZE_GET_RESET_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr,
RwDefinitions::SIZE_CLEAR_RESET_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
RwDefinitions::SIZE_GET_RW_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr,
RwDefinitions::SIZE_INIT_RW);
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, nullptr,
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
RwDefinitions::SIZE_SET_SPEED_REPLY);
this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset,
RwDefinitions::SIZE_GET_TELEMETRY_REPLY);
insertInCommandMap(rws::REQUEST_ID);
insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true);
insertInCommandMap(rws::RESET_MCU);
insertInCommandMap(rws::SET_SPEED);
insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 5, nullptr, 0, false, true, rws::REPLY_ID);
insertInCommandAndReplyMap(rws::GET_TM, 5, nullptr, 0, false, true, rws::REPLY_ID);
}
ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
uint8_t replyByte = *start;
switch (replyByte) {
case (RwDefinitions::GET_LAST_RESET_STATUS): {
*foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
*foundId = RwDefinitions::GET_LAST_RESET_STATUS;
break;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
*foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS;
*foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS;
break;
}
case (RwDefinitions::GET_RW_STATUS): {
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
*foundId = RwDefinitions::GET_RW_STATUS;
break;
}
case (RwDefinitions::INIT_RW_CONTROLLER): {
*foundLen = RwDefinitions::SIZE_INIT_RW;
*foundId = RwDefinitions::INIT_RW_CONTROLLER;
break;
}
case (RwDefinitions::SET_SPEED): {
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
*foundId = RwDefinitions::SET_SPEED;
break;
}
case (RwDefinitions::GET_TEMPERATURE): {
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
*foundId = RwDefinitions::GET_TEMPERATURE;
break;
}
case (RwDefinitions::GET_TM): {
*foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY;
*foundId = RwDefinitions::GET_TM;
break;
}
default: {
sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
*foundLen = remainingSize;
return returnvalue::FAILED;
}
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
return IGNORE_FULL_PACKET;
}
if (remainingSize > 0) {
*foundLen = remainingSize;
*foundId = rws::REPLY_ID;
}
sizeOfReply = *foundLen;
return returnvalue::OK;
}
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
/** Check result code */
if (*(packet + 1) == RwDefinitions::STATE_ERROR) {
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
<< std::endl;
return EXECUTION_FAILED;
}
/** Received in little endian byte order */
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2);
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
return CRC_ERROR;
}
switch (id) {
case (RwDefinitions::GET_LAST_RESET_STATUS): {
handleResetStatusReply(packet);
break;
RwReplies replies(packet);
ReturnValue_t result = returnvalue::OK;
ReturnValue_t status;
auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) {
// This is just the packet length of the frame from the RW. The actual
// data is one more flag byte to show whether the value was read at least once.
auto packetLen = rws::idToPacketLen(id);
// arrayprinter::print(packetPtr, packetLen);
uint16_t replyCrc;
size_t dummy = 0;
SerializeAdapter::deSerialize(&replyCrc, packetPtr + packetLen - 2, &dummy,
SerializeIF::Endianness::LITTLE);
if (CRC::crc16ccitt(packetPtr, packetLen - 2) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl;
return CRC_ERROR;
}
case (RwDefinitions::GET_RW_STATUS): {
handleGetRwStatusReply(packet);
break;
if (packetPtr[1] == rws::STATE_ERROR) {
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
<< std::endl;
result = EXECUTION_FAILED;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS):
case (RwDefinitions::INIT_RW_CONTROLLER):
case (RwDefinitions::SET_SPEED):
// no reply data expected
break;
case (RwDefinitions::GET_TEMPERATURE): {
handleTemperatureReply(packet);
break;
}
case (RwDefinitions::GET_TM): {
handleGetTelemetryReply(packet);
break;
}
default: {
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
return returnvalue::OK;
};
if (replies.wasSetSpeedReplyRead()) {
status = checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply());
if (status != returnvalue::OK) {
result = status;
}
}
return returnvalue::OK;
if (replies.wasRwStatusRead()) {
status = checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply());
if (status == returnvalue::OK) {
handleGetRwStatusReply(replies.getRwStatusReply());
} else {
result = status;
}
}
if (replies.wasGetLastStatusReplyRead()) {
status = checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS,
replies.getGetLastResetStatusReply());
if (status == returnvalue::OK) {
handleResetStatusReply(replies.getGetLastResetStatusReply());
} else {
result = status;
}
}
if (replies.wasClearLastRsetStatusReplyRead()) {
status = checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS,
replies.getClearLastResetStatusReply());
if (status != returnvalue::OK) {
result = status;
}
}
if (replies.wasReadTemperatureReplySet()) {
status = checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply());
if (status == returnvalue::OK) {
handleTemperatureReply(replies.getReadTemperatureReply());
} else {
result = status;
}
}
if (internalState == InternalState::GET_TM) {
if (replies.wasHkDataReplyRead()) {
status = checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply());
if (status == returnvalue::OK) {
handleGetTelemetryReply(replies.getHkDataReply());
} else {
result = status;
}
internalState = InternalState::DEFAULT;
}
}
if (internalState == InternalState::INIT_RW_CONTROLLER) {
if (replies.wasInitRwControllerReplyRead()) {
status =
checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
if (status != returnvalue::OK) {
result = status;
}
internalState = InternalState::DEFAULT;
}
}
if (internalState == InternalState::RESET_MCU) {
internalState = InternalState::DEFAULT;
}
return result;
}
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed);
localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime);
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 5.0));
poolManager.subscribeForRegularPeriodicPacket(
@ -286,26 +302,15 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
return returnvalue::OK;
}
void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
commandBuffer[0] = static_cast<uint8_t>(id);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 3;
}
ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
int32_t speed =
*commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3);
ReturnValue_t RwHandler::checkSpeedAndRampTime() {
int32_t speed = 0;
uint16_t rampTime = 0;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED;
}
uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
if (rampTime < 10 || rampTime > 20000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
return INVALID_RAMP_TIME;
@ -314,33 +319,14 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return returnvalue::OK;
}
void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
/** Speed (0.1 RPM) */
commandBuffer[1] = *(commandData + 3);
commandBuffer[2] = *(commandData + 2);
commandBuffer[3] = *(commandData + 1);
commandBuffer[4] = *commandData;
/** Ramp time (ms) */
commandBuffer[5] = *(commandData + 5);
commandBuffer[6] = *(commandData + 4);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 9;
}
void RwHandler::handleResetStatusReply(const uint8_t* packet) {
PoolReadGuard rg(&lastResetStatusSet);
uint8_t offset = 2;
uint8_t resetStatus = packet[offset];
if (resetStatus != 0) {
internalState = InternalState::CLEAR_RESET_STATUS;
// internalState = InternalState::CLEAR_RESET_STATUS;
lastResetStatusSet.lastNonClearedResetStatus = resetStatus;
triggerEvent(RwDefinitions::RESET_OCCURED, resetStatus, 0);
triggerEvent(rws::RESET_OCCURED, resetStatus, 0);
}
lastResetStatusSet.currentResetStatus = resetStatus;
if (debugMode) {
@ -379,10 +365,10 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
statusSet.setValidity(true, true);
if (statusSet.state == RwDefinitions::STATE_ERROR) {
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(RwDefinitions::ERROR_STATE, statusSet.state.value, 0);
triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0);
sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl;
}
@ -413,6 +399,24 @@ void RwHandler::handleTemperatureReply(const uint8_t* packet) {
}
}
LocalPoolDataSetBase* RwHandler::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case (rws::SetIds::STATUS_SET_ID): {
return &statusSet;
}
case (rws::SetIds::LAST_RESET_ID): {
return &lastResetStatusSet;
}
case (rws::SetIds::SPEED_CMD_SET): {
return &rwSpeedActuationSet;
}
case (rws::SetIds::TM_SET_ID): {
return &tmDataset;
}
}
return nullptr;
}
void RwHandler::handleGetTelemetryReply(const uint8_t* packet) {
PoolReadGuard rg(&tmDataset);
uint8_t offset = 2;

64
mission/devices/RwHandler.h
View File

@ -3,12 +3,14 @@
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include <mission/devices/devicedefinitions/RwDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <string.h>
#include "events/subsystemIdRanges.h"
#include "returnvalues/classIds.h"
static constexpr bool ACTUATION_WIRETAPPING = false;
class GpioIF;
/**
@ -34,30 +36,12 @@ class RwHandler : public DeviceHandlerBase {
* to high to enable the device.
*/
RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF,
gpioId_t enableGpio);
gpioId_t enableGpio, uint8_t rwIdx);
void setDebugMode(bool enable);
virtual ~RwHandler();
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call
static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing
//! start sign 0x7E
static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid
//! substitution combination
static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3);
//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E
static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
//! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames.
static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5);
//! [EXPORT] : [COMMENT] Expected a start marker as first byte
static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6);
protected:
void doStartUp() override;
void doShutDown() override;
@ -72,6 +56,7 @@ class RwHandler : 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:
//! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in
@ -85,42 +70,57 @@ class RwHandler : public DeviceHandlerBase {
static const ReturnValue_t EXECUTION_FAILED = MAKE_RETURN_CODE(0xA3);
//! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc
static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4);
static const ReturnValue_t VALUE_NOT_READ = MAKE_RETURN_CODE(0xA5);
GpioIF* gpioComIF = nullptr;
gpioId_t enableGpio = gpio::NO_GPIO;
bool debugMode = false;
RwDefinitions::StatusSet statusSet;
RwDefinitions::LastResetSatus lastResetStatusSet;
RwDefinitions::TmDataset tmDataset;
rws::StatusSet statusSet;
rws::LastResetSatus lastResetStatusSet;
rws::TmDataset tmDataset;
rws::RwSpeedActuationSet rwSpeedActuationSet;
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
uint8_t commandBuffer[32];
uint8_t rwIdx;
enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0});
PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
InternalState internalState = InternalState::GET_RESET_STATUS;
enum class InternalState {
DEFAULT,
GET_TM,
INIT_RW_CONTROLLER,
RESET_MCU,
// GET_RESET_STATUS,
// CLEAR_RESET_STATUS,
// READ_TEMPERATURE,
// SET_SPEED,
// GET_RW_SATUS
};
size_t sizeOfReply = 0;
InternalState internalState = InternalState::DEFAULT;
/**
* @brief This function can be used to build commands which do not contain any data apart
* from the command id and the CRC.
* @param commandId The command id of the command to build.
*/
void prepareSimpleCommand(DeviceCommandId_t id);
// void prepareSimpleCommand(DeviceCommandId_t id);
/**
* @brief This function checks if the receiced speed and ramp time to set are in a valid
* range.
* @return returnvalue::OK if successful, otherwise error code.
*/
ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen);
ReturnValue_t checkSpeedAndRampTime();
/**
* @brief This function prepares the set speed command from the commandData received with
* an action message.
* @brief This function prepares the set speed command from the dataSet received with
* an action message or set in the software.
* @return returnvalue::OK if successful, otherwise error code.
*/
void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen);
// ReturnValue_t prepareSetSpeedCmd();
/**
* @brief This function writes the last reset status retrieved with the get last reset status

4
mission/devices/SolarArrayDeploymentHandler.cpp
View File

@ -11,6 +11,7 @@
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw_hal/common/gpio/GpioCookie.h"
#include "mission/trace.h"
static constexpr bool DEBUG_MODE = true;
@ -37,6 +38,9 @@ SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;
ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
using namespace std::filesystem;
#if OBSW_THREAD_TRACING == 1
trace::threadTrace(opCounter, "SA DEPL");
#endif
if (opDivider.checkAndIncrement()) {
auto activeSdc = sdcMan.getActiveSdCard();
if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and

4
mission/devices/SolarArrayDeploymentHandler.h
View File

@ -19,6 +19,7 @@
#include "fsfw/timemanager/Countdown.h"
#include "fsfw_hal/common/gpio/GpioIF.h"
#include "mission/memory/SdCardMountedIF.h"
#include "mission/trace.h"
#include "returnvalues/classIds.h"
enum DeploymentChannels : uint8_t { SA_1 = 1, SA_2 = 2 };
@ -172,6 +173,9 @@ class SolarArrayDeploymentHandler : public ExecutableObjectIF,
bool firstAutonomousCycle = true;
ActionId_t activeCmd = HasActionsIF::INVALID_ACTION_ID;
std::optional<uint64_t> initUptime;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif
PeriodicOperationDivider opDivider = PeriodicOperationDivider(5);
uint8_t retryCounter = 3;

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

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

6
mission/devices/devicedefinitions/imtqHandlerDefinitions.h
View File

@ -492,14 +492,14 @@ 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_;
}
xDipole = xDipole_;
if (yDipole.value != yDipole_) {
yDipole = yDipole_;
}
yDipole = yDipole_;
if (zDipole.value != zDipole_) {
zDipole = zDipole_;
}
zDipole = zDipole_;
currentTorqueDurationMs = currentTorqueDurationMs_;
}

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

@ -0,0 +1,54 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
#include "rwHelpers.h"
void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
size_t& encodedLen) {
encodedBuffer[0] = rws::FRAME_DELIMITER;
encodedLen = 1;
for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) {
if (sourceBuf[sourceIdx] == 0x7E) {
encodedBuffer[encodedLen++] = 0x7D;
encodedBuffer[encodedLen++] = 0x5E;
} else if (sourceBuf[sourceIdx] == 0x7D) {
encodedBuffer[encodedLen++] = 0x7D;
encodedBuffer[encodedLen++] = 0x5D;
} else {
encodedBuffer[encodedLen++] = sourceBuf[sourceIdx];
}
}
encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER;
}
size_t rws::idToPacketLen(DeviceCommandId_t id) {
switch (id) {
case (rws::GET_RW_STATUS): {
return rws::SIZE_GET_RW_STATUS;
}
case (rws::SET_SPEED): {
return rws::SIZE_SET_SPEED_REPLY;
}
case (rws::CLEAR_LAST_RESET_STATUS): {
return rws::SIZE_CLEAR_RESET_STATUS;
}
case (rws::GET_LAST_RESET_STATUS): {
return rws::SIZE_GET_RESET_STATUS;
}
case (rws::GET_TEMPERATURE): {
return rws::SIZE_GET_TEMPERATURE_REPLY;
}
case (rws::GET_TM): {
return rws::SIZE_GET_TELEMETRY_REPLY;
}
case (rws::INIT_RW_CONTROLLER): {
return rws::SIZE_INIT_RW;
}
default: {
sif::error << "no reply buffer for rw command " << id << std::endl;
return 0;
}
}
}
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */

209
mission/devices/devicedefinitions/RwDefinitions.h → mission/devices/devicedefinitions/rwHelpers.h
View File

@ -1,14 +1,60 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include "eive/resultClassIds.h"
#include "events/subsystemIdRanges.h"
#include "objects/systemObjectList.h"
namespace RwDefinitions {
namespace rws {
void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
size_t& encodedLen);
size_t idToPacketLen(DeviceCommandId_t id);
static const size_t SIZE_GET_RESET_STATUS = 5;
static const size_t SIZE_CLEAR_RESET_STATUS = 4;
static const size_t SIZE_INIT_RW = 4;
static const size_t SIZE_GET_RW_STATUS = 14;
static const size_t SIZE_SET_SPEED_REPLY = 4;
static const size_t SIZE_GET_TEMPERATURE_REPLY = 8;
/** Max size when requesting telemetry */
static const size_t SIZE_GET_TELEMETRY_REPLY = 91;
//! This is the end and start marker of the frame datalinklayer. Also called frame delimiter
//! in the NanoAvionics datasheet.
static constexpr uint8_t FRAME_DELIMITER = 0x7E;
enum class SpecialRwRequest : uint8_t {
REQUEST_NONE = 0,
RESET_MCU = 1,
INIT_RW_CONTROLLER = 2,
GET_TM = 3,
NUM_REQUESTS
};
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call
static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing
//! start sign 0x7E
static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2);
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid
//! substitution combination
static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3);
//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E
static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
//! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames.
static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5);
//! [EXPORT] : [COMMENT] Expected a start marker as first byte
static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6);
//! [EXPORT] : [COMMENT] Timeout when reading reply
static const ReturnValue_t SPI_READ_TIMEOUT = MAKE_RETURN_CODE(0xB7);
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER;
@ -17,7 +63,8 @@ static constexpr Event ERROR_STATE = MAKE_EVENT(1, severity::HIGH);
static constexpr Event RESET_OCCURED = event::makeEvent(SUBSYSTEM_ID, 2, severity::LOW);
static const uint32_t SPI_REPLY_DELAY = 70000; // us
//! Minimal delay as specified by the datasheet.
static const uint32_t SPI_REPLY_DELAY = 20000; // us
enum PoolIds : lp_id_t {
TEMPERATURE_C,
@ -51,7 +98,10 @@ enum PoolIds : lp_id_t {
SPI_BYTES_WRITTEN,
SPI_BYTES_READ,
SPI_REG_OVERRUN_ERRORS,
SPI_TOTAL_ERRORS
SPI_TOTAL_ERRORS,
RW_SPEED,
RAMP_TIME,
};
enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING };
@ -65,29 +115,28 @@ enum LastResetStatusBitPos : uint8_t {
LOW_POWER_RESET = 5
};
static const DeviceCommandId_t RESET_MCU = 1;
static const DeviceCommandId_t GET_LAST_RESET_STATUS = 2;
static const DeviceCommandId_t CLEAR_LAST_RESET_STATUS = 3;
static const DeviceCommandId_t GET_RW_STATUS = 4;
/** This command is needed to recover from error state */
static const DeviceCommandId_t INIT_RW_CONTROLLER = 5;
static const DeviceCommandId_t SET_SPEED = 6;
static const DeviceCommandId_t GET_TEMPERATURE = 8;
static const DeviceCommandId_t GET_TM = 9;
enum DeviceCommandId : DeviceCommandId_t {
RESET_MCU = 1,
static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
static const uint32_t STATUS_SET_ID = GET_RW_STATUS;
static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS;
static const uint32_t TM_SET_ID = GET_TM;
GET_LAST_RESET_STATUS = 2,
CLEAR_LAST_RESET_STATUS = 3,
GET_RW_STATUS = 4,
INIT_RW_CONTROLLER = 5,
SET_SPEED = 6,
GET_TEMPERATURE = 8,
GET_TM = 9
};
static const size_t SIZE_GET_RESET_STATUS = 5;
static const size_t SIZE_CLEAR_RESET_STATUS = 4;
static const size_t SIZE_INIT_RW = 4;
static const size_t SIZE_GET_RW_STATUS = 14;
static const size_t SIZE_SET_SPEED_REPLY = 4;
static const size_t SIZE_GET_TEMPERATURE_REPLY = 8;
/** Max size when requesting telemetry */
static const size_t SIZE_GET_TELEMETRY_REPLY = 91;
static constexpr DeviceCommandId_t REQUEST_ID = 0x77;
static constexpr DeviceCommandId_t REPLY_ID = 0x78;
enum SetIds : uint32_t {
TEMPERATURE_SET_ID = DeviceCommandId::GET_TEMPERATURE,
STATUS_SET_ID = DeviceCommandId::GET_RW_STATUS,
LAST_RESET_ID = DeviceCommandId::GET_LAST_RESET_STATUS,
TM_SET_ID = DeviceCommandId::GET_TM,
SPEED_CMD_SET = 10,
};
/** Set speed command has maximum size */
static const size_t MAX_CMD_SIZE = 9;
@ -106,9 +155,10 @@ static const uint8_t TM_SET_ENTRIES = 24;
*/
class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
public:
StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, STATUS_SET_ID) {}
StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::STATUS_SET_ID) {}
StatusSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {}
StatusSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, rws::SetIds::STATUS_SET_ID)) {}
lp_var_t<int32_t> temperatureCelcius =
lp_var_t<int32_t>(sid.objectId, PoolIds::TEMPERATURE_C, this);
@ -124,9 +174,11 @@ class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
*/
class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
public:
LastResetSatus(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LAST_RESET_ID) {}
LastResetSatus(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, rws::SetIds::LAST_RESET_ID) {}
LastResetSatus(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {}
LastResetSatus(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, rws::SetIds::LAST_RESET_ID)) {}
// If a reset occurs, the status code will be cached into this variable
lp_var_t<uint8_t> lastNonClearedResetStatus =
@ -143,9 +195,9 @@ class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
*/
class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
public:
TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TM_SET_ID) {}
TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::TM_SET_ID) {}
TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {}
TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, rws::SetIds::TM_SET_ID)) {}
lp_var_t<uint8_t> lastResetStatus =
lp_var_t<uint8_t>(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this);
@ -192,6 +244,95 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
};
} // namespace RwDefinitions
class RwSpeedActuationSet : public StaticLocalDataSet<2> {
friend class RwHandler;
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */
public:
RwSpeedActuationSet(HasLocalDataPoolIF& owner)
: StaticLocalDataSet(&owner, rws::SetIds::SPEED_CMD_SET) {}
RwSpeedActuationSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, rws::SetIds::SPEED_CMD_SET)) {}
void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) {
if (rwSpeed.value != rwSpeed_) {
rwSpeed = rwSpeed_;
}
if (rampTime.value != rampTime_) {
rampTime = rampTime_;
}
}
void getRwSpeed(int32_t& rwSpeed_, uint16_t& rampTime_) {
rwSpeed_ = rwSpeed.value;
rampTime_ = rampTime.value;
}
private:
lp_var_t<int32_t> rwSpeed = lp_var_t<int32_t>(sid.objectId, rws::PoolIds::RW_SPEED, this);
lp_var_t<uint16_t> rampTime = lp_var_t<uint16_t>(sid.objectId, rws::PoolIds::RAMP_TIME, this);
};
} // namespace rws
/**
* Raw pointer overlay to hold the different frames received from the reaction
* wheel in a raw buffer and send them to the device handler.
*/
struct RwReplies {
friend class RwPollingTask;
public:
RwReplies(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) { initPointers(); }
const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply + 1; }
bool wasClearLastRsetStatusReplyRead() const { return clearLastResetStatusReply[0]; }
const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply + 1; }
bool wasGetLastStatusReplyRead() const { return getLastResetStatusReply[0]; }
const uint8_t* getHkDataReply() const { return hkDataReply + 1; }
bool wasHkDataReplyRead() const { return hkDataReply[0]; }
const uint8_t* getInitRwControllerReply() const { return initRwControllerReply + 1; }
bool wasInitRwControllerReplyRead() const { return initRwControllerReply[0]; }
const uint8_t* getRawData() const { return rawData; }
const uint8_t* getReadTemperatureReply() const { return readTemperatureReply + 1; }
bool wasReadTemperatureReplySet() const { return readTemperatureReply[0]; }
const uint8_t* getRwStatusReply() const { return rwStatusReply + 1; }
bool wasRwStatusRead() const { return rwStatusReply[0]; }
const uint8_t* getSetSpeedReply() const { return setSpeedReply + 1; }
bool wasSetSpeedReplyRead() const { return setSpeedReply[0]; }
private:
RwReplies(uint8_t* rwData) : rawData(rwData) { initPointers(); }
/**
* The first byte of the reply buffers contains a flag which shows whether that
* frame was read from the reaction wheel at least once.
*/
void initPointers() {
rwStatusReply = rawData;
setSpeedReply = rawData + rws::SIZE_GET_RW_STATUS + 1;
getLastResetStatusReply = setSpeedReply + rws::SIZE_SET_SPEED_REPLY + 1;
clearLastResetStatusReply = getLastResetStatusReply + rws::SIZE_GET_RESET_STATUS + 1;
readTemperatureReply = clearLastResetStatusReply + rws::SIZE_CLEAR_RESET_STATUS + 1;
hkDataReply = readTemperatureReply + rws::SIZE_GET_TEMPERATURE_REPLY + 1;
initRwControllerReply = hkDataReply + rws::SIZE_GET_TELEMETRY_REPLY + 1;
dummyPointer = initRwControllerReply + rws::SIZE_INIT_RW + 1;
}
uint8_t* rawData;
uint8_t* rwStatusReply;
uint8_t* setSpeedReply;
uint8_t* getLastResetStatusReply;
uint8_t* clearLastResetStatusReply;
uint8_t* readTemperatureReply;
uint8_t* hkDataReply;
uint8_t* initRwControllerReply;
uint8_t* dummyPointer;
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */

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

@ -42,6 +42,11 @@ ReturnValue_t AcsSubsystem::initialize() {
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;
}
return Subsystem::initialize();
}
@ -64,12 +69,13 @@ void AcsSubsystem::handleEventMessages() {
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
}
}
if (event.getEvent() == acs::SAFE_RATE_RECOVERY) {
if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
event.getEvent() == acs::MULTIPLE_RW_INVALID) {
CommandMessage msg;
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
if (result != returnvalue::OK) {
sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl;
sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl;
}
}
break;

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

@ -167,7 +167,12 @@ bool RwAssembly::isUseable(object_id_t object, Mode_t mode) {
return false;
}
ReturnValue_t RwAssembly::initialize() { return SubsystemBase::initialize(); }
ReturnValue_t RwAssembly::initialize() {
for (auto objId : helper.rwIds) {
updateChildModeByObjId(objId, MODE_OFF, SUBMODE_NONE);
}
return AssemblyBase::initialize();
}
void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) {
if (targetMode == MODE_OFF) {

10
mission/trace.cpp Normal file
View File

@ -0,0 +1,10 @@
#include "trace.h"
#include "fsfw/serviceinterface.h"
void trace::threadTrace(uint32_t& counter, const char* name, unsigned div) {
counter++;
if (counter % div == 0) {
sif::debug << name << " running" << std::endl;
}
}

14
mission/trace.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef MISSION_TRACE_H_
#define MISSION_TRACE_H_
#include <cstdint>
#define OBSW_THREAD_TRACING 0
namespace trace {
void threadTrace(uint32_t& counter, const char* name, unsigned div = 5);
}
#endif /* MISSION_TRACE_H_ */