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,
|
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||||
&susDataProcessed, &attitudeEstimationData, &acsParameters);
|
&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
|
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
|
||||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||||
if (not mekfInvalidFlag) {
|
if (not mekfInvalidFlag) {
|
||||||
@ -354,33 +319,6 @@ void AcsController::performDetumble() {
|
|||||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
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);
|
updateCtrlValData(safeCtrlStrat);
|
||||||
updateActuatorCmdData(cmdDipoleMtqs);
|
updateActuatorCmdData(cmdDipoleMtqs);
|
||||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||||
@ -612,6 +550,61 @@ void AcsController::performPointingCtrl() {
|
|||||||
acsParameters.rwHandlingParameters.rampTime);
|
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) {
|
void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
|
||||||
if (not safeCtrlFailureFlag) {
|
if (not safeCtrlFailureFlag) {
|
||||||
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
|
||||||
|
@ -46,11 +46,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
uint16_t startAtIndex) override;
|
uint16_t startAtIndex) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void performAttitudeControl();
|
|
||||||
void performSafe();
|
|
||||||
void performDetumble();
|
|
||||||
void performPointingCtrl();
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
|
||||||
static constexpr double ZERO_VEC3[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 };
|
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
||||||
InternalState internalState = InternalState::STARTUP;
|
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;
|
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
|
||||||
|
|
||||||
/** Device command IDs */
|
/** Device command IDs */
|
||||||
@ -137,6 +138,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
void modeChanged(Mode_t mode, Submode_t submode);
|
void modeChanged(Mode_t mode, Submode_t submode);
|
||||||
void announceMode(bool recursive);
|
void announceMode(bool recursive);
|
||||||
|
|
||||||
|
void performAttitudeControl();
|
||||||
|
void performSafe();
|
||||||
|
void performDetumble();
|
||||||
|
void performPointingCtrl();
|
||||||
|
|
||||||
|
void handleDetumbling();
|
||||||
|
|
||||||
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
|
||||||
|
|
||||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||||
|
Loading…
Reference in New Issue
Block a user