Marius Eggert
d33ae9ede7
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
# Conflicts: # mission/controller/AcsController.cpp # mission/controller/AcsController.h # mission/controller/acs/AcsParameters.h # mission/controller/acs/ActuatorCmd.h # mission/controller/acs/Guidance.cpp # mission/controller/acs/Guidance.h # mission/controller/acs/MultiplicativeKalmanFilter.cpp # mission/controller/acs/OutputValues.h # mission/controller/acs/SensorProcessing.cpp # mission/controller/acs/SensorProcessing.h # mission/controller/acs/control/Detumble.cpp # mission/controller/acs/control/Detumble.h # mission/controller/acs/control/PtgCtrl.cpp # mission/controller/acs/util/MathOperations.h
71 lines
2.0 KiB
C++
71 lines
2.0 KiB
C++
|
|
/*
|
|
* Detumble.cpp
|
|
*
|
|
* Created on: 17 Aug 2022
|
|
* Author: Robin Marquardt
|
|
*/
|
|
|
|
#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/sign.h>
|
|
#include <math.h>
|
|
|
|
#include "../util/MathOperations.h"
|
|
|
|
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); }
|
|
|
|
Detumble::~Detumble() {}
|
|
|
|
void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
|
|
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
|
|
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
|
|
}
|
|
|
|
ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
|
|
const double *magField, const bool *magFieldValid,
|
|
double *magMom) {
|
|
|
|
if (!magRateValid || !magFieldValid) {
|
|
return DETUMBLE_NO_SENSORDATA;
|
|
}
|
|
double gain = detumbleParameter->gainD;
|
|
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
|
|
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid,
|
|
double *magMom) {
|
|
if (!magRateValid) {
|
|
return DETUMBLE_NO_SENSORDATA;
|
|
}
|
|
|
|
double dipolMax = magnetorquesParameter->DipolMax;
|
|
for (int i = 0; i < 3; i++) {
|
|
magMom[i] = -dipolMax * sign(magRate[i]);
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
|
|
}
|
|
|
|
ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
|
|
const double *magField, const bool *magFieldValid,
|
|
double *magMom) {
|
|
|
|
if (!satRateValid || !magFieldValid) {
|
|
return DETUMBLE_NO_SENSORDATA;
|
|
}
|
|
double gain = detumbleParameter->gainD;
|
|
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
|
|
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
|
|
return returnvalue::OK;
|
|
|
|
}
|