eive-obsw/mission/controller/AcsController.cpp

108 lines
3.3 KiB
C++
Raw Normal View History

2022-08-15 09:58:18 +02:00
#include "AcsController.h"
2022-08-12 12:29:28 +02:00
#include <fsfw/datapool/PoolReadGuard.h>
2022-10-20 15:08:33 +02:00
#include "mission/devices/torquer.h"
2022-08-15 09:58:18 +02:00
AcsController::AcsController(object_id_t objectId)
2022-09-29 19:40:00 +02:00
: ExtendedControllerBase(objectId), mgmData(this) {}
2022-08-12 12:29:28 +02:00
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-08-15 09:58:18 +02:00
}
2022-08-12 12:29:28 +02:00
2022-08-15 10:50:19 +02:00
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;
}
2022-10-20 15:08:33 +02:00
{
2022-10-20 16:09:18 +02:00
// TODO: Calculate actuator output
// PoolReadGuard pg(&dipoleSet);
// MutexGuard mg(torquer::lazyLock());
// torquer::NEW_ACTUATION_FLAG = true;
// TODO: Insert correct values here
// dipoleSet.setDipoles(500, 500, 500, 150);
2022-10-20 15:08:33 +02:00
}
2022-08-16 11:27:16 +02:00
{
PoolReadGuard pg(&mgmData);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-18 15:32:24 +02:00
copyMgmData();
2022-08-16 11:27:16 +02:00
}
2022-08-15 11:19:08 +02:00
}
2022-08-15 10:50:19 +02:00
}
2022-08-12 12:29:28 +02:00
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2022-08-15 11:19:08 +02:00
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);
2022-08-30 15:53:20 +02:00
poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0});
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-08-15 09:58:18 +02:00
}
2022-08-16 11:56:17 +02:00
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
if (sid == mgmData.getSid()) {
return &mgmData;
}
return nullptr;
}
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2022-08-15 09:58:18 +02:00
}
2022-08-15 10:50:19 +02:00
void AcsController::copyMgmData() {
2022-08-15 11:19:08 +02:00
{
PoolReadGuard pg(&mgm0Lis3Set);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-15 11:19:08 +02:00
std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
}
}
{
PoolReadGuard pg(&mgm1Rm3100Set);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-15 11:19:08 +02:00
std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
}
}
{
PoolReadGuard pg(&mgm2Lis3Set);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-15 11:19:08 +02:00
std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
}
}
{
PoolReadGuard pg(&mgm3Rm3100Set);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-15 11:19:08 +02:00
std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
}
}
{
PoolReadGuard pg(&imtqMgmSet);
2022-08-24 17:27:47 +02:00
if (pg.getReadResult() == returnvalue::OK) {
2022-08-18 17:27:39 +02:00
std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float));
2022-08-15 11:19:08 +02:00
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
}
}
2022-08-15 10:50:19 +02:00
}