cleanup and detumble switch
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
This commit is contained in:
parent
34dde2640e
commit
543d147b37
@ -174,47 +174,31 @@ void AcsController::performSafe() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid());
|
||||
switch (safeCtrlStrat) {
|
||||
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_MEKF):
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
|
||||
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
|
||||
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
|
||||
satRateSafe, inertiaEive, magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_NONMEKF):
|
||||
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
|
||||
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
||||
susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive,
|
||||
magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_DAMPING):
|
||||
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
|
||||
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
|
||||
satRateSafe, sunTargetDir, magMomMtq, errAng);
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
break;
|
||||
case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
if (not safeCtrlFailureFlag) {
|
||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 1, 0);
|
||||
safeCtrlFailureFlag = true;
|
||||
}
|
||||
safeCtrlFailureCounter++;
|
||||
if (safeCtrlFailureCounter > 150) {
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
}
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
if (not safeCtrlFailureFlag) {
|
||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 0, 1);
|
||||
safeCtrlFailureFlag = true;
|
||||
}
|
||||
safeCtrlFailureCounter++;
|
||||
if (safeCtrlFailureCounter > 150) {
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
}
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -270,11 +254,27 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::MEKF_RECOVERY);
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(),
|
||||
gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid());
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
|
||||
acsParameters.detumbleParameter.gainD);
|
||||
switch (safeCtrlStrat) {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
|
||||
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainD);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
|
||||
magMomMtq, acsParameters.detumbleParameter.gainD);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
|
||||
safeCtrlFailure(1, 0);
|
||||
break;
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
@ -498,6 +498,18 @@ void AcsController::performPointingCtrl() {
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
}
|
||||
|
||||
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
||||
if (not safeCtrlFailureFlag) {
|
||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
||||
safeCtrlFailureFlag = true;
|
||||
}
|
||||
safeCtrlFailureCounter++;
|
||||
if (safeCtrlFailureCounter > 150) {
|
||||
safeCtrlFailureFlag = false;
|
||||
safeCtrlFailureCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
uint16_t dipoleTorqueDuration) {
|
||||
{
|
||||
@ -581,7 +593,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
|
||||
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 = SafeCtrl::SafeModeStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
|
||||
ctrlValData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
|
||||
@ -12,18 +12,17 @@
|
||||
#include <mission/acs/imtqHelpers.h>
|
||||
#include <mission/acs/rwHelpers.h>
|
||||
#include <mission/acs/susMax1227Helpers.h>
|
||||
#include <mission/controller/acs/ActuatorCmd.h>
|
||||
#include <mission/controller/acs/Guidance.h>
|
||||
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
||||
#include <mission/controller/acs/Navigation.h>
|
||||
#include <mission/controller/acs/SensorProcessing.h>
|
||||
#include <mission/controller/acs/control/Detumble.h>
|
||||
#include <mission/controller/acs/control/PtgCtrl.h>
|
||||
#include <mission/controller/acs/control/SafeCtrl.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
#include <mission/utility/trace.h>
|
||||
|
||||
#include "acs/ActuatorCmd.h"
|
||||
#include "acs/Guidance.h"
|
||||
#include "acs/MultiplicativeKalmanFilter.h"
|
||||
#include "acs/Navigation.h"
|
||||
#include "acs/SensorProcessing.h"
|
||||
#include "acs/control/Detumble.h"
|
||||
#include "acs/control/PtgCtrl.h"
|
||||
#include "acs/control/SafeCtrl.h"
|
||||
#include "controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
|
||||
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
||||
public:
|
||||
static constexpr dur_millis_t INIT_DELAY = 500;
|
||||
@ -106,6 +105,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
void modeChanged(Mode_t mode, Submode_t submode);
|
||||
void announceMode(bool recursive);
|
||||
|
||||
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
||||
|
||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
uint16_t dipoleTorqueDuration);
|
||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user