fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: add validity flags to global pos message (#23787)

Browse Source 
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Marco Hauswirth 2024-10-23 10:19:04 +02:00 committed by GitHub
parent 808153b049
commit 0c451552c7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
16 changed files with 87 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/VehicleGlobalPosition.msg
View File

@ -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

4
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@ -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

2
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -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

2
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -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;

2
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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);

12
src/modules/ekf2/EKF/ekf.h
View File

@ -220,13 +220,17 @@ public:
// 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;
}

10
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -109,7 +109,7 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
double current_lon = static_cast<double>(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();

26
src/modules/ekf2/EKF2.cpp
View File

@ -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 &timestamp)
// 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 &timestamp)
_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 &timestamp)
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();

6
src/modules/ekf2/test/test_EKF_airspeed.cpp
View File

@ -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));

18
src/modules/ekf2/test/test_EKF_basics.cpp
View File

@ -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

12
src/modules/ekf2/test/test_EKF_externalVision.cpp
View File

@ -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)

2
src/modules/ekf2/test/test_EKF_fake_pos.cpp
View File

@ -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());
}

58
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
View File

@ -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);

2
src/modules/ekf2/test/test_EKF_gps.cpp
View File

@ -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);

4
src/modules/ekf2/test/test_EKF_yaw_estimator.cpp
View File

@ -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());
}

26
src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp
View File

@ -72,39 +72,21 @@ private:
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;