Merge pull request 'Fix for Pointing Control Strategy Handling' (#842) from ptg-strat-fix into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #842
This commit is contained in:
commit
d4f45a6dd8
@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release:
|
|||||||
|
|
||||||
# [unreleased]
|
# [unreleased]
|
||||||
|
|
||||||
|
## Fixed
|
||||||
|
|
||||||
|
- Pointing strategy now actually uses fused rotation rate source instead of its valid flag.
|
||||||
|
- All datasets now get updated during pointing mode, even if the strategy is a fault one.
|
||||||
|
|
||||||
# [v7.5.3] 2023-12-19
|
# [v7.5.3] 2023-12-19
|
||||||
|
|
||||||
## Fixed
|
## Fixed
|
||||||
|
@ -403,14 +403,17 @@ void AcsController::performPointingCtrl() {
|
|||||||
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
|
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
|
||||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
|
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
|
||||||
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
|
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++;
|
ptgCtrlLostCounter++;
|
||||||
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
|
if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
|
||||||
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
|
triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
|
||||||
ptgCtrlLostCounter = 0;
|
ptgCtrlLostCounter = 0;
|
||||||
}
|
}
|
||||||
|
updateCtrlValData(ptgCtrlStrat);
|
||||||
|
updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
|
||||||
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||||
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
||||||
acsParameters.rwHandlingParameters.rampTime);
|
acsParameters.rwHandlingParameters.rampTime);
|
||||||
@ -437,7 +440,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
|
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -593,7 +596,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
||||||
|
|
||||||
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
|
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
|
||||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
||||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||||
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
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);
|
PoolReadGuard pg(&ctrlValData);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
||||||
@ -682,13 +685,13 @@ void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
|
|||||||
ctrlValData.errAng.setValid(false);
|
ctrlValData.errAng.setValid(false);
|
||||||
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
ctrlValData.tgtRotRate.setValid(false);
|
ctrlValData.tgtRotRate.setValid(false);
|
||||||
ctrlValData.safeStrat.value = safeModeStrat;
|
ctrlValData.safeStrat.value = ctrlStrat;
|
||||||
ctrlValData.safeStrat.setValid(true);
|
ctrlValData.safeStrat.setValid(true);
|
||||||
ctrlValData.setValidity(true, false);
|
ctrlValData.setValidity(true, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
|
||||||
PoolReadGuard pg(&ctrlValData);
|
PoolReadGuard pg(&ctrlValData);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
|
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);
|
ctrlValData.errAng.setValid(true);
|
||||||
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
||||||
ctrlValData.tgtRotRate.setValid(false);
|
ctrlValData.tgtRotRate.setValid(false);
|
||||||
ctrlValData.safeStrat.value = safeModeStrat;
|
ctrlValData.safeStrat.value = ctrlStrat;
|
||||||
ctrlValData.safeStrat.setValid(true);
|
ctrlValData.safeStrat.setValid(true);
|
||||||
ctrlValData.setValidity(true, false);
|
ctrlValData.setValidity(true, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
|
||||||
const double *tgtRotRate) {
|
const double *tgtRotRate,
|
||||||
|
acs::ControlModeStrategy ctrlStrat) {
|
||||||
PoolReadGuard pg(&ctrlValData);
|
PoolReadGuard pg(&ctrlValData);
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
|
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
|
||||||
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
|
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
|
||||||
ctrlValData.errAng.value = errAng;
|
ctrlValData.errAng.value = errAng;
|
||||||
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
|
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
|
||||||
ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
|
ctrlValData.safeStrat.value = ctrlStrat;
|
||||||
ctrlValData.setValidity(true, true);
|
ctrlValData.setValidity(true, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -52,6 +52,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
void performPointingCtrl();
|
void performPointingCtrl();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
||||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||||
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
||||||
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
|
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
|
||||||
@ -143,10 +144,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
|
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
|
||||||
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
|
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
|
||||||
const int16_t* mtqTargetDipole);
|
const int16_t* mtqTargetDipole);
|
||||||
void updateCtrlValData(uint8_t safeModeStrat);
|
void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
|
||||||
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
|
void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
|
||||||
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
|
void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
|
||||||
const double* tgtRotRate);
|
const double* tgtRotRate, acs::ControlModeStrategy cStrat);
|
||||||
|
|
||||||
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
|
ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
|
||||||
ReturnValue_t writeTleToFs(const uint8_t* tle);
|
ReturnValue_t writeTleToFs(const uint8_t* tle);
|
||||||
|
@ -21,9 +21,8 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
|
|||||||
return acs::ControlModeStrategy::PTGCTRL_STR;
|
return acs::ControlModeStrategy::PTGCTRL_STR;
|
||||||
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||||
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
||||||
} else {
|
|
||||||
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
|
||||||
}
|
}
|
||||||
|
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||||
|
Loading…
Reference in New Issue
Block a user