dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add control logic for fusion modes

This commit is contained in:
Paul Riseborough 2016-02-12 14:38:24 +11:00 committed by Roman
parent 22c177c951
commit dba58aa4c6
1 changed files with 35 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
EKF/control.cpp
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 (_control_status.flags.yaw_align) {
resetPosition();
resetVelocity();
resetPosition();
resetVelocity();
_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
// or the more accurate 3-axis fusion
if (!_control_status.flags.armed) {
// always use simple mag fusion for initial startup
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
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_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 if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// always use 3-axis mag fusion
_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 {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = 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 (_control_status.flags.mag_3D && !_control_status.flags.gps) {
// 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
// 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;
} else {
_control_status.flags.mag_dec = false;
}
}
void Ekf::calculateVehicleStatus()