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

Merge branch 'develop' into eggert/acs-ctrl-action-cmds

Browse Source
This commit is contained in:
Marius Eggert
2023-02-24 09:18:25 +01:00
parent 89518a035a 104d9647fe
commit 7392012a00
45 changed files with 852 additions and 475 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mission/comDefs.h
View File

@ -24,7 +24,7 @@ enum class CcsdsSubmode : uint8_t {
DATARATE_HIGH = 2,
DATARATE_DEFAULT = 3
};
enum class ParameterId : uint8_t { DATARATE = 0 };
enum class ParameterId : uint8_t { DATARATE = 0, TRANSMITTER_TIMEOUT = 1 };
} // namespace com

78
mission/controller/AcsController.cpp
View File

@ -452,64 +452,52 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
return returnvalue::OK;
}
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
}
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]) {
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
const int32_t *rwTargetSpeed,
const int16_t *mtqTargetDipole) {
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
void AcsController::updateCtrlValData(double errAng) {
double unitQuat[4] = {0, 0, 0, 1};
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.tgtQuat.setValid(false);
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.errQuat.setValid(false);
ctrlValData.errAng.value = errAng;
ctrlValData.errAng.setValid(true);
ctrlValData.setValidity(true, false);
}
}
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(true, true);
}
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) {
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(true, true);
}
}
void AcsController::disableCtrlValData() {
double unitQuat[4] = {0, 0, 0, 1};
double errAng = 0;
{
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
ctrlValData.setValidity(false, true);
}
PoolReadGuard pg(&ctrlValData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
ctrlValData.errAng.value = 0;
ctrlValData.setValidity(false, true);
}
}

12
mission/controller/AcsController.h
View File

@ -40,6 +40,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl();
private:
static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
AcsParameters acsParameters;
SensorProcessing sensorProcessing;
Navigation navigation;
@ -94,11 +98,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
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);
void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(double errAng);
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng);
void disableCtrlValData();
/* ACS Sensor Values */

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

