Skip to content

Commit

Permalink
ekf2: cleanup legacy EKF solution_status_flags
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Jul 15, 2024
1 parent 1cd7d54 commit 30561c4
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 54 deletions.
22 changes: 0 additions & 22 deletions src/modules/ekf2/EKF/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -407,7 +407,7 @@ class Ekf final : public EstimatorInterface
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; }

Expand Down
80 changes: 50 additions & 30 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion src/modules/ekf2/EKF2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 30561c4

Please sign in to comment.