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

Merge pull request 'Enable ACS Controller to command RWs' (#375) from eggert/rw-cmd-acs-ctrl into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source 
Reviewed-on: #375
This commit is contained in:
Robin Müller 2023-02-14 18:53:01 +01:00
commit 1553900145
15 changed files with 384 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
CHANGELOG.md
View File

@ -17,8 +17,19 @@ change warranting a new major release:
# [unreleased]
## Added
- Function for the ACS controller to command MTQ and RWs called by all subroutines
- RwHandler now handles commanding of RW speeds via RwSpeedActuationSet
- Tracing supports which allows checking whether threads are running as usual.
## Changed
- Remove 2 TCS threads.
- Move low level polling into ACS PST, move high level device handlers into TCS system task.
- ActCmds now returns command vectors as integers as required by the actuators
and scales them to the appropriate range
- All RwHandler are now polled five times per ACS cycle
- Remove 2 TCS threads. Move low level polling into ACS PST, move high level device handlers into
TCS system task.
- Further reduce number of threads:
@ -27,10 +38,6 @@ change warranting a new major release:
3. Group all other components into PUS medium priority task
4. Add SCEX device handler to PL task, remove dedicated thread
## Added
- Tracing supports which allows checking whether threads are running as usual.
## Removed
- lwgps dependency not compiled anymore, is not used

4
bsp_q7s/core/ObjectFactory.cpp
View File

@ -683,8 +683,8 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF,
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED,
&rwSpiCallback::spiCallback, nullptr);
auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF,
rwGpioIds[idx]);
rwCookies[idx]->setCallbackArgs(rws[idx]);
rwGpioIds[idx], idx);
rwCookies[idx]->setCallbackArgs(rwHandler);
#if OBSW_TEST_RW == 1
rws[idx]->setStartUpImmediately();
#endif

134
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
View File

@ -241,7 +241,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
@ -294,7 +293,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
}
}
// SUS: 16 ms
bool addSus0 = true;
bool addSus1 = true;
bool addSus2 = true;
@ -442,7 +440,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -465,7 +462,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -488,7 +484,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -511,7 +506,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -523,6 +517,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -533,7 +528,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -556,7 +550,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
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);
@ -640,6 +633,129 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
}
if (cfg.scheduleRws) {
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
//
// thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
// thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
}
if (cfg.scheduleRws) {
// this is the torquing cycle
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
@ -685,6 +801,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
}
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0);
return returnvalue::OK;
}

116
mission/controller/AcsController.cpp
View File

@ -119,10 +119,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) {
@ -140,8 +140,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);
@ -183,18 +183,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() {
@ -211,8 +208,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) <
@ -231,10 +228,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) {
@ -242,18 +235,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() {
@ -276,7 +266,7 @@ void AcsController::performPointingCtrl() {
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
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:
@ -396,44 +386,62 @@ 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)};
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);
}
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,

12
mission/controller/AcsController.h
View File

@ -17,6 +17,7 @@
#include "eive/objects.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
#include "mission/trace.h"
@ -74,11 +75,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);
RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1);
RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2);
RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3);
RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4);
/* ACS Datasets */
// MGMs
acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);

6
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;
}
@ -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;
}

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