@ -111,8 +111,7 @@ void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double ve
if (sightAngleSun < critSightAngle) {
strBlindAvoidFlag = true;
}
}
else {
} else {
if (sightAngleSun < blindEnd * exclAngle) {
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
double blindRefRate[3] = {0, 0, 0};

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

@ -1098,7 +1098,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
}
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus) {
MekfStatus mekfStatus) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {
@ -1114,9 +1114,8 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
}
}
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData,
MekfStatus mekfStatus, double quat[4],
double satRotRate[3]) {
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
double quat[4], double satRotRate[3]) {
{
PoolReadGuard pg(mekfData);
if (pg.getReadResult() == returnvalue::OK) {

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

@ -110,7 +110,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;

3
mission/core/CMakeLists.txt
View File

@ -1 +1,2 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp
pollingSeqTables.cpp)

665
mission/core/pollingSeqTables.cpp Normal file
View File

@ -0,0 +1,665 @@
#include "pollingSeqTables.h"
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/objectmanager/ObjectManagerIF.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/FixedTimeslotTaskIF.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include "OBSWConfig.h"
#include "eive/definitions.h"
#include "mission/devices/devicedefinitions/Max31865Definitions.h"
#ifndef RPI_TEST_ADIS16507
#define RPI_TEST_ADIS16507 0
#endif
#ifndef RPI_TEST_GPS_HANDLER
#define RPI_TEST_GPS_HANDLER 0
#endif
ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs();
#if OBSW_ADD_SYRLINKS == 1
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
static_cast<void>(length);
#if OBSW_ADD_PL_PCDU == 1
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
#endif
#if OBSW_ADD_RAD_SENSORS == 1
/* Radiation sensor */
thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
return thisSequence->checkSequence();
}
// I don't think this needs to be in a PST because linux takes care of bus serialization, but
// keep it like this for now, it works
ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
// Length of a communication cycle
uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length);
#if OBSW_ADD_BPX_BATTERY_HANDLER == 1
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::GET_READ);
#endif
// These are actually part of another bus, but this works, so keep it like this for now
#if OBSW_ADD_TMP_DEVICES == 1
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.4,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.4, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.4, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_0, length * 0.4, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_TCS_1, length * 0.4, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, DeviceHandlerIF::GET_READ);
// damaged
/*
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, DeviceHandlerIF::GET_READ);
*/
thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4, DeviceHandlerIF::GET_READ);
#endif
static_cast<void>(length);
return thisSequence->checkSequence();
}
ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs();
// PCDU handlers receives two messages and both must be handled
thisSequence->addSlot(objects::PCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::P60DOCK_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
if (thisSequence->checkSequence() != returnvalue::OK) {
sif::error << "GomSpace PST initialization failed" << std::endl;
return returnvalue::FAILED;
}
static_cast<void>(length);
return returnvalue::OK;
}
ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
/* Length of a communication cycle */
uint32_t length = thisSequence->getPeriodMs();
bool notEmpty = false;
#if RPI_TEST_ADIS16507 == 1
notEmpty = true;
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
#endif
#if RPI_TEST_GPS_HANDLER == 1
notEmpty = true;
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.5, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
#endif
static_cast<void>(length);
if (not notEmpty) {
return returnvalue::FAILED;
}
if (thisSequence->checkSequence() != returnvalue::OK) {
sif::error << "Test PST initialization failed" << std::endl;
return returnvalue::FAILED;
}
return returnvalue::OK;
}
ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
/* Length of a communication cycle */
uint32_t length = thisSequence->getPeriodMs();
bool enableAside = true;
bool enableBside = true;
if (cfg.scheduleAcsBoard) {
if (enableAside) {
// A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
}
if (enableBside) {
// B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ);
}
}
// SUS: 16 ms
bool addSus0 = true;
bool addSus1 = true;
bool addSus2 = true;
bool addSus3 = true;
bool addSus4 = true;
bool addSus5 = true;
bool addSus6 = true;
bool addSus7 = true;
bool addSus8 = true;
bool addSus9 = true;
bool addSus10 = true;
bool addSus11 = true;
if (cfg.scheduleSus) {
if (addSus0) {
/* Write setup */
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus1) {
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus2) {
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus3) {
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus4) {
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus5) {
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus6) {
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus7) {
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus8) {
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus9) {
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus10) {
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
if (addSus11) {
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
}
}
if (cfg.scheduleStr) {
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
}
if (cfg.scheduleImtq) {
// This is the MTM measurement cycle
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::DHB_OP);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::START_MEASURE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
imtq::ComStep::START_MEASURE_GET);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
imtq::ComStep::READ_MEASURE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
imtq::ComStep::READ_MEASURE_GET);
}
thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0);
if (cfg.scheduleImtq) {
// This is the torquing cycle.
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
imtq::ComStep::START_ACTUATE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
imtq::ComStep::START_ACTUATE_GET);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
imtq::ComStep::READ_ACTUATE_SEND);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
imtq::ComStep::READ_ACTUATE_GET);
}
if (cfg.scheduleRws) {
// this is the torquing cycle
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
DeviceHandlerIF::GET_READ);
}
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0);
return returnvalue::OK;
}

64
mission/core/pollingSeqTables.h Normal file
View File

