ACS Ctrl Bug Bash #439
@ -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;
|
||||||
|
Loading…
Reference in New Issue
Block a user