Merge pull request 'RW Status Check for ACS Ctrl' (#382) from eggert/rw-status-check into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #382 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
f41cf14df5
10
CHANGELOG.md
10
CHANGELOG.md
@ -17,6 +17,13 @@ change warranting a new major release:
|
|||||||
|
|
||||||
# [unreleased]
|
# [unreleased]
|
||||||
|
|
||||||
|
## Added
|
||||||
|
|
||||||
|
- In case the ACS Controller does recognize more than one RW to be invalid and therefore not
|
||||||
|
available, it does not perform pointing control but aborts shortly after `sensorProcessing`. If the
|
||||||
|
problem persits for 5 ACS cycles, the `MULTIPLE_RW_INVALID` event is triggered, which invokes the
|
||||||
|
transition of the `AcsSubsystem` to safe mode.
|
||||||
|
|
||||||
## Changed
|
## Changed
|
||||||
|
|
||||||
- Igrf13 model vector now outputs as uT instead of nT
|
- Igrf13 model vector now outputs as uT instead of nT
|
||||||
@ -25,6 +32,9 @@ change warranting a new major release:
|
|||||||
|
|
||||||
## Fixed
|
## Fixed
|
||||||
- Fixed values for GYR sensor fusion
|
- Fixed values for GYR sensor fusion
|
||||||
|
- Fixed speed types for `rwHandlingParameter`
|
||||||
|
- Pseudo inverse used for allocating torque to RWs and RW antistiction now actually consider the
|
||||||
|
state of the RWs
|
||||||
|
|
||||||
# [v1.27.2] 2023-02-14
|
# [v1.27.2] 2023-02-14
|
||||||
|
|
||||||
|
@ -23,6 +23,8 @@ static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
|
|||||||
static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
|
static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
|
||||||
//!< The system has recovered from a safe rate rotation violation.
|
//!< The system has recovered from a safe rate rotation violation.
|
||||||
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
||||||
|
//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
|
||||||
|
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
|
||||||
|
|
||||||
extern const char* getModeStr(AcsMode mode);
|
extern const char* getModeStr(AcsMode mode);
|
||||||
|
|
||||||
|
@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
|
|||||||
detumble(&acsParameters),
|
detumble(&acsParameters),
|
||||||
ptgCtrl(&acsParameters),
|
ptgCtrl(&acsParameters),
|
||||||
detumbleCounter{0},
|
detumbleCounter{0},
|
||||||
|
multipleRwUnavailableCounter{0},
|
||||||
parameterHelper(this),
|
parameterHelper(this),
|
||||||
mgmDataRaw(this),
|
mgmDataRaw(this),
|
||||||
mgmDataProcessed(this),
|
mgmDataProcessed(this),
|
||||||
@ -263,7 +264,16 @@ void AcsController::performPointingCtrl() {
|
|||||||
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
|
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
|
||||||
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
|
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
|
||||||
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}};
|
||||||
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||||
|
if (result == returnvalue::FAILED) {
|
||||||
|
multipleRwUnavailableCounter++;
|
||||||
|
if (multipleRwUnavailableCounter > 4) {
|
||||||
|
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
} else {
|
||||||
|
multipleRwUnavailableCounter = 0;
|
||||||
|
}
|
||||||
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
|
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
|
||||||
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
||||||
double mgtDpDes[3] = {0, 0, 0};
|
double mgtDpDes[3] = {0, 0, 0};
|
||||||
@ -385,10 +395,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (enableAntiStiction) {
|
if (enableAntiStiction) {
|
||||||
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
|
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
|
||||||
int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
|
||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value};
|
|
||||||
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||||
|
@ -50,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
|||||||
PtgCtrl ptgCtrl;
|
PtgCtrl ptgCtrl;
|
||||||
|
|
||||||
uint8_t detumbleCounter;
|
uint8_t detumbleCounter;
|
||||||
|
uint8_t multipleRwUnavailableCounter;
|
||||||
|
|
||||||
ParameterHelper parameterHelper;
|
ParameterHelper parameterHelper;
|
||||||
|
|
||||||
|
@ -294,17 +294,17 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(rwMatrices.pseudoInverse);
|
parameterWrapper->set(rwMatrices.pseudoInverse);
|
||||||
break;
|
break;
|
||||||
case 0x2:
|
case 0x2:
|
||||||
parameterWrapper->set(rwMatrices.without0);
|
|
||||||
break;
|
|
||||||
case 0x3:
|
|
||||||
parameterWrapper->set(rwMatrices.without1);
|
parameterWrapper->set(rwMatrices.without1);
|
||||||
break;
|
break;
|
||||||
case 0x4:
|
case 0x3:
|
||||||
parameterWrapper->set(rwMatrices.without2);
|
parameterWrapper->set(rwMatrices.without2);
|
||||||
break;
|
break;
|
||||||
case 0x5:
|
case 0x4:
|
||||||
parameterWrapper->set(rwMatrices.without3);
|
parameterWrapper->set(rwMatrices.without3);
|
||||||
break;
|
break;
|
||||||
|
case 0x5:
|
||||||
|
parameterWrapper->set(rwMatrices.without4);
|
||||||
|
break;
|
||||||
case 0x6:
|
case 0x6:
|
||||||
parameterWrapper->set(rwMatrices.nullspace);
|
parameterWrapper->set(rwMatrices.nullspace);
|
||||||
break;
|
break;
|
||||||
|
@ -789,8 +789,8 @@ class AcsParameters : public HasParametersIF {
|
|||||||
struct RwHandlingParameters {
|
struct RwHandlingParameters {
|
||||||
double inertiaWheel = 0.000028198;
|
double inertiaWheel = 0.000028198;
|
||||||
double maxTrq = 0.0032; // 3.2 [mNm]
|
double maxTrq = 0.0032; // 3.2 [mNm]
|
||||||
double stictionSpeed = 100; // 80; // RPM
|
int32_t stictionSpeed = 100; // RPM
|
||||||
double stictionReleaseSpeed = 120; // RPM
|
int32_t stictionReleaseSpeed = 120; // RPM
|
||||||
double stictionTorque = 0.0006;
|
double stictionTorque = 0.0006;
|
||||||
|
|
||||||
uint16_t rampTime = 10;
|
uint16_t rampTime = 10;
|
||||||
@ -802,13 +802,13 @@ class AcsParameters : public HasParametersIF {
|
|||||||
{0.3907, 0.3907, 0.3907, 0.3907}};
|
{0.3907, 0.3907, 0.3907, 0.3907}};
|
||||||
double pseudoInverse[4][3] = {
|
double pseudoInverse[4][3] = {
|
||||||
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
|
{0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
|
||||||
double without0[4][3] = {
|
|
||||||
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
|
|
||||||
double without1[4][3] = {
|
double without1[4][3] = {
|
||||||
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
|
{0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
|
||||||
double without2[4][3] = {
|
double without2[4][3] = {
|
||||||
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
|
{0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
|
||||||
double without3[4][3] = {
|
double without3[4][3] = {
|
||||||
|
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
|
||||||
|
double without4[4][3] = {
|
||||||
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
|
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
|
||||||
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
|
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
|
||||||
} rwMatrices;
|
} rwMatrices;
|
||||||
|
@ -610,104 +610,33 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
|
|||||||
// under 150 arcsec ??
|
// under 150 arcsec ??
|
||||||
}
|
}
|
||||||
|
|
||||||
void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
|
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||||
if (sensorValues->rw1Set.isValid() && sensorValues->rw2Set.isValid() &&
|
double *rwPseudoInv) {
|
||||||
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
|
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
|
bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
|
bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
|
bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
|
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
|
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
|
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
|
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
|
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
|
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
|
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
|
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
|
|
||||||
|
|
||||||
}
|
if (rw1valid && rw2valid && rw3valid && rw4valid) {
|
||||||
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.pseudoInverse, 12 * sizeof(double));
|
||||||
else if (!(sensorValues->rw1Set.isValid()) && sensorValues->rw2Set.isValid() &&
|
return returnvalue::OK;
|
||||||
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
|
} else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without1, 12 * sizeof(double));
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
|
return returnvalue::OK;
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
|
} else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without2, 12 * sizeof(double));
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
|
return returnvalue::OK;
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
|
} else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without3, 12 * sizeof(double));
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
|
return returnvalue::OK;
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
|
} else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
|
std::memcpy(rwPseudoInv, acsParameters.rwMatrices.without4, 12 * sizeof(double));
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
|
return returnvalue::OK;
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
|
} else {
|
||||||
}
|
|
||||||
|
|
||||||
else if ((sensorValues->rw1Set.isValid()) && !(sensorValues->rw2Set.isValid()) &&
|
|
||||||
sensorValues->rw3Set.isValid() && sensorValues->rw4Set.isValid()) {
|
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
|
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
|
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
|
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
|
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
|
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
|
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
|
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
|
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
|
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
|
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
|
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
|
|
||||||
}
|
|
||||||
|
|
||||||
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
|
|
||||||
!(sensorValues->rw3Set.isValid()) && sensorValues->rw4Set.isValid()) {
|
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
|
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
|
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
|
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
|
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
|
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
|
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
|
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
|
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
|
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
|
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
|
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
|
|
||||||
}
|
|
||||||
|
|
||||||
else if ((sensorValues->rw1Set.isValid()) && (sensorValues->rw2Set.isValid()) &&
|
|
||||||
(sensorValues->rw3Set.isValid()) && !(sensorValues->rw4Set.isValid())) {
|
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
|
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
|
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
|
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
|
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
|
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
|
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
|
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
|
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
|
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
|
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
|
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
|
|
||||||
}
|
|
||||||
|
|
||||||
else {
|
|
||||||
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
|
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
|
||||||
// Does not make sense, but is implemented that way in MATLAB ?!
|
// Does not make sense, but is implemented that way in MATLAB ?!
|
||||||
// Thought: It does not really play a role, because in case there are more then one
|
// Thought: It does not really play a role, because in case there are more then one
|
||||||
// reaction wheel invalid the pointing control is destined to fail.
|
// reaction wheel invalid the pointing control is destined to fail.
|
||||||
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
|
return returnvalue::FAILED;
|
||||||
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
|
|
||||||
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
|
|
||||||
rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
|
|
||||||
rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
|
|
||||||
rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
|
|
||||||
rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
|
|
||||||
rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
|
|
||||||
rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
|
|
||||||
rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
|
|
||||||
rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
|
|
||||||
rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -67,7 +67,7 @@ class Guidance {
|
|||||||
|
|
||||||
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
||||||
// reation wheel maybe can be done in "commanding.h"
|
// reation wheel maybe can be done in "commanding.h"
|
||||||
void getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
|
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AcsParameters acsParameters;
|
AcsParameters acsParameters;
|
||||||
|
@ -161,8 +161,14 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara
|
|||||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
|
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand) {
|
||||||
double *torqueCommand) {
|
bool rwAvailable[4] = {
|
||||||
|
(sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()),
|
||||||
|
(sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()),
|
||||||
|
(sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()),
|
||||||
|
(sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid())};
|
||||||
|
int32_t omegaRw[4] = {sensorValues->rw1Set.currSpeed.value, sensorValues->rw2Set.currSpeed.value,
|
||||||
|
sensorValues->rw3Set.currSpeed.value, sensorValues->rw4Set.currSpeed.value};
|
||||||
for (uint8_t i = 0; i < 4; i++) {
|
for (uint8_t i = 0; i < 4; i++) {
|
||||||
if (rwAvailable[i]) {
|
if (rwAvailable[i]) {
|
||||||
if (torqueMemory[i] != 0) {
|
if (torqueMemory[i] != 0) {
|
||||||
|
@ -54,11 +54,10 @@ class PtgCtrl {
|
|||||||
const int32_t *speedRw3, double *rwTrqNs);
|
const int32_t *speedRw3, double *rwTrqNs);
|
||||||
|
|
||||||
/* @brief: Commands the stiction torque in case wheel speed is to low
|
/* @brief: Commands the stiction torque in case wheel speed is to low
|
||||||
* @param: rwAvailable Boolean Flag for all reaction wheels
|
* @param: sensorValues class containing all RW related values
|
||||||
* omegaRw current wheel speed of reaction wheels
|
|
||||||
* torqueCommand modified torque after antistiction
|
* torqueCommand modified torque after antistiction
|
||||||
*/
|
*/
|
||||||
void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
|
void rwAntistiction(ACS::SensorValues *sensorValues, double *torqueCommand);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AcsParameters::RwHandlingParameters *rwHandlingParameters;
|
AcsParameters::RwHandlingParameters *rwHandlingParameters;
|
||||||
|
@ -42,6 +42,11 @@ ReturnValue_t AcsSubsystem::initialize() {
|
|||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl;
|
sif::error << "AcsSubsystem: Subscribing for acs::SAFE_RATE_RECOVERY failed" << std::endl;
|
||||||
}
|
}
|
||||||
|
result =
|
||||||
|
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::MULTIPLE_RW_INVALID));
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
|
||||||
|
}
|
||||||
return Subsystem::initialize();
|
return Subsystem::initialize();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -64,12 +69,13 @@ void AcsSubsystem::handleEventMessages() {
|
|||||||
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
|
sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (event.getEvent() == acs::SAFE_RATE_RECOVERY) {
|
if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
|
||||||
|
event.getEvent() == acs::MULTIPLE_RW_INVALID) {
|
||||||
CommandMessage msg;
|
CommandMessage msg;
|
||||||
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
|
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
|
||||||
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
|
ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
|
||||||
if (result != returnvalue::OK) {
|
if (result != returnvalue::OK) {
|
||||||
sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl;
|
sif::error << "AcsSubsystem: sending SAFE mode cmd to self has failed" << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
Loading…
Reference in New Issue
Block a user