@ -0,0 +1,64 @@
#ifndef POLLINGSEQUENCEFACTORY_H_
#define POLLINGSEQUENCEFACTORY_H_
#include "OBSWConfig.h"
#if defined(RASPBERRY_PI)
#include "rpiConfig.h"
#elif defined(XIPHOS_Q7S)
#include "q7sConfig.h"
#endif
#include "fsfw/returnvalues/returnvalue.h"
class FixedTimeslotTaskIF;
/**
* All device handlers are scheduled by adding them into
* Polling Sequence Tables (PST) to satisfy stricter timing requirements of
* device communication, a device handler has four different communication steps:
* 1. DeviceHandlerIF::SEND_WRITE -> Send write via interface
* 2. DeviceHandlerIF::GET_WRITE -> Get confirmation for write
* 3. DeviceHandlerIF::SEND_READ -> Send read request
* 4. DeviceHandlerIF::GET_READ -> Read from interface
* The PST specifies precisely when the respective ComIF functions are called
* during the communication cycle time.
* The task is created using the FixedTimeslotTaskIF,
* which utilises the underlying Operating System Abstraction Layer (OSAL)
*
* @param thisSequence FixedTimeslotTaskIF * object is passed inside the
* Factory class when creating the PST
* @return
*/
namespace pst {
struct AcsPstCfg {
bool scheduleAcsBoard = true;
bool scheduleImtq = true;
bool scheduleRws = true;
bool scheduleSus = true;
bool scheduleStr = true;
};
/**
* @brief This function creates the PST for all gomspace devices.
* @details
* Scheduled in a separate PST because the gomspace library uses blocking calls when requesting
* data from devices.
*/
ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstSpiAndSyrlinks(FixedTimeslotTaskIF* thisSequence);
ReturnValue_t pstTcsAndAcs(FixedTimeslotTaskIF* thisSequence, AcsPstCfg cfg);
ReturnValue_t pstI2c(FixedTimeslotTaskIF* thisSequence);
/**
* Generic test PST
* @param thisSequence
* @return
*/
ReturnValue_t pstTest(FixedTimeslotTaskIF* thisSequence);
} // namespace pst
#endif /* POLLINGSEQUENCEINIT_H_ */

1
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -385,6 +385,7 @@ ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl);
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
poolManager.subscribeForRegularPeriodicPacket(

2
mission/devices/GyroADIS1650XHandler.h
View File

@ -63,6 +63,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
InternalState internalState = InternalState::STARTUP;
bool commandExecuted = false;
PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
void prepareReadCommand(uint8_t *regList, size_t len);
BurstModes getBurstMode();

2
mission/devices/ImtqHandler.cpp
View File

@ -256,7 +256,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
ReturnValue_t result;
ReturnValue_t status = returnvalue::OK;
if(getMode() != MODE_NORMAL) {
if (getMode() != MODE_NORMAL) {
// Ignore replies during transitions.
return returnvalue::OK;
}

14
mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h
View File

@ -147,11 +147,15 @@ class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
setAllVariablesReadOnly();
}
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER);
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS);
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL);
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER);
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER);
lp_var_t<uint16_t> diagStatReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this);
lp_var_t<uint8_t> filterSetting =
lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS, this);
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL, this);
lp_var_t<uint16_t> mscCtrlReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this);
lp_var_t<uint16_t> decRateReg =
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this);
private:
friend class GyroADIS1650XHandler;

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

@ -108,8 +108,6 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
static constexpr pcdu::Switches SWITCH_A = pcdu::Switches::PDU1_CH7_ACS_A_SIDE_3V3;
static constexpr pcdu::Switches SWITCH_B = pcdu::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3;
bool tryingOtherSide = false;
bool dualModeErrorSwitch = true;
AcsBoardHelper helper;
GpioIF* gpioIF = nullptr;

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

