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

better rw failure handling and use new classid location

This commit is contained in:
Marius Eggert 2024-01-26 10:59:57 +01:00
parent 10841a01b7
commit 7daeb9a148
2 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
mission/controller/AcsController.cpp
View File

@ -447,9 +447,10 @@ void AcsController::performPointingCtrl() {
}
uint8_t enableAntiStiction = true;
bool allRwAvailable = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
if (result == returnvalue::FAILED) {
if (result == Guidance::MULTIPLE_RW_UNAVAILABLE) {
if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
triggerEvent(acs::MULTIPLE_RW_INVALID);
@ -457,8 +458,10 @@ void AcsController::performPointingCtrl() {
}
multipleRwUnavailableCounter++;
return;
} else {
multipleRwUnavailableCounter = 0;
}
multipleRwUnavailableCounter = 0;
if (result == Guidance::SINGLE_RW_UNAVAILABLE) {
allRwAvailable = false;
}
// Variables required for guidance
@ -476,10 +479,11 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
@ -500,7 +504,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
@ -521,7 +525,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
@ -545,7 +549,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);
@ -568,7 +572,7 @@ void AcsController::performPointingCtrl() {
errorSatRotRate, errorAngle);
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
*rwPseudoInv, torquePtgRws);
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters,
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
rwTrqNs);

10
mission/controller/AcsController.h
View File

@ -108,13 +108,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
static const DeviceCommandId_t UPDATE_TLE = 0x3;
static const DeviceCommandId_t READ_TLE = 0x4;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
static constexpr ReturnValue_t FILE_DELETION_FAILED =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 0);
//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1);
static constexpr ReturnValue_t WRITE_FILE_FAILED =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 1);
//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2);
static constexpr ReturnValue_t READ_FILE_FAILED =
returnvalue::makeCode(acsctrl::IF_ACS_CTRL_ID, 2);
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;