diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-16 22:12:07 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-16 22:12:07 +0200 |
commit | 692e8f84a93a932986004d896554a70380ea11e9 (patch) | |
tree | b9cdef755d925813dd277200134b1d05c0550c9a /src/modules/commander | |
parent | a72015c260dbb4e70d23c35120269cef61a439cc (diff) | |
download | px4-firmware-692e8f84a93a932986004d896554a70380ea11e9.tar.gz px4-firmware-692e8f84a93a932986004d896554a70380ea11e9.tar.bz2 px4-firmware-692e8f84a93a932986004d896554a70380ea11e9.zip |
commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 21 |
1 files changed, 20 insertions, 1 deletions
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; |