eive-obsw/mission/controller/acs/control/Detumble.cpp

48 lines
2.0 KiB
C++
Raw Normal View History

2022-09-27 11:06:11 +02:00
#include "Detumble.h"
2022-12-01 15:56:55 +01:00
#include <fsfw/globalfunctions/math/VectorOperations.h>
2022-12-01 15:56:55 +01:00
#include <math.h>
Detumble::Detumble() {}
2022-12-01 15:56:55 +01:00
Detumble::~Detumble() {}
2023-11-02 16:59:09 +01:00
acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
2023-12-06 17:27:43 +01:00
const bool satRotRateValid,
const bool magFieldRateValid,
const bool useFullDetumbleLaw) {
2023-04-06 10:46:58 +02:00
if (not magFieldValid) {
2023-11-14 13:22:35 +01:00
return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
2023-04-14 11:38:05 +02:00
} else if (satRotRateValid and useFullDetumbleLaw) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
2023-04-06 10:46:58 +02:00
} else if (magFieldRateValid) {
2023-11-02 16:59:09 +01:00
return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
2023-04-06 10:46:58 +02:00
} else {
2023-11-14 13:22:35 +01:00
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
2022-12-14 10:13:28 +01:00
}
}
2023-04-06 10:46:58 +02:00
void Detumble::bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
double gain) {
// convert uT to T
double magFieldBT[3];
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// control law
double factor = gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldBT, magFieldNormed, 3);
VectorOperations<double>::cross(satRotRateB, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMomB, 3);
}
2022-10-21 16:46:09 +02:00
2023-04-06 10:46:58 +02:00
void Detumble::bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB,
double gain) {
// convert uT to T
2023-04-06 10:46:58 +02:00
double magFieldBT[3], magRateBT[3];
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
VectorOperations<double>::mulScalar(magRateB, 1e-6, magRateBT, 3);
// control law
2023-04-06 10:46:58 +02:00
double factor = -gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(magRateBT, factor, magMomB, 3);
2022-10-21 16:46:09 +02:00
}