From e87221a8a3efb90e0b840f2fabcf81d622e121cb Mon Sep 17 00:00:00 2001 From: Robin Marquardt Date: Fri, 21 Oct 2022 16:46:09 +0200 Subject: [PATCH] added gyro detumble --- mission/controller/acs/control/Detumble.cpp | 14 ++++++++++++++ mission/controller/acs/control/Detumble.h | 4 ++++ 2 files changed, 18 insertions(+) diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 67903c7c..f12d5962 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -63,3 +63,17 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateVa 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 = detumbleCtrlParameters->gainD; + double factor = -gain / pow(VectorOperations::norm(magField,3),2); + VectorOperations::mulScalar(satRate, factor, magMom, 3); + return returnvalue::OK; + +} diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index b85aaf86..e49c88ac 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -38,6 +38,10 @@ public: ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom); + ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, + const double *magField, const bool *magFieldValid, + double *magMom); + private: AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters; AcsParameters::MagnetorquesParameter* magnetorquesParameter;