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

EV yaw: move EV yaw reset out of resetMagHeading function

Browse Source 
Also remove the unnecessary 2nd "manual reset" done in the start function
This commit is contained in:
bresch
2021-05-11 11:59:08 +02:00
committed by Paul Riseborough
parent 48722a2a92
commit 6248646bff
3 changed files with 21 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -4
View File
@@ -222,12 +222,13 @@ void Ekf::controlExternalVisionFusion()
if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless data is arriving frequently
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
startEvYawFusion();
if (resetYawToEv()) {
_control_status.flags.yaw_align = true;
startEvYawFusion();
}
}
}
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
@@ -342,7 +343,6 @@ void Ekf::controlExternalVisionFusion()
stopEvFusion();
_warning_events.flags.vision_data_stopped = true;
ECL_WARN("vision data stopped");
}
}
EKF/ekf.h
+5 -1
View File
@@ -673,10 +673,14 @@ private:
// update the terrain vertical position estimate using an optical flow measurement
void fuseFlowForTerrain();
// reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
// reset the heading and magnetic field states using the declination and magnetometer measurements
// return true if successful
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true);
// reset the heading using the external vision measurements
// return true if successful
bool resetYawToEv();
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
bool realignYawGPS();
EKF/ekf_helper.cpp
+12 -18
View File
@@ -496,14 +496,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
float yaw_new;
float yaw_new_variance = 0.0f;
if (_control_status.flags.ev_yaw) {
yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
if (increase_yaw_var) {
yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
}
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
@@ -520,7 +513,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
return true;
} else {
// there is no yaw observation
// there is no magnetic yaw observation
return false;
}
@@ -538,6 +531,16 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
return true;
}
bool Ekf::resetYawToEv()
{
const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw_new, yaw_new_variance, true);
return true;
}
// Return the magnetic declination in radians to be used by the alignment and fusion processing
float Ekf::getMagDeclination()
{
@@ -1512,15 +1515,6 @@ void Ekf::startEvVelFusion()
void Ekf::startEvYawFusion()
{
// reset the yaw angle to the value from the vision quaternion
const float yaw = getEuler321Yaw(_ev_sample_delayed.quat);
const float yaw_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
resetQuatStateYaw(yaw, yaw_variance, true);
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_dec = false;