better rw failure handling and use new classid location
This commit is contained in:
parent
10841a01b7
commit
7daeb9a148
@ -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);
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user