fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add arbitrary pitch offset for range sensor

Browse Source
This commit is contained in:
Paul Riseborough
2017-02-22 14:12:05 +01:00
committed by Roman
parent d9688fae7d
commit cf9c8de167
6 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+7 -3
View File
@@ -83,8 +83,12 @@ void Ekf::controlFusionModes()
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
&& (_R_rng_to_earth_2_2 > 0.7071f);
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
@@ -244,7 +248,7 @@ void Ekf::controlOpticalFlowFusion()
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
float range = heightAboveGndEst / _R_to_earth(2, 2);
float range = heightAboveGndEst / _R_rng_to_earth_2_2;
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should have reliable OF measurements so
@@ -738,7 +742,7 @@ void Ekf::controlRangeFinderFusion()
// correct the range data for position offset relative to the IMU
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
if (_terrain_initialised) {