eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Fix Desaturation for Faulty Wheels #865

Merged
meggert merged 7 commits from fix-desaturation into main 2024-02-29 12:56:19 +01:00
Conversation 0 Commits 7 Files Changed 8 +81 -61
8 changed files with 81 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CHANGELOG.md
View File

@ -29,6 +29,7 @@ will consitute of a breaking change warranting a new major release:
- The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
and is triggered by the `AcsController` now.
- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
- Fixed calculation of desaturation torque for faulty RWs.
## Changed
@ -42,6 +43,7 @@ will consitute of a breaking change warranting a new major release:
also limits the rotation for the reference target quaternion to prevent spikes in required
rotation rates.
- Updated QUEST and Sun Vector Params to new values.
- Removed the satellites's angular momentum from desaturation calculation.
## Added

42
mission/controller/AcsController.cpp
View File

@ -396,7 +396,7 @@ void AcsController::performPointingCtrl() {
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);
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail);
if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@ -433,10 +433,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_TARGET:
@ -456,10 +456,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_TARGET_GS:
@ -476,10 +476,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_NADIR:
@ -498,10 +498,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
case acs::PTG_INERTIAL:
@ -520,10 +520,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break;
default:
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;

2
mission/controller/AcsController.h
View File

@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipoleMtqs[3] = {0, 0, 0};
acsctrl::RwAvail rwAvail;
#if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0;
#endif

24
mission/controller/acs/Guidance.cpp
View File

@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
if (rw1valid and rw2valid and rw3valid and rw4valid) {
if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
} else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
} else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
} else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
} else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
not rwAvail->rw4avail) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE;

3
mission/controller/acs/Guidance.h
View File

@ -44,7 +44,8 @@ class Guidance {
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double &errorAngle);
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
acsctrl::RwAvail *rwAvail);
private:
const AcsParameters *acsParameters;

44
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -1,11 +1,5 @@
#include "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {}
@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
// Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(
@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
4);
}
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail,
AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid,
const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw0, const int32_t speedRw1,
const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
if (not magFieldBValid or not pointingLawParameters->desatOn) {
return;
@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// calculate angular momentum of the satellite
double angMomentumSat[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
angMomentumSat, 3, 3, 1);
// calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
// relocate RW speed zero to nullspace RW speed
double refSpeedRws[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
if (not allRwAvailable) {
if (not rwAvail->rw1avail) {
refSpeedRws[0] = 0.0;
} else if (not rwAvail->rw2avail) {
refSpeedRws[1] = 0.0;
} else if (not rwAvail->rw3avail) {
refSpeedRws[2] = 0.0;
} else if (not rwAvail->rw4avail) {
refSpeedRws[3] = 0.0;
}
}
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
// convert speed from 10 RPM to 1 RPM
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
// convert to rad/s
@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
// calculate total angular momentum
double angMomentumTotal[3] = {0, 0, 0};
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
angMomentumTotal, 3);
// resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);

18
mission/controller/acs/control/PtgCtrl.h
View File

@ -1,11 +1,16 @@
#ifndef PTGCTRL_H_
#define PTGCTRL_H_
#include <math.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
class PtgCtrl {
/*
@ -38,10 +43,11 @@ class PtgCtrl {
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const double *satRate,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *mgtDpDes);
void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
AcsParameters::PointingLawParameters *pointingLawParameters,
const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
double *mgtDpDes);
/* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after anti-stiction

7
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
//! [EXPORT] : [COMMENT] Multiple RWs have failed.
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
struct RwAvail {
bool rw1avail = false;
bool rw2avail = false;
bool rw3avail = false;
bool rw4avail = false;
};
enum SetIds : uint32_t {
MGM_SENSOR_DATA,
MGM_PROCESSED_DATA,