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

ekf2-fake pos: add valid fake position fusion

Browse Source 
This is similar to fake pos but is only used when the ekf has an
external information telling it that the vehicle is not changing
position. This information can then be used to keep a valid local
position even when the vehicle isn't exactly at rest.
This commit is contained in:
bresch
2024-08-19 15:36:04 +02:00
committed by Mathieu Bresciani
parent 64b0586dad
commit 1a0f97ebbd
13 changed files with 215 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+11
View File
@@ -1925,6 +1925,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_aux_gpos = _ekf.control_status_flags().aux_gpos;
status_flags.cs_rng_terrain = _ekf.control_status_flags().rng_terrain;
status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain;
status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos;
status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
@@ -2592,6 +2594,15 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
flags.gnd_effect = vehicle_land_detected.in_ground_effect;
}
launch_detection_status_s launch_detection_status;
if (_launch_detection_status_sub.copy(&launch_detection_status)
&& (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) {
flags.constant_pos = (launch_detection_status.launch_detection_state ==
launch_detection_status_s::STATE_WAITING_FOR_LAUNCH);
}
_ekf.setSystemFlagData(flags);
}
}