From 0c451552c7c85bcc2dfe186e1659dd5ea1728a61 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Wed, 23 Oct 2024 10:19:04 +0200 Subject: [PATCH] EKF2: add validity flags to global pos message (#23787) Co-authored-by: Mathieu Bresciani --- msg/VehicleGlobalPosition.msg | 3 + .../checks/estimatorCheck.cpp | 4 +- .../barometer/baro_height_control.cpp | 2 +- .../aid_sources/magnetometer/mag_control.cpp | 2 +- src/modules/ekf2/EKF/ekf.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 12 ++-- src/modules/ekf2/EKF/ekf_helper.cpp | 10 ++-- src/modules/ekf2/EKF2.cpp | 26 ++++----- src/modules/ekf2/test/test_EKF_airspeed.cpp | 6 +- src/modules/ekf2/test/test_EKF_basics.cpp | 18 +++--- .../ekf2/test/test_EKF_externalVision.cpp | 12 ++-- src/modules/ekf2/test/test_EKF_fake_pos.cpp | 2 +- .../ekf2/test/test_EKF_fusionLogic.cpp | 58 +++++++++---------- src/modules/ekf2/test/test_EKF_gps.cpp | 2 +- .../ekf2/test/test_EKF_yaw_estimator.cpp | 4 +- .../mavlink/streams/GLOBAL_POSITION_INT.hpp | 26 ++------- 16 files changed, 87 insertions(+), 102 deletions(-) diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index e37155fe25a9..30aaf5652a96 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/VehicleGlobalPosition.msg @@ -13,6 +13,9 @@ float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) float32 alt_ellipsoid # Altitude above ellipsoid, (meters) +bool lat_lon_valid +bool alt_valid + float32 delta_alt # Reset delta for altitude float32 delta_terrain # Reset delta for terrain uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index aa400e27c1ae..aa1195ca2fbb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -746,8 +746,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } } + const bool global_pos_valid = gpos.lat_lon_valid && gpos.alt_valid; + failsafe_flags.global_position_invalid = - !checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, + !checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index dc6d36bcb5c7..f6ab66f73660 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -202,7 +202,7 @@ void Ekf::stopBaroHgtFusion() #if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const { - if (_control_status.flags.wind && local_position_is_valid()) { + if (_control_status.flags.wind && isLocalHorizontalPositionValid()) { // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with a different scale in the positive and // negative X and Y directions. Used to correct baro data for positional errors diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 13caff5ca2d6..cff897d10649 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -88,7 +88,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse); if (global_origin_valid() - && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) + && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) ) { // position of local NED origin in GPS / WGS84 frame double latitude_deg; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 9b87b3d0e13b..f453559c0ef7 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -298,7 +298,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl Vector3f pos_correction; // apply a first order correction using velocity at the delayed time horizon and the delta time - if ((timestamp_observation > 0) && local_position_is_valid()) { + if ((timestamp_observation > 0) && isLocalHorizontalPositionValid()) { timestamp_observation = math::min(_time_latest_us, timestamp_observation); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3192aaf21405..ec9a00154ff3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -220,13 +220,17 @@ class Ekf final : public EstimatorInterface // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation // and have not started using synthetic position observations to constrain drift - bool global_position_is_valid() const + bool isGlobalHorizontalPositionValid() const { - return (_pos_ref.isInitialized() && local_position_is_valid()); + return _pos_ref.isInitialized() && isLocalHorizontalPositionValid(); } - // return true if the local position estimate is valid - bool local_position_is_valid() const + bool isGlobalVerticalPositionValid() const + { + return _pos_ref.isInitialized() && isLocalVerticalPositionValid(); + } + + bool isLocalHorizontalPositionValid() const { return !_horizontal_deadreckon_time_exceeded; } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 26757aacbc9f..d4b3e6031ebd 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -109,7 +109,7 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f double current_lon = static_cast(NAN); // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && local_position_is_valid()) { + if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) { _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); current_pos_available = true; } @@ -184,7 +184,7 @@ bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double long _pos_ref.initReference(latitude, longitude, _time_delayed_us); // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (local_position_is_valid()) { + if (isLocalHorizontalPositionValid()) { double est_lat; double est_lon; _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon); @@ -671,16 +671,16 @@ uint16_t Ekf::get_ekf_soln_status() const soln_status.flags.attitude = attitude_valid(); // 2 ESTIMATOR_VELOCITY_HORIZ True if the horizontal velocity estimate is good - soln_status.flags.velocity_horiz = local_position_is_valid(); + soln_status.flags.velocity_horiz = isLocalHorizontalPositionValid(); // 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(); + soln_status.flags.pos_horiz_rel = isLocalHorizontalPositionValid(); // 16 ESTIMATOR_POS_HORIZ_ABS True if the horizontal position (absolute) estimate is good - soln_status.flags.pos_horiz_abs = global_position_is_valid(); + soln_status.flags.pos_horiz_abs = isGlobalHorizontalPositionValid(); // 32 ESTIMATOR_POS_VERT_ABS True if the vertical position (absolute) estimate is good soln_status.flags.pos_vert_abs = isVerticalAidingActive(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 20129678e482..f83ba6e1e912 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -417,7 +417,7 @@ int EKF2::print_status(bool verbose) { PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n", _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(), - _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); + _ekf.isLocalHorizontalPositionValid(), _ekf.isGlobalHorizontalPositionValid()); perf_print_counter(_ekf_update_perf); perf_print_counter(_msg_missed_imu_perf); @@ -1174,23 +1174,22 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) // Position of local NED origin in GPS / WGS84 frame _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); + global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid(); global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters + global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid(); + #if defined(CONFIG_EKF2_GNSS) global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt); -#else - global_pos.alt_ellipsoid = global_pos.alt; #endif - // delta_alt, alt_reset_counter - // global altitude has opposite sign of local down position + // global altitude has opposite sign of local down position float delta_z = 0.f; uint8_t z_reset_counter = 0; _ekf.get_posD_reset(&delta_z, &z_reset_counter); global_pos.delta_alt = -delta_z; global_pos.alt_reset_counter = z_reset_counter; - // lat_lon_reset_counter float delta_xy[2] {}; uint8_t xy_reset_counter = 0; _ekf.get_posNE_reset(delta_xy, &xy_reset_counter); @@ -1198,16 +1197,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); - global_pos.terrain_alt = NAN; - global_pos.terrain_alt_valid = false; - #if defined(CONFIG_EKF2_TERRAIN) - if (_ekf.isTerrainEstimateValid()) { - // Terrain altitude in m, WGS84 - global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); - global_pos.terrain_alt_valid = true; - } + // Terrain altitude in m, WGS84 + global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); + global_pos.terrain_alt_valid = _ekf.isTerrainEstimateValid(); float delta_hagl = 0.f; _ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter); @@ -1566,8 +1560,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.ay = vel_deriv(1); lpos.az = vel_deriv(2); - lpos.xy_valid = _ekf.local_position_is_valid(); - lpos.v_xy_valid = _ekf.local_position_is_valid(); + lpos.xy_valid = _ekf.isLocalHorizontalPositionValid(); + lpos.v_xy_valid = _ekf.isLocalHorizontalPositionValid(); // TODO: some modules (e.g.: mc_pos_control) don't handle v_z_valid != z_valid properly lpos.z_valid = _ekf.isLocalVerticalPositionValid() || _ekf.isLocalVerticalVelocityValid(); diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index b9dc3e367279..d1a9e918963a 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -207,7 +207,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) @@ -239,7 +239,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) _sensor_simulator.runSeconds(10.f); EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: an external position reset is sent ResetLoggingChecker reset_logging_checker(_ekf); @@ -260,7 +260,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) EXPECT_NEAR(altitude_est, altitude_new, 0.01f); EXPECT_NEAR(latitude_est, latitude_new, 1e-3f); EXPECT_NEAR(longitude_est, longitude_new, 1e-3f); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); reset_logging_checker.capturePostResetState(); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index e95443344235..afe56992050d 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -283,7 +283,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) // Global origin has been initialized but since there is no position aiding, the global // position is still not valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _sensor_simulator.runSeconds(1); @@ -307,9 +307,9 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev) _sensor_simulator.runSeconds(1); // THEN; since there is no origin, only the local position can be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_FALSE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _latitude_new = 45.0000005; _longitude_new = 111.0000005; @@ -320,8 +320,8 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev) // THEN: local and global positions are valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } TEST_F(EkfBasicsTest, global_position_from_opt_flow) @@ -338,9 +338,9 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow) _sensor_simulator.runSeconds(1); // THEN; since there is no origin, only the local position can be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_FALSE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _latitude_new = 45.0000005; _longitude_new = 111.0000005; @@ -351,8 +351,8 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow) // THEN: local and global positions are valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } // TODO: Add sampling tests diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 5b1439edd9f2..f87151eeafaf 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -85,8 +85,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.runSeconds(2); @@ -95,8 +95,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _ekf_wrapper.enableExternalVisionHeadingFusion(); _sensor_simulator.runSeconds(2); @@ -105,8 +105,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfExternalVisionTest, visionVelocityReset) diff --git a/src/modules/ekf2/test/test_EKF_fake_pos.cpp b/src/modules/ekf2/test/test_EKF_fake_pos.cpp index 3cd755a3a56e..fb3b5991e1f9 100644 --- a/src/modules/ekf2/test/test_EKF_fake_pos.cpp +++ b/src/modules/ekf2/test/test_EKF_fake_pos.cpp @@ -117,5 +117,5 @@ TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos) _sensor_simulator.runSeconds(60); EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index 9367b86da33a..2b89c8aa8f71 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -84,13 +84,13 @@ TEST_F(EkfFusionLogicTest, doNoFusion) // GIVEN: a tilt and heading aligned filter // WHEN: having no aiding source // THEN: EKF should not have a valid position estimate - EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); _sensor_simulator.runSeconds(4); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doGpsFusion) @@ -104,8 +104,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should intend to fuse GPS EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // THEN: Local and global position should be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: GPS data is not send for 11s _sensor_simulator.stopGps(); @@ -113,8 +113,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should stop to intend to fuse GPS EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: GPS data is send again for 11s _sensor_simulator.startGps(); @@ -122,8 +122,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should to intend to fuse GPS EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: clients decides to stop GPS fusion _ekf_wrapper.disableGpsFusion(); @@ -230,8 +230,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Flow data is not send and we enable flow fusion _sensor_simulator.stopFlow(); @@ -242,8 +242,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Flow data is sent and we enable flow fusion _sensor_simulator.startFlow(); @@ -253,8 +253,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should intend to fuse flow EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Stop sending flow data _sensor_simulator.stopFlow(); @@ -263,8 +263,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionPositionFusion) @@ -279,8 +279,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: stop sending vision data _sensor_simulator.stopExternalVision(); @@ -291,8 +291,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) @@ -307,8 +307,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: stop sending vision data _sensor_simulator.stopExternalVision(); @@ -319,8 +319,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) @@ -336,8 +336,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // THEN: Yaw state should be reset to vision EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); @@ -350,8 +350,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // THEN: Yaw state shoud be reset to mag EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index b03a8701d52a..ac683564a473 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -107,7 +107,7 @@ TEST_F(EkfGpsTest, gpsFixLoss) // THEN: after dead-reconing for a couple of seconds, the local position gets invalidated _sensor_simulator.runSeconds(6); EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); - EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); // The control logic takes a bit more time to deactivate the GNSS fusion completely _sensor_simulator.runSeconds(5); diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index a49cd5c66fc1..1a132e5db509 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -111,6 +111,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); } diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp index 6d2997fe7188..d5b015cc07e0 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -72,39 +72,21 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream mavlink_global_position_int_t msg{}; - if (lpos.z_valid && lpos.z_global) { - msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f; - - } else { - // fall back to baro altitude - vehicle_air_data_s air_data{}; - _air_data_sub.copy(&air_data); - - if (air_data.timestamp > 0) { - msg.alt = air_data.baro_alt_meter * 1000.0f; - } - } - home_position_s home{}; _home_sub.copy(&home); if ((home.timestamp > 0) && home.valid_alt) { - if (lpos.z_valid) { - msg.relative_alt = -(lpos.z - home.z) * 1000.0f; - - } else { - msg.relative_alt = msg.alt - (home.alt * 1000.0f); - } + msg.relative_alt = (gpos.alt - home.alt) * 1000.0f; } else { - if (lpos.z_valid) { - msg.relative_alt = -lpos.z * 1000.0f; - } + msg.relative_alt = gpos.alt * 1000.0f; } msg.time_boot_ms = gpos.timestamp / 1000; + msg.lat = gpos.lat * 1e7; msg.lon = gpos.lon * 1e7; + msg.alt = gpos.alt * 1000.0f; msg.vx = lpos.vx * 100.0f; msg.vy = lpos.vy * 100.0f;