eive-obsw/mission/controller/acs/control/Detumble.cpp
Marius Eggert 5f17f365e3
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
fixed local includes
2022-09-27 11:06:11 +02:00

66 lines
1.5 KiB
C++

/*
* Detumble.cpp
*
* Created on: 17 Aug 2022
* Author: Robin Marquardt
*/
#include "Detumble.h"
#include "../util/MathOperations.h"
#include <math.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>
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 = detumbleCtrlParameters->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;
}