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

add SafeModeStrategy to detumble laws

Browse Source
This commit is contained in:
Marius Eggert 2023-04-06 10:46:58 +02:00
parent 205a672680
commit 34dde2640e
2 changed files with 35 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
mission/controller/acs/control/Detumble.cpp
View File

@ -1,61 +1,45 @@
#include "Detumble.h" #include "Detumble.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/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <math.h> #include <math.h>
#include "../util/MathOperations.h"
Detumble::Detumble() {} Detumble::Detumble() {}
Detumble::~Detumble() {} Detumble::~Detumble() {}
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
const double *magField, const bool magFieldValid, double *magMom, const bool magFieldRateValid) {
double gain) { if (not magFieldValid) {
if (!magRateValid || !magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
return DETUMBLE_NO_SENSORDATA; } else if (satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
} else if (magFieldRateValid) {
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
} else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
} }
// convert uT to T
double magFieldT[3], magRateT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
// control law
double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
VectorOperations<double>::mulScalar(magRateT, factor, magMom, 3);
return returnvalue::OK;
} }
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, void Detumble::bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double dipolMax) { double gain) {
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
return returnvalue::OK;
}
ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid,
const double *magField, const bool *magFieldValid,
double *magMom, double gain) {
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
// convert uT to T // convert uT to T
double magFieldT[3]; double magFieldBT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// control law // control law
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2); double factor = gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3); VectorOperations<double>::normalize(magFieldBT, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct); VectorOperations<double>::cross(satRotRateB, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3); VectorOperations<double>::mulScalar(crossProduct, factor, magMomB, 3);
return returnvalue::OK; }
void Detumble::bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB,
double gain) {
// convert uT to T
double magFieldBT[3], magRateBT[3];
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
VectorOperations<double>::mulScalar(magRateB, 1e-6, magRateBT, 3);
// control law
double factor = -gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(magRateBT, factor, magMomB, 3);
} }

20
mission/controller/acs/control/Detumble.h
View File

@ -2,30 +2,22 @@
#define ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_
#include <fsfw/returnvalues/returnvalue.h> #include <fsfw/returnvalues/returnvalue.h>
#include <mission/acs/defs.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <time.h>
#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "eive/resultClassIds.h"
class Detumble { class Detumble {
public: public:
Detumble(); Detumble();
virtual ~Detumble(); virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); const bool magFieldRateValid);
ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
const bool magFieldValid, double *magMom, double gain); double gain);
ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, void bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB, double gain);
double dipolMax);
ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom, double gain);
private: private:
}; };