v7.5.0 #828
@ -96,12 +96,11 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
|
|||||||
|
|
||||||
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
|
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
|
||||||
QuaternionOperations::normalize(qBI, qBI);
|
QuaternionOperations::normalize(qBI, qBI);
|
||||||
|
}
|
||||||
// Low Pass
|
// Low Pass
|
||||||
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
|
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
|
||||||
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
|
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
{
|
{
|
||||||
PoolReadGuard pg{attitudeEstimation};
|
PoolReadGuard pg{attitudeEstimation};
|
||||||
if (pg.getReadResult() == returnvalue::OK) {
|
if (pg.getReadResult() == returnvalue::OK) {
|
||||||
|
Loading…
Reference in New Issue
Block a user