fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: pass gpsSample around where required

Browse Source 
- this minimizes potential misuse accessing _gps_sample_delayed and
makes the dependency clear
This commit is contained in:
Daniel Agar
2022-09-05 10:27:19 -04:00
parent 0c860fa227
commit d996af4647
9 changed files with 75 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+15 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,14 +49,21 @@ void Ekf::controlGpsFusion()
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
updateGpsYaw(_gps_sample_delayed);
updateGpsVel(_gps_sample_delayed);
updateGpsPos(_gps_sample_delayed);
const gpsSample &gps_sample{_gps_sample_delayed};
updateGpsYaw(gps_sample);
updateGpsVel(gps_sample);
updateGpsPos(gps_sample);
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
if (gps_checks_passing && !gps_checks_failing) {
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.sacc);
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
@@ -144,7 +151,7 @@ void Ekf::controlGpsFusion()
_inhibit_ev_yaw_use = true;
} else {
startGpsFusion();
startGpsFusion(gps_sample);
}
} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) {
@@ -152,8 +159,8 @@ void Ekf::controlGpsFusion()
if (resetYawToEKFGSF()) {
_information_events.flags.yaw_aligned_to_imu_gps = true;
ECL_INFO("Yaw aligned using IMU and GPS");
resetVelocityToGps(_gps_sample_delayed);
resetHorizontalPositionToGps(_gps_sample_delayed);
resetVelocityToGps(gps_sample);
resetHorizontalPositionToGps(gps_sample);
}
}
}