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

Improve naming and brackets

Browse Source
This commit is contained in:
kamilritz
2020-01-07 08:14:59 +11:00
committed by Paul Riseborough
parent 1c68709949
commit f99dbd8ca3
3 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+1 -1
View File
@@ -192,7 +192,7 @@ bool Ekf::initialiseFilter()
setControlBaroHeight(); setControlBaroHeight();
// reset variables that are shared with post alignment GPS checks // reset variables that are shared with post alignment GPS checks
_gps_pos_derived_filt(2) = 0.0f; _gps_pos_deriv_filt(2) = 0.0f;
_gps_alt_ref = 0.0f; _gps_alt_ref = 0.0f;
if(!initialiseTilt()){ if(!initialiseTilt()){
EKF/ekf.h
+1 -1
View File
@@ -445,7 +445,7 @@ private:
float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
// variables used for the GPS quality checks // variables used for the GPS quality checks
Vector3f _gps_pos_derived_filt; ///< GPS NED position derivative (m/sec) Vector3f _gps_pos_deriv_filt; ///< GPS NED position derivative (m/sec)
Vector2f _gps_velNE_filt; ///< filtered GPS North and East velocity (m/sec) Vector2f _gps_velNE_filt; ///< filtered GPS North and East velocity (m/sec)
float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec) float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec)
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
EKF/gps_checks.cpp
+8 -8
View File
@@ -146,11 +146,11 @@ bool Ekf::gps_is_good(const gps_message &gps)
if (!_control_status.flags.in_air && _vehicle_at_rest) { if (!_control_status.flags.in_air && _vehicle_at_rest) {
// Calculate position movement since last measurement // Calculate position movement since last measurement
float delta_posN = 0.0f; float delta_posN = 0.0f;
float delta_posE = 0.0f; float delta_pos_e = 0.0f;
// calculate position movement since last GPS fix // calculate position movement since last GPS fix
if (_gps_pos_prev.timestamp > 0) { if (_gps_pos_prev.timestamp > 0) {
map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_posE); map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_pos_e);
} else { } else {
// no previous position has been set // no previous position has been set
@@ -159,23 +159,23 @@ bool Ekf::gps_is_good(const gps_message &gps)
} }
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
const Vector3f vel_limit{_params.req_hdrift, _params.req_hdrift, _params.req_vdrift}; const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
Vector3f pos_derived{delta_posN, delta_posE, (_gps_alt_prev - 1e-3f * (float)gps.alt)}; Vector3f pos_derived(delta_posN, delta_pos_e, (_gps_alt_prev - 1e-3f * (float)gps.alt));
pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit); pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
// Apply a low pass filter // Apply a low pass filter
_gps_pos_derived_filt = pos_derived * filter_coef + _gps_pos_derived_filt * (1.0f-filter_coef); _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
// Calculate the horizontal drift speed and fail if too high // Calculate the horizontal drift speed and fail if too high
_gps_drift_metrics[0] = Vector2f{_gps_pos_derived_filt.xy()}.norm(); _gps_drift_metrics[0] = Vector2f(_gps_pos_deriv_filt.xy()).norm();
_gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift); _gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift);
// Fail if the vertical drift speed is too high // Fail if the vertical drift speed is too high
_gps_drift_metrics[1] = fabsf(_gps_pos_derived_filt(2)); _gps_drift_metrics[1] = fabsf(_gps_pos_deriv_filt(2));
_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift); _gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
// Check the magnitude of the filtered horizontal GPS velocity // Check the magnitude of the filtered horizontal GPS velocity
Vector2f gps_velNE = matrix::constrain(Vector2f{gps.vel_ned[0], gps.vel_ned[1]}, // TODO: when vel_ned vector3f use .xy() Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned[0], gps.vel_ned[1]), // TODO: when vel_ned vector3f use .xy()
-10.0f * _params.req_hdrift, -10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift); 10.0f * _params.req_hdrift);
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef); _gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);