fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/gps_checks.cpp
View File

@ -143,7 +143,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
} else {
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
@ -176,10 +176,10 @@ bool Ekf::gps_is_good(struct gps_message *gps)
// Calculate the vertical drift velocity and limit to 10x the threshold
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
_gps_alt_ref = gps->alt * 1e-3f;
_gps_alt_ref = 1e-3f * (float)gps->alt;
// Apply a low pass filter to the vertical velocity
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);