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: move synthetic_position flag to control_status.flags.fake_pos

Browse Source
This commit is contained in:
bresch 2022-09-08 14:41:50 +02:00 committed by Daniel Agar
parent 86f7e15f7a
commit aa716936bf
8 changed files with 48 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/estimator_status.msg
View File

@ -16,7 +16,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position
uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle
uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail
uint32 control_mode_flags # Bitmask to indicate EKF logic state
uint64 control_mode_flags # Bitmask to indicate EKF logic state
uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete
uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete
uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused

1
msg/estimator_status_flags.msg
View File

@ -36,6 +36,7 @@ bool cs_rng_fault # 28 - true when the range finder has been declare
bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements
bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing
bool cs_fake_pos # 32 - true when fake position measurements are being fused
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

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

@ -500,40 +500,41 @@ union gps_check_fail_status_u {
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint32_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete
uint32_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete
uint32_t gps : 1; ///< 2 - true if GPS measurement fusion is intended
uint32_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended
uint32_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
uint32_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
uint32_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
uint32_t in_air : 1; ///< 7 - true when the vehicle is airborne
uint32_t wind : 1; ///< 8 - true when wind velocity is being estimated
uint32_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference
uint32_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference
uint32_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference
uint32_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended
uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
uint32_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused
uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint32_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
uint32_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
uint32_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
uint32_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
uint32_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
uint32_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
uint64_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete
uint64_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete
uint64_t gps : 1; ///< 2 - true if GPS measurement fusion is intended
uint64_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended
uint64_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
uint64_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
uint64_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne
uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated
uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference
uint64_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference
uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference
uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended
uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
uint64_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused
uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
uint64_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
uint64_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest
uint64_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
} flags;
uint32_t value;
uint64_t value;
};
// Mavlink bitmask containing state of estimator solution

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

@ -659,7 +659,7 @@ void Ekf::controlAirDataFusion()
const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
_control_status.flags.wind = false;
}
@ -673,7 +673,7 @@ void Ekf::controlAirDataFusion()
_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position;
const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos;
const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr;
const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
@ -707,7 +707,7 @@ void Ekf::controlAirDataFusion()
void Ekf::controlBetaFusion()
{
if (_using_synthetic_position) {
if (_control_status.flags.fake_pos) {
return;
}
@ -733,7 +733,7 @@ void Ekf::controlBetaFusion()
void Ekf::controlDragFusion()
{
if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer &&
!_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
!_control_status.flags.fake_pos && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) {
if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion

3
src/modules/ekf2/EKF/ekf.h
View File

@ -272,7 +272,7 @@ public:
// return true if the local position estimate is valid
bool local_position_is_valid() const
{
return (!_deadreckon_time_exceeded && !_using_synthetic_position);
return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos);
}
bool isTerrainEstimateValid() const { return _hagl_valid; };
@ -465,7 +465,6 @@ private:
uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec)
uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift
uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)

10
src/modules/ekf2/EKF/fake_pos_control.cpp
View File

@ -53,7 +53,7 @@ void Ekf::controlFakePosFusion()
const bool continuing_conditions_passing = !isHorizontalAidingActive();
const bool starting_conditions_passing = continuing_conditions_passing;
if (_using_synthetic_position) {
if (_control_status.flags.fake_pos) {
if (continuing_conditions_passing) {
fuseFakePosition();
@ -83,9 +83,9 @@ void Ekf::controlFakePosFusion()
void Ekf::startFakePosFusion()
{
if (!_using_synthetic_position) {
if (!_control_status.flags.fake_pos) {
ECL_INFO("start fake position fusion");
_using_synthetic_position = true;
_control_status.flags.fake_pos = true;
_fuse_hpos_as_odom = false; // TODO: needed?
resetFakePosFusion();
}
@ -105,9 +105,9 @@ void Ekf::resetFakePosFusion()
void Ekf::stopFakePosFusion()
{
if (_using_synthetic_position) {
if (_control_status.flags.fake_pos) {
ECL_INFO("stop fake position fusion");
_using_synthetic_position = false;
_control_status.flags.fake_pos = false;
resetEstimatorAidStatus(_aid_src_fake_pos);
}

1
src/modules/ekf2/EKF2.cpp
View File

@ -1428,6 +1428,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning;
status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent;
status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;

2
src/modules/ekf2/EKF2.hpp
View File

@ -314,7 +314,7 @@ private:
hrt_abstime _last_event_flags_publish{0};
hrt_abstime _last_status_flags_publish{0};
uint32_t _filter_control_status{0};
uint64_t _filter_control_status{0};
uint32_t _filter_fault_status{0};
uint32_t _innov_check_fail_status{0};