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

Refactor ev fusion start into helper functions (#872)

Browse Source
This commit is contained in:
kritz
2020-07-27 11:42:52 +02:00
committed by GitHub
parent 8c65ca29e4
commit 88c52aba5e
3 changed files with 40 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -24
View File
@@ -200,16 +200,12 @@ void Ekf::controlExternalVisionFusion()
if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
// turn on use of external vision measurements for position
if (_params.fusion_mode & MASK_USE_EVPOS && !_control_status.flags.ev_pos) {
_control_status.flags.ev_pos = true;
resetHorizontalPosition();
ECL_INFO_TIMESTAMPED("starting vision pos fusion");
startEvPosFusion();
}
// turn on use of external vision measurements for velocity
if (_params.fusion_mode & MASK_USE_EVVEL && !_control_status.flags.ev_vel) {
_control_status.flags.ev_vel = true;
resetVelocity();
ECL_INFO_TIMESTAMPED("starting vision vel fusion");
startEvVelFusion();
}
}
}
@@ -218,24 +214,7 @@ void Ekf::controlExternalVisionFusion()
if (!_control_status.flags.gps && (_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)) {
// reset the yaw angle to the value from the vision quaternion
const Eulerf euler_obs(_ev_sample_delayed.quat);
const float yaw = euler_obs(2);
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;
stopMagHdgFusion();
stopMag3DFusion();
ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
startEvYawFusion();
}
}
EKF/ekf.h
+4 -3
View File
@@ -854,12 +854,13 @@ private:
void startGpsYawFusion();
void stopGpsYawFusion();
void startEvPosFusion();
void startEvVelFusion();
void startEvYawFusion();
void stopEvFusion();
void stopEvPosFusion();
void stopEvVelFusion();
void stopEvYawFusion();
void stopAuxVelFusion();
EKF/ekf_helper.cpp
+33
View File
@@ -1575,6 +1575,39 @@ void Ekf::stopGpsYawFusion()
_control_status.flags.gps_yaw = false;
}
void Ekf::startEvPosFusion() {
_control_status.flags.ev_pos = true;
resetHorizontalPosition();
ECL_INFO_TIMESTAMPED("starting vision pos fusion");
}
void Ekf::startEvVelFusion() {
_control_status.flags.ev_vel = true;
resetVelocity();
ECL_INFO_TIMESTAMPED("starting vision vel fusion");
}
void Ekf::startEvYawFusion() {
// reset the yaw angle to the value from the vision quaternion
const Eulerf euler_obs(_ev_sample_delayed.quat);
const float yaw = euler_obs(2);
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;
stopMagHdgFusion();
stopMag3DFusion();
ECL_INFO_TIMESTAMPED("starting vision yaw fusion");
}
void Ekf::stopEvFusion()
{
stopEvPosFusion();