eive-obsw/mission/controller/AcsController.cpp

279 lines
8.9 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>
//#include <fsfw/timemanager/Clock.h>
2022-09-30 10:13:27 +02:00
2022-08-15 09:58:18 +02:00
AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
2022-10-06 15:37:41 +02:00
sensorProcessing(&acsParameters),
navigation(&acsParameters),
actuatorCmd(&acsParameters),
detumble(&acsParameters),
detumbleCounter{0},
mgmData(this),
susData(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: {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
// performSafe();
break;
case SUBMODE_DETUMBLE:
performDetumble();
break;
case SUBMODE_PTG_GS:
// performPointingCtrl();
break;
}
}
2022-08-15 10:50:19 +02:00
break;
}
default:
break;
}
{
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-09-28 15:27:51 +02:00
{
PoolReadGuard pg(&susData);
if (pg.getReadResult() == returnvalue::OK) {
copySusData();
}
}
mode = MODE_ON;
submode = SUBMODE_DETUMBLE;
}
2022-10-06 15:37:41 +02:00
void AcsController::performSafe() {}
2022-08-12 12:29:28 +02:00
void AcsController::performDetumble() {
2022-10-06 15:37:41 +02:00
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
// sensorValues.read();
// outputValues.read();
timeval now; // = {0,0};
Clock::getClock_timeval(&now);
sensorProcessing.process(&susData, now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
&outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
&outputValues.magFieldEstValid, magMomMtq);
double dipolCmdUnits[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
else if (outputValues.satRateEstValid &&
VectorOperations<double>::norm(outputValues.satRateEst, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
}
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
submode = SUBMODE_SAFE;
detumbleCounter = 0;
2022-10-06 15:37:41 +02:00
}
}
2022-10-06 15:37:41 +02:00
void AcsController::performPointingCtrl() {}
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
// MGM
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-10-06 15:37:41 +02:00
poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0});
// SUS
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec);
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec);
2022-10-06 15:37:41 +02:00
poolManager.subscribeForRegularPeriodicPacket({susData.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;
2022-09-30 10:13:27 +02:00
} else if (sid == susData.getSid()) {
return &susData;
2022-08-16 11:56:17 +02:00
}
return nullptr;
}
2022-08-15 09:58:18 +02:00
ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
if (mode == MODE_OFF) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
} else {
return INVALID_SUBMODE;
}
} else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
if ((submode > 5) || (submode < 2)) {
return INVALID_SUBMODE;
} else {
return returnvalue::OK;
}
}
return INVALID_MODE;
2022-08-15 09:58:18 +02:00
}
2022-08-15 10:50:19 +02:00
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
void AcsController::announceMode(bool recursive) {}
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
}
2022-09-28 15:27:51 +02:00
void AcsController::copySusData() {
{
PoolReadGuard pg(&susSets[0]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus0.value, susSets[0].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[1]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus1.value, susSets[1].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[2]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus2.value, susSets[2].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[3]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus3.value, susSets[3].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[4]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus4.value, susSets[4].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[5]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus5.value, susSets[5].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[6]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus6.value, susSets[6].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[7]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus7.value, susSets[7].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[8]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus8.value, susSets[8].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[9]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus9.value, susSets[9].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[10]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus10.value, susSets[10].channels.value, 6 * sizeof(uint16_t));
}
}
{
PoolReadGuard pg(&susSets[11]);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(susData.sus11.value, susSets[11].channels.value, 6 * sizeof(uint16_t));
}
}
}