|
|
|
@ -403,14 +403,17 @@ void AcsController::performPointingCtrl() {
|
|
|
|
|
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
|
|
|
|
|
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
|
|
|
|
|
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
|
|
|
|
|
fusedRotRateData.rotRateSource.isValid(), useMekf);
|
|
|
|
|
fusedRotRateData.rotRateSource.value, useMekf);
|
|
|
|
|
|
|
|
|
|
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
|
|
|
|
|
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
|
|
|
|
|
ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
|
|
|
|
|
ptgCtrlLostCounter++;
|
|
|
|
|
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
|
|
|
|
|
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
|
|
|
|
|
ptgCtrlLostCounter = 0;
|
|
|
|
|
}
|
|
|
|
|
updateCtrlValData(ptgCtrlStrat);
|
|
|
|
|
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
|
|
|
|
|
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
|
|
|
|
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
|
|
|
|
acsParameters.rwHandlingParameters.rampTime);
|
|
|
|
@ -437,7 +440,7 @@ void AcsController::performPointingCtrl() {
|
|
|
|
|
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
|
|
|
|
|
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
|
|
|
|
|
<< std::endl;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
@ -593,7 +596,7 @@ void AcsController::performPointingCtrl() {
|
|
|
|
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
|
|
|
|
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
|
|
|
|
|
|
|
|
|
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
|
|
|
|
|
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
|
|
|
|
|
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
|
|
|
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
|
|
|
|
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
|
|
|
@ -671,7 +674,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
|
|
|
|
|
void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
|
|
|
|
|
PoolReadGuard pg(&ctrlValData);
|
|
|
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
|
|
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
|
|
@ -682,13 +685,13 @@ void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
|
|
|
|
|
ctrlValData.errAng.setValid(false);
|
|
|
|
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
|
|
|
|
ctrlValData.tgtRotRate.setValid(false);
|
|
|
|
|
ctrlValData.safeStrat.value = safeModeStrat;
|
|
|
|
|
ctrlValData.safeStrat.value = ctrlStrat;
|
|
|
|
|
ctrlValData.safeStrat.setValid(true);
|
|
|
|
|
ctrlValData.setValidity(true, false);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
|
|
|
|
void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
|
|
|
|
|
PoolReadGuard pg(&ctrlValData);
|
|
|
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
|
|
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
|
|
|
@ -699,21 +702,22 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
|
|
|
|
ctrlValData.errAng.setValid(true);
|
|
|
|
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
|
|
|
|
ctrlValData.tgtRotRate.setValid(false);
|
|
|
|
|
ctrlValData.safeStrat.value = safeModeStrat;
|
|
|
|
|
ctrlValData.safeStrat.value = ctrlStrat;
|
|
|
|
|
ctrlValData.safeStrat.setValid(true);
|
|
|
|
|
ctrlValData.setValidity(true, false);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
|
|
|
|
const double *tgtRotRate) {
|
|
|
|
|
const double *tgtRotRate,
|
|
|
|
|
acs::ControlModeStrategy ctrlStrat) {
|
|
|
|
|
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.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
|
|
|
|
|
ctrlValData.safeStrat.value = ctrlStrat;
|
|
|
|
|
ctrlValData.setValidity(true, true);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|