Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

ekf2: move gps buffer pop to controlGpsFusion()

Browse Source 
- controlGpsFusion() now owns yaw estimator update
This commit is contained in:
Daniel Agar
2023-02-21 11:05:16 -05:00
parent a867bb7d88
commit 9ec3f30ae1
4 changed files with 39 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+1 -32
View File
@@ -102,41 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
}
}
if (_gps_buffer) {
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
_time_prev_gps_us = _gps_sample_delayed.time_us;
_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
if (_gps_data_ready) {
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;
// correct position and height for offset relative to IMU
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_gps_sample_delayed.pos -= pos_offset_earth.xy();
_gps_sample_delayed.hgt += pos_offset_earth(2);
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
&& _gps_sample_delayed.vel.isAllFinite()
) {
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
}
}
}
// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
runYawEKFGSF(imu_delayed);
// control use of observations for aiding
controlMagFusion();
controlOpticalFlowFusion(imu_delayed);
controlGpsFusion();
controlGpsFusion(imu_delayed);
controlAirDataFusion(imu_delayed);
controlBetaFusion(imu_delayed);
controlDragFusion();
src/modules/ekf2/EKF/ekf.h
+1 -5
View File
@@ -491,8 +491,6 @@ private:
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec)
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
uint64_t _time_last_v_pos_aiding{0};
uint64_t _time_last_v_vel_aiding{0};
@@ -856,7 +854,7 @@ private:
void resetOnGroundMotionForOpticalFlowChecks();
// control fusion of GPS observations
void controlGpsFusion();
void controlGpsFusion(const imuSample &imu_delayed);
bool shouldResetGpsFusion() const;
bool isYawFailure() const;
@@ -1038,8 +1036,6 @@ private:
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
void runYawEKFGSF(const imuSample &imu_delayed);
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
src/modules/ekf2/EKF/ekf_helper.cpp
-5
View File
@@ -1346,11 +1346,6 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
void Ekf::runYawEKFGSF(const imuSample &imu_delayed)
{
_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
}
void Ekf::resetGpsDriftCheckFilters()
{
_gps_velNE_filt.setZero();
src/modules/ekf2/EKF/gps_control.cpp
+37 -6
View File
@@ -39,13 +39,41 @@
#include "ekf.h"
#include <mathlib/mathlib.h>
void Ekf::controlGpsFusion()
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
{
if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
stopGpsFusion();
return;
}
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
if (_gps_data_ready) {
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;
// correct position and height for offset relative to IMU
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_gps_sample_delayed.pos -= pos_offset_earth.xy();
_gps_sample_delayed.hgt += pos_offset_earth(2);
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
if ((_gps_sample_delayed.sacc > 0.f) && (_gps_sample_delayed.sacc < _params.req_sacc)
&& _gps_sample_delayed.vel.isAllFinite()
) {
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), math::max(_gps_sample_delayed.sacc, _params.gps_vel_noise));
}
}
// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
@@ -236,8 +264,8 @@ bool Ekf::shouldResetGpsFusion() const
* with no aiding we need to do something
*/
const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max);
const bool is_reset_required = has_horizontal_aiding_timed_out
|| isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
@@ -280,7 +308,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
const bool continuing_conditions_passing = !gps_checks_failing;
const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
2 * GNSS_YAW_MAX_INTERVAL);
const bool starting_conditions_passing = continuing_conditions_passing
&& _control_status.flags.tilt_align
@@ -336,7 +365,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
}
}
} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
} else if (_control_status.flags.gps_yaw
&& !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
// No yaw data in the message anymore. Stop until it comes back.
stopGpsYawFusion();
}