fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity
46,346 Commits 852 Branches 161 Tags
c199a284f5681afb562394e263786b8ff444aa91
Commit Graph
100 Commits
This Branch
This Branch
All Branches
Author SHA1 Message Date
bresch d579fb540c ekf2-terrain: do not consider negative hagl as valid 2024-10-30 12:14:43 -04:00
bresch 6119b08ef4 mpc: reset integrators when controllers are not used 
Starting with a non-zero integrator could appear as a strong disturbance
when engaging position mode.
 2024-10-02 10:47:25 -04:00
bresch dbf51da99e ekf2: rework amsl to ellipsoid altitude conversion 2024-10-01 10:40:18 -04:00
bresch 5bfa6b3359 ekf2: remove redundant ned_origin_initialised flag 2024-10-01 10:37:20 -04:00
Mathieu Bresciani 28d960fc21 fix VehicleCommand.msg syntax 2024-09-30 13:42:50 -04:00
bresch 35efe44a6b ekf2: fusion always succeeds, do not return boolean 2024-09-24 10:48:14 -04:00
bresch 23f343aa3a ekf2-rng: consider height covariance for terrain reset to range 
Range does not provide a direct terrain observation but a measurement
relative to the height state. Correlation between height and terrain
must be set properly.
 2024-09-24 10:18:25 -04:00
bresch 15ddd94349 ekf2: move implementation to ekf_helper to reduce flash usage 
saves about 4.8kB of flash on FMUv5
 2024-09-23 10:27:42 -04:00
Mathieu BrescianiandMatthias Grob 03aec2e188 HeadingSmoothing: fix angle wrapping and add unit tests 
Co-authored-by: Matthias Grob <maetugr@gmail.com>
 2024-09-10 15:06:19 +02:00
