eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

acs ctrl state machine done
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit Details

This commit is contained in:
Marius Eggert 2024-01-29 09:32:01 +01:00
parent ba6eac505e
commit c67f65369c
2 changed files with 69 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
mission/controller/AcsController.cpp
View File

@ -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);

20
mission/controller/AcsController.h
View File

@ -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,