101 lines
3.1 KiB
C++
101 lines
3.1 KiB
C++
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include "AcsController.h"
|
|
|
|
AcsController::AcsController(object_id_t objectId)
|
|
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
|
|
mgmData(this) {}
|
|
|
|
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void AcsController::performControlOperation() {
|
|
switch (internalState) {
|
|
case InternalState::STARTUP: {
|
|
initialCountdown.resetTimer();
|
|
internalState = InternalState::INITIAL_DELAY;
|
|
return;
|
|
}
|
|
case InternalState::INITIAL_DELAY: {
|
|
if (initialCountdown.hasTimedOut()) {
|
|
internalState = InternalState::READY;
|
|
}
|
|
return;
|
|
}
|
|
case InternalState::READY: {
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
{
|
|
PoolReadGuard pg(&mgmData);
|
|
if (pg.getReadResult() == RETURN_OK) {
|
|
copyMgmData();
|
|
}
|
|
}
|
|
}
|
|
|
|
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
|
|
poolManager.subscribeForPeriodicPacket(mgmData.getSid(), false, 5.0, false);
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
|
if (sid == mgmData.getSid()) {
|
|
return &mgmData;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
|
|
uint32_t *msToReachTheMode) {
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ModeDefinitionHelper AcsController::getModeDefinitionHelper() {
|
|
return ModeDefinitionHelper::create<ControllerModes, DefaultSubmode>();
|
|
}
|
|
|
|
void AcsController::copyMgmData() {
|
|
{
|
|
PoolReadGuard pg(&mgm0Lis3Set);
|
|
if(pg.getReadResult() == RETURN_OK) {
|
|
std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm1Rm3100Set);
|
|
if(pg.getReadResult() == RETURN_OK) {
|
|
std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm2Lis3Set);
|
|
if(pg.getReadResult() == RETURN_OK) {
|
|
std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm3Rm3100Set);
|
|
if(pg.getReadResult() == RETURN_OK) {
|
|
std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&imtqMgmSet);
|
|
if(pg.getReadResult() == RETURN_OK) {
|
|
std::memcpy(mgmData.imtqCal.value, imtqMgmSet.mgmXyz.value, 3 * sizeof(int32_t));
|
|
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
|
|
}
|
|
}
|
|
}
|