From 1541e4b414fe63ec9d6c3077375db7e20c20ef4a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 10 Dec 2020 09:13:12 -0500 Subject: [PATCH] EKF: update control status and fault status getters - the fault status union exceeded 16 bits, but the getter was get_filter_fault_status(uint16_t *val) --- EKF/estimator_interface.h | 7 +- test/sensor_simulator/ekf_wrapper.cpp | 36 +++------ test/test_EKF_basics.cpp | 105 ++++++++++++-------------- 3 files changed, 64 insertions(+), 84 deletions(-) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 85cd710b43..2b78a50b16 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -196,14 +196,17 @@ public: } // get EKF mode status - void get_control_mode(uint32_t *val) const { *val = _control_status.value; } + const filter_control_status_u &control_status() const { return _control_status; } const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; } + + const filter_control_status_u &control_status_prev() const { return _control_status_prev; } const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; } // get EKF internal fault status - void get_filter_fault_status(uint16_t *val) const { *val = _fault_status.value; } + const fault_status_u &fault_status() const { return _fault_status; } const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; } + const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; } diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index f40b9117a6..b60227ecd6 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -62,9 +62,7 @@ void EkfWrapper::disableGpsFusion() bool EkfWrapper::isIntendingGpsFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.gps; + return _ekf->control_status_flags().gps; } void EkfWrapper::enableGpsHeadingFusion() @@ -79,9 +77,7 @@ void EkfWrapper::disableGpsHeadingFusion() bool EkfWrapper::isIntendingGpsHeadingFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.gps_yaw; + return _ekf->control_status_flags().gps_yaw; } void EkfWrapper::enableFlowFusion() @@ -96,9 +92,7 @@ void EkfWrapper::disableFlowFusion() bool EkfWrapper::isIntendingFlowFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.opt_flow; + return _ekf->control_status_flags().opt_flow; } void EkfWrapper::enableExternalVisionPositionFusion() @@ -113,9 +107,7 @@ void EkfWrapper::disableExternalVisionPositionFusion() bool EkfWrapper::isIntendingExternalVisionPositionFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.ev_pos; + return _ekf->control_status_flags().ev_pos; } void EkfWrapper::enableExternalVisionVelocityFusion() @@ -130,9 +122,7 @@ void EkfWrapper::disableExternalVisionVelocityFusion() bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.ev_vel; + return _ekf->control_status_flags().ev_vel; } void EkfWrapper::enableExternalVisionHeadingFusion() @@ -147,9 +137,7 @@ void EkfWrapper::disableExternalVisionHeadingFusion() bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.ev_yaw; + return _ekf->control_status_flags().ev_yaw; } void EkfWrapper::enableExternalVisionAlignment() @@ -164,23 +152,17 @@ void EkfWrapper::disableExternalVisionAlignment() bool EkfWrapper::isIntendingMagHeadingFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.mag_hdg; + return _ekf->control_status_flags().mag_hdg; } bool EkfWrapper::isIntendingMag3DFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.mag_3D; + return _ekf->control_status_flags().mag_3D; } bool EkfWrapper::isWindVelocityEstimated() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.wind; + return _ekf->control_status_flags().wind; } void EkfWrapper::enableTerrainRngFusion() diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 31d31bae96..65227169bc 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -78,34 +78,31 @@ TEST_F(EkfBasicsTest, initialControlMode) { // GIVEN: reasonable static sensor data for some duration // THEN: EKF control status should be reasonable - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - - EXPECT_EQ(1, (int) control_status.flags.tilt_align); - EXPECT_EQ(1, (int) control_status.flags.yaw_align); - EXPECT_EQ(0, (int) control_status.flags.gps); - EXPECT_EQ(0, (int) control_status.flags.opt_flow); - EXPECT_EQ(1, (int) control_status.flags.mag_hdg); - EXPECT_EQ(0, (int) control_status.flags.mag_3D); - EXPECT_EQ(0, (int) control_status.flags.mag_dec); - EXPECT_EQ(0, (int) control_status.flags.in_air); - EXPECT_EQ(0, (int) control_status.flags.wind); - EXPECT_EQ(1, (int) control_status.flags.baro_hgt); - EXPECT_EQ(0, (int) control_status.flags.rng_hgt); - EXPECT_EQ(0, (int) control_status.flags.gps_hgt); - EXPECT_EQ(0, (int) control_status.flags.ev_pos); - EXPECT_EQ(0, (int) control_status.flags.ev_yaw); - EXPECT_EQ(0, (int) control_status.flags.ev_hgt); - EXPECT_EQ(0, (int) control_status.flags.fuse_beta); - EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed); - EXPECT_EQ(0, (int) control_status.flags.fixed_wing); - EXPECT_EQ(0, (int) control_status.flags.mag_fault); - EXPECT_EQ(0, (int) control_status.flags.gnd_effect); - EXPECT_EQ(0, (int) control_status.flags.rng_stuck); - EXPECT_EQ(0, (int) control_status.flags.gps_yaw); - EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight); - EXPECT_EQ(0, (int) control_status.flags.ev_vel); - EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z); + EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align); + EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gps); + EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); + EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); + EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fuse_beta); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_field_disturbed); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); + EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); + EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); } TEST_F(EkfBasicsTest, convergesToZero) @@ -135,33 +132,31 @@ TEST_F(EkfBasicsTest, gpsFusion) _sensor_simulator.runSeconds(11); // THEN: EKF should fuse GPS, but no other position sensor - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - EXPECT_EQ(1, (int) control_status.flags.tilt_align); - EXPECT_EQ(1, (int) control_status.flags.yaw_align); - EXPECT_EQ(1, (int) control_status.flags.gps); - EXPECT_EQ(0, (int) control_status.flags.opt_flow); - EXPECT_EQ(1, (int) control_status.flags.mag_hdg); - EXPECT_EQ(0, (int) control_status.flags.mag_3D); - EXPECT_EQ(0, (int) control_status.flags.mag_dec); - EXPECT_EQ(0, (int) control_status.flags.in_air); - EXPECT_EQ(0, (int) control_status.flags.wind); - EXPECT_EQ(1, (int) control_status.flags.baro_hgt); - EXPECT_EQ(0, (int) control_status.flags.rng_hgt); - EXPECT_EQ(0, (int) control_status.flags.gps_hgt); - EXPECT_EQ(0, (int) control_status.flags.ev_pos); - EXPECT_EQ(0, (int) control_status.flags.ev_yaw); - EXPECT_EQ(0, (int) control_status.flags.ev_hgt); - EXPECT_EQ(0, (int) control_status.flags.fuse_beta); - EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed); - EXPECT_EQ(0, (int) control_status.flags.fixed_wing); - EXPECT_EQ(0, (int) control_status.flags.mag_fault); - EXPECT_EQ(0, (int) control_status.flags.gnd_effect); - EXPECT_EQ(0, (int) control_status.flags.rng_stuck); - EXPECT_EQ(0, (int) control_status.flags.gps_yaw); - EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight); - EXPECT_EQ(0, (int) control_status.flags.ev_vel); - EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z); + EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align); + EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(1, (int) _ekf->control_status_flags().gps); + EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow); + EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec); + EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air); + EXPECT_EQ(0, (int) _ekf->control_status_flags().wind); + EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_pos); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_hgt); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fuse_beta); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_field_disturbed); + EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); + EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); + EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); + EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); } TEST_F(EkfBasicsTest, accelBiasEstimation)