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

commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-16 22:12:07 +02:00
parent a72015c260
commit 692e8f84a9
5 changed files with 51 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+20 -1
View File
@@ -1012,7 +1012,26 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
local_eph_good = false;
} else {
local_eph_good = true;
}
} else {
if (local_position.eph < eph_epv_threshold) {
local_eph_good = true;
} else {
local_eph_good = false;
}
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;