bresch 15e9c65a8f dist-sensor: reduce enum names 2024-09-09 15:40:40 +02:00
bresch 8bca467c15 dist-sensor: use enum instead of integer 2024-09-09 15:40:40 +02:00
Mathieu Bresciani 2cda0efd84 Commander: extend COM_ARM_WO_GPS to disable warning (#23628) 2024-08-28 17:33:58 +02:00
bresch bab256bfe6 ekf2: handle external altitude resets 2024-08-28 11:02:26 +02:00
bresch cd2170deea ekf2-origin: backcompute based on lpos validity 2024-08-28 11:02:26 +02:00
bresch 130fefb1e7 ekf2: initialize origin from corrent position when possible 2024-08-28 11:02:26 +02:00
bresch af752536b9 ekf2: extract setting origin from current lat/lon/alt 
This is not only needed when GNSS is available but also for other global
sources of position (e.g.: aux global pos and manual pos updates)
 2024-08-28 11:02:26 +02:00
bresch 9169a7c5fc ekf2: split horizontal and vertical origin reset 
Allow partial resets (only lat/lon or only altitude)
 2024-08-28 11:02:26 +02:00
breschandMarco Hauswirth 9183c479a5 ekf2: correctly compute vel variance from flow variance 
Co-authored-by: Marco Hauswirth <marco.hauswirth@auterion.com>
 2024-08-26 11:28:36 -04:00
bresch 1a0f97ebbd ekf2-fake pos: add valid fake position fusion 
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.
 2024-08-23 11:17:21 +02:00
bresch 64b0586dad ekf2: return validity based on dead-reckoning time only 2024-08-23 11:17:21 +02:00
bresch 0931179579 ekf2: extract WMM update logic 2024-08-20 10:32:27 -04:00
bresch ad1d9e1312 failsafe: do not add additional hold delay if failsafe action is hold 2024-08-16 16:26:20 +02:00
bresch ea673b0b5b navigator: check hagl failsafe centrally 2024-08-16 16:26:20 +02:00
bresch 4ed3e9e210 navigator: add failure enum 2024-08-14 11:08:02 +02:00
bresch 25fcb3c913 comander: trigger failsafe when navigator reports failure 2024-08-14 11:08:02 +02:00
bresch 9f69e9ee6c navigator: publish navigator_state 
feedback to commander
 2024-08-14 11:08:02 +02:00
bresch 1fa878ad88 navigator: add navigation state ID to every mode class 2024-08-14 11:08:02 +02:00
bresch aad607e0dd ekf2: send airspeed data to ekf backend regardless of sign 
On ground the airspeed can sometimes be slightly negative but the ekf
should still know that airspeed data is flowing regularly
 2024-08-13 17:43:45 +02:00
bresch 478875c006 ekf tools: compare gyro integral with attitude estimate 2024-08-12 15:26:08 +02:00
bresch 3f17f15505 commander: allow FW manual pos control without global position 2024-08-09 12:51:09 +02:00
bresch 8b12b7c641 commander: extend local position 'relaxed' validity 
Relaxed position is valid as long as a velocity aiding source is active
(e.g.: optical flow or airspeed+sideslip)
 2024-08-09 12:51:09 +02:00
bresch f04aa2494b FW pos control: do not requre global pos in manual position control 2024-08-09 12:51:09 +02:00
bresch d2478d00cf ekf2: only allow ref sensor to reset height 2024-08-06 10:19:13 -04:00
bresch 7b3d168af1 baro tuning: make hpf argument optional 
This filter is often not needed. Setting the default value to -1 makes it
optional.
 2024-07-29 14:20:41 +02:00
bresch 0b1eba948a ekf2-flow: add param to force using internal gyro 
In some cases the vibration environment of the optical flow sensor is
worse than near the autopilot.
 2024-07-25 19:54:19 -04:00
bresch 79e0e00d8c ekf2: block process noise increment without constraining the variance 
The wind variance can be reset to a value larger than the wind init
variance (e.g.: when the reset occurs when flying close to the N or E
axis). Constraining the variance after a covariance initialization would
artificially increase the correlation and could destabilize the filter.
 2024-07-24 17:11:53 +02:00
bresch 81575049df ekf2: reword EKF2_GPS_CHECK param 2024-07-16 10:15:19 -04:00
bresch 20c0d3a096 ekf2: enable all GNSS checks by default 2024-07-16 10:15:19 -04:00
bresch 9d6c2baa90 ekf2-flow: only allow flow when in range 
Also, as the flow makes the link between range and horizontal velocity,
only allow it to start if at least one of the two is known. Otherwise
the EKF will struggle to estimate both values at the same time.
 2024-07-15 11:40:33 -04:00
bresch a50ef2eb5e ekf2-terrain: make terrain validity based on uncertainty 
When using optical flow for terrain aiding, we cannot assume that the
terrain estimate is valid if flow is fused as is can only be observed
during motion. When no direct observation is available, the terrain is
assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.
 2024-06-26 11:05:38 +02:00
bresch a665764b0e ekf2: remove unused EKF2_TERR_MASK 2024-06-26 11:05:38 +02:00
bresch 7903ddf5df ekf2-terrain: terrain is not a separate estimator 2024-06-26 11:05:38 +02:00
bresch 9001c23926 ekf2: clean up hagl vs terrain naming 
Terrain is the state: terrain vertical position
Hagl (height above ground level) is the vertical distance between the
vertical position and the terrain vertical position
 2024-06-26 11:05:38 +02:00
bresch 68980b59e2 ekf2: add terrain state 2024-06-26 11:05:38 +02:00
bresch 0a665a526c ekf2: add mag type init 
In this mode, the mag is used to inilialize the heading.
During flight, the heading is predicted using gyros and corrected
by GNSS measurements if available.
 2024-06-03 13:28:12 +02:00
bresch 53210dd8f3 ekf2-mag: with NE aiding, constrain heading drift only before takeoff 
After takeoff, the heading is easily observable
 2024-05-31 10:38:17 -04:00
bresch 3dac9af09e ekf2-mag: do not reset on WMM change when NE aiding is active 
The mag field states are observed. No need to reset them.
 2024-05-31 10:38:17 -04:00
bresch ee765e7859 ekf2-mag: do not reset when NE aiding is active 2024-05-31 10:38:17 -04:00
bresch 6515935b52 ekf2-mag: do not limit the earth mag field estimate 
The EKF can recover from an initial bad earth mag field estimate.
Constraining the field is not necessary and can lead to an unpredicted
behavior of the filter.
Declination fusion is safe to run even when the horizontal field is 0
 2024-05-31 10:38:17 -04:00
bresch 774b6ed3b8 ekf2-mag: do not use yaw emergency estimator to reset mag states 
On slowly moving vehicles (e.g.: boats, rovers), the yaw estimator has
worse convergence than the main EKF. Resetting the mag states using the
yaw estimator as reference can lead to poor heading. Also, the EKF can
recover really well from initially incorrect mag states.
 2024-05-31 10:38:17 -04:00
bresch c3d984703c ekf2-mag: remove immediate declination fusion after reset 2024-05-31 10:38:17 -04:00
bresch a6007e4b93 ekf2-mag: turn around update_all_states condition 
Non-functional
 2024-05-31 10:38:17 -04:00
bresch c11c75d32e ekf2-mag: always add process noise until initial value 2024-05-31 10:38:17 -04:00
bresch 993782cffa ekf2: only trigger position timeout reset when hpos fusion is active 2024-05-29 20:49:14 -04:00
bresch f9e2ab8d44 msg-rates-setpoint: fix frame name NED -> FRD 2024-05-27 09:35:34 +02:00
bresch ccbcbbe268 wind_est_replay: report scale instead of inverse_scale 
The estimator internally estimates the scale inverse, but the interface
should be the scale as "airspeed_corrected = scale * airspeed"
 2024-05-24 17:25:39 +02:00
bresch ea39032b45 mag_ctrl: combine common conditions for mag_hdg and mag_3d 2024-05-17 11:19:04 +02:00
bresch d796009302 mag_ctrl: do not fuse synthetic mag but do not zero the innovation 2024-05-17 11:19:04 +02:00
bresch b42799fac2 wind_est_replay: allow setting the initial scale factor 2024-05-17 09:17:08 +02:00
bresch 440465702e wind_est_replay: fix cov matrix format and data indexing 2024-05-17 09:17:08 +02:00
bresch 9dc7719d4a ekf2: Only reset to GNSS heading if necessary 
When North-East (e.g.: GNSS pos/vel) aiding is active, the heading
estimate is constrained and consistent with the vel/pos aiding. Reset to
GNSS heading should only occur if no N-E aiding is active or if the
filter is not yes aligned. Otherwise, just wait for the consistency
check to pass again (will pass at some point if the heading uncertainty
of the filter is getting too high).
 2024-04-29 07:22:01 -07:00
bresch 7cefc3172a estimatorCheck: get param only if handle is valid 2024-04-25 14:48:23 -04:00
bresch b508df39a2 imu consistency: don't scale param threshold 2024-04-24 15:51:07 -04:00
bresch 8bf1cf0b15 ekf2_params: reduce "short" description 2024-04-24 15:09:57 -04:00
bresch 4cf51e5833 baro comp: set hpf optional 2024-04-22 15:59:50 +02:00
bresch a8a67fbf8f ekf2: set horizon using specific parameter 
Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.
 2024-04-17 09:16:16 -04:00
bresch 7a973eddce ekf2: migrate param to yaml 2024-04-16 11:14:13 -04:00
bresch 42a16bd91d order yaml fileds 2024-04-16 11:14:13 -04:00
bresch 5f61e3b785 ekf2: update change indicator 2024-04-05 22:26:03 -04:00
bresch b79d3854e4 ekf2: remove option to continuously fuse mag declination 
Declination fusion is only used when not observable (no global aiding).
 2024-04-05 22:26:03 -04:00
bresch dae246d7e2 ekf2: do not continuously use mag decl fusion when GNSS fusion is active 
This prevents over-constraining the heading from mag fusion. An
incorrect mag yaw rotation can be absorbed as a declination error.
 2024-04-05 22:26:03 -04:00
bresch 2f51db7284 ekf2: limit mag heading fusion to prevent heading overconfidence 2024-04-05 22:26:03 -04:00
bresch 51883fe5d4 ekf2: integrate mag heading into mag 3D 2024-04-05 22:26:03 -04:00
bresch 0c9e4013d2 ekf2: improve tilt leveling speed 
Starting with no yaw uncertainty makes the tilt more observable when
using fake position fusion during the quasi-stationary alignment phase.
 2024-04-05 22:26:03 -04:00
bresch 12fefbcfbd ekf2: use global definition of quaternion error 2024-04-05 22:26:03 -04:00
bresch 62b8db153b mpc: fix PositionControl unit test 
The unit test assumes the position controller is in "decoupled" mode
 2024-03-20 14:16:12 +01:00
bresch 638e17d551 ekf: update change indicator 2024-03-20 14:13:49 +01:00
bresch 6d819343aa ekf2: fix direct state measurement update for suboptimal K case 
The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).
 2024-03-20 14:13:49 +01:00
