diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3f44f327..2310a99b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -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::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); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index aa2b9411..bb4e3c37 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -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;