acs ctrl state machine done
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
This commit is contained in:
parent
ba6eac505e
commit
c67f65369c
@ -178,41 +178,6 @@ void AcsController::performAttitudeControl() {
|
||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &attitudeEstimationData, &acsParameters);
|
||||
|
||||
if (not submode == acs::SafeSubmode::DETUMBLE) {
|
||||
}
|
||||
|
||||
switch (detumbleState) {
|
||||
case DetumbleState::NO_DETUMBLE:
|
||||
if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
|
||||
break;
|
||||
}
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
|
||||
}
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_PTG:
|
||||
triggerEvent(acs::PTG_RATE_VIOLATION);
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_SAFE:
|
||||
detumbleCounter = 0;
|
||||
// Triggers detumble mode transition in subsystem
|
||||
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
||||
startTransition(mode, acs::SafeSubmode::DETUMBLE);
|
||||
break;
|
||||
case DetumbleState::IN_DETUMBLE:
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl;
|
||||
}
|
||||
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
@ -354,33 +319,6 @@ void AcsController::performDetumble() {
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
if (acsParameters.safeModeControllerParameters.useMekf) {
|
||||
if (attitudeEstimationData.satRotRateMekf.isValid() and
|
||||
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (acsParameters.safeModeControllerParameters.useGyr) {
|
||||
if (gyrDataProcessed.gyrVecTot.isValid() and
|
||||
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
}
|
||||
} else if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
detumbleCounter = 0;
|
||||
// Triggers safe mode transition in subsystem
|
||||
triggerEvent(acs::SAFE_RATE_RECOVERY);
|
||||
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
||||
}
|
||||
|
||||
updateCtrlValData(safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
@ -612,6 +550,61 @@ void AcsController::performPointingCtrl() {
|
||||
acsParameters.rwHandlingParameters.rampTime);
|
||||
}
|
||||
|
||||
void AcsController::handleDetumbling() {
|
||||
switch (detumbleState) {
|
||||
case DetumbleState::NO_DETUMBLE:
|
||||
if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
|
||||
break;
|
||||
}
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
|
||||
}
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_PTG:
|
||||
triggerEvent(acs::PTG_RATE_VIOLATION);
|
||||
detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION;
|
||||
break;
|
||||
case DetumbleState::PTG_TO_SAFE_TRANSITION:
|
||||
if (mode == acs::AcsMode::SAFE) {
|
||||
detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
|
||||
}
|
||||
break;
|
||||
case DetumbleState::DETUMBLE_FROM_SAFE:
|
||||
detumbleCounter = 0;
|
||||
// Triggers detumble mode transition in subsystem
|
||||
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
||||
startTransition(mode, acs::SafeSubmode::DETUMBLE);
|
||||
break;
|
||||
case DetumbleState::IN_DETUMBLE:
|
||||
if (fusedRotRateData.rotRateTotal.isValid() and
|
||||
VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
|
||||
acsParameters.detumbleParameter.omegaDetumbleEnd) {
|
||||
detumbleCounter++;
|
||||
} else if (detumbleCounter > 0) {
|
||||
detumbleCounter -= 1;
|
||||
}
|
||||
|
||||
if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
|
||||
detumbleCounter = 0;
|
||||
// Triggers safe mode transition in subsystem
|
||||
triggerEvent(acs::RATE_RECOVERY);
|
||||
startTransition(mode, acs::SafeSubmode::DEFAULT);
|
||||
detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid DetumbleState: " << detumbleState << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
||||
if (not safeCtrlFailureFlag) {
|
||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
||||
|
@ -46,11 +46,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
uint16_t startAtIndex) override;
|
||||
|
||||
protected:
|
||||
void performAttitudeControl();
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
void performPointingCtrl();
|
||||
|
||||
private:
|
||||
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
||||
static constexpr double ZERO_VEC3[3] = {0, 0, 0};
|
||||
@ -101,7 +96,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
||||
InternalState internalState = InternalState::STARTUP;
|
||||
|
||||
enum class DetumbleState { NO_DETUMBLE, DETUMBLE_FROM_SAFE, DETUMBLE_FROM_PTG, IN_DETUMBLE };
|
||||
enum class DetumbleState {
|
||||
NO_DETUMBLE,
|
||||
DETUMBLE_FROM_PTG,
|
||||
PTG_TO_SAFE_TRANSITION,
|
||||
DETUMBLE_FROM_SAFE,
|
||||
IN_DETUMBLE
|
||||
};
|
||||
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
|
||||
|
||||
/** Device command IDs */
|
||||
@ -137,6 +138,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
void modeChanged(Mode_t mode, Submode_t submode);
|
||||
void announceMode(bool recursive);
|
||||
|
||||
void performAttitudeControl();
|
||||
void performSafe();
|
||||
void performDetumble();
|
||||
void performPointingCtrl();
|
||||
|
||||
void handleDetumbling();
|
||||
|
||||
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
||||
|
||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
|
Loading…
Reference in New Issue
Block a user