bresch cb2bb2e098 ekf2: add no gyro bias estimate test case 
This makes the ekf unstable and creates NANs during initialization
 2024-03-20 14:13:49 +01:00
bresch c9221b91ad ekf2: fix gnss yaw unit test 2024-03-20 14:04:19 +01:00
bresch 2e6dd243af mpc: add possibility to generate tilt using full 3D accel 
Using full 3D acceleration provides better horizontal acceleration
tracking but also creates a sometimes unwanted behavior because the tilt
is directly coupled with the vertical acceleration setpoint.
 2024-03-19 12:29:34 -04:00
bresch 7c6ecd95a8 mc_wind_estimator_tuning: optionally use GNSS velocity 
Sometimes GNSS is logged but not used
 2024-03-14 21:06:24 +01:00
bresch b2f1122372 ekf2: remove old yaw 321 and 312 derivations 2024-03-14 11:33:45 -04:00
bresch ee63f3e664 update change indicator 2024-03-14 11:33:45 -04:00
bresch e3f67d5c1a ekf2: new yaw derivation 
Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis
 2024-03-14 11:33:45 -04:00
bresch 0d0978b3b9 ekf2: update change indicator 2024-03-07 11:06:31 -05:00
bresch 0639f5370c ekf2: fix mag and wind covariance prediction 2024-03-07 11:06:31 -05:00
bresch 2bacb4b65d ekf2: update change indicator 2024-03-07 15:11:47 +01:00
bresch 421f13e4b5 ekf2: fix joseph covariance update for Schmidt-Kalman filter 
If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
 2024-03-07 15:11:47 +01:00
