diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index fbd1f90afd65..44754f5fddd6 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -106,8 +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 -bool pre_flt_fail # true when estimator has failed pre-flight checks and the vehicle should not be flown regardless of flight mode - +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) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 0d796a59480f..a4c61b5b30b3 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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) { + 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: Position unknown"); + 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; diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9c0d1df0a900..85dfc341735b 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################# + +add_subdirectory(Utility) + px4_add_module( MODULE modules__ekf2 MAIN ekf2 @@ -42,4 +45,5 @@ px4_add_module( ecl_EKF ecl_geo perf + Ekf2Utility ) diff --git a/src/modules/ekf2/Utility/CMakeLists.txt b/src/modules/ekf2/Utility/CMakeLists.txt new file mode 100644 index 000000000000..df1c33ebfecb --- /dev/null +++ b/src/modules/ekf2/Utility/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################# + +px4_add_library(Ekf2Utility + PreFlightChecker.cpp +) + +target_include_directories(Ekf2Utility + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(Ekf2Utility PRIVATE mathlib) + +px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS Ekf2Utility) diff --git a/src/modules/ekf2/Utility/InnovationLpf.hpp b/src/modules/ekf2/Utility/InnovationLpf.hpp new file mode 100644 index 000000000000..a542f6aa4a0e --- /dev/null +++ b/src/modules/ekf2/Utility/InnovationLpf.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * First order "alpha" IIR digital filter with input saturation + */ + +#include + +class InnovationLpf final +{ +public: + InnovationLpf() = default; + ~InnovationLpf() = default; + + void reset(float val = 0.f) { _x = val; } + + /** + * Update the filter with a new value and returns the filtered state + * The new value is constained by the limit set in setSpikeLimit + * @param val new input + * @param alpha normalized weight of the new input + * @param spike_limit the amplitude of the saturation at the input of the filter + * @return filtered output + */ + float update(float val, float alpha, float spike_limit) + { + float val_constrained = math::constrain(val, -spike_limit, spike_limit); + float beta = 1.f - alpha; + + _x = beta * _x + alpha * val_constrained; + + return _x; + } + + /** + * Helper function to compute alpha from dt and the inverse of tau + * @param dt sampling time in seconds + * @param tau_inv inverse of the time constant of the filter + * @return alpha, the normalized weight of a new measurement + */ + static float computeAlphaFromDtAndTauInv(float dt, float tau_inv) + { + return dt * tau_inv; + } + +private: + float _x; ///< current state of the filter +}; diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp new file mode 100644 index 000000000000..36dc3e695e35 --- /dev/null +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PreFlightCheckHelper.cpp + * Class handling the EKF2 innovation pre flight checks + */ + +#include "PreFlightChecker.hpp" + +void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov) +{ + const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv); + + _has_heading_failed = preFlightCheckHeadingFailed(innov, alpha); + _has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha); + _has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha); + _has_height_failed = preFlightCheckHeightFailed(innov, alpha); +} + +bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha) +{ + const float heading_test_limit = selectHeadingTestLimit(); + const float heading_innov_spike_lim = 2.0f * heading_test_limit; + + const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim); + + return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit); +} + +float PreFlightChecker::selectHeadingTestLimit() +{ + // Select the max allowed heading innovaton depending on whether we are not aiding navigation using + // observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion). + const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding; + + return (is_ne_aiding && !_can_observe_heading_in_flight) + ? _nav_heading_innov_test_lim // more restrictive test limit + : _heading_innov_test_lim; // less restrictive test limit +} + +bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha) +{ + bool has_failed = false; + + if (_is_using_gps_aiding || _is_using_ev_pos_aiding) { + const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov); + Vector2f vel_ne_innov_lpf; + vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim); + vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim); + has_failed |= checkInnov2DFailed(vel_ne_innov, vel_ne_innov_lpf, _vel_innov_test_lim); + } + + if (_is_using_flow_aiding) { + const Vector2f flow_innov = Vector2f(innov.flow_innov); + Vector2f flow_innov_lpf; + flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); + flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); + has_failed |= checkInnov2DFailed(flow_innov, flow_innov_lpf, _flow_innov_test_lim); + } + + return has_failed; +} + +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); + return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim); +} + +bool PreFlightChecker::preFlightCheckHeightFailed(const ekf2_innovations_s &innov, const float alpha) +{ + const float hgt_innov = innov.vel_pos_innov[5]; + const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim); + return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim); +} + +bool PreFlightChecker::checkInnovFailed(const float innov, const float innov_lpf, const float test_limit) +{ + return fabsf(innov_lpf) > test_limit || fabsf(innov) > 2.0f * test_limit; +} + +bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, const float test_limit) +{ + return innov_lpf.norm_squared() > sq(test_limit) + || innov.norm_squared() > sq(2.0f * test_limit); +} + +uint8_t PreFlightChecker::prefltFailBoolToBitMask(const bool heading_failed, const bool horiz_vel_failed, + const bool vert_vel_failed, const bool height_failed) +{ + return heading_failed | (horiz_vel_failed << 1) | (vert_vel_failed << 2) | (height_failed << 3); +} + +void PreFlightChecker::reset() +{ + _is_using_gps_aiding = false; + _is_using_flow_aiding = false; + _is_using_ev_pos_aiding = false; + _has_heading_failed = false; + _has_horiz_vel_failed = false; + _has_vert_vel_failed = false; + _has_height_failed = false; + _filter_vel_n_innov.reset(); + _filter_vel_e_innov.reset(); + _filter_vel_d_innov.reset(); + _filter_hgt_innov.reset(); + _filter_heading_innov.reset(); + _filter_flow_x_innov.reset(); + _filter_flow_y_innov.reset(); +} diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp new file mode 100644 index 000000000000..0bff50dafef6 --- /dev/null +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PreFlightChecker.hpp + * Class handling the EKF2 innovation pre flight checks + * + * First call the update(...) function and then get the results + * using the hasXxxFailed() getters + */ + +#pragma once + +#include +#include + +#include + +#include "InnovationLpf.hpp" + +using matrix::Vector2f; + +class PreFlightChecker +{ +public: + PreFlightChecker() = default; + ~PreFlightChecker() = default; + + /* + * Reset all the internal states of the class to their default value + */ + void reset(); + + /* + * Update the internal states + * @param dt the sampling time + * @param innov the ekf2_innovation_s struct containing the current innovations + */ + void update(float dt, const ekf2_innovations_s &innov); + + /* + * If set to true, the checker will use a less conservative heading innovation check + */ + void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; } + + void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } + void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } + void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } + + bool hasHeadingFailed() const { return _has_heading_failed; } + bool hasHorizVelFailed() const { return _has_horiz_vel_failed; } + bool hasVertVelFailed() const { return _has_vert_vel_failed; } + bool hasHeightFailed() const { return _has_height_failed; } + + /* + * Overall state of the pre fligh checks + * @return true if any of the check failed + */ + bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); } + + /* + * Horizontal checks overall result + * @return true if one of the horizontal checks failed + */ + bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; } + + /* + * Vertical checks overall result + * @return true if one of the vertical checks failed + */ + bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; } + + /* + * Check if the innovation fails the test + * To pass the test, the following conditions should be true: + * innov <= test_limit + * innov_lpf <= 2 * test_limit + * @param innov the current unfiltered innovation + * @param innov_lpf the low-pass filtered innovation + * @param test_limit the magnitude test limit + * @return true if the check failed the test, false otherwise + */ + static bool checkInnovFailed(float innov, float innov_lpf, float test_limit); + + /* + * Check if the a innovation of a 2D vector fails the test + * To pass the test, the following conditions should be true: + * innov <= test_limit + * innov_lpf <= 2 * test_limit + * @param innov the current unfiltered innovation + * @param innov_lpf the low-pass filtered innovation + * @param test_limit the magnitude test limit + * @return true if the check failed the test, false otherwise + */ + static bool checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, float test_limit); + + /* + * Packs the boolean flags into a bit field + */ + 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; } + +private: + bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha); + float selectHeadingTestLimit(); + + bool preFlightCheckHorizVelFailed(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_vert_vel_failed{}; + bool _has_height_failed{}; + + bool _can_observe_heading_in_flight{}; + bool _is_using_gps_aiding{}; + bool _is_using_flow_aiding{}; + bool _is_using_ev_pos_aiding{}; + + // Low-pass filters for innovation pre-flight checks + InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec) + InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec) + InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec) + InnovationLpf _filter_hgt_innov; ///< Preflight low pass filter height innovation (m) + InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad) + InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) + InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) + + // Preflight low pass filter time constant inverse (1/sec) + static constexpr float _innov_lpf_tau_inv = 0.2f; + // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) + static constexpr float _vel_innov_test_lim = 0.5f; + // Maximum permissible height innovation to pass pre-flight checks (m) + static constexpr float _hgt_innov_test_lim = 1.5f; + // Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad) + static constexpr float _nav_heading_innov_test_lim = 0.25f; + // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) + static constexpr float _heading_innov_test_lim = 0.52f; + // Maximum permissible flow innovation to pass pre-flight checks + static constexpr float _flow_innov_test_lim = 0.1f; + // Preflight velocity innovation spike limit (m/sec) + static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; + // Preflight position innovation spike limit (m) + static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; + // Preflight flow innovation spike limit (rad) + static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; +}; diff --git a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp new file mode 100644 index 000000000000..03ae5a942aa6 --- /dev/null +++ b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test code for PreFlightChecker class + * Run this test only using make tests TESTFILTER=PreFlightChecker + */ + +#include + +#include "PreFlightChecker.hpp" + +class PreFlightCheckerTest : public ::testing::Test +{ +}; + +TEST_F(PreFlightCheckerTest, testInnovFailed) +{ + const float test_limit = 1.0; ///< is the limit for innovation_lpf, the limit for innovation is 2*test_limit + const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5}; + const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1}; + const bool expected_result[9] = {false, false, true, false, true, true, true, true, true}; + + for (int i = 0; i < 9; i++) { + EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], test_limit), expected_result[i]); + } + + // Smaller test limit, all the checks should fail except the first + EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations[0], innovations_lpf[0], 0.0)); + + for (int i = 1; i < 9; i++) { + EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], 0.0)); + } + + // Larger test limit, none of the checks should fail + for (int i = 0; i < 9; i++) { + EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations[i], innovations_lpf[i], 2.0)); + } +} + +TEST_F(PreFlightCheckerTest, testInnov2dFailed) +{ + const float test_limit = 1.0; + Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}}; + Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}}; + const bool expected_result[4] = {false, true, true, true}; + + for (int i = 0; i < 4; i++) { + EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], test_limit), expected_result[i]); + } + + // Smaller test limit, all the checks should fail except the first + EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0)); + + for (int i = 1; i < 4; i++) { + EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], 0.0)); + } + + // Larger test limit, none of the checks should fail + for (int i = 0; i < 4; i++) { + EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[i], innovations_lpf[i], 1.42)); + } +} + +TEST_F(PreFlightCheckerTest, testBitMask) +{ + PreFlightChecker preflt_checker; + + const bool heading_failed = true; + const bool horiz_vel_failed = true; + const bool down_vel_failed = false; + const bool height_failed = true; + + int bitmask = PreFlightChecker::prefltFailBoolToBitMask(heading_failed, horiz_vel_failed, down_vel_failed, + height_failed); + + EXPECT_EQ(bitmask, 0b1011); +} diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index dd0e9e684980..3b16ec7237c5 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -78,6 +78,8 @@ #include #include +#include "Utility/PreFlightChecker.hpp" + // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm #define BLEND_MASK_USE_SPD_ACC 1 #define BLEND_MASK_USE_HPOS_ACC 2 @@ -116,6 +118,12 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Wor private: int getRangeSubIndex(); ///< get subscription index of first downward-facing range sensor + PreFlightChecker _preflt_checker; + void runPreFlightChecks(float dt, const filter_control_status_u &control_status, + const vehicle_status_s &vehicle_status, + const ekf2_innovations_s &innov); + void resetPreFlightChecks(); + template void update_mag_bias(Param &mag_bias_param, int axis_index); @@ -200,27 +208,6 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Wor // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved - // Used to filter velocity innovations during pre-flight checks - bool _preflt_horiz_fail = false; ///< true if preflight horizontal innovation checks are failed - bool _preflt_vert_fail = false; ///< true if preflight vertical innovation checks are failed - bool _preflt_fail = false; ///< true if any preflight innovation checks are failed - Vector2f _vel_ne_innov_lpf = {}; ///< Preflight low pass filtered NE axis velocity innovations (m/sec) - float _vel_d_innov_lpf = {}; ///< Preflight low pass filtered D axis velocity innovations (m/sec) - float _hgt_innov_lpf = 0.0f; ///< Preflight low pass filtered height innovation (m) - float _yaw_innov_magnitude_lpf = 0.0f; ///< Preflight low pass filtered yaw innovation magntitude (rad) - - static constexpr float _innov_lpf_tau_inv = 0.2f; ///< Preflight low pass filter time constant inverse (1/sec) - static constexpr float _vel_innov_test_lim = - 0.5f; ///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec) - static constexpr float _hgt_innov_test_lim = - 1.5f; ///< Maximum permissible height innovation to pass pre-flight checks (m) - static constexpr float _nav_yaw_innov_test_lim = - 0.25f; ///< Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad) - static constexpr float _yaw_innov_test_lim = - 0.52f; ///< Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) - const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec) - const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m) - // set pose/velocity as invalid if standard deviation is bigger than max_std_dev // TODO: the user should be allowed to set these values by a parameter static constexpr float ep_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated position @@ -1303,10 +1290,10 @@ void Ekf2::Run() lpos.az = vel_deriv[2]; // TODO: better status reporting - lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; - lpos.z_valid = !_preflt_vert_fail; - lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; - lpos.v_z_valid = !_preflt_vert_fail; + lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.z_valid = !_preflt_checker.hasVertFailed(); + lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed(); + lpos.v_z_valid = !_preflt_checker.hasVertFailed(); // Position of local NED origin in GPS / WGS84 frame map_projection_reference_s ekf_origin; @@ -1470,7 +1457,7 @@ void Ekf2::Run() _vehicle_visual_odometry_aligned_pub.publish(aligned_ev_odom); } - if (_ekf.global_position_is_valid() && !_preflt_fail) { + if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { // generate and publish global position data vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); @@ -1552,7 +1539,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 = _preflt_fail; + 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); @@ -1673,64 +1663,11 @@ void Ekf2::Run() // calculate noise filtered velocity innovations which are used for pre-flight checking if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - // calculate coefficients for LPF applied to innovation sequences - float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); - float beta = 1.0f - alpha; - - // filter the velocity and innvovations - _vel_ne_innov_lpf(0) = beta * _vel_ne_innov_lpf(0) + alpha * constrain(innovations.vel_pos_innov[0], - -_vel_innov_spike_lim, _vel_innov_spike_lim); - _vel_ne_innov_lpf(1) = beta * _vel_ne_innov_lpf(1) + alpha * constrain(innovations.vel_pos_innov[1], - -_vel_innov_spike_lim, _vel_innov_spike_lim); - _vel_d_innov_lpf = beta * _vel_d_innov_lpf + alpha * constrain(innovations.vel_pos_innov[2], - -_vel_innov_spike_lim, _vel_innov_spike_lim); - - // set the max allowed yaw innovaton depending on whether we are not aiding navigation using - // observations in the NE reference frame. - bool doing_ne_aiding = control_status.flags.gps || control_status.flags.ev_pos; - - float yaw_test_limit; - - if (doing_ne_aiding && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { - // use a smaller tolerance when doing NE inertial frame aiding as a rotary wing - // vehicle which cannot use GPS course to realign heading in flight - yaw_test_limit = _nav_yaw_innov_test_lim; - - } else { - // use a larger tolerance when not doing NE inertial frame aiding or - // if a fixed wing vehicle which can realign heading using GPS course - yaw_test_limit = _yaw_innov_test_lim; - } - - // filter the yaw innovations - _yaw_innov_magnitude_lpf = beta * _yaw_innov_magnitude_lpf + alpha * constrain(innovations.heading_innov, - -2.0f * yaw_test_limit, 2.0f * yaw_test_limit); - - _hgt_innov_lpf = beta * _hgt_innov_lpf + alpha * constrain(innovations.vel_pos_innov[5], -_hgt_innov_spike_lim, - _hgt_innov_spike_lim); - - // check the yaw and horizontal velocity innovations - float vel_ne_innov_length = sqrtf(innovations.vel_pos_innov[0] * innovations.vel_pos_innov[0] + - innovations.vel_pos_innov[1] * innovations.vel_pos_innov[1]); - _preflt_horiz_fail = (_vel_ne_innov_lpf.norm() > _vel_innov_test_lim) - || (vel_ne_innov_length > 2.0f * _vel_innov_test_lim) - || (_yaw_innov_magnitude_lpf > yaw_test_limit); - - // check the vertical velocity and position innovations - _preflt_vert_fail = (fabsf(_vel_d_innov_lpf) > _vel_innov_test_lim) - || (fabsf(innovations.vel_pos_innov[2]) > 2.0f * _vel_innov_test_lim) - || (fabsf(_hgt_innov_lpf) > _hgt_innov_test_lim); - - // master pass-fail status - _preflt_fail = _preflt_horiz_fail || _preflt_vert_fail; + float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f; + runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations); } else { - _vel_ne_innov_lpf.zero(); - _vel_d_innov_lpf = 0.0f; - _hgt_innov_lpf = 0.0f; - _preflt_horiz_fail = false; - _preflt_vert_fail = false; - _preflt_fail = false; + resetPreFlightChecks(); } _estimator_innovations_pub.publish(innovations); @@ -1742,6 +1679,26 @@ void Ekf2::Run() } } +void Ekf2::runPreFlightChecks(const float dt, + const filter_control_status_u &control_status, + const vehicle_status_s &vehicle_status, + const ekf2_innovations_s &innov) +{ + const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + _preflt_checker.setVehicleCanObserveHeadingInFlight(can_observe_heading_in_flight); + _preflt_checker.setUsingGpsAiding(control_status.flags.gps); + _preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow); + _preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos); + + _preflt_checker.update(dt, innov); +} + +void Ekf2::resetPreFlightChecks() +{ + _preflt_checker.reset(); +} + int Ekf2::getRangeSubIndex() { for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {