|
|
|
@ -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 &&
|
|
|
|
@ -321,18 +323,18 @@ void AcsController::performDetumble() {
|
|
|
|
|
if (acsParameters.safeModeControllerParameters.useMekf) {
|
|
|
|
|
if (mekfData.satRotRateMekf.isValid() and
|
|
|
|
|
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
|
|
|
|
detumbleCounter++;
|
|
|
|
|
}
|
|
|
|
|
} else if (acsParameters.safeModeControllerParameters.useGyr) {
|
|
|
|
|
if (gyrDataProcessed.gyrVecTot.isValid() and
|
|
|
|
|
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
|
|
|
|
detumbleCounter++;
|
|
|
|
|
}
|
|
|
|
|
} else if (fusedRotRateData.rotRateTotal.isValid() and
|
|
|
|
|
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
|
|
|
|
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
|
|
|
|
detumbleCounter++;
|
|
|
|
|
} else if (detumbleCounter > 0) {
|
|
|
|
|
detumbleCounter -= 1;
|
|
|
|
@ -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
|
|
|
|
|