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;
|
uint8_t enableAntiStiction = true;
|
||||||
|
bool allRwAvailable = true;
|
||||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||||
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||||
if (result == returnvalue::FAILED) {
|
if (result == Guidance::MULTIPLE_RW_UNAVAILABLE) {
|
||||||
if (multipleRwUnavailableCounter >=
|
if (multipleRwUnavailableCounter >=
|
||||||
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
|
||||||
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||||
@ -457,8 +458,10 @@ void AcsController::performPointingCtrl() {
|
|||||||
}
|
}
|
||||||
multipleRwUnavailableCounter++;
|
multipleRwUnavailableCounter++;
|
||||||
return;
|
return;
|
||||||
} else {
|
}
|
||||||
multipleRwUnavailableCounter = 0;
|
multipleRwUnavailableCounter = 0;
|
||||||
|
if (result == Guidance::SINGLE_RW_UNAVAILABLE) {
|
||||||
|
allRwAvailable = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Variables required for guidance
|
// Variables required for guidance
|
||||||
@ -476,10 +479,11 @@ void AcsController::performPointingCtrl() {
|
|||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
|
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
rwTrqNs);
|
rwTrqNs);
|
||||||
|
|
||||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||||
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
|
||||||
ptgCtrl.ptgDesaturation(
|
ptgCtrl.ptgDesaturation(
|
||||||
@ -500,7 +504,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.targetModeControllerParameters,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
rwTrqNs);
|
rwTrqNs);
|
||||||
@ -521,7 +525,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
rwTrqNs);
|
rwTrqNs);
|
||||||
@ -545,7 +549,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.nadirModeControllerParameters,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
rwTrqNs);
|
rwTrqNs);
|
||||||
@ -568,7 +572,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
errorSatRotRate, errorAngle);
|
errorSatRotRate, errorAngle);
|
||||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||||
*rwPseudoInv, torquePtgRws);
|
*rwPseudoInv, torquePtgRws);
|
||||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.inertialModeControllerParameters,
|
||||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
rwTrqNs);
|
rwTrqNs);
|
||||||
|
@ -108,13 +108,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
static const DeviceCommandId_t UPDATE_TLE = 0x3;
|
||||||
static const DeviceCommandId_t READ_TLE = 0x4;
|
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.
|
//! [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.
|
//! [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.
|
//! [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 initialize() override;
|
||||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||||
|
Loading…
Reference in New Issue
Block a user