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

fixed detumbleCtrl magField and magRate units

Browse Source
This commit is contained in:
Marius Eggert 2023-03-07 10:11:12 +01:00
parent ba0e07b5c6
commit faf4937e76
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,8 +19,13 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
// convert uT to T
double magFieldT[3], magRateT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
// control law
double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
VectorOperations<double>::mulScalar(magRateT, factor, magMom, 3);
return returnvalue::OK;
}
@ -43,9 +48,13 @@ ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateVa
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
// convert uT to T
double magFieldT[3];
VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
// control law
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
VectorOperations<double>::normalize(magField, magFieldNormed, 3);
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
return returnvalue::OK;