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

ekf2: perform GNSS checks at delayed-time horizon

Browse Source 
- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic
This commit is contained in:
bresch
2023-12-18 18:11:20 +01:00
committed by Mathieu Bresciani
parent b0566cd8aa
commit a653073d4f
15 changed files with 371 additions and 419 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+15 -29
View File
@@ -160,25 +160,6 @@ enum class MagCheckMask : uint8_t {
FORCE_WMM = (1 << 2)
};
struct gpsMessage {
uint64_t time_usec{};
int32_t lat{}; ///< Latitude in 1E-7 degrees
int32_t lon{}; ///< Longitude in 1E-7 degrees
int32_t alt{}; ///< Altitude in 1E-3 meters (millimeters) above MSL
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
float yaw_accuracy{}; ///< yaw measurement accuracy (rad, [0, 2PI])
uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
float eph{}; ///< GPS horizontal position accuracy in m
float epv{}; ///< GPS vertical position accuracy in m
float sacc{}; ///< GPS speed accuracy in m/s
float vel_m_s{}; ///< GPS ground speed (m/sec)
Vector3f vel_ned{}; ///< GPS ground speed NED
bool vel_ned_valid{}; ///< GPS ground speed is valid
uint8_t nsats{}; ///< number of satellites used
float pdop{}; ///< position dilution of precision
};
struct imuSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f delta_ang{}; ///< delta angle in body frame (integrated gyro measurements) (rad)
@@ -188,16 +169,21 @@ struct imuSample {
bool delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping
};
struct gpsSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector2f pos{}; ///< NE earth frame gps horizontal position measurement (m)
float hgt{}; ///< gps height measurement (m)
Vector3f vel{}; ///< NED earth frame gps velocity measurement (m/sec)
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float hacc{}; ///< 1-std horizontal position error (m)
float vacc{}; ///< 1-std vertical position error (m)
float sacc{}; ///< 1-std speed error (m/sec)
float yaw_acc{}; ///< 1-std yaw error (rad)
struct gnssSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
double lat{}; ///< latitude (degrees)
double lon{}; ///< longitude (degrees)
float alt{}; ///< GNSS altitude above MSL (m)
Vector3f vel{}; ///< NED earth frame GNSS velocity measurement (m/sec)
float hacc{}; ///< 1-std horizontal position error (m)
float vacc{}; ///< 1-std vertical position error (m)
float sacc{}; ///< 1-std speed error (m/sec)
uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
uint8_t nsats{}; ///< number of satellites used
float pdop{}; ///< position dilution of precision
float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float yaw_acc{}; ///< 1-std yaw error (rad)
float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
};
struct magSample {