eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Fixes for Pointing Modes #851

Merged
meggert merged 24 commits from ptg-fixes into main 2024-01-29 17:05:46 +01:00
Conversation 1 Commits 24 Files Changed 12 +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
Showing only changes of commit 20920fe22f - Show all commits

3
CHANGELOG.md
View File

@ -26,6 +26,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],
@ -600,6 +548,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);

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;
@ -126,6 +130,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: