Remove q_valid flag from attitude topic

This commit is contained in:
Lorenz Meier
2016-10-18 20:46:08 +02:00
parent d349bd570f
commit 526fb8f515
11 changed files with 31 additions and 46 deletions
@@ -617,7 +617,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
matrix::Quaternion<float> q(Ro);
memcpy(&att.q[0],&q._data[0],sizeof(att.q));
att.q_valid = true;
if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
@@ -625,7 +624,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("ERR: NaN estimate!");
PX4_ERR("ERR: NaN estimate!");
}
perf_end(ekf_loop_perf);
@@ -826,7 +826,6 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.q[1] = _ekf->states[1];
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;