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

Refactor gps fusion commencing

Browse Source
This commit is contained in:
Kamil Ritz
2020-06-06 13:48:05 +02:00
committed by Mathieu Bresciani
parent 38cbd1a182
commit 74780aa512
1 changed files with 7 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+7 -15
View File
@@ -572,25 +572,17 @@ void Ekf::controlGpsFusion()
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
// if we are not already aiding with optical flow, then we need to reset the position and velocity
// otherwise we only need to reset the position
_control_status.flags.gps = true;
resetHorizontalPositionToGps();
// when adding with optical flow,
// velocity reset is not necessary
if (!_control_status.flags.opt_flow) {
if (!resetHorizontalPosition() || !resetVelocity()) {
_control_status.flags.gps = false;
}
} else if (!resetHorizontalPosition()) {
_control_status.flags.gps = false;
resetVelocityToGps();
}
if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("starting GPS fusion");
_time_last_gps = _time_last_imu;
}
ECL_INFO_TIMESTAMPED("starting GPS fusion");
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
}
}