fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 16:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: cleanup legacy EKF solution_status_flags

Browse Source
This commit is contained in:
Daniel Agar
2024-07-15 15:50:02 -04:00
committed by Mathieu Bresciani
parent 1cd7d54170
commit 48f1687d3a
4 changed files with 52 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
-22
View File
@@ -613,28 +613,6 @@ uint64_t mag_heading_consistent :
uint64_t value;
};
// Mavlink bitmask containing state of estimator solution
union ekf_solution_status_u {
struct {
uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good
uint16_t velocity_horiz : 1; ///< 1 - True if the horizontal velocity estimate is good
uint16_t velocity_vert : 1; ///< 2 - True if the vertical velocity estimate is good
uint16_t pos_horiz_rel : 1; ///< 3 - True if the horizontal position (relative) estimate is good
uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good
uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good
uint16_t const_pos_mode :
1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
uint16_t pred_pos_horiz_rel :
1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
uint16_t pred_pos_horiz_abs :
1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch
uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data
} flags;
uint16_t value;
};
// define structure used to communicate information events
union information_event_status_u {
struct {
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -407,7 +407,7 @@ public:
float getHeightAboveGroundInnovationTestRatio() const;
// return a bitmask integer that describes which state estimates are valid
void get_ekf_soln_status(uint16_t *status) const;
uint16_t get_ekf_soln_status() const;
HeightSensor getHeightSensorRef() const { return _height_sensor_ref; }
src/modules/ekf2/EKF/ekf_helper.cpp
+50 -30
View File
@@ -531,50 +531,70 @@ float Ekf::getHeightAboveGroundInnovationTestRatio() const
return NAN;
}
void Ekf::get_ekf_soln_status(uint16_t *status) const
uint16_t Ekf::get_ekf_soln_status() const
{
ekf_solution_status_u soln_status{};
// TODO: Is this accurate enough?
// LEGACY Mavlink bitmask containing state of estimator solution (see Mavlink ESTIMATOR_STATUS_FLAGS)
union ekf_solution_status_u {
struct {
uint16_t attitude : 1;
uint16_t velocity_horiz : 1;
uint16_t velocity_vert : 1;
uint16_t pos_horiz_rel : 1;
uint16_t pos_horiz_abs : 1;
uint16_t pos_vert_abs : 1;
uint16_t pos_vert_agl : 1;
uint16_t const_pos_mode : 1;
uint16_t pred_pos_horiz_rel : 1;
uint16_t pred_pos_horiz_abs : 1;
uint16_t gps_glitch : 1;
uint16_t accel_error : 1;
} flags;
uint16_t value;
} soln_status{};
// 1 ESTIMATOR_ATTITUDE True if the attitude estimate is good
soln_status.flags.attitude = attitude_valid();
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta
&& _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt
|| _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos
|| _control_status.flags.opt_flow) && (_fault_status.value == 0);
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& (_fault_status.value == 0);
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
// 2 ESTIMATOR_VELOCITY_HORIZ True if the horizontal velocity estimate is good
soln_status.flags.velocity_horiz = local_position_is_valid();
// 4 ESTIMATOR_VELOCITY_VERT True if the vertical velocity estimate is good
soln_status.flags.velocity_vert = isLocalVerticalVelocityValid() || isLocalVerticalPositionValid();
// 8 ESTIMATOR_POS_HORIZ_REL True if the horizontal position (relative) estimate is good
soln_status.flags.pos_horiz_rel = local_position_is_valid();
// 16 ESTIMATOR_POS_HORIZ_ABS True if the horizontal position (absolute) estimate is good
soln_status.flags.pos_horiz_abs = global_position_is_valid();
// 32 ESTIMATOR_POS_VERT_ABS True if the vertical position (absolute) estimate is good
soln_status.flags.pos_vert_abs = isVerticalAidingActive();
// 64 ESTIMATOR_POS_VERT_AGL True if the vertical position (above ground) estimate is good
#if defined(CONFIG_EKF2_TERRAIN)
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
#endif // CONFIG_EKF2_TERRAIN
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
bool mag_innov_good = true;
// 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest;
#if defined(CONFIG_EKF2_MAGNETOMETER)
// 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
mag_innov_good = false;
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
// 512 ESTIMATOR_PRED_POS_HORIZ_ABS True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gps || _control_status.flags.aux_gpos;
// 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GPS glitch
#if defined(CONFIG_EKF2_GNSS)
const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
#else
(void)mag_innov_good;
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad);
#endif // CONFIG_EKF2_GNSS
soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
*status = soln_status.value;
// 2048 ESTIMATOR_ACCEL_ERROR True if the EKF has detected bad accelerometer data
soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical || _fault_status.flags.bad_acc_clipping;
return soln_status.value;
}
void Ekf::fuse(const VectorState &K, float innovation)
src/modules/ekf2/EKF2.cpp
+1 -1
View File
@@ -1825,7 +1825,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.beta_test_ratio = _ekf.getSyntheticSideslipInnovationTestRatio();
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
status.solution_status_flags = _ekf.get_ekf_soln_status();
// reset counters
status.reset_count_vel_ne = _ekf.state_reset_status().reset_count.velNE;