ACS Ctrl Bug Bash #439

Merged
muellerr merged 42 commits from acs-bug-bash into develop 2023-03-10 19:21:19 +01:00
Showing only changes of commit faf4937e76 - Show all commits

View File

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