From b550ad22b9ede8c6e252cf66976a50ba9c959337 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 10 Jun 2021 15:21:13 +0300 Subject: [PATCH] EKF2: Always publish global position For historical reasons, there is a check in EKF2 to only publish global position if it has moved for 1mm. This is no longer necessary, and also this if doesn't save any cpu cycles in real conditions where GPS errors are always much bigger that this. When using simulated or fake GPS, based on some other positioning system, the GPS coordinate input can be very accurate due to quantization or true accuracy. In this case this check bites and perfectly valid position doesn't get published. Just removed this if. Signed-off-by: Jukka Laitinen --- src/modules/ekf2/EKF2.cpp | 57 ++++++++++++++++++--------------------- src/modules/ekf2/EKF2.hpp | 2 -- 2 files changed, 26 insertions(+), 33 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c7cb997b8b..351b70d9aa 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -628,47 +628,42 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { - // only publish if position has changed by at least 1 mm (map_projection_reproject is relatively expensive) const Vector3f position{_ekf.getPosition()}; - if ((_last_local_position_for_gpos - position).longerThan(0.001f)) { - // generate and publish global position data - vehicle_global_position_s global_pos; - global_pos.timestamp_sample = timestamp; + // generate and publish global position data + vehicle_global_position_s global_pos; + global_pos.timestamp_sample = timestamp; - // Position of local NED origin in GPS / WGS84 frame - map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon); + // Position of local NED origin in GPS / WGS84 frame + map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon); - float delta_xy[2]; - _ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter); + float delta_xy[2]; + _ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter); - global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters - global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); + global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters + global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); - // global altitude has opposite sign of local down position - float delta_z; - uint8_t z_reset_counter; - _ekf.get_posD_reset(&delta_z, &z_reset_counter); - global_pos.delta_alt = -delta_z; + // global altitude has opposite sign of local down position + float delta_z; + uint8_t z_reset_counter; + _ekf.get_posD_reset(&delta_z, &z_reset_counter); + global_pos.delta_alt = -delta_z; - _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); + _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); - if (_ekf.isTerrainEstimateValid()) { - // Terrain altitude in m, WGS84 - global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); - global_pos.terrain_alt_valid = true; + if (_ekf.isTerrainEstimateValid()) { + // Terrain altitude in m, WGS84 + global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); + global_pos.terrain_alt_valid = true; - } else { - global_pos.terrain_alt = NAN; - global_pos.terrain_alt_valid = false; - } - - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _global_position_pub.publish(global_pos); - - _last_local_position_for_gpos = position; + } else { + global_pos.terrain_alt = NAN; + global_pos.terrain_alt_valid = false; } + + global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning + global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _global_position_pub.publish(global_pos); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 3c201f1979..99e2266e9e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -212,8 +212,6 @@ private: uint32_t _device_id_gyro{0}; uint32_t _device_id_mag{0}; - Vector3f _last_local_position_for_gpos{}; - Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; Vector3f _last_mag_bias_published{};