Browse Source

EKF: Explicitly define type conversion for GPS height

master
Paul Riseborough 9 years ago
parent
commit
687fcc70be
  1. 6
      EKF/gps_checks.cpp

6
EKF/gps_checks.cpp

@ -143,7 +143,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) @@ -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) @@ -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);

Loading…
Cancel
Save