yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: Always publish global position

Browse Source 
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 <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen
2021-06-10 15:21:13 +03:00
committed by Lorenz Meier
parent 8415692f2a
commit b550ad22b9
2 changed files with 26 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
src/modules/ekf2/EKF2.cpp
View File

@@ -628,47 +628,42 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp) void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
{ {
if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { 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()}; const Vector3f position{_ekf.getPosition()};
if ((_last_local_position_for_gpos - position).longerThan(0.001f)) { // generate and publish global position data
// generate and publish global position data vehicle_global_position_s global_pos;
vehicle_global_position_s global_pos; global_pos.timestamp_sample = timestamp;
global_pos.timestamp_sample = timestamp;
// Position of local NED origin in GPS / WGS84 frame // 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); map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
float delta_xy[2]; float delta_xy[2];
_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter); _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 = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
// global altitude has opposite sign of local down position // global altitude has opposite sign of local down position
float delta_z; float delta_z;
uint8_t z_reset_counter; uint8_t z_reset_counter;
_ekf.get_posD_reset(&delta_z, &z_reset_counter); _ekf.get_posD_reset(&delta_z, &z_reset_counter);
global_pos.delta_alt = -delta_z; 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()) { if (_ekf.isTerrainEstimateValid()) {
// Terrain altitude in m, WGS84 // Terrain altitude in m, WGS84
global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
global_pos.terrain_alt_valid = true; global_pos.terrain_alt_valid = true;
} else { } else {
global_pos.terrain_alt = NAN; global_pos.terrain_alt = NAN;
global_pos.terrain_alt_valid = false; 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;
} }
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);
} }
} }

2
src/modules/ekf2/EKF2.hpp
View File

@@ -212,8 +212,6 @@ private:
uint32_t _device_id_gyro{0}; uint32_t _device_id_gyro{0};
uint32_t _device_id_mag{0}; uint32_t _device_id_mag{0};
Vector3f _last_local_position_for_gpos{};
Vector3f _last_accel_bias_published{}; Vector3f _last_accel_bias_published{};
Vector3f _last_gyro_bias_published{}; Vector3f _last_gyro_bias_published{};
Vector3f _last_mag_bias_published{}; Vector3f _last_mag_bias_published{};