diff options
| author | Sebastian Verling <sverling@student.ethz.ch> | 2015-02-23 18:44:38 +0100 |
|---|---|---|
| committer | Sebastian Verling <sverling@student.ethz.ch> | 2015-02-23 18:44:38 +0100 |
| commit | a77420ede84b293e41855ddc7b23b647cc64da96 (patch) | |
| tree | 7cfefa8771a9a63ce15d13f1ba2707de05faa330 /src/modules/ekf_att_pos_estimator | |
| parent | aeef634f9b111d1c50809905802d63b7087c9ac4 (diff) | |
| download | px4-firmware-a77420ede84b293e41855ddc7b23b647cc64da96.tar.gz px4-firmware-a77420ede84b293e41855ddc7b23b647cc64da96.tar.bz2 px4-firmware-a77420ede84b293e41855ddc7b23b647cc64da96.zip | |
corrected rate offset calculation such that units match
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 12 |
1 files changed, 6 insertions, 6 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7d3cb5396..903158129 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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) { |