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

Always trigger Detumble #846

Merged
meggert merged 10 commits from detumble-fix into main 2024-01-29 17:02:30 +01:00
Conversation 0 Commits 10 Files Changed 5 +92 -61
5 changed files with 92 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CHANGELOG.md
View File

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

6
mission/acs/defs.h
View File

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

112
mission/controller/AcsController.cpp
View File

@ -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) {
muellerr commented 2024-01-29 12:08:21 +01:00
Review
Copy Link

GYR and MEKF are not relevant anymore? (I just compared to the deleted code above..)

GYR and MEKF are not relevant anymore? (I just compared to the deleted code above..)
meggert commented 2024-01-29 14:10:57 +01:00
Review
Copy Link

for now they are irrelevant. the MEKF might come back someday

for now they are irrelevant. the MEKF might come back someday
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);

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

11
mission/system/EiveSystem.cpp
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(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: