Skip to content

Commit

Permalink
ekf: Use booleans instead of bitmask for ekf preflt checks
Browse files Browse the repository at this point in the history
Rename "down" to "vert"
  • Loading branch information
bresch committed Oct 21, 2019
1 parent f26ea57 commit d303e77
Show file tree
Hide file tree
Showing 5 changed files with 34 additions and 35 deletions.
10 changes: 4 additions & 6 deletions msg/estimator_status.msg
Original file line number Diff line number Diff line change
Expand Up @@ -106,12 +106,10 @@ uint16 solution_status_flags # Bitmask indicating which filter kinematic state o

float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time

uint8 pre_flt_fail_flags # Bitmask indicating which innovation preflight check failed
# 0 - True if the heading innovation check failed
# 1 - True if the horizontal velocity innovation check failed
# 2 - True if the vertival velocity innovation check failed
# 3 - True if the height innovation check failed

bool pre_flt_fail_innov_heading
bool pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_height

# legacy local position estimator (LPE) flags
uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt)
Expand Down
18 changes: 16 additions & 2 deletions src/modules/commander/PreflightCheck.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -494,9 +494,23 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
}

// Check if preflight check performed by estimator has failed
if (status.pre_flt_fail_flags > 0) {
if (status.pre_flt_fail_innov_heading ||
status.pre_flt_fail_innov_vel_horiz ||
status.pre_flt_fail_innov_vel_vert ||
status.pre_flt_fail_innov_height) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: estimator not ready (%d)", status.pre_flt_fail_flags);
if (status.pre_flt_fail_innov_heading) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");

} else if (status.pre_flt_fail_innov_vel_horiz) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");

} else if (status.pre_flt_fail_innov_vel_horiz) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");

} else if (status.pre_flt_fail_innov_height) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
}
}

success = false;
Expand Down
10 changes: 5 additions & 5 deletions src/modules/ekf2/Utility/PreFlightChecker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)

_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
_has_down_vel_failed = preFlightCheckDownVelFailed(innov, alpha);
_has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha);
_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
}

Expand Down Expand Up @@ -92,7 +92,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in
return has_failed;
}

bool PreFlightChecker::preFlightCheckDownVelFailed(const ekf2_innovations_s &innov, const float alpha)
bool PreFlightChecker::preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, const float alpha)
{
const float vel_d_innov = innov.vel_pos_innov[2];
const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
Expand All @@ -118,9 +118,9 @@ bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov, const Vector2f
}

uint8_t PreFlightChecker::prefltFailBoolToBitMask(const bool heading_failed, const bool horiz_vel_failed,
const bool down_vel_failed, const bool height_failed)
const bool vert_vel_failed, const bool height_failed)
{
return heading_failed | (horiz_vel_failed << 1) | (down_vel_failed << 2) | (height_failed << 3);
return heading_failed | (horiz_vel_failed << 1) | (vert_vel_failed << 2) | (height_failed << 3);
}

void PreFlightChecker::reset()
Expand All @@ -130,7 +130,7 @@ void PreFlightChecker::reset()
_is_using_ev_pos_aiding = false;
_has_heading_failed = false;
_has_horiz_vel_failed = false;
_has_down_vel_failed = false;
_has_vert_vel_failed = false;
_has_height_failed = false;
_filter_vel_n_innov.reset();
_filter_vel_e_innov.reset();
Expand Down
26 changes: 5 additions & 21 deletions src/modules/ekf2/Utility/PreFlightChecker.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ class PreFlightChecker

bool hasHeadingFailed() const { return _has_heading_failed; }
bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
bool hasDownVelFailed() const { return _has_down_vel_failed; }
bool hasVertVelFailed() const { return _has_vert_vel_failed; }
bool hasHeightFailed() const { return _has_height_failed; }

/*
Expand All @@ -98,23 +98,7 @@ class PreFlightChecker
* Vertical checks overall result
* @return true if one of the vertical checks failed
*/
bool hasVertFailed() const { return _has_down_vel_failed || _has_height_failed; }

/*
* Bitfield representation of the failures
* @return a bitfield where the bits represent
* #0 _has_heading_failed
* #1 _has_horiz_vel_failed
* #2 _has_down_vel_failed
* #3 _has_height_failed
*/
uint8_t getBitmask() const
{
return prefltFailBoolToBitMask(_has_heading_failed,
_has_horiz_vel_failed,
_has_down_vel_failed,
_has_height_failed);
}
bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; }

/*
* Check if the innovation fails the test
Expand Down Expand Up @@ -143,7 +127,7 @@ class PreFlightChecker
/*
* Packs the boolean flags into a bit field
*/
static uint8_t prefltFailBoolToBitMask(bool heading_failed, bool horiz_vel_failed, bool down_vel_failed,
static uint8_t prefltFailBoolToBitMask(bool heading_failed, bool horiz_vel_failed, bool vert_vel_failed,
bool height_failed);

static constexpr float sq(float var) { return var * var; }
Expand All @@ -153,14 +137,14 @@ class PreFlightChecker
float selectHeadingTestLimit();

bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha);
bool preFlightCheckDownVelFailed(const ekf2_innovations_s &innov, float alpha);
bool preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, float alpha);
bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha);

void resetPreFlightChecks();

bool _has_heading_failed{};
bool _has_horiz_vel_failed{};
bool _has_down_vel_failed{};
bool _has_vert_vel_failed{};
bool _has_height_failed{};

bool _can_observe_heading_in_flight{};
Expand Down
5 changes: 4 additions & 1 deletion src/modules/ekf2/ekf2_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1550,7 +1550,10 @@ void Ekf2::Run()
status.time_slip = _last_time_slip_us / 1e6f;
status.health_flags = 0.0f; // unused
status.timeout_flags = 0.0f; // unused
status.pre_flt_fail_flags = _preflt_checker.getBitmask();
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();

_estimator_status_pub.publish(status);

Expand Down

0 comments on commit d303e77

Please sign in to comment.