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