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

yaw_align: let the mag control logic do the alignement and resets

Browse Source
This commit is contained in:
bresch 2021-05-11 12:21:05 +02:00 committed by Paul Riseborough
parent 2cc023038b
commit a4fe2b1e72
2 changed files with 4 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/control.cpp
View File

@ -53,11 +53,10 @@ void Ekf::controlFusionModes()
// whilst we are aligning the tilt, monitor the variances
const Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// Once the tilt variances have reduced to equivalent of 3deg uncertainty
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); // TODO: is this needed?
// send alignment status message to the console
const char* height_source = nullptr;
@ -435,12 +434,6 @@ void Ekf::controlOpticalFlowFusion()
&& !_control_status.flags.opt_flow // we are not yet using flow data
&& !_inhibit_flow_use)
{
// If the heading is not aligned, reset the yaw and magnetic field states
// TODO: ekf2 should always try to align itself if not already aligned
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
}
// If the heading is valid and use is not inhibited , start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout

4
EKF/ekf.cpp
View File

@ -203,7 +203,9 @@ bool Ekf::initialiseFilter()
}
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
// but do not mark the yaw alignement complete as it needs to be
// reset once the leveling phase is done
resetMagHeading(_mag_lpf.getState(), false, false);
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();