.idea
.run
archive
arduino
automation
bsp_egse
bsp_hosted
bsp_linux_board
bsp_q7s
bsp_te0720_1cfa
cmake
common
doc
dummies
fsfw
generators
hooks
linux
misc
mission
acs
cfdp
com
config
controller
acs
control
CMakeLists.txt
Detumble.cpp
Detumble.h
PtgCtrl.cpp
PtgCtrl.h
SafeCtrl.cpp
SafeCtrl.h
util
AcsParameters.cpp
AcsParameters.h
ActuatorCmd.cpp
ActuatorCmd.h
CMakeLists.txt
FusedRotationEstimation.cpp
FusedRotationEstimation.h
Guidance.cpp
Guidance.h
Igrf13Model.cpp
Igrf13Model.h
MultiplicativeKalmanFilter.cpp
MultiplicativeKalmanFilter.h
Navigation.cpp
Navigation.h
SensorProcessing.cpp
SensorProcessing.h
SensorValues.cpp
SensorValues.h
SusConverter.cpp
SusConverter.h
controllerdefinitions
AcsController.cpp
AcsController.h
CMakeLists.txt
PowerController.cpp
PowerController.h
ThermalController.cpp
ThermalController.h
tcsDefs.h
memory
payload
power
system
tcs
tmtc
utility
CMakeLists.txt
SolarArrayDeploymentHandler.cpp
SolarArrayDeploymentHandler.h
genericFactory.cpp
genericFactory.h
persistentTmStoreDefs.h
pollingSeqTables.cpp
pollingSeqTables.h
scheduling.cpp
scheduling.h
sysDefs.h
scripts
test
thirdparty
tmtc
unittest
watchdog
.clang-format
.dockerignore
.gitignore
.gitmodules
CHANGELOG.md
CMakeLists.txt
Justfile
LICENSE
NOTICE
README.md
clone-submodules-no-privlibs.sh
docker-compose.yml
q7s-env-em.sh
q7s-env.sh
release-checklist.md
48 lines
1.9 KiB
C++
48 lines
1.9 KiB
C++
#include "Detumble.h"
|
|
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
|
|
Detumble::Detumble() {}
|
|
|
|
Detumble::~Detumble() {}
|
|
|
|
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
|
|
const bool satRotRateValid,
|
|
const bool magFieldRateValid,
|
|
const bool useFullDetumbleLaw) {
|
|
if (not magFieldValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
|
} else if (satRotRateValid and useFullDetumbleLaw) {
|
|
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
|
|
} else if (magFieldRateValid) {
|
|
return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
|
|
} else {
|
|
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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);
|
|
}
|