fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:57:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Don't push bad values from lidar to EKF2 (#5196)

Browse Source 
Report terrain altitude
This commit is contained in:
Vasily Evseenko
2016-08-02 10:30:40 +04:00
committed by Lorenz Meier
parent a58c2f416f
commit 457526ebe7
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+11 -3
View File
@@ -486,6 +486,9 @@ void Ekf2::task_main()
if (range_finder_updated) {
orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder);
if (range_finder.min_distance >= range_finder.current_distance || range_finder.max_distance <= range_finder.current_distance) {
range_finder_updated = false;
}
}
orb_check(_ev_pos_sub, &vision_position_updated);
@@ -816,9 +819,14 @@ void Ekf2::task_main()
global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
// TODO: implement terrain estimator
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
if (lpos.dist_bottom_valid) {
global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = true; // Terrain altitude estimate is valid
} else {
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
}
// TODO use innovatun consistency check timouts to set this
global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning