Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 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 the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) { if (_control_status.flags.yaw_align) {
// if we are not already aiding with optical flow, then we need to reset the position and velocity resetHorizontalPositionToGps();
// otherwise we only need to reset the position
_control_status.flags.gps = true;
// when adding with optical flow,
// velocity reset is not necessary
if (!_control_status.flags.opt_flow) { if (!_control_status.flags.opt_flow) {
if (!resetHorizontalPosition() || !resetVelocity()) { resetVelocityToGps();
_control_status.flags.gps = false;
}
} else if (!resetHorizontalPosition()) {
_control_status.flags.gps = false;
} }
if (_control_status.flags.gps) { ECL_INFO_TIMESTAMPED("starting GPS fusion");
ECL_INFO_TIMESTAMPED("starting GPS fusion"); _control_status.flags.gps = true;
_time_last_gps = _time_last_imu; _time_last_gps = _time_last_imu;
}
} }
} }