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

EKF: Add control logic for fusion modes

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-14 22:01:53 +01:00
committed by Roman
parent 22c177c951
commit dba58aa4c6
1 changed files with 35 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+35 -16
View File
@@ -62,8 +62,8 @@ void Ekf::controlFusionModes()
// If the heading is valid, reset the positon and velocity and start using gps aiding // If the heading is valid, reset the positon and velocity and start using gps aiding
if (_control_status.flags.yaw_align) { if (_control_status.flags.yaw_align) {
resetPosition(); resetPosition();
resetVelocity(); resetVelocity();
_control_status.flags.gps = true; _control_status.flags.gps = true;
} }
} }
@@ -101,31 +101,50 @@ void Ekf::controlFusionModes()
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
// or the more accurate 3-axis fusion // or the more accurate 3-axis fusion
if (!_control_status.flags.armed) { if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
// always use simple mag fusion for initial startup if (!_control_status.flags.armed) {
// always use simple mag fusion for initial startup
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
} else {
if (_control_status.flags.in_air) {
// always use 3-axis mag fusion when airborne
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = true;
} else {
// always use simple heading fusion when on the ground
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use simple heading fusion
_control_status.flags.mag_hdg = true; _control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false; _control_status.flags.mag_3D = false;
} else { } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
if (_control_status.flags.in_air) { // always use 3-axis mag fusion
// always use 3-axis mag fusion when airborne _control_status.flags.mag_hdg = false;
_control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = true;
_control_status.flags.mag_3D = true;
} else { } else {
// always use simple heading fusion when on the ground // do no magnetometer fusion at all
_control_status.flags.mag_hdg = true; _control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false; _control_status.flags.mag_3D = false;
}
} }
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination needs to be fused as an observation to prevent long term heading drift // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
if (_control_status.flags.mag_3D && !_control_status.flags.gps) { // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
_control_status.flags.mag_dec = true; _control_status.flags.mag_dec = true;
} else { } else {
_control_status.flags.mag_dec = false; _control_status.flags.mag_dec = false;
} }
} }
void Ekf::calculateVehicleStatus() void Ekf::calculateVehicleStatus()