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

Heading reset to magnetometer from GPS or EV (#969)

Browse Source
This commit is contained in:
isidroas 2021-02-16 14:37:34 +01:00 committed by GitHub
parent d4258cc66d
commit 0c5291d194
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 25 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -333,7 +333,7 @@ void Ekf::controlExternalVisionFusion()
fuseHeading();
}
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw)
&& isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) {
// Turn off EV fusion mode if no data has been received

2
EKF/ekf.h
View File

@ -382,6 +382,7 @@ private:
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited
@ -778,6 +779,7 @@ private:
void controlMagFusion();
bool noOtherYawAidingThanMag() const;
bool otherHeadingSourcesHaveStopped();
void checkHaglYawResetReq();
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }

11
EKF/mag_control.cpp
View File

@ -72,6 +72,8 @@ void Ekf::controlMagFusion()
return;
}
_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
if (noOtherYawAidingThanMag() && _mag_data_ready) {
if (_control_status.flags.in_air) {
checkHaglYawResetReq();
@ -327,3 +329,12 @@ void Ekf::run3DMagAndDeclFusions()
}
}
bool Ekf::otherHeadingSourcesHaveStopped()
{
// detect rising edge of noOtherYawAidingThanMag()
bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
return result;
}

11
test/test_EKF_fusionLogic.cpp
View File

@ -270,6 +270,7 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
{
// WHEN: allow vision position to be fused and we send vision data
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
_ekf_wrapper.enableExternalVisionHeadingFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
@ -281,19 +282,23 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion)
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
// THEN: Yaw state should be reset to vision
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
// WHEN: stop sending vision data
_sensor_simulator.stopExternalVision();
_sensor_simulator.runSeconds(6);
_sensor_simulator.runSeconds(7.1);
// THEN: EKF should stop to intend to fuse vision position estimate
// and EKF should not have a valid local position estimate anymore
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
// TODO: it is still intending to fuse ev_yaw. There should be some fallback to mag if possible
EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
EXPECT_FALSE(_ekf->local_position_is_valid());
EXPECT_FALSE(_ekf->global_position_is_valid());
// THEN: Yaw state shoud be reset to mag
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
}
TEST_F(EkfFusionLogicTest, doBaroHeightFusion)

3
test/test_EKF_gps_yaw.cpp
View File

@ -195,6 +195,8 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
// BUT WHEN: the GPS yaw is suddenly invalid
gps_heading = NAN;
_sensor_simulator._gps.setYaw(gps_heading);
@ -204,4 +206,5 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
// the estimator should fall back to mag fusion
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
}