Merge pull request 'Always trigger Detumble' (#846) from detumble-fix into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good

Reviewed-on: #846
This commit is contained in:
Marius Eggert 2024-01-29 17:02:29 +01:00
commit 83fc8a633e
5 changed files with 92 additions and 61 deletions

View File

@ -23,6 +23,9 @@ will consitute of a breaking change warranting a new major release:
## Changed ## Changed
- Increased allowed mode transition time for PLOC SUPV. - Increased allowed mode transition time for PLOC SUPV.
- Detumbling can now be triggered from all modes of the `AcsController`. In case the
current mode is a higher pointing mode, the STR will be set to faulty, to trigger a
transition to safe first. Then, in a second step, a transition to detumble is triggered.
## Fixed ## Fixed

View File

@ -66,8 +66,10 @@ enum Source : uint8_t {
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation. //! [EXPORT] : [COMMENT] The limits for the rotation in pointing mode were violated.
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM);
//! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation.
static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
//! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes
//! cannot be maintained. //! cannot be maintained.
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);

View File

@ -195,6 +195,8 @@ void AcsController::performAttitudeControl() {
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
handleDetumbling();
switch (mode) { switch (mode) {
case acs::SAFE: case acs::SAFE:
switch (submode) { switch (submode) {
@ -284,33 +286,6 @@ void AcsController::performSafe() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
// detumble check and switch
if (acsParameters.safeModeControllerParameters.useMekf) {
if (attitudeEstimationData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else if (acsParameters.safeModeControllerParameters.useGyr) {
if (gyrDataProcessed.gyrVecTot.isValid() and
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} else 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) {
detumbleCounter = 0;
// Triggers detumble mode transition in subsystem
triggerEvent(acs::SAFE_RATE_VIOLATION);
startTransition(mode, acs::SafeSubmode::DETUMBLE);
}
updateCtrlValData(errAng, safeCtrlStrat); updateCtrlValData(errAng, safeCtrlStrat);
updateActuatorCmdData(cmdDipoleMtqs); updateActuatorCmdData(cmdDipoleMtqs);
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
@ -346,33 +321,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],
@ -596,6 +544,62 @@ 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);
detumbleState = DetumbleState::IN_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" << 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);

View File

@ -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,6 +96,15 @@ 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_PTG,
PTG_TO_SAFE_TRANSITION,
DETUMBLE_FROM_SAFE,
IN_DETUMBLE
};
DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
/** Device command IDs */ /** Device command IDs */
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1; static const DeviceCommandId_t RESET_MEKF = 0x1;
@ -134,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,

View File

@ -174,6 +174,7 @@ ReturnValue_t EiveSystem::initialize() {
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL)); manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::PTG_RATE_VIOLATION));
return Subsystem::initialize(); return Subsystem::initialize();
} }
@ -224,6 +225,16 @@ void EiveSystem::handleEventMessages() {
} }
break; break;
} }
case acs::PTG_RATE_VIOLATION: {
CommandMessage msg;
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
ReturnValue_t result = MessageQueueSenderIF::sendMessage(
strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
<< std::endl;
}
}
} }
break; break;
default: default: