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

added Antistiction, added Nadir Pointing, added performSafe()
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Marquardt 2022-11-08 13:48:50 +01:00
parent 75ab11fc35
commit 20936faec6
9 changed files with 213 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
mission/controller/AcsController.cpp
View File

@ -8,6 +8,7 @@ AcsController::AcsController(object_id_t objectId)
navigation(&acsParameters),
actuatorCmd(&acsParameters),
guidance(&acsParameters),
safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters),
detumbleCounter{0},
@ -35,7 +36,7 @@ void AcsController::performControlOperation() {
if (mode != MODE_OFF) {
switch (submode) {
case SUBMODE_SAFE:
// performSafe();
performSafe();
break;
case SUBMODE_DETUMBLE:
@ -45,9 +46,14 @@ void AcsController::performControlOperation() {
case SUBMODE_PTG_GS:
performPointingCtrl();
break;
case SUBMODE_PTG_SUN:
performPointingCtrlSun();
break;
performPointingCtrl();
break;
case SUBMODE_PTG_NADIR:
performPointingCtrl();
break;
}
}
break;
@ -75,7 +81,40 @@ void AcsController::performControlOperation() {
// DEBUG END
}
void AcsController::performSafe() {}
void AcsController::performSafe() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now;
Clock::getClock_timeval(&now);
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
// Give desired satellite rate and sun direction to align
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
// IF MEKF is working
double magMomMtq[3] = {0, 0, 0};
bool magMomMtqValid = false;
if ( !validMekf ) { // Valid = 0, Failed = 1
safeCtrl.safeMekf(now, (outputValues.quatMekfBJ), &(outputValues.quatMekfBJValid),
(outputValues.magFieldModel), &(outputValues.magFieldModelValid),
(outputValues.sunDirModel), &(outputValues.sunDirModelValid),
(outputValues.satRateMekf), &(outputValues.satRateMekfValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
}
else {
safeCtrl.safeNoMekf(now, outputValues.sunDirEst, &outputValues.sunDirEstValid,
outputValues.sunVectorDerivative, &(outputValues.sunVectorDerivativeValid),
outputValues.magFieldEst, &(outputValues.magFieldEstValid),
outputValues.magneticFieldVectorDerivative, &(outputValues.magneticFieldVectorDerivativeValid),
sunTargetDir, satRateSafe, magMomMtq, &magMomMtqValid);
}
}
void AcsController::performDetumble() {
ACS::SensorValues sensorValues;
@ -121,7 +160,18 @@ void AcsController::performPointingCtrl() {
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
switch (submode) {
case SUBMODE_PTG_GS:
guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break;
case SUBMODE_PTG_SUN:
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
break;
case SUBMODE_PTG_NADIR:
guidance.targetQuatNadirPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
break;
}
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -132,45 +182,21 @@ void AcsController::performPointingCtrl() {
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
if (acsParameters.pointingModeControllerParameters.enableAntiStiction) {
bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET?
double rwSpeed[4] = {&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value)};
ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
}
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}
void AcsController::performPointingCtrlSun() {
ACS::SensorValues sensorValues;
ACS::OutputValues outputValues;
timeval now; // Übergabe ?
sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
ReturnValue_t validMekf;
navigation.useMekf(&sensorValues, &outputValues, &validMekf);
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
double rwTrqNs[4] = {0, 0, 0, 0};
ptgCtrl.ptgNullspace(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input
actuatorCmd.cmdSpeedToRws(
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
rwTrqNs, cmdSpeedRws);
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol
ptgCtrl.ptgDesaturation(
outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,

3
mission/controller/AcsController.h
View File

@ -9,6 +9,7 @@
#include "acs/Guidance.h"
#include "acs/Navigation.h"
#include "acs/SensorProcessing.h"
#include "acs/control/SafeCtrl.h"
#include "acs/control/Detumble.h"
#include "acs/control/PtgCtrl.h"
#include "controllerdefinitions/AcsCtrlDefinitions.h"
@ -34,7 +35,6 @@ class AcsController : public ExtendedControllerBase {
void performSafe();
void performDetumble();
void performPointingCtrl();
void performPointingCtrlSun();
private:
AcsParameters acsParameters;
@ -43,6 +43,7 @@ class AcsController : public ExtendedControllerBase {
ActuatorCmd actuatorCmd;
Guidance guidance;
SafeCtrl safeCtrl;
Detumble detumble;
PtgCtrl ptgCtrl;

7
mission/controller/acs/AcsParameters.h
View File

@ -776,6 +776,9 @@ class AcsParameters /*: public HasParametersIF*/ {
double rw3orientationMatrix[3][3];
double inertiaWheel = 0.000028198;
double maxTrq = 0.0032; // 3.2 [mNm]
double stictionSpeed = 80; //RPM
double stictionReleaseSpeed = 120; //RPM
double stictionTorque = 0.0006;
} rwHandlingParameters;
struct RwMatrices {
@ -842,10 +845,12 @@ class AcsParameters /*: public HasParametersIF*/ {
double desatMomentumRef[3] = {0, 0, 0};
double deSatGainFactor = 1000;
bool desatOn = true;
bool enableAntiStiction = true;
double omegaEarth = 0.000072921158553;
} inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
double nadirRefDirection[3] = {-1, 0, 0}; //Camera
} pointingModeControllerParameters, inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
struct StrParameters {
double exclusionAngle = 20 * M_PI / 180;

23
mission/controller/acs/ActuatorCmd.cpp
View File

@ -23,26 +23,27 @@ ActuatorCmd::~ActuatorCmd(){
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
using namespace Math;
void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
// Scaling the commanded torque to a maximum value
double torque[4] = {0, 0, 0, 0};
double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
double maxValue = 0;
for (int i = 0; i < 4; i++) { // size of torque, always 4 ?
if (abs(torque[i]) > maxValue) {
maxValue = abs(torque[i]);
if (abs(rwTrq[i]) > maxValue) {
maxValue = abs(rwTrq[i]);
}
}
if (maxValue > maxTrq) {
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
double scalingFactor = maxTrq / maxValue;
VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
}
}
void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3,
const double *rwTorque, double *rwCmdSpeed) {
using namespace Math;
// Calculating the commanded speed in RPM for every reaction wheel
double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
@ -52,7 +53,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
// W_RW = Torque_RW / I_RW * delta t [rad/s]
double factor = commandTime / inertiaWheel * radToRpm;
VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
}

12
mission/controller/acs/ActuatorCmd.h
View File

@ -20,6 +20,14 @@ public:
ActuatorCmd(AcsParameters *acsParameters_); //Input mode ?
virtual ~ActuatorCmd();
/*
* @brief: scalingTorqueRws() scales the torque via maximum part in case this part is higher
* then the maximum torque
* @param: rwTrq given torque for reaction wheels
* rwTrqScaled possible scaled torque
*/
void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
/*
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction wheels, also
* will calculate the needed revolutions per minute for the RWs, which will be given
@ -29,8 +37,8 @@ public:
* rwCmdSpeed output revolutions per minute for every reaction wheel
*/
void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTrqIn,
const double *rwTrqNS, double *rwCmdSpeed);
const int32_t *speedRw2, const int32_t *speedRw3, const double *rwTorque,
double *rwCmdSpeed);
/*
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques

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

@ -234,6 +234,73 @@ void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *ou
refSatRate[2] = 0;
}
void Guidance::targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
double targetDirE[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
// Transformation between ECEF and IJK frame
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
// Transformation between ECEF and Body frame
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double quatBJ[4] = {0, 0, 0, 0};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ);
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
// Target Direction in the body frame
double targetDirB[3] = {0, 0, 0};
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
// rotation quaternion from two vectors
double refDir[3] = {0, 0, 0};
refDir[0] = acsParameters.targetModeControllerParameters.nadirRefDirection[0];
refDir[1] = acsParameters.targetModeControllerParameters.nadirRefDirection[1];
refDir[2] = acsParameters.targetModeControllerParameters.nadirRefDirection[2];
double noramlizedTargetDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
VectorOperations<double>::normalize(refDir, refDir, 3);
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
double normRefDir = VectorOperations<double>::norm(refDir, 3);
double crossDir[3] = {0, 0, 0};
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
targetQuat[0] = crossDir[0];
targetQuat[1] = crossDir[1];
targetQuat[2] = crossDir[2];
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};

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

@ -30,6 +30,10 @@ public:
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing
void targetQuatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );

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

@ -16,7 +16,7 @@
#include <fsfw/globalfunctions/sign.h>
#include <math.h>
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_): torqueMemory {0, 0, 0, 0}, omegaMemory {0, 0, 0, 0} {
loadAcsParameters(acsParameters_);
}
@ -164,3 +164,36 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}
void PtgCtrl::rwAntistiction(const bool* rwAvailable, const double* omegaRw,
double* torqueCommand) {
for (uint8_t i = 0; i < 4; i++) {
if (rwAvailable[i]) {
if (torqueMemory[i] != 0) {
if ((omegaRw[i] * torqueMemory[i])
> rwHandlingParameters->stictionReleaseSpeed) {
torqueMemory[i] = 0;
} else {
torqueCommand[i] = torqueMemory[i]
* rwHandlingParameters->stictionTorque;
}
} else {
if ((omegaRw[i] < rwHandlingParameters->stictionSpeed)
&& (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
if (omegaRw[i] < omegaMemory[i]) {
torqueMemory[i] = -1;
} else {
torqueMemory[i] = 1;
}
torqueCommand[i] = torqueMemory[i]
* rwHandlingParameters->stictionTorque;
}
}
} else {
torqueMemory[i] = 0;
}
omegaMemory[i] = omegaRw[i];
}
}
}

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

@ -52,11 +52,22 @@ class PtgCtrl {
void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
const int32_t *speedRw3, double *rwTrqNs);
/* @brief: Commands the stiction torque in case wheel speed is to low
* @param: rwAvailable Boolean Flag for all reaction wheels
* omegaRw current wheel speed of reaction wheels
* torqueCommand modified torque after antistiction
*/
void rwAntistiction(const bool* rwAvailable, const double* omegaRw,
double* torqueCommand);
private:
AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
AcsParameters::RwHandlingParameters *rwHandlingParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
AcsParameters::RwMatrices *rwMatrices;
double torqueMemory[4];
double omegaMemory[4];
};
#endif /* ACS_CONTROL_PTGCTRL_H_ */