bresch 6f9a378247 yaw_est: force set gyro bias when at rest 
The gyro bias estimate from EFK2 is really good when at rest and should
be used by the yaw estimator to prevent heading drifts due to poor
heading observability.
 2024-03-05 14:00:06 -05:00
bresch 28db3e1c8c ekf2: update change indicator 2024-02-27 12:33:43 -05:00
bresch e9d43015ce ekf2: fix unit tests failing due to mag fusion changes 2024-02-27 12:33:43 -05:00
bresch b46fc9a67d ekf2 sensor_sim: set correct world mag field 2024-02-27 12:33:43 -05:00
bresch b80f15f7b5 ekf2-mag_auto: always use mag 3D after takeoff 2024-02-27 12:33:43 -05:00
bresch 051baec9c4 ekf2: allow wind dead-reckoning after manual position reset 
Reset velocity using airspeed and start navigating
 2024-02-27 09:34:05 -05:00
bresch 08a2a6c836 update EKF2 change indicator 2024-02-26 12:32:59 -05:00
bresch d501d8e1d4 ekf2: use Joseph stabilized update in direct state observations 2024-02-26 12:32:59 -05:00
bresch 9d9766c6cf ekf2: use Joseph stabilized covariance update 2024-02-26 12:32:59 -05:00
bresch 37a40d3fc2 baro static pressure compensation tuning: remove dependency to baro bias 
`estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.
 2024-02-23 10:07:13 +01:00
bresch c221da27a7 ekf2: set attitude validity flag using centralized function 2024-02-20 11:33:30 -05:00