@ -1,13 +1,36 @@
#include "ComSubsystem.h"
#include <fsfw/events/EventManagerIF.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <linux/ipcore/PdecHandler.h>
#include <mission/comDefs.h>
#include <mission/config/comCfg.h>
#include "mission/config/comCfg.h"
#include <utility>
ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
uint32_t maxNumberOfTables, uint32_t transmitterTimeout)
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) {
com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK);
auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
eventQueue =
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
transmitterCountdown.setTimeout(transmitterTimeout);
}
void ComSubsystem::performChildOperation() {
readEventQueue();
// Execute default rate sequence after transition has been completed
if (rememberBitLock and not isInTransition) {
startRxAndTxLowRateSeq();
rememberBitLock = false;
}
if (countdownActive) {
checkTransmitterCountdown();
}
Subsystem::performChildOperation();
}
MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }
@ -27,6 +50,17 @@ ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentif
parameterWrapper->set(datarateCfg);
com::setCurrentDatarate(static_cast<com::Datarate>(newVal));
return returnvalue::OK;
} else if ((domainId == 0) and
(uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::TRANSMITTER_TIMEOUT))) {
uint32_t newVal = 0;
ReturnValue_t result = newValues->getElement(&newVal);
if (result != returnvalue::OK) {
return result;
}
parameterWrapper->set(transmitterTimeout);
transmitterTimeout = newVal;
transmitterCountdown.setTimeout(transmitterTimeout);
return returnvalue::OK;
}
return returnvalue::OK;
}
@ -44,5 +78,121 @@ ReturnValue_t ComSubsystem::initialize() {
if (result != returnvalue::OK) {
return result;
}
EventManagerIF *manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->registerListener(eventQueue->getId());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event "
"listener"
<< std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->subscribeToEventRange(eventQueue->getId(),
event::getEventId(PdecHandler::CARRIER_LOCK),
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC "
"handler"
<< std::endl;
#endif
return result;
}
return Subsystem::initialize();
}
void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) {
// Depending on the submode the transmitter timeout is enabled or
// disabled here
if (mode == com::Submode::RX_ONLY) {
transmitterCountdown.timeOut();
countdownActive = false;
} else if (isTxMode(mode)) {
// Only start transmitter countdown if transmitter is not already on
if (not isTxMode(this->mode)) {
transmitterCountdown.resetTimer();
countdownActive = true;
}
}
Subsystem::startTransition(mode, submode);
}
void ComSubsystem::readEventQueue() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
handleEventMessage(&event);
break;
default:
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
<< std::endl;
break;
}
}
}
void ComSubsystem::handleEventMessage(EventMessage *eventMessage) {
Event event = eventMessage->getEvent();
switch (event) {
case PdecHandler::BIT_LOCK_PDEC: {
handleBitLockEvent();
break;
}
case PdecHandler::CARRIER_LOCK: {
handleCarrierLockEvent();
break;
}
default:
sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl;
break;
}
}
void ComSubsystem::handleBitLockEvent() {
if (isTxMode(mode)) {
// Tx already on
return;
}
if (isInTransition) {
rememberBitLock = true;
return;
}
startRxAndTxLowRateSeq();
}
void ComSubsystem::handleCarrierLockEvent() {
if (!enableTxWhenCarrierLock) {
return;
}
startRxAndTxLowRateSeq();
}
void ComSubsystem::startRxAndTxLowRateSeq() {
// Turns transmitter on
startTransition(com::Submode::RX_AND_TX_LOW_DATARATE, SUBMODE_NONE);
}
void ComSubsystem::checkTransmitterCountdown() {
if (transmitterCountdown.hasTimedOut()) {
startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
countdownActive = false;
}
}
bool ComSubsystem::isTxMode(Mode_t mode) {
if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) ||
(mode == com::Submode::RX_AND_TX_LOW_DATARATE) ||
(mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) {
return true;
}
return false;
}

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

@ -1,6 +1,7 @@
#ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_
#define MISSION_SYSTEM_COMSUBSYSTEM_H_
#include <fsfw/events/EventMessage.h>
#include <fsfw/parameters/HasParametersIF.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/subsystem/Subsystem.h>
@ -9,21 +10,71 @@
class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
public:
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
/**
* @brief Constructor
*
* @param setObjectId
* @param maxNumberOfSequences
* @param maxNumberOfTables
* @param transmitterTimeout Maximum time the transmitter of the syrlinks
* will
* be enabled
*/
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
uint32_t transmitterTimeout);
virtual ~ComSubsystem() = default;
MessageQueueId_t getCommandQueue() const override;
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
uint16_t startAtIndex) override;
virtual void performChildOperation() override;
private:
static const Mode_t INITIAL_MODE = 0;
ReturnValue_t handleCommandMessage(CommandMessage *message) override;
ReturnValue_t initialize() override;
void startTransition(Mode_t mode, Submode_t submode) override;
void readEventQueue();
void handleEventMessage(EventMessage *eventMessage);
void handleBitLockEvent();
void handleCarrierLockEvent();
void checkTransmitterCountdown();
/**
* @brief Enables transmitter in low rate mode
*/
void startRxAndTxLowRateSeq();
/**
* @brief Returns true if mode is a mode where the transmitter is on
*/
bool isTxMode(Mode_t mode);
uint8_t datarateCfg = static_cast<uint8_t>(com::Datarate::LOW_RATE_MODULATION_BPSK);
// Maximum time after which the transmitter will be turned of. This is a
// protection mechanism due prevent the syrlinks from overheating
uint32_t transmitterTimeout = 0;
ParameterHelper paramHelper;
MessageQueueIF *eventQueue = nullptr;
bool enableTxWhenCarrierLock = false;
// Countdown will be started as soon as the transmitter was enabled
Countdown transmitterCountdown;
// Transmitter countdown only active when sysrlinks transmitter is on (modes:
// rx and tx low rate, rx and tx high rate, rx and tx default rate)
bool countdownActive = false;
// True when bit lock occurred while COM subsystem is in a transition. This
// variable is used to remember the bit lock and execute the default rate
// sequence after the active transition has been completed
bool rememberBitLock = false;
};
#endif /* MISSION_SYSTEM_COMSUBSYSTEM_H_ */

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

