Browse Source

corrected rate offset calculation such that units match

sbg
Sebastian Verling 10 years ago
parent
commit
a77420ede8
  1. 12
      src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

12
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@ -772,14 +772,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() @@ -772,14 +772,14 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU;
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU;
_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10];
_att.rate_offsets[1] = _ekf->states[11];
_att.rate_offsets[2] = _ekf->states[12];
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU;
/* lazily publish the attitude only once available */
if (_att_pub > 0) {

Loading…
Cancel
Save