From 692e8f84a93a932986004d896554a70380ea11e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 May 2014 22:12:07 +0200 Subject: commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic --- src/modules/commander/commander.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd..e4ae357d7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; -- cgit v1.2.3