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

ekf2: add GNSS yaw max interval

Browse Source 
yaw data usually comes at lower rate than vel/pos
This commit is contained in:
bresch 2022-11-08 14:59:29 +01:00 committed by Daniel Agar
parent 9834c7917b
commit 06702da003
3 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/ekf2/EKF/common.h
View File

@ -66,10 +66,11 @@ using math::Utilities::sq;
using math::Utilities::updateYawInRotMat;
// maximum sensor intervals in usec
#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
#define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
#define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
#define GNSS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GNSS measurements (uSec)
#define GNSS_YAW_MAX_INTERVAL (uint64_t)15e5 ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
// bad accelerometer detection and mitigation
#define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)

4
src/modules/ekf2/EKF/control.cpp
View File

@ -86,7 +86,7 @@ void Ekf::controlFusionModes()
}
if (_gps_buffer) {
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
_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;
@ -418,7 +418,7 @@ 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 * GPS_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

4
src/modules/ekf2/EKF/gnss_height_control.cpp
View File

@ -92,7 +92,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
&& _gps_checks_passed;
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)
&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)
&& _gps_checks_passed
&& gps_checks_passing
&& !gps_checks_failing;
@ -158,7 +158,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
}
} else if (_control_status.flags.gps_hgt
&& !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) {
&& !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopGpsHgtFusion();