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

62 lines
2.2 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/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>
2022-12-01 15:56:55 +01:00
#include <math.h>
2022-12-01 15:56:55 +01:00
#include "../util/MathOperations.h"
Detumble::Detumble() {}
2022-12-01 15:56:55 +01:00
Detumble::~Detumble() {}
2022-12-14 10:13:28 +01:00
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
const double *magField, const bool magFieldValid, double *magMom,
double gain) {
2022-12-14 10:13:28 +01:00
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
// 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);
2022-12-14 10:13:28 +01:00
return returnvalue::OK;
}
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double dipolMax) {
if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA;
}
for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]);
}
2023-02-01 15:37:10 +01:00
return returnvalue::OK;
}
2022-10-21 16:46:09 +02:00
ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid,
2023-02-01 15:37:10 +01:00
const double *magField, const bool *magFieldValid,
double *magMom, double gain) {
2023-02-01 15:37:10 +01:00
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
// convert uT to T
double magFieldT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
// control law
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
2023-02-01 15:37:10 +01:00
return returnvalue::OK;
2022-10-21 16:46:09 +02:00
}