eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Fix for Pointing Control Strategy Handling #842

Merged
meggert merged 4 commits from ptg-strat-fix into main 2024-01-16 16:50:15 +01:00
Conversation 0 Commits 4 Files Changed 4 +24 -15
4 changed files with 24 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CHANGELOG.md
View File

@ -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

24
mission/controller/AcsController.cpp
View File

@ -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);
} }
} }

7
mission/controller/AcsController.h
View File

@ -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);

3
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -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,