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

EKF: Explicitly define type conversion for GPS height

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-16 17:18:20 +11:00
parent e334a5dc57
commit 687fcc70be
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_checks.cpp
+3 -3
View File
@@ -143,7 +143,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
} else { } else {
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
_gps_alt_ref = gps->alt * 1e-3f; _gps_alt_ref = 1e-3f * (float)gps->alt;
} }
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient // Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
@@ -176,10 +176,10 @@ bool Ekf::gps_is_good(struct gps_message *gps)
// Calculate the vertical drift velocity and limit to 10x the threshold // Calculate the vertical drift velocity and limit to 10x the threshold
vel_limit = 10.0f * _params.req_vdrift; vel_limit = 10.0f * _params.req_vdrift;
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit); float velD = fminf(fmaxf((_gps_alt_ref - 1e-3f * (float)gps->alt) / dt, -vel_limit), vel_limit);
// Save the current height as the reference for next time // Save the current height as the reference for next time
_gps_alt_ref = gps->alt * 1e-3f; _gps_alt_ref = 1e-3f * (float)gps->alt;
// Apply a low pass filter to the vertical velocity // Apply a low pass filter to the vertical velocity
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);