@ -10,6 +10,7 @@ enum class HandlerState { SWITCH_PENDING, IDLE };
class Stack5VHandler {
public:
//! [EXPORT] : [SKIP]
static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0);
Stack5VHandler(PowerSwitchIF& switcher);

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

@ -50,8 +50,6 @@ class SusAssembly : public DualLaneAssemblyBase {
SusAssHelper helper;
PowerSwitchIF* pwrSwitcher = nullptr;
bool tryingOtherSide = false;
bool dualModeErrorSwitch = true;
ReturnValue_t initialize() override;
// AssemblyBase overrides

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

@ -1,6 +1,5 @@
#include "comModeTree.h"
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <fsfw/modes/HasModesIF.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/subsystem/Subsystem.h>
@ -11,7 +10,8 @@
const auto check = subsystem::checkInsert;
ComSubsystem satsystem::com::SUBSYSTEM = ComSubsystem(objects::COM_SUBSYSTEM, 12, 24);
ComSubsystem satsystem::com::SUBSYSTEM =
ComSubsystem(objects::COM_SUBSYSTEM, 12, 24, TRANSMITTER_TIMEOUT);
static const auto OFF = HasModesIF::MODE_OFF;
static const auto ON = HasModesIF::MODE_ON;
@ -19,39 +19,48 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL;
auto COM_SEQUENCE_RX_ONLY =
std::make_pair(::com::Submode::RX_ONLY, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TGT =
std::make_pair((::com::Submode::RX_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_0 =
std::make_pair((::com::Submode::RX_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_1 =
std::make_pair((::com::Submode::RX_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TGT = std::make_pair(
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_0 = std::make_pair(
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_ONLY_TRANS_1 = std::make_pair(
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 3>());
auto COM_SEQUENCE_RX_AND_TX_LOW_RATE =
std::make_pair(::com::Submode::RX_AND_TX_LOW_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT = std::make_pair(
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 = std::make_pair(
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 = std::make_pair(
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3,
FixedArrayList<ModeListEntry, 3>());
auto COM_SEQUENCE_RX_AND_TX_HIGH_RATE =
std::make_pair(::com::Submode::RX_AND_TX_HIGH_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT = std::make_pair(
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 = std::make_pair(
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 = std::make_pair(
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3,
FixedArrayList<ModeListEntry, 3>());
auto COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE =
std::make_pair(::com::Submode::RX_AND_TX_DEFAULT_DATARATE, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT = std::make_pair(
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 = std::make_pair(
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 = std::make_pair(
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2,
FixedArrayList<ModeListEntry, 3>());
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 =
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3,
FixedArrayList<ModeListEntry, 3>());
namespace {
@ -68,7 +77,7 @@ Subsystem& satsystem::com::init() {
buildTxAndRxLowRateSequence(SUBSYSTEM, entry);
buildTxAndRxHighRateSequence(SUBSYSTEM, entry);
buildTxAndRxDefaultRateSequence(SUBSYSTEM, entry);
SUBSYSTEM.setInitialMode(NML, ::com::Submode::RX_ONLY);
SUBSYSTEM.setInitialMode(COM_SEQUENCE_RX_ONLY.first);
return SUBSYSTEM;
}
@ -95,7 +104,7 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
// Build RX Only table. We could track the state of the CCSDS IP core handler
// as well but I do not think this is necessary because enabling that should
// not intefere with the Syrlinks Handler.
// not interfere with the Syrlinks Handler.
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);

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

@ -1,6 +1,7 @@
#ifndef MISSION_SYSTEM_TREE_COMMODETREE_H_
#define MISSION_SYSTEM_TREE_COMMODETREE_H_
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <mission/system/objects/ComSubsystem.h>
namespace satsystem {
@ -8,6 +9,11 @@ namespace satsystem {
namespace com {
extern ComSubsystem SUBSYSTEM;
// The syrlinks must not transmitting longer then 15 minutes otherwise the
// transceiver might be damaged due to overheating
// 15 minutes in milliseconds
static const uint32_t TRANSMITTER_TIMEOUT = 900000;
Subsystem& init();
} // namespace com

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

@ -7,6 +7,7 @@
#include "acsModeTree.h"
#include "comModeTree.h"
#include "eive/objects.h"
#include "mission/comDefs.h"
#include "payloadModeTree.h"
#include "tcsModeTree.h"
#include "util.h"
@ -85,6 +86,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
// Build SAFE transition 0. Two transitions to reduce number of consecutive events and because
// consecutive commanding of TCS and ACS can lead to SPI issues.
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
ctxc);

109
mission/tmtc/CcsdsIpCoreHandler.cpp
View File

@ -1,12 +1,10 @@
#include "CcsdsIpCoreHandler.h"
#include <fsfw/subsystem/helper.h>
#include <linux/ipcore/PdecHandler.h>
#include <linux/ipcore/PtmeConfig.h>
#include <mission/config/comCfg.h>
#include "eive/definitions.h"
#include "fsfw/events/EventManagerIF.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serialize/SerializeAdapter.h"
@ -16,8 +14,7 @@
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
object_id_t tcDestination, PtmeConfig* ptmeConfig,
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData,
uint32_t transmitterTimeout)
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData)
: SystemObject(objectId),
ptmeId(ptmeId),
tcDestination(tcDestination),
@ -27,8 +24,7 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
ptmeConfig(ptmeConfig),
gpioIF(gpioIF),
enTxClock(enTxClock),
enTxData(enTxData),
transmitterTimeout(transmitterTimeout) {
enTxData(enTxData) {
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
eventQueue =
@ -38,11 +34,9 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {}
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
checkEvents();
readCommandQueue();
handleTelemetry();
handleTelecommands();
checkTxTimer();
return returnvalue::OK;
}
@ -98,46 +92,11 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() {
iter->second->setPtmeObject(ptme);
}
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CcsdsHandler::initialize: Invalid event manager" << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = manager->registerListener(eventQueue->getId());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "CcsdsHandler::initialize: Failed to register CCSDS handler as event "
"listener"
<< std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
;
}
result = manager->subscribeToEventRange(eventQueue->getId(),
event::getEventId(PdecHandler::CARRIER_LOCK),
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CcsdsHandler::initialize: Failed to subscribe to events from PDEC "
"handler"
<< std::endl;
#endif
return result;
}
result = ptmeConfig->initialize();
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
transmitterCountdown.setTimeout(transmitterTimeout);
transmitterCountdown.timeOut();
#if OBSW_SYRLINKS_SIMULATED == 1
// Update data on rising edge
ptmeConfig->invertTxClock(false);
@ -286,54 +245,6 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu
return EXECUTION_FINISHED;
}
void CcsdsIpCoreHandler::checkEvents() {
EventMessage event;
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
result = eventQueue->receiveMessage(&event)) {
switch (event.getMessageId()) {
case EventMessage::EVENT_MESSAGE:
handleEvent(&event);
break;
default:
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
<< std::endl;
break;
}
}
}
void CcsdsIpCoreHandler::handleEvent(EventMessage* eventMessage) {
Event event = eventMessage->getEvent();
switch (event) {
case PdecHandler::BIT_LOCK_PDEC: {
handleBitLockEvent();
break;
}
case PdecHandler::CARRIER_LOCK: {
handleCarrierLockEvent();
break;
}
default:
sif::debug << "CcsdsHandler::handleEvent: Did not subscribe to this event" << std::endl;
break;
}
}
void CcsdsIpCoreHandler::handleBitLockEvent() {
if (transmitterCountdown.isBusy()) {
// Transmitter already enabled
return;
}
enableTransmit();
}
void CcsdsIpCoreHandler::handleCarrierLockEvent() {
if (!enableTxWhenCarrierLock) {
return;
}
enableTransmit();
}
void CcsdsIpCoreHandler::forwardLinkstate() {
VirtualChannelMapIter iter;
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
@ -342,27 +253,12 @@ void CcsdsIpCoreHandler::forwardLinkstate() {
}
void CcsdsIpCoreHandler::enableTransmit() {
if (transmitterCountdown.isBusy()) {
// Transmitter already enabled
return;
}
#ifndef TE0720_1CFA
gpioIF->pullHigh(enTxClock);
gpioIF->pullHigh(enTxData);
#endif
linkState = UP;
forwardLinkstate();
transmitterCountdown.resetTimer();
}
void CcsdsIpCoreHandler::checkTxTimer() {
if (linkState == DOWN) {
return;
}
if (transmitterCountdown.hasTimedOut()) {
disableTransmit();
// TODO: set mode to off (move timer to subsystem)
}
}
void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
@ -431,7 +327,6 @@ void CcsdsIpCoreHandler::disableTransmit() {
#endif
linkState = DOWN;
forwardLinkstate();
transmitterCountdown.timeOut();
}
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }

26
mission/tmtc/CcsdsIpCoreHandler.h
View File

@ -59,8 +59,7 @@ class CcsdsIpCoreHandler : public SystemObject,
* @param enTxData GPIO ID of RS485 tx data enable
*/
CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData,
uint32_t transmitterTimeout = 900000);
PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData);
~CcsdsIpCoreHandler();
@ -154,29 +153,16 @@ class CcsdsIpCoreHandler : public SystemObject,
PtmeConfig* ptmeConfig = nullptr;
GpioIF* gpioIF = nullptr;
// GPIO to enable RS485 transceiver for TX clock
gpioId_t enTxClock = gpio::NO_GPIO;
// GPIO to enable RS485 transceiver for TX data signal
gpioId_t enTxData = gpio::NO_GPIO;
// Syrlinks must not be transmitting more than 15 minutes (according to datasheet)
// Value initialized by constructor argument
const uint32_t transmitterTimeout = 0;
// Countdown to disable transmitter after 15 minutes
Countdown transmitterCountdown;
// When true transmitting is started as soon as carrier lock has been detected
bool enableTxWhenCarrierLock = false;
bool linkState = DOWN;
void readCommandQueue(void);
void handleTelemetry();
void handleTelecommands();
void checkEvents();
void handleEvent(EventMessage* eventMessage);
void handleBitLockEvent();
void handleCarrierLockEvent();
/**
* @brief Forward link state to virtual channels.
@ -188,12 +174,6 @@ class CcsdsIpCoreHandler : public SystemObject,
*/
void enableTransmit();
/**
* @brief Checks Tx timer for timeout and disables RS485 tx clock and tx data in case
* timer has expired.
*/
void checkTxTimer();
/**
* @brief Disables the transmitter by pulling the enable tx clock and tx data pin of the
* RS485 transceiver chips to high.

2
mission/tmtc/CfdpTmFunnel.cpp
View File

@ -21,7 +21,7 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
break;
}
count++;
if(count == 500) {
if (count == 500) {
sif::error << "CfdpTmFunnel: Possible message storm detected" << std::endl;
break;
}

2
mission/tmtc/PusTmFunnel.cpp
View File

@ -21,7 +21,7 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
break;
}
count++;
if(count == 500) {
if (count == 500) {
sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
break;
}

2
mission/tmtc/VirtualChannel.cpp
View File

@ -50,7 +50,7 @@ ReturnValue_t VirtualChannel::performOperation() {
}
count++;
if(count == 500) {
if (count == 500) {
sif::error << "VirtualChannel: Possible message storm detected" << std::endl;
break;
}