added fused rotation calculation to detumble and write detumble strat to dataset
This commit is contained in:
parent
1f61f7d2e8
commit
bea918e861
@ -273,6 +273,8 @@ void AcsController::performDetumble() {
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
|
||||
&gyrDataProcessed, &fusedRotRateData);
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData, &acsParameters);
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
@ -345,7 +347,7 @@ void AcsController::performDetumble() {
|
||||
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
||||
}
|
||||
|
||||
disableCtrlValData();
|
||||
updateCtrlValData(safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration);
|
||||
@ -616,6 +618,23 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::updateCtrlValData(uint8_t 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 = 0;
|
||||
ctrlValData.errAng.setValid(false);
|
||||
std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
|
||||
ctrlValData.tgtRotRate.setValid(false);
|
||||
ctrlValData.safeStrat.value = safeModeStrat;
|
||||
ctrlValData.safeStrat.setValid(true);
|
||||
ctrlValData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
@ -646,17 +665,6 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
@ -117,10 +117,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(const double* tgtQuat, const double* errQuat, double errAng,
|
||||
const double* tgtRotRate);
|
||||
void disableCtrlValData();
|
||||
|
||||
/* ACS Sensor Values */
|
||||
ACS::SensorValues sensorValues;
|
||||
|
Loading…
Reference in New Issue
Block a user