diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d6d2fab6..260b793a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -8,7 +8,6 @@ AcsController::AcsController(object_id_t objectId) : ExtendedControllerBase(objectId), guidance(&acsParameters), safeCtrl(&acsParameters), - detumble(&acsParameters), ptgCtrl(&acsParameters), parameterHelper(this), mgmDataRaw(this), @@ -219,7 +218,8 @@ void AcsController::performDetumble() { double magMomMtq[3] = {0, 0, 0}; detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); + mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, + acsParameters.detumbleParameter.gainD); actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 222a0ec2..28cbefbf 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -17,33 +17,27 @@ #include "../util/MathOperations.h" -Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } +Detumble::Detumble() {} Detumble::~Detumble() {} -void Detumble::loadAcsParameters(AcsParameters *acsParameters_) { - detumbleParameter = &(acsParameters_->detumbleParameter); - magnetorquesParameter = &(acsParameters_->magnetorquerParameter); -} - ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom) { + const double *magField, const bool magFieldValid, double *magMom, + double gain) { if (!magRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); VectorOperations::mulScalar(magRate, factor, magMom, 3); return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, - double *magMom) { +ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax) { if (!magRateValid) { return DETUMBLE_NO_SENSORDATA; } - double dipolMax = magnetorquesParameter->dipolMax; for (int i = 0; i < 3; i++) { magMom[i] = -dipolMax * sign(magRate[i]); } @@ -51,14 +45,16 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal return returnvalue::OK; } -ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid, +ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, const bool *magFieldValid, - double *magMom) { + double *magMom, double gain) { if (!satRateValid || !magFieldValid) { return DETUMBLE_NO_SENSORDATA; } - double gain = detumbleParameter->gainD; - double factor = -gain / pow(VectorOperations::norm(magField, 3), 2); - VectorOperations::mulScalar(satRate, factor, magMom, 3); + double factor = gain / pow(VectorOperations::norm(magField, 3), 2); + double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; + VectorOperations::normalize(magField, magFieldNormed, 3); + VectorOperations::cross(satRate, magFieldNormed, crossProduct); + VectorOperations::mulScalar(crossProduct, factor, magMom, 3); return returnvalue::OK; } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index dee82271..2bf3607d 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -12,30 +12,22 @@ class Detumble { public: - Detumble(AcsParameters *acsParameters_); + Detumble(); virtual ~Detumble(); static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); - /* @brief: Load AcsParameters for 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); + const bool magFieldValid, double *magMom, double gain); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom); + ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, + double dipolMax); - 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); + ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, + const bool *magFieldValid, double *magMom, double gain); private: - AcsParameters::DetumbleParameter *detumbleParameter; - AcsParameters::MagnetorquerParameter *magnetorquesParameter; }; #endif /*ACS_CONTROL_DETUMBLE_H_*/