fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

estimator_aid_source split GNSS pos (3d) -> pos (2d) + hgt

Browse Source 
- per estimator air source status only keep a single set of flags and
timestamp that applies to the entire source
This commit is contained in:
Daniel Agar
2022-10-13 11:28:50 -04:00
parent 8a9a091ff3
commit 2de990fd4b
26 changed files with 578 additions and 691 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+30 -15
View File
@@ -52,22 +52,42 @@ void Ekf::controlGpsFusion()
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_sample, gps_checks_passing, gps_checks_failing);
// GNSS velocity
const Vector3f velocity{gps_sample.vel};
const float vel_var = sq(gps_sample.sacc);
const Vector3f vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
updateVelocityAidSrcStatus(gps_sample.time_us, velocity, vel_obs_var, fmaxf(_params.gps_vel_innov_gate, 1.f), _aid_src_gnss_vel);
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
// GNSS position
const float pos_var_lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float pos_var = sq(fmaxf(gps_sample.hacc, pos_var_lower_limit));
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, pos_var_lower_limit);
pos_var = fminf(pos_var, upper_limit);
}
updateHorizontalPositionAidSrcStatus(gps_sample.time_us, gps_sample.pos, Vector2f(pos_var, pos_var), fmaxf(_params.gps_pos_innov_gate, 1.f), _aid_src_gnss_pos);
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
// 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);
_yawEstimator.setVelocity(velocity.xy(), gps_sample.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
const bool mandatory_conditions_passing = _control_status.flags.tilt_align
const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))
&& _control_status.flags.tilt_align
&& _control_status.flags.yaw_align
&& _NED_origin_initialised;
@@ -79,16 +99,11 @@ void Ekf::controlGpsFusion()
if (continuing_conditions_passing
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
if (_params.gnss_ctrl & GnssCtrl::VEL) {
fuseGpsVel();
}
if ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VPOS)) {
fuseGpsPos();
}
fuseVelocity(_aid_src_gnss_vel);
fuseHorizontalPosition(_aid_src_gnss_pos);
if (shouldResetGpsFusion()) {
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1'000'000);
/* A reset is not performed when getting GPS back after a significant period of no data
* because the timeout could have been caused by bad GPS.
@@ -98,7 +113,7 @@ void Ekf::controlGpsFusion()
&& _control_status.flags.in_air
&& !was_gps_signal_lost
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
&& isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
// to improve its estimate if the previous reset was not successful.
if (resetYawToEKFGSF()) {
@@ -116,8 +131,8 @@ void Ekf::controlGpsFusion()
ECL_WARN("GPS fusion timeout - resetting");
}
resetVelocityToGps(_gps_sample_delayed);
resetHorizontalPositionToGps(_gps_sample_delayed);
resetVelocityToGps(gps_sample);
resetHorizontalPositionToGps(gps_sample);
}
} else {