diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp new file mode 100644 index 00000000..283d6dc1 --- /dev/null +++ b/mission/controller/acs/control/Detumble.cpp @@ -0,0 +1,65 @@ + +/* + * Detumble.cpp + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + + +#include "Detumble.h" +#include +#include +#include +#include +#include +#include +#include + + +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::norm(magField,3),2); + VectorOperations::mulScalar(magRate, factor, magMom, 3); + return RETURN_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 RETURN_OK; + +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h new file mode 100644 index 00000000..54d62179 --- /dev/null +++ b/mission/controller/acs/control/Detumble.h @@ -0,0 +1,46 @@ +/* + * Detumble.h + * + * Created on: 17 Aug 2022 + * Author: Robin Marquardt + */ + +#ifndef ACS_CONTROL_DETUMBLE_H_ +#define ACS_CONTROL_DETUMBLE_H_ + +#include +#include +#include +#include +#include +#include "acs/config/classIds.h" +#include +#include + +class Detumble : public HasReturnvaluesIF { + +public: + Detumble(AcsParameters *acsParameters_); + virtual ~Detumble(); + + static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE; + static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); + + /* @brief: Load AcsParameters für this class + * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters + */ + void loadAcsParameters(AcsParameters *acsParameters_); + + ReturnValue_t bDotLaw(const double *magRate, const bool *magRateValid, + const double *magField, const bool *magFieldValid, + double *magMom); + + ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + +private: + AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; + AcsParameters::MagnetorquesParameter* magnetorquesParameter; +}; + +#endif /* ACS_CONTROL_DETUMBLE_H_ */ +