LPE: Use euler angles derived from quaternion

This commit is contained in:
Lorenz Meier
2016-10-18 20:46:08 +02:00
parent ac936a28dd
commit 13833e5fd6
5 changed files with 34 additions and 21 deletions
@@ -67,8 +67,8 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
_time_last_sonar = _timeStamp;
y.setZero();
y(0) = (d + _sonar_z_offset.get()) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
cosf(_eul(0)) *
cosf(_eul(1));
return OK;
}