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

Merge pull request #322 from PX4/pr-ekfMagFusionMode 

EKF Add mag fusion mode for VTOL
This commit is contained in:
Paul Riseborough 2017-10-13 07:27:54 +11:00 committed by GitHub
parent 9c65968c3d dbff89fbcb
commit a34bba87fc
3 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -179,6 +179,7 @@ struct dragSample {
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states.
// Maximum sensor intervals in usec
#define GPS_MAX_INTERVAL 5e5 ///< Maximum allowable time interval between GPS measurements (uSec)

12
EKF/control.cpp
View File

@ -1105,7 +1105,7 @@ void Ekf::controlMagFusion()
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) {
// Check if height has increased sufficiently to be away from ground magnetic anomalies
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
@ -1188,7 +1188,15 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_hdg = true;
}
// perform switch-over from only updating the mag states to updating all states
/*
Control switch-over between only updating the mag states to updating all states
When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates.
When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed
to access the magnetic field states.
*/
_control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW)
&& _control_status.flags.fixed_wing;
if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
// When re-commencing use of magnetometer to correct vehicle states
// set the field state variance to the observation variance and zero

6
EKF/ekf_helper.cpp
View File

@ -506,7 +506,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
@ -579,7 +579,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
@ -616,7 +616,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// using magnetic heading tuning parameter
angle_err_var_vec(2) = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
}