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
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #846
This commit is contained in:
commit
83fc8a633e
@ -23,6 +23,9 @@ will consitute of a breaking change warranting a new major release:
|
||||
## Changed
|
||||
|
||||
- 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
|
||||
|
||||
|
@ -66,8 +66,10 @@ enum Source : uint8_t {
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
||||
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
|
||||
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation.
|
||||
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
||||
//! [EXPORT] : [COMMENT] The limits for the rotation in pointing mode were violated.
|
||||
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
|
||||
//! cannot be maintained.
|
||||
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
|
||||
|
@ -195,6 +195,8 @@ void AcsController::performAttitudeControl() {
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
|
||||
handleDetumbling();
|
||||
|
||||
switch (mode) {
|
||||
case acs::SAFE:
|
||||
switch (submode) {
|
||||
@ -284,33 +286,6 @@ void AcsController::performSafe() {
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
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);
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
@ -346,33 +321,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],
|
||||
@ -596,6 +544,62 @@ 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);
|
||||
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) {
|
||||
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,6 +96,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
enum class InternalState { STARTUP, INITIAL_DELAY, READY };
|
||||
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 */
|
||||
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
|
||||
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 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,
|
||||
|
@ -174,6 +174,7 @@ ReturnValue_t EiveSystem::initialize() {
|
||||
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_CRITICAL));
|
||||
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::PTG_RATE_VIOLATION));
|
||||
return Subsystem::initialize();
|
||||
}
|
||||
|
||||
@ -224,6 +225,16 @@ void EiveSystem::handleEventMessages() {
|
||||
}
|
||||
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;
|
||||
default:
|
||||
|
Loading…
Reference in New Issue
Block a user