eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

cleaned up detumble and fixed gyr detumble law to actual bdot law
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-28 10:12:25 +01:00
parent d86b9d55da
commit 04a561b9a0
3 changed files with 20 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mission/controller/AcsController.cpp
View File

@ -8,7 +8,6 @@ AcsController::AcsController(object_id_t objectId)
: ExtendedControllerBase(objectId), : ExtendedControllerBase(objectId),
guidance(&acsParameters), guidance(&acsParameters),
safeCtrl(&acsParameters), safeCtrl(&acsParameters),
detumble(&acsParameters),
ptgCtrl(&acsParameters), ptgCtrl(&acsParameters),
parameterHelper(this), parameterHelper(this),
mgmDataRaw(this), mgmDataRaw(this),
@ -219,7 +218,8 @@ void AcsController::performDetumble() {
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
acsParameters.detumbleParameter.gainD);
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
*acsParameters.magnetorquerParameter.inverseAlignment, *acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipolMax); acsParameters.magnetorquerParameter.dipolMax);

28
mission/controller/acs/control/Detumble.cpp
View File

@ -17,33 +17,27 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParameters_); } Detumble::Detumble() {}
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, 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) { if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2); double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3); VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK; return returnvalue::OK;
} }
ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom,
double *magMom) { double dipolMax) {
if (!magRateValid) { if (!magRateValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double dipolMax = magnetorquesParameter->dipolMax;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
magMom[i] = -dipolMax * sign(magRate[i]); magMom[i] = -dipolMax * sign(magRate[i]);
} }
@ -51,14 +45,16 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
return returnvalue::OK; 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, const double *magField, const bool *magFieldValid,
double *magMom) { double *magMom, double gain) {
if (!satRateValid || !magFieldValid) { if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA; return DETUMBLE_NO_SENSORDATA;
} }
double gain = detumbleParameter->gainD; double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2); double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3); VectorOperations<double>::normalize(magField, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
return returnvalue::OK; return returnvalue::OK;
} }

20
mission/controller/acs/control/Detumble.h
View File

@ -12,30 +12,22 @@
class Detumble { class Detumble {
public: public:
Detumble(AcsParameters *acsParameters_); Detumble();
virtual ~Detumble(); virtual ~Detumble();
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); 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, 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 bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom, double gain);
ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
const bool *magFieldValid, double *magMom);
private: private:
AcsParameters::DetumbleParameter *detumbleParameter;
AcsParameters::MagnetorquerParameter *magnetorquesParameter;
}; };
#endif /*ACS_CONTROL_DETUMBLE_H_*/ #endif /*ACS_CONTROL_DETUMBLE_H_*/