diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 8b17f804ce..9f91c3c9e0 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -129,85 +129,79 @@ bool Ekf::gps_is_good(struct gps_message *gps) _gps_error_norm = fmaxf((gps->eph / _params.req_hacc) , (gps->epv / _params.req_vacc)); _gps_error_norm = fmaxf(_gps_error_norm , (gps->sacc / _params.req_sacc)); - // Calculate position movement since last measurement - float delta_posN = 0.0f; - float delta_PosE = 0.0f; + // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient + const float filt_time_const = 10.0f; + float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const); + float filter_coef = dt / filt_time_const; + + // The following checks are only valid when the vehicle is at rest double lat = gps->lat * 1.0e-7; double lon = gps->lon * 1.0e-7; + if (!_control_status.flags.in_air && _vehicle_at_rest) { + // Calculate position movement since last measurement + float delta_posN = 0.0f; + float delta_PosE = 0.0f; - // calculate position movement since last GPS fix - if (_gps_pos_prev.timestamp > 0) { - map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE); + // calculate position movement since last GPS fix + if (_gps_pos_prev.timestamp > 0) { + map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE); - } else { - // no previous position has been set - map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); - _gps_alt_prev = 1e-3f * (float)gps->alt; - } - - // Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient - const float filt_time_const = 10.0f; - float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const); - float filter_coef = dt / filt_time_const; + } else { + // no previous position has been set + map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); + _gps_alt_prev = 1e-3f * (float)gps->alt; - // save GPS fix for next time - map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); + } - // Calculate the horizontal drift velocity components and limit to 10x the threshold - float vel_limit = 10.0f * _params.req_hdrift; - float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit); - float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit); + // Calculate the horizontal drift velocity components and limit to 10x the threshold + float vel_limit = 10.0f * _params.req_hdrift; + float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit); + float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit); - // Apply a low pass filter - _gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef); - _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); + // Apply a low pass filter + _gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef); + _gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef); - // Calculate the horizontal drift speed and fail if too high - // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.in_air) { + // Calculate the horizontal drift speed and fail if too high float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); - } else { - _gps_check_fail_status.flags.hdrift = false; - } + // Calculate the vertical drift velocity and limit to 10x the threshold + float vz_drift_limit = 10.0f * _params.req_vdrift; + float gps_alt_m = 1e-3f * (float)gps->alt; + float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vz_drift_limit, vz_drift_limit); - // Calculate the vertical drift velocity and limit to 10x the threshold - vel_limit = 10.0f * _params.req_vdrift; - float gps_alt_m = 1e-3f * (float)gps->alt; - float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vel_limit, vel_limit); - _gps_alt_prev = gps_alt_m; + // Apply a low pass filter to the vertical velocity + _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); - // Apply a low pass filter to the vertical velocity - _gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef); - - // Fail if the vertical drift speed is too high - // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.in_air) { + // Fail if the vertical drift speed is too high _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); - } else { - _gps_check_fail_status.flags.vdrift = false; - } - - // Check the magnitude of the filtered horizontal GPS velocity - // This check can only be used if the vehicle is stationary during alignment - if (!_control_status.flags.in_air) { - vel_limit = 10.0f * _params.req_hdrift; - float gps_velN = fminf(fmaxf(gps->vel_ned[0], -vel_limit), vel_limit); - float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vel_limit), vel_limit); + // Check the magnitude of the filtered horizontal GPS velocity + float vxy_drift_limit = 10.0f * _params.req_hdrift; + float gps_velN = fminf(fmaxf(gps->vel_ned[0], -vxy_drift_limit), vxy_drift_limit); + float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vxy_drift_limit), vxy_drift_limit); _gps_velN_filt = gps_velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef); _gps_velE_filt = gps_velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef); float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt); _gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift); - } else { + } else if (_control_status.flags.in_air) { + // These checks are always declared as passed when flying + // If on ground and moving, the last result before movemenent commenced is kept + _gps_check_fail_status.flags.hdrift = false; + _gps_check_fail_status.flags.vdrift = false; _gps_check_fail_status.flags.hspeed = false; + } + // save GPS fix for next time + map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu); + _gps_alt_prev = 1e-3f * (float)gps->alt; + // Check the filtered difference between GPS and EKF vertical velocity - vel_limit = 10.0f * _params.req_vdrift; - float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit); + float vz_diff_limit = 10.0f * _params.req_vdrift; + float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vz_diff_limit), vz_diff_limit); _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);