fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 00:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Split angular alignment into tilt and yaw and use yaw and magnetic field alignment function

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-12 14:23:44 +11:00
committed by Roman
parent 2bbe7f9a1c
commit aa58b3e98c
2 changed files with 17 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+9 -8
View File
@@ -211,14 +211,15 @@ union gps_check_fail_status_u {
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
uint8_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete
uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
uint8_t gps : 1; // 2 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic heading is being fused
uint8_t mag_3D : 1; // 5 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 6 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 7 - true when the vehicle is airborne
uint8_t armed : 1; // 8 - true when the vehicle motors are armed
} flags;
uint16_t value;
};
EKF/ekf.cpp
+8 -18
View File
@@ -246,27 +246,17 @@ bool Ekf::initialiseFilter(void)
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
_control_status.flags.tilt_align = true;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// TODO replace this with a conditional test based on fitered angle error states.
_control_status.flags.angle_align = true;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init;
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
// calculate the averaged barometer reading
_baro_at_alignment = _baro_sum / (float)_baro_counter;