379 lines
13 KiB
C++
379 lines
13 KiB
C++
#include "AcsController.h"
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
|
|
AcsController::AcsController(object_id_t objectId)
|
|
: ExtendedControllerBase(objectId, objects::NO_OBJECT),
|
|
sensorProcessing(&acsParameters),
|
|
navigation(&acsParameters),
|
|
actuatorCmd(&acsParameters),
|
|
guidance(&acsParameters),
|
|
safeCtrl(&acsParameters),
|
|
detumble(&acsParameters),
|
|
ptgCtrl(&acsParameters),
|
|
detumbleCounter{0},
|
|
mgmData(this),
|
|
susData(this) {}
|
|
|
|
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
|
return returnvalue::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: {
|
|
if (mode != MODE_OFF) {
|
|
switch (submode) {
|
|
case SUBMODE_SAFE:
|
|
performSafe();
|
|
break;
|
|
|
|
case SUBMODE_DETUMBLE:
|
|
performDetumble();
|
|
break;
|
|
|
|
case SUBMODE_PTG_TARGET:
|
|
performPointingCtrl();
|
|
break;
|
|
|
|
case SUBMODE_PTG_SUN:
|
|
performPointingCtrl();
|
|
break;
|
|
|
|
case SUBMODE_PTG_NADIR:
|
|
performPointingCtrl();
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
{
|
|
PoolReadGuard pg(&mgmData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
copyMgmData();
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&susData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
copySusData();
|
|
}
|
|
}
|
|
|
|
// DEBUG : REMOVE AFTER COMPLETION
|
|
mode = MODE_ON;
|
|
submode = SUBMODE_DETUMBLE;
|
|
// DEBUG END
|
|
}
|
|
|
|
void AcsController::performSafe() {
|
|
ACS::SensorValues sensorValues;
|
|
ACS::OutputValues outputValues;
|
|
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
|
ReturnValue_t validMekf;
|
|
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
|
// Give desired satellite rate and sun direction to align
|
|
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
|
|
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
|
|
// IF MEKF is working
|
|
double magMomMtq[3] = {0, 0, 0};
|
|
bool magMomMtqValid = false;
|
|
if ( !validMekf ) { // Valid = 0, Failed = 1
|
|
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
|
|
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
|
|
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
|
|
(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
|
|
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
|
|
}
|
|
else {
|
|
|
|
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
|
|
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
|
|
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
|
|
outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
|
|
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
|
|
|
|
}
|
|
double dipolCmdUnits[3] = {0, 0, 0};
|
|
actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
|
|
}
|
|
|
|
void AcsController::performDetumble() {
|
|
ACS::SensorValues sensorValues;
|
|
ACS::OutputValues outputValues;
|
|
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(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;
|
|
}
|
|
}
|
|
|
|
void AcsController::performPointingCtrl() {
|
|
ACS::SensorValues sensorValues;
|
|
ACS::OutputValues outputValues;
|
|
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
|
|
ReturnValue_t validMekf;
|
|
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
|
|
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
|
|
|
switch (submode) {
|
|
case SUBMODE_PTG_TARGET:
|
|
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
|
break;
|
|
case SUBMODE_PTG_SUN:
|
|
guidance.sunQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
|
break;
|
|
case SUBMODE_PTG_NADIR:
|
|
guidance.quatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
|
|
break;
|
|
case SUBMODE_PTG_INERTIAL:
|
|
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
|
break;
|
|
}
|
|
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
|
|
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
|
|
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
|
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
|
|
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
|
|
double rwTrqNs[4] = {0, 0, 0, 0};
|
|
ptgCtrl.ptgNullspace(
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
|
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
|
|
|
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
|
|
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
|
|
int32_t rwSpeed[4] = {(sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
|
|
(sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
|
|
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
|
|
}
|
|
|
|
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
|
|
actuatorCmd.cmdSpeedToRws(
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
|
|
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
|
|
ptgCtrl.ptgDesaturation(
|
|
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
|
|
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
|
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
|
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
|
|
}
|
|
|
|
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
// MGM
|
|
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.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);
|
|
poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0});
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
|
if (sid == mgmData.getSid()) {
|
|
return &mgmData;
|
|
} else if (sid == susData.getSid()) {
|
|
return &susData;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
|
|
|
|
void AcsController::announceMode(bool recursive) {}
|
|
|
|
void AcsController::copyMgmData() {
|
|
{
|
|
PoolReadGuard pg(&mgm0Lis3Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm1Rm3100Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm2Lis3Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&mgm3Rm3100Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&imtqMgmSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float));
|
|
mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
|
|
}
|
|
}
|
|
}
|
|
|
|
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));
|
|
}
|
|
}
|
|
}
|