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

GPS yaw: Extract and refactor GPS yaw control logic

Browse Source 
- Rename GPS yaw fusion functions:
resetGpsAntYaw -> resetYawToGps
fuseGpsAntYaw -> fuseGpsYaw

- Move last fusion time in reset function
This commit is contained in:
bresch
2020-07-02 13:35:13 +02:00
committed by Mathieu Bresciani
parent 7f4fedde6a
commit b0f79caf34
3 changed files with 45 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+38 -34
View File
@@ -518,40 +518,7 @@ void Ekf::controlGpsFusion()
// Detect if coming back after significant time without GPS data
bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
if (!(_params.fusion_mode & MASK_USE_GPSYAW)
|| _is_gps_yaw_faulty) {
stopGpsYawFusion();
} else {
if (ISFINITE(_gps_sample_delayed.yaw)) {
if (!_control_status.flags.gps_yaw
&& _control_status.flags.tilt_align
&& !_gps_hgt_intermittent) {
// Activate GPS yaw fusion
if (resetGpsAntYaw()) {
_control_status.flags.yaw_align = true;
startGpsYawFusion();
}
}
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
}
// Check if the data is constantly fused by the estimator,
// if not, declare the sensor faulty and stop the fusion
// By doing this, another source of yaw aiding is allowed to start
if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)
&& _control_status.flags.gps_yaw) {
_is_gps_yaw_faulty = true;
stopGpsYawFusion();
}
}
controlGpsYawFusion();
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
@@ -761,6 +728,43 @@ void Ekf::controlGpsFusion()
}
}
void Ekf::controlGpsYawFusion()
{
if (!(_params.fusion_mode & MASK_USE_GPSYAW)
|| _is_gps_yaw_faulty) {
stopGpsYawFusion();
return;
}
if (ISFINITE(_gps_sample_delayed.yaw)) {
if (_control_status.flags.gps_yaw) {
fuseGpsYaw();
} else {
// Try to activate GPS yaw fusion
if (_control_status.flags.tilt_align
&& !_gps_hgt_intermittent) {
if (resetYawToGps()) {
_control_status.flags.yaw_align = true;
startGpsYawFusion();
}
}
}
}
// Check if the data is constantly fused by the estimator,
// if not, declare the sensor faulty and stop the fusion
// By doing this, another source of yaw aiding is allowed to start
if (_control_status.flags.gps_yaw
&& isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
_is_gps_yaw_faulty = true;
stopGpsYawFusion();
}
}
void Ekf::controlHeightSensorTimeouts()
{
/*