1043 lines
46 KiB
C++
1043 lines
46 KiB
C++
#include "AcsController.h"
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <mission/acs/defs.h>
|
|
#include <mission/config/torquer.h>
|
|
|
|
AcsController::AcsController(object_id_t objectId, bool enableHkSets)
|
|
: ExtendedControllerBase(objectId),
|
|
enableHkSets(enableHkSets),
|
|
guidance(&acsParameters),
|
|
safeCtrl(&acsParameters),
|
|
ptgCtrl(&acsParameters),
|
|
parameterHelper(this),
|
|
mgmDataRaw(this),
|
|
mgmDataProcessed(this),
|
|
susDataRaw(this),
|
|
susDataProcessed(this),
|
|
gyrDataRaw(this),
|
|
gyrDataProcessed(this),
|
|
gpsDataProcessed(this),
|
|
mekfData(this),
|
|
ctrlValData(this),
|
|
actuatorCmdData(this) {}
|
|
|
|
ReturnValue_t AcsController::initialize() {
|
|
ReturnValue_t result = parameterHelper.initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
return ExtendedControllerBase::initialize();
|
|
}
|
|
|
|
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
|
ReturnValue_t result = actionHelper.handleActionMessage(message);
|
|
if (result == returnvalue::OK) {
|
|
return result;
|
|
}
|
|
result = parameterHelper.handleParameterMessage(message);
|
|
if (result == returnvalue::OK) {
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t *data, size_t size) {
|
|
switch (actionId) {
|
|
case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
|
|
ReturnValue_t result = guidance.solarArrayDeploymentComplete();
|
|
if (result == returnvalue::FAILED) {
|
|
return FILE_DELETION_FAILED;
|
|
}
|
|
return HasActionsIF::EXECUTION_FINISHED;
|
|
}
|
|
case RESET_MEKF: {
|
|
navigation.resetMekf(&mekfData);
|
|
return HasActionsIF::EXECUTION_FINISHED;
|
|
}
|
|
case RESTORE_MEKF_NONFINITE_RECOVERY: {
|
|
mekfLost = false;
|
|
return HasActionsIF::EXECUTION_FINISHED;
|
|
}
|
|
default: {
|
|
return HasActionsIF::INVALID_ACTION_ID;
|
|
}
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
|
|
|
|
ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
|
|
ParameterWrapper *parameterWrapper,
|
|
const ParameterWrapper *newValues,
|
|
uint16_t startAtIndex) {
|
|
return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
|
|
startAtIndex);
|
|
}
|
|
|
|
void AcsController::performControlOperation() {
|
|
#if OBSW_THREAD_TRACING == 1
|
|
trace::threadTrace(opCounter, "ACS & TCS PST");
|
|
#endif
|
|
{
|
|
PoolReadGuard pg(&mgmDataRaw);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
copyMgmData();
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&susDataRaw);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
copySusData();
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&gyrDataRaw);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
copyGyrData();
|
|
}
|
|
}
|
|
|
|
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 (mode) {
|
|
case acs::SAFE:
|
|
switch (submode) {
|
|
case SUBMODE_NONE:
|
|
performSafe();
|
|
break;
|
|
case acs::DETUMBLE:
|
|
performDetumble();
|
|
break;
|
|
}
|
|
break;
|
|
case acs::PTG_IDLE:
|
|
case acs::PTG_TARGET:
|
|
case acs::PTG_TARGET_GS:
|
|
case acs::PTG_NADIR:
|
|
case acs::PTG_INERTIAL:
|
|
performPointingCtrl();
|
|
break;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void AcsController::performSafe() {
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
|
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
|
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
|
&susDataProcessed, &mekfData, &acsParameters);
|
|
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
|
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
|
if (not mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
|
|
mekfInvalidFlag = true;
|
|
}
|
|
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
|
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
|
|
navigation.resetMekf(&mekfData);
|
|
mekfLost = true;
|
|
}
|
|
} else if (mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_RECOVERY);
|
|
mekfInvalidFlag = false;
|
|
}
|
|
|
|
// get desired satellite rate, sun direction to align to and inertia
|
|
double sunTargetDir[3] = {0, 0, 0};
|
|
guidance.getTargetParamsSafe(sunTargetDir);
|
|
|
|
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
|
acs::ctrlStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
|
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
|
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
|
acsParameters.safeModeControllerParameters.useMekf,
|
|
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
|
|
switch (safeCtrlStrat) {
|
|
case (acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF):
|
|
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
|
|
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
|
|
magMomMtq, errAng);
|
|
safeCtrlFailureFlag = false;
|
|
safeCtrlFailureCounter = 0;
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF):
|
|
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
|
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
|
|
safeCtrlFailureFlag = false;
|
|
safeCtrlFailureCounter = 0;
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING):
|
|
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
|
sunTargetDir, magMomMtq, errAng);
|
|
safeCtrlFailureFlag = false;
|
|
safeCtrlFailureCounter = 0;
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING):
|
|
errAng = NAN;
|
|
safeCtrlFailureFlag = false;
|
|
safeCtrlFailureCounter = 0;
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
|
safeCtrlFailure(1, 0);
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
|
safeCtrlFailure(0, 1);
|
|
break;
|
|
default:
|
|
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
|
|
break;
|
|
}
|
|
|
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
|
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
|
|
|
// detumble check and switch
|
|
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
|
|
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
|
|
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
|
detumbleCounter++;
|
|
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
|
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
|
|
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
|
detumbleCounter++;
|
|
} else if (detumbleCounter > 0) {
|
|
detumbleCounter -= 1;
|
|
}
|
|
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
|
detumbleCounter = 0;
|
|
// Triggers detumble mode transition in subsystem
|
|
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
|
startTransition(mode, acs::SafeSubmode::DETUMBLE);
|
|
}
|
|
|
|
updateCtrlValData(errAng, safeCtrlStrat);
|
|
updateActuatorCmdData(cmdDipoleMtqs);
|
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
|
acsParameters.magnetorquerParameter.torqueDuration);
|
|
}
|
|
|
|
void AcsController::performDetumble() {
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
|
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
|
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
|
&susDataProcessed, &mekfData, &acsParameters);
|
|
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
|
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
|
if (not mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
|
|
mekfInvalidFlag = true;
|
|
}
|
|
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
|
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
|
|
navigation.resetMekf(&mekfData);
|
|
mekfLost = true;
|
|
}
|
|
} else if (mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_RECOVERY);
|
|
mekfInvalidFlag = false;
|
|
}
|
|
acs::ctrlStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
|
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
|
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
|
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
|
double magMomMtq[3] = {0, 0, 0};
|
|
switch (safeCtrlStrat) {
|
|
case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL):
|
|
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
|
|
magMomMtq, acsParameters.detumbleParameter.gainFull);
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
|
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
|
|
magMomMtq, acsParameters.detumbleParameter.gainBdot);
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
|
safeCtrlFailure(1, 0);
|
|
break;
|
|
case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
|
safeCtrlFailure(0, 1);
|
|
break;
|
|
default:
|
|
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
|
|
break;
|
|
}
|
|
|
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
|
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
|
|
|
if (mekfData.satRotRateMekf.isValid() &&
|
|
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
|
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
|
detumbleCounter++;
|
|
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
|
|
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
|
|
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
|
detumbleCounter++;
|
|
} else if (detumbleCounter > 0) {
|
|
detumbleCounter -= 1;
|
|
}
|
|
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
|
detumbleCounter = 0;
|
|
// Triggers safe mode transition in subsystem
|
|
triggerEvent(acs::SAFE_RATE_RECOVERY);
|
|
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
|
}
|
|
|
|
disableCtrlValData();
|
|
updateActuatorCmdData(cmdDipoleMtqs);
|
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
|
acsParameters.magnetorquerParameter.torqueDuration);
|
|
}
|
|
|
|
void AcsController::performPointingCtrl() {
|
|
timeval now;
|
|
Clock::getClock_timeval(&now);
|
|
|
|
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
|
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
|
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
|
&susDataProcessed, &mekfData, &acsParameters);
|
|
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
|
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
|
mekfInvalidCounter++;
|
|
if (not mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
|
|
mekfInvalidFlag = true;
|
|
}
|
|
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
|
|
triggerEvent(acs::MEKF_AUTOMATIC_RESET);
|
|
navigation.resetMekf(&mekfData);
|
|
mekfLost = true;
|
|
}
|
|
if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
|
|
// Trigger this so STR FDIR can set the device faulty.
|
|
EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
|
|
mekfInvalidCounter = 0;
|
|
}
|
|
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
|
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
|
acsParameters.rwHandlingParameters.rampTime);
|
|
return;
|
|
} else {
|
|
if (mekfInvalidFlag) {
|
|
triggerEvent(acs::MEKF_RECOVERY);
|
|
mekfInvalidFlag = false;
|
|
}
|
|
mekfInvalidCounter = 0;
|
|
}
|
|
uint8_t enableAntiStiction = true;
|
|
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
|
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
|
if (result == returnvalue::FAILED) {
|
|
if (multipleRwUnavailableCounter >=
|
|
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
|
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
|
}
|
|
multipleRwUnavailableCounter++;
|
|
rwInvalidFlag = true;
|
|
} else {
|
|
multipleRwUnavailableCounter = 0;
|
|
rwInvalidFlag = false;
|
|
}
|
|
|
|
// Variables required for ptgStrat
|
|
acs::ctrlStrategy ptgStrat;
|
|
double quat[4] = {0, 0, 0, 0}, satRotRate[3] = {0, 0, 0};
|
|
// Variables required for guidance
|
|
double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 0},
|
|
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
|
|
// Variables required for setting actuators
|
|
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
|
|
mgtDpDes[3] = {0, 0, 0};
|
|
|
|
switch (mode) {
|
|
case acs::PTG_IDLE:
|
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.idleModeControllerParameters, quat,
|
|
satRotRate);
|
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
break;
|
|
}
|
|
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
|
|
targetSatRotRate);
|
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
|
|
errorSatRotRate, errorAngle);
|
|
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
|
*rwPseudoInv, torquePtgRws);
|
|
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
rwTrqNs);
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
|
ptgCtrl.ptgDesaturation(
|
|
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
|
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
|
break;
|
|
|
|
case acs::PTG_TARGET:
|
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.targetModeControllerParameters, quat,
|
|
satRotRate);
|
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
break;
|
|
}
|
|
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
|
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
|
targetSatRotRate);
|
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
|
acsParameters.targetModeControllerParameters.quatRef,
|
|
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
|
errorSatRotRate, errorAngle);
|
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
|
*rwPseudoInv, torquePtgRws);
|
|
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
rwTrqNs);
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
|
ptgCtrl.ptgDesaturation(
|
|
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
|
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
|
break;
|
|
|
|
case acs::PTG_TARGET_GS:
|
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.gsTargetModeControllerParameters, quat,
|
|
satRotRate);
|
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
break;
|
|
}
|
|
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
|
|
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
|
|
errorSatRotRate, errorAngle);
|
|
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
|
*rwPseudoInv, torquePtgRws);
|
|
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
rwTrqNs);
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
|
ptgCtrl.ptgDesaturation(
|
|
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
|
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
|
break;
|
|
|
|
case acs::PTG_NADIR:
|
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.nadirModeControllerParameters, quat,
|
|
satRotRate);
|
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
break;
|
|
}
|
|
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
|
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
|
targetSatRotRate);
|
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
|
acsParameters.nadirModeControllerParameters.quatRef,
|
|
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
|
errorSatRotRate, errorAngle);
|
|
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
|
*rwPseudoInv, torquePtgRws);
|
|
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
rwTrqNs);
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
|
ptgCtrl.ptgDesaturation(
|
|
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
|
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
|
break;
|
|
|
|
case acs::PTG_INERTIAL:
|
|
ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.inertialModeControllerParameters, quat,
|
|
satRotRate);
|
|
if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
|
|
ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
break;
|
|
}
|
|
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
|
sizeof(targetQuat));
|
|
guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
|
|
acsParameters.inertialModeControllerParameters.quatRef,
|
|
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
|
|
errorSatRotRate, errorAngle);
|
|
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
|
*rwPseudoInv, torquePtgRws);
|
|
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
rwTrqNs);
|
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
|
ptgCtrl.ptgDesaturation(
|
|
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
|
mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
|
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
|
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
|
break;
|
|
default:
|
|
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
|
break;
|
|
}
|
|
|
|
if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL and
|
|
ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
|
|
actuatorCmd.cmdSpeedToRws(
|
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
|
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
|
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
|
if (enableAntiStiction) {
|
|
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
|
}
|
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
|
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes,
|
|
cmdDipoleMtqs);
|
|
}
|
|
|
|
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgStrat);
|
|
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
|
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
|
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
|
acsParameters.rwHandlingParameters.rampTime);
|
|
}
|
|
|
|
void AcsController::handlePtgCtrlStrat(acs::ctrlStrategy &ptgStrat,
|
|
AcsParameters::PointingLawParameters ptgLawParameters,
|
|
double *quat, double *satRotRate) {
|
|
ptgStrat = ptgCtrl.ptgCtrlStrategy(
|
|
not rwInvalidFlag, ptgLawParameters.useMekf, not mekfInvalidFlag,
|
|
mgmDataProcessed.mgmVecTot.isValid(), ptgLawParameters.desatOn,
|
|
sensorValues.strSet.isTrustWorthy.value, gyrDataProcessed.gyrVecTot.isValid());
|
|
switch (ptgStrat) {
|
|
case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF:
|
|
case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT:
|
|
std::memcpy(quat, mekfData.quatMekf.value, 4 * sizeof(double));
|
|
std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
|
|
break;
|
|
case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF:
|
|
case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT:
|
|
quat[0] = sensorValues.strSet.caliQx.value;
|
|
quat[1] = sensorValues.strSet.caliQy.value;
|
|
quat[2] = sensorValues.strSet.caliQz.value;
|
|
quat[3] = sensorValues.strSet.caliQw.value;
|
|
std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
|
|
break;
|
|
default:;
|
|
}
|
|
}
|
|
|
|
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
|
if (not safeCtrlFailureFlag) {
|
|
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
|
safeCtrlFailureFlag = true;
|
|
}
|
|
safeCtrlFailureCounter++;
|
|
if (safeCtrlFailureCounter > 150) {
|
|
safeCtrlFailureFlag = false;
|
|
safeCtrlFailureCounter = 0;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
|
uint16_t dipoleTorqueDuration) {
|
|
{
|
|
PoolReadGuard pg(&dipoleSet);
|
|
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
|
|
torquer::LOCK_CTX);
|
|
torquer::NEW_ACTUATION_FLAG = true;
|
|
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t AcsController::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) {
|
|
{
|
|
PoolReadGuard pg(&dipoleSet);
|
|
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
|
|
torquer::LOCK_CTX);
|
|
torquer::NEW_ACTUATION_FLAG = true;
|
|
dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
|
|
}
|
|
{
|
|
PoolReadGuard pg(&rw1SpeedSet);
|
|
rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
|
|
}
|
|
{
|
|
PoolReadGuard pg(&rw2SpeedSet);
|
|
rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
|
|
}
|
|
{
|
|
PoolReadGuard pg(&rw3SpeedSet);
|
|
rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
|
|
}
|
|
{
|
|
PoolReadGuard pg(&rw4SpeedSet);
|
|
rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
|
|
updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
|
|
}
|
|
|
|
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, acs::ctrlStrategy safeModeStrat) {
|
|
PoolReadGuard pg(&ctrlValData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
ctrlValData.tgtQuat.setValid(false);
|
|
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
ctrlValData.errQuat.setValid(false);
|
|
ctrlValData.errAng.value = errAng;
|
|
ctrlValData.errAng.setValid(true);
|
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
|
ctrlValData.tgtRotRate.setValid(false);
|
|
ctrlValData.ctrlStrat.value = safeModeStrat;
|
|
ctrlValData.ctrlStrat.setValid(true);
|
|
ctrlValData.setValidity(true, false);
|
|
}
|
|
}
|
|
|
|
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
|
const double *tgtRotRate, acs::ctrlStrategy ptgModeStrat) {
|
|
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;
|
|
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
|
|
ctrlValData.ctrlStrat.value = ptgModeStrat;
|
|
ctrlValData.setValidity(true, true);
|
|
}
|
|
}
|
|
|
|
void AcsController::disableCtrlValData() {
|
|
PoolReadGuard pg(&ctrlValData);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
ctrlValData.errAng.value = 0;
|
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
|
ctrlValData.setValidity(false, true);
|
|
}
|
|
}
|
|
|
|
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
// MGM Raw
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
|
|
// MGM Processed
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
|
|
poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.0});
|
|
// SUS Raw
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
|
|
// SUS Processed
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
|
|
poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.0});
|
|
// GYR Raw
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.0});
|
|
// GYR Processed
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
|
|
poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
|
|
// GPS Processed
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
|
|
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
|
|
// MEKF
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
|
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
|
|
// Ctrl Values
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::CTRL_STRAT, &ctrlStrat);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
|
|
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.0});
|
|
// Actuator CMD
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
|
|
localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
|
|
poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
|
|
switch (sid.ownerSetId) {
|
|
case acsctrl::MGM_SENSOR_DATA:
|
|
return &mgmDataRaw;
|
|
case acsctrl::MGM_PROCESSED_DATA:
|
|
return &mgmDataProcessed;
|
|
case acsctrl::SUS_SENSOR_DATA:
|
|
return &susDataRaw;
|
|
case acsctrl::SUS_PROCESSED_DATA:
|
|
return &susDataProcessed;
|
|
case acsctrl::GYR_SENSOR_DATA:
|
|
return &gyrDataRaw;
|
|
case acsctrl::GYR_PROCESSED_DATA:
|
|
return &gyrDataProcessed;
|
|
case acsctrl::GPS_PROCESSED_DATA:
|
|
return &gpsDataProcessed;
|
|
case acsctrl::MEKF_DATA:
|
|
return &mekfData;
|
|
case acsctrl::CTRL_VAL_DATA:
|
|
return &ctrlValData;
|
|
case acsctrl::ACTUATOR_CMD_DATA:
|
|
return &actuatorCmdData;
|
|
default:
|
|
return nullptr;
|
|
}
|
|
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 (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) {
|
|
if (mode == acs::AcsMode::SAFE) {
|
|
if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) {
|
|
return INVALID_SUBMODE;
|
|
} else {
|
|
return returnvalue::OK;
|
|
}
|
|
} else if (not(submode == SUBMODE_NONE)) {
|
|
return INVALID_SUBMODE;
|
|
} else {
|
|
return returnvalue::OK;
|
|
}
|
|
}
|
|
return INVALID_MODE;
|
|
}
|
|
|
|
void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
|
|
return ExtendedControllerBase::modeChanged(mode, submode);
|
|
}
|
|
|
|
void AcsController::announceMode(bool recursive) {
|
|
const char *modeStr = "UNKNOWN";
|
|
if (mode == HasModesIF::MODE_OFF) {
|
|
modeStr = "OFF";
|
|
} else {
|
|
modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
|
|
}
|
|
const char *submodeStr = "UNKNOWN";
|
|
if (submode == HasModesIF::SUBMODE_NONE) {
|
|
submodeStr = "NONE";
|
|
}
|
|
if (mode == acs::AcsMode::SAFE) {
|
|
acs::SafeSubmode safeSubmode = static_cast<acs::SafeSubmode>(this->submode);
|
|
if (safeSubmode == acs::SafeSubmode::DETUMBLE) {
|
|
submodeStr = "DETUMBLE";
|
|
}
|
|
}
|
|
sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode"
|
|
<< std::endl;
|
|
return ExtendedControllerBase::announceMode(recursive);
|
|
}
|
|
|
|
void AcsController::copyMgmData() {
|
|
{
|
|
PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataRaw.mgm0Lis3.value, sensorValues.mgm0Lis3Set.fieldStrengths.value,
|
|
3 * sizeof(float));
|
|
mgmDataRaw.mgm0Lis3.setValid(sensorValues.mgm0Lis3Set.fieldStrengths.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.mgm1Rm3100Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataRaw.mgm1Rm3100.value, sensorValues.mgm1Rm3100Set.fieldStrengths.value,
|
|
3 * sizeof(float));
|
|
mgmDataRaw.mgm1Rm3100.setValid(sensorValues.mgm1Rm3100Set.fieldStrengths.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.mgm2Lis3Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataRaw.mgm2Lis3.value, sensorValues.mgm2Lis3Set.fieldStrengths.value,
|
|
3 * sizeof(float));
|
|
mgmDataRaw.mgm2Lis3.setValid(sensorValues.mgm2Lis3Set.fieldStrengths.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
|
|
3 * sizeof(float));
|
|
mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.imtqMgmSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(mgmDataRaw.imtqRaw.value, sensorValues.imtqMgmSet.mtmRawNt.value,
|
|
3 * sizeof(float));
|
|
mgmDataRaw.imtqRaw.setValid(sensorValues.imtqMgmSet.mtmRawNt.isValid());
|
|
mgmDataRaw.actuationCalStatus.value = sensorValues.imtqMgmSet.coilActuationStatus.value;
|
|
mgmDataRaw.actuationCalStatus.setValid(sensorValues.imtqMgmSet.coilActuationStatus.isValid());
|
|
}
|
|
}
|
|
}
|
|
|
|
void AcsController::copySusData() {
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[0]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[1]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[2]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[3]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[4]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[5]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[6]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[7]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[8]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[9]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[10]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.susSets[11]);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value,
|
|
6 * sizeof(uint16_t));
|
|
susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
|
|
}
|
|
}
|
|
}
|
|
|
|
void AcsController::copyGyrData() {
|
|
{
|
|
PoolReadGuard pg(&sensorValues.gyr0AdisSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value;
|
|
gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value;
|
|
gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value;
|
|
gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() &&
|
|
sensorValues.gyr0AdisSet.angVelocY.isValid() &&
|
|
sensorValues.gyr0AdisSet.angVelocZ.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.gyr1L3gSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value;
|
|
gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value;
|
|
gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value;
|
|
gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() &&
|
|
sensorValues.gyr1L3gSet.angVelocY.isValid() &&
|
|
sensorValues.gyr1L3gSet.angVelocZ.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.gyr2AdisSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value;
|
|
gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value;
|
|
gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value;
|
|
gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() &&
|
|
sensorValues.gyr2AdisSet.angVelocY.isValid() &&
|
|
sensorValues.gyr2AdisSet.angVelocZ.isValid());
|
|
}
|
|
}
|
|
{
|
|
PoolReadGuard pg(&sensorValues.gyr3L3gSet);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value;
|
|
gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value;
|
|
gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value;
|
|
gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() &&
|
|
sensorValues.gyr3L3gSet.angVelocY.isValid() &&
|
|
sensorValues.gyr3L3gSet.angVelocZ.isValid());
|
|
}
|
|
}
|
|
}
|