@ -792,6 +792,8 @@ class AcsParameters : public HasParametersIF {
double stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006;
uint16_t rampTime = 10;
} rwHandlingParameters;
struct 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 {

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

@ -38,27 +38,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 +75,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:

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

75
mission/devices/RwHandler.cpp
View File

@ -7,13 +7,15 @@
#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;
}
@ -42,6 +44,10 @@ void RwHandler::doShutDown() {
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) {
case InternalState::SET_SPEED:
*id = RwDefinitions::SET_SPEED;
internalState = InternalState::GET_RESET_STATUS;
break;
case InternalState::GET_RESET_STATUS:
*id = RwDefinitions::GET_LAST_RESET_STATUS;
internalState = InternalState::READ_TEMPERATURE;
@ -52,7 +58,7 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
break;
case InternalState::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
internalState = InternalState::GET_RESET_STATUS;
internalState = InternalState::CLEAR_RESET_STATUS;
break;
case InternalState::CLEAR_RESET_STATUS:
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
@ -97,16 +103,37 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
return returnvalue::OK;
}
case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) {
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);
result = prepareSetSpeedCmd();
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
@ -243,6 +270,9 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed);
localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime);
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
@ -295,17 +325,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
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,23 +342,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return returnvalue::OK;
}
void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) {
ReturnValue_t RwHandler::prepareSetSpeedCmd() {
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);
uint8_t* serPtr = commandBuffer + 1;
size_t serSize = 1;
rwSpeedActuationSet.setValidityBufferGeneration(false);
ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
SerializeIF::Endianness::LITTLE);
rwSpeedActuationSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
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);
commandBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 9;
return result;
}
void RwHandler::handleResetStatusReply(const uint8_t* packet) {

26
mission/devices/RwHandler.h
View File

@ -9,6 +9,8 @@
#include "events/subsystemIdRanges.h"
#include "returnvalues/classIds.h"
static constexpr bool ACTUATION_WIRETAPPING = false;
class GpioIF;
/**
@ -34,7 +36,7 @@ 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);
@ -93,10 +95,21 @@ class RwHandler : public DeviceHandlerBase {
RwDefinitions::StatusSet statusSet;
RwDefinitions::LastResetSatus lastResetStatusSet;
RwDefinitions::TmDataset tmDataset;
RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
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});
enum class InternalState {
GET_RESET_STATUS,
CLEAR_RESET_STATUS,
READ_TEMPERATURE,
SET_SPEED,
GET_RW_SATUS
};
InternalState internalState = InternalState::GET_RESET_STATUS;
@ -114,13 +127,14 @@ class RwHandler : public DeviceHandlerBase {
* 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

64
mission/devices/devicedefinitions/RwDefinitions.h
View File

@ -51,7 +51,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 };
@ -75,10 +78,13 @@ static const DeviceCommandId_t SET_SPEED = 6;
static const DeviceCommandId_t GET_TEMPERATURE = 8;
static const DeviceCommandId_t GET_TM = 9;
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;
enum SetIds : uint32_t {
TEMPERATURE_SET_ID = GET_TEMPERATURE,
STATUS_SET_ID = GET_RW_STATUS,
LAST_RESET_ID = GET_LAST_RESET_STATUS,
TM_SET_ID = GET_TM,
SPEED_CMD_SET = 10,
};
static const size_t SIZE_GET_RESET_STATUS = 5;
static const size_t SIZE_CLEAR_RESET_STATUS = 4;
@ -106,9 +112,11 @@ 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, RwDefinitions::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, RwDefinitions::SetIds::STATUS_SET_ID)) {}
lp_var_t<int32_t> temperatureCelcius =
lp_var_t<int32_t>(sid.objectId, PoolIds::TEMPERATURE_C, this);
@ -124,9 +132,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, RwDefinitions::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, RwDefinitions::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 +153,11 @@ 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, RwDefinitions::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, RwDefinitions::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 +204,36 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
};
class RwSpeedActuationSet : public StaticLocalDataSet<2> {
friend class RwHandler;
public:
RwSpeedActuationSet(HasLocalDataPoolIF& owner)
: StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {}
RwSpeedActuationSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, RwDefinitions::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, RwDefinitions::PoolIds::RW_SPEED, this);
lp_var_t<uint16_t> rampTime =
lp_var_t<uint16_t>(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this);
};
} // namespace RwDefinitions
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */

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_;
}

2
tmtc

@ -1 +1 @@
Subproject commit 20e107c7ae373e7f36fca68957dbc36f4dd76f3b
Subproject commit 1e143ea6faa608baf3118512416f5a495dbd606c