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] # [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 ## 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 - Remove 2 TCS threads. Move low level polling into ACS PST, move high level device handlers into
TCS system task. TCS system task.
- Further reduce number of threads: - 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 3. Group all other components into PUS medium priority task
4. Add SCEX device handler to PL task, remove dedicated thread 4. Add SCEX device handler to PL task, remove dedicated thread
## Added
- Tracing supports which allows checking whether threads are running as usual.
## Removed ## Removed
- lwgps dependency not compiled anymore, is not used - 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, RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED,
&rwSpiCallback::spiCallback, nullptr); &rwSpiCallback::spiCallback, nullptr);
auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF,
rwGpioIds[idx]); rwGpioIds[idx], idx);
rwCookies[idx]->setCallbackArgs(rws[idx]); rwCookies[idx]->setCallbackArgs(rwHandler);
#if OBSW_TEST_RW == 1 #if OBSW_TEST_RW == 1
rws[idx]->setStartUpImmediately(); rws[idx]->setStartUpImmediately();
#endif #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, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
if (enableBside) { if (enableBside) {
// B side // B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, 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 // SUS: 16 ms
bool addSus0 = true; bool addSus0 = true;
bool addSus1 = true; bool addSus1 = true;
bool addSus2 = 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, thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus6) { if (addSus6) {
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); 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, thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus7) { if (addSus7) {
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); 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, thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus8) { if (addSus8) {
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); 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, thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus9) { if (addSus9) {
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
@ -523,6 +517,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE); 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, thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus10) { if (addSus10) {
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0, thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); 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, thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
} }
if (addSus11) { if (addSus11) {
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0, thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
@ -640,6 +633,129 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
} }
if (cfg.scheduleRws) { 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, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, 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, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0); thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0);
return returnvalue::OK; return returnvalue::OK;
} }

116
mission/controller/AcsController.cpp
View File

@ -119,10 +119,10 @@ void AcsController::performSafe() {
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
&mekfData, &validMekf); &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}; double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working // if MEKF is working
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
bool magMomMtqValid = false; bool magMomMtqValid = false;
if (validMekf == returnvalue::OK) { if (validMekf == returnvalue::OK) {
@ -140,8 +140,8 @@ void AcsController::performSafe() {
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
} }
double dipolCmdUnits[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
{ {
PoolReadGuard pg(&ctrlValData); PoolReadGuard pg(&ctrlValData);
@ -183,18 +183,15 @@ void AcsController::performSafe() {
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); 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.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// torquer::NEW_ACTUATION_FLAG = true; // acsParameters.rwHandlingParameters.rampTime);
// dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
// torqueDuration);
// }
} }
void AcsController::performDetumble() { void AcsController::performDetumble() {
@ -211,8 +208,8 @@ void AcsController::performDetumble() {
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
if (mekfData.satRotRateMekf.isValid() && if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) < VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -231,10 +228,6 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); 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); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
@ -242,18 +235,15 @@ void AcsController::performDetumble() {
actuatorCmdData.rwTargetTorque.setValid(false); actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false); 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.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false); actuatorCmdData.setValidity(true, false);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// torquer::NEW_ACTUATION_FLAG = true; // acsParameters.rwHandlingParameters.rampTime);
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
// torqueDuration);
// }
} }
void AcsController::performPointingCtrl() { void AcsController::performPointingCtrl() {
@ -276,7 +266,7 @@ void AcsController::performPointingCtrl() {
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 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 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) { switch (submode) {
case acs::PTG_IDLE: case acs::PTG_IDLE:
@ -396,44 +386,62 @@ void AcsController::performPointingCtrl() {
if (enableAntiStiction) { if (enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
int32_t rwSpeed[4] = { int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value};
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
} }
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
&(sensorValues.rw2Set.currSpeed.value), sensorValues.rw2Set.currSpeed.value,
&(sensorValues.rw3Set.currSpeed.value), sensorValues.rw3Set.currSpeed.value,
&(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
for (int i = 0; i < 3; ++i) {
cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
}
int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
for (int i = 0; i < 4; ++i) {
cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
}
{ {
PoolReadGuard pg(&actuatorCmdData); PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true); actuatorCmdData.setValidity(true, true);
} }
} }
// {
// PoolReadGuard pg(&dipoleSet); // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// MutexGuard mg(torquer::lazyLock()); // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// torquer::NEW_ACTUATION_FLAG = true; // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
// dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], // acsParameters.rwHandlingParameters.rampTime);
// torqueDuration); }
// }
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, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,

12
mission/controller/AcsController.h
View File

@ -17,6 +17,7 @@
#include "eive/objects.h" #include "eive/objects.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/RwDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
#include "mission/trace.h" #include "mission/trace.h"
@ -74,11 +75,20 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void modeChanged(Mode_t mode, Submode_t submode); void modeChanged(Mode_t mode, Submode_t submode);
void announceMode(bool recursive); 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 Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;
/* ACS Datasets */ /* ACS Actuation Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); 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 // MGMs
acsctrl::MgmDataRaw mgmDataRaw; acsctrl::MgmDataRaw mgmDataRaw;
PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3); 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: case 0x4:
parameterWrapper->set(rwHandlingParameters.stictionTorque); parameterWrapper->set(rwHandlingParameters.stictionTorque);
break; break;
case 0x5:
parameterWrapper->set(rwHandlingParameters.rampTime);
break;
default: default:
return INVALID_IDENTIFIER_ID; return INVALID_IDENTIFIER_ID;
} }
@ -584,6 +587,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x5: case 0x5:
parameterWrapper->set(magnetorquesParameter.DipolMax); parameterWrapper->set(magnetorquesParameter.DipolMax);
break; break;
case 0x6:
parameterWrapper->set(magnetorquesParameter.torqueDuration);
break;
default: default:
return INVALID_IDENTIFIER_ID; 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 stictionSpeed = 100; // 80; // RPM
double stictionReleaseSpeed = 120; // RPM double stictionReleaseSpeed = 120; // RPM
double stictionTorque = 0.0006; double stictionTorque = 0.0006;
uint16_t rampTime = 10;
} rwHandlingParameters; } rwHandlingParameters;
struct RwMatrices { struct RwMatrices {
@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF {
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
double DipolMax = 0.2; // [Am^2] double DipolMax = 0.2; // [Am^2]
uint16_t torqueDuration = 300; // [ms]
} magnetorquesParameter; } magnetorquesParameter;
struct DetumbleParameter { 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, void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, const int32_t speedRw2, const int32_t speedRw3,
const double *rwTorque, double *rwCmdSpeed) { const double *rwTorque, int32_t *rwCmdSpeed) {
using namespace Math; using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel // 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 deltaSpeed[4] = {0, 0, 0, 0};
double commandTime = acsParameters.onBoardParams.sampleTime, double commandTime = acsParameters.onBoardParams.sampleTime,
inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel; inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s] // W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm; double factor = commandTime / inertiaWheel * radToRpm;
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4); 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 // Convert to actuator frame
double dipolMomentActuatorDouble[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment, MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
dipolMoment, dipolMomentActuator, 3, 3, 1); dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
// Scaling along largest element if dipol exceeds maximum // Scaling along largest element if dipol exceeds maximum
double maxDipol = acsParameters.magnetorquesParameter.DipolMax; double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
double maxValue = 0; double maxValue = 0;
@ -69,8 +75,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu
} }
if (maxValue > maxDipol) { if (maxValue > maxDipol) {
double scalingFactor = maxDipol / maxValue; 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 // 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 * rwCmdSpeed output revolutions per minute for every
* reaction wheel * reaction wheel
*/ */
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed); const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
/* /*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
@ -37,7 +37,7 @@ class ActuatorCmd {
* @param: dipolMoment given dipol moment in spacecraft frame * @param: dipolMoment given dipol moment in spacecraft frame
* dipolMomentActuator resulting dipol moment in actuator reference 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: protected:
private: private:

12
mission/devices/ImtqHandler.cpp
View File

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

75
mission/devices/RwHandler.cpp
View File

@ -7,13 +7,15 @@
#include "OBSWConfig.h" #include "OBSWConfig.h"
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, 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), : DeviceHandlerBase(objectId, comIF, comCookie),
gpioComIF(gpioComIF), gpioComIF(gpioComIF),
enableGpio(enableGpio), enableGpio(enableGpio),
statusSet(this), statusSet(this),
lastResetStatusSet(this), lastResetStatusSet(this),
tmDataset(this) { tmDataset(this),
rwSpeedActuationSet(*this),
rwIdx(rwIdx) {
if (comCookie == nullptr) { if (comCookie == nullptr) {
sif::error << "RwHandler: Invalid com cookie" << std::endl; sif::error << "RwHandler: Invalid com cookie" << std::endl;
} }
@ -42,6 +44,10 @@ void RwHandler::doShutDown() {
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) { switch (internalState) {
case InternalState::SET_SPEED:
*id = RwDefinitions::SET_SPEED;
internalState = InternalState::GET_RESET_STATUS;
break;
case InternalState::GET_RESET_STATUS: case InternalState::GET_RESET_STATUS:
*id = RwDefinitions::GET_LAST_RESET_STATUS; *id = RwDefinitions::GET_LAST_RESET_STATUS;
internalState = InternalState::READ_TEMPERATURE; internalState = InternalState::READ_TEMPERATURE;
@ -52,7 +58,7 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
break; break;
case InternalState::GET_RW_SATUS: case InternalState::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS; *id = RwDefinitions::GET_RW_STATUS;
internalState = InternalState::GET_RESET_STATUS; internalState = InternalState::CLEAR_RESET_STATUS;
break; break;
case InternalState::CLEAR_RESET_STATUS: case InternalState::CLEAR_RESET_STATUS:
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS; *id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
@ -97,16 +103,37 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
return returnvalue::OK; return returnvalue::OK;
} }
case (RwDefinitions::SET_SPEED): { case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) { if (commandData != nullptr && commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl; << " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH; 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) { if (result != returnvalue::OK) {
return result; return result;
} }
prepareSetSpeedCmd(commandData, commandDataLen); }
}
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;
}
result = prepareSetSpeedCmd();
return result; return result;
} }
case (RwDefinitions::GET_TEMPERATURE): { 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, ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) { 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::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, 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; rawPacketLen = 3;
} }
ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t RwHandler::checkSpeedAndRampTime() {
int32_t speed = int32_t speed = 0;
*commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3); uint16_t rampTime = 0;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) { if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl; sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED; return INVALID_SPEED;
} }
uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
if (rampTime < 10 || rampTime > 20000) { if (rampTime < 10 || rampTime > 20000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl; sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
return INVALID_RAMP_TIME; return INVALID_RAMP_TIME;
@ -314,23 +342,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return returnvalue::OK; 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); commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
uint8_t* serPtr = commandBuffer + 1;
/** Speed (0.1 RPM) */ size_t serSize = 1;
commandBuffer[1] = *(commandData + 3); rwSpeedActuationSet.setValidityBufferGeneration(false);
commandBuffer[2] = *(commandData + 2); ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
commandBuffer[3] = *(commandData + 1); SerializeIF::Endianness::LITTLE);
commandBuffer[4] = *commandData; rwSpeedActuationSet.setValidityBufferGeneration(true);
/** Ramp time (ms) */ if (result != returnvalue::OK) {
commandBuffer[5] = *(commandData + 5); return result;
commandBuffer[6] = *(commandData + 4); }
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF); uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF); 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; rawPacket = commandBuffer;
rawPacketLen = 9; rawPacketLen = 9;
return result;
} }
void RwHandler::handleResetStatusReply(const uint8_t* packet) { void RwHandler::handleResetStatusReply(const uint8_t* packet) {

26
mission/devices/RwHandler.h
View File

@ -9,6 +9,8 @@
#include "events/subsystemIdRanges.h" #include "events/subsystemIdRanges.h"
#include "returnvalues/classIds.h" #include "returnvalues/classIds.h"
static constexpr bool ACTUATION_WIRETAPPING = false;
class GpioIF; class GpioIF;
/** /**
@ -34,7 +36,7 @@ class RwHandler : public DeviceHandlerBase {
* to high to enable the device. * to high to enable the device.
*/ */
RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF, 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); void setDebugMode(bool enable);
@ -93,10 +95,21 @@ class RwHandler : public DeviceHandlerBase {
RwDefinitions::StatusSet statusSet; RwDefinitions::StatusSet statusSet;
RwDefinitions::LastResetSatus lastResetStatusSet; RwDefinitions::LastResetSatus lastResetStatusSet;
RwDefinitions::TmDataset tmDataset; RwDefinitions::TmDataset tmDataset;
RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; 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; InternalState internalState = InternalState::GET_RESET_STATUS;
@ -114,13 +127,14 @@ class RwHandler : public DeviceHandlerBase {
* range. * range.
* @return returnvalue::OK if successful, otherwise error code. * @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 * @brief This function prepares the set speed command from the dataSet received with
* an action message. * 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 * @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_WRITTEN,
SPI_BYTES_READ, SPI_BYTES_READ,
SPI_REG_OVERRUN_ERRORS, 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 }; 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_TEMPERATURE = 8;
static const DeviceCommandId_t GET_TM = 9; static const DeviceCommandId_t GET_TM = 9;
static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; enum SetIds : uint32_t {
static const uint32_t STATUS_SET_ID = GET_RW_STATUS; TEMPERATURE_SET_ID = GET_TEMPERATURE,
static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS; STATUS_SET_ID = GET_RW_STATUS,
static const uint32_t TM_SET_ID = GET_TM; 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_GET_RESET_STATUS = 5;
static const size_t SIZE_CLEAR_RESET_STATUS = 4; 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> { class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
public: 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> temperatureCelcius =
lp_var_t<int32_t>(sid.objectId, PoolIds::TEMPERATURE_C, this); 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> { class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
public: 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 // If a reset occurs, the status code will be cached into this variable
lp_var_t<uint8_t> lastNonClearedResetStatus = lp_var_t<uint8_t> lastNonClearedResetStatus =
@ -143,9 +153,11 @@ class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
*/ */
class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> { class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
public: 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> lastResetStatus =
lp_var_t<uint8_t>(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); 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); 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 } // namespace RwDefinitions
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */ #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_, void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
uint16_t currentTorqueDurationMs_) { uint16_t currentTorqueDurationMs_) {
if (xDipole.value != xDipole_) { if (xDipole.value != xDipole_) {
}
xDipole = xDipole_; xDipole = xDipole_;
}
if (yDipole.value != yDipole_) { if (yDipole.value != yDipole_) {
}
yDipole = yDipole_; yDipole = yDipole_;
if (zDipole.value != zDipole_) {
} }
if (zDipole.value != zDipole_) {
zDipole = zDipole_; zDipole = zDipole_;
}
currentTorqueDurationMs = currentTorqueDurationMs_; currentTorqueDurationMs = currentTorqueDurationMs_;
} }

2
tmtc

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