diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-28 17:22:24 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-28 17:22:24 +0200 |
commit | f0a21c4f5f9a840eda797fbe162220fa98f77a37 (patch) | |
tree | 4361bd1b19c5a65dd91c29046e9676bf8fd67529 /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
parent | 196edd8a4faa36f837ff440ed41fc9656f96e20f (diff) | |
parent | 045ee8c7c7c4618a8a03f9c9342f53d2fc7333c9 (diff) | |
download | px4-firmware-f0a21c4f5f9a840eda797fbe162220fa98f77a37.tar.gz px4-firmware-f0a21c4f5f9a840eda797fbe162220fa98f77a37.tar.bz2 px4-firmware-f0a21c4f5f9a840eda797fbe162220fa98f77a37.zip |
Merge remote-tracking branch 'upstream/navigator_rewrite' into navigator_rewrite_estimator
Conflicts:
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 23 |
1 files changed, 20 insertions, 3 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 334177ad8..cb160c775 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 @@ -298,7 +298,7 @@ private: /** * Check filter sanity state - * + * * @return zero if ok, non-zero for a filter error condition. */ int check_filter_state(); @@ -1256,8 +1256,8 @@ FixedwingEstimator::task_main() float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change - if ((_ekf->velNED[0] != _gps.vel_n_m_s || - _ekf->velNED[1] != _gps.vel_e_m_s || + if ((_ekf->velNED[0] != _gps.vel_n_m_s || + _ekf->velNED[1] != _gps.vel_e_m_s || _ekf->velNED[2] != _gps.vel_d_m_s) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; @@ -1494,6 +1494,23 @@ FixedwingEstimator::task_main() } + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + } } } |