datasets are now always written in pointing modes
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
EIVE/eive-obsw/pipeline/pr-main This commit looks good

This commit is contained in:
Marius Eggert 2024-01-16 16:28:53 +01:00
parent 777b61376c
commit ebe4ca8084
2 changed files with 15 additions and 11 deletions

View File

@ -411,6 +411,8 @@ void AcsController::performPointingCtrl() {
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 +439,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 +595,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 +673,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 +684,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 +701,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);
}
}

View File

@ -52,6 +52,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
void performPointingCtrl();
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_VEC4[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 double* rwTargetTorque, const int32_t* rwTargetSpeed,
const int16_t* mtqTargetDipole);
void updateCtrlValData(uint8_t safeModeStrat);
void updateCtrlValData(double errAng, uint8_t safeModeStrat);
void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
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 writeTleToFs(const uint8_t* tle);