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,601 Commits 852 Branches 161 Tags
a60591378cca4a2434763052850f7329f6f265de
Commit Graph
100 Commits
This Branch
This Branch
All Branches
Author SHA1 Message Date
bresch 6b10f1ca12 ekf: update change indicator 
The calculation of "alpha" in the accel_horiz and height_rate lowpass
filters is slightly different than before.
 2024-12-18 14:11:08 +01:00
bresch 480c232bfd ekf2: use alpha filter class 2024-12-18 14:11:08 +01:00
bresch fa5a781e20 AlphaFilter: set time constant instead of alpha 
Then the update function can set the dt at every iteration if needed
 2024-12-18 14:11:08 +01:00
bresch 37401d6fd1 AlphaFilter: allow setting dt in update call 2024-12-18 14:11:08 +01:00
bresch 4a73195007 yaw_est: store attitude as quaternion instead of DCM 
This saves flash and makes code simpler
 2024-12-18 09:33:11 +01:00
bresch 6969e5b6b4 ekf2: do not pre-compute airspeed Kalman gain 
The generated code is not much faster than the simple matrix-vector
multiplication
 2024-12-17 22:32:16 -05:00
bresch 3941b19968 ekf2_replay: log groundtruth messages 2024-12-13 17:40:08 +01:00
bresch 3165a77e26 SIH-AGP: add sensor failure simulation 2024-12-13 17:40:08 +01:00
bresch 2a08d4bdb8 add SensorAgpSim to generate Aux Global Position data 2024-12-13 17:40:08 +01:00
bresch f9140fcd50 ekf2: set baro bias when GNSS is alt ref 
Do this even when GNSS altitude fusion is disabled.
 2024-12-13 14:55:46 +01:00
bresch fad9ae855d EKF2: reset aid src when resetting state to measurement 
Filtered innovations and test ratios can be large before the reset and
would trigger pre-flight warnings
 2024-12-13 14:26:57 +01:00
bresch cc92979b06 ekf-agp: fix timeout 2024-12-12 15:49:04 +01:00
bresch 8626019ae0 EKF2: reset global position using variance 2024-12-03 13:04:25 +01:00
bresch 6b637f82f8 lla; fix conversion to ECEF 2024-12-03 13:03:54 +01:00
bresch b06ff99a3e disable SIH on x21-777 and v6u to save flash 2024-11-29 14:21:29 +01:00
bresch 8b1975cb98 SIH: lower IMU noise before takeoff 
This speeds-up the EKF alignment
 2024-11-29 14:21:29 +01:00
bresch b30ea40c6d SIH: set GNSS delay to 0 as delay is not simulated 2024-11-29 14:21:29 +01:00
bresch cd18138b1c SIH: add transport rate acceleration to local acceleration 2024-11-29 14:21:29 +01:00
bresch 674aa474e7 SIH: use LatLonAlt class 2024-11-29 14:21:29 +01:00
bresch 189122d553 lla: add gravity constant at equator 2024-11-29 14:21:29 +01:00
bresch b6658df169 lla: move to lib directory 2024-11-29 14:21:29 +01:00
bresch 7cf42727fb lla: add functions to convert from and to ECEF 2024-11-29 14:21:29 +01:00
bresch 7ee69d616d SIH: rework FW ground contact 2024-11-29 14:21:29 +01:00
bresch 5d33971712 SIH: refactor MC ground contact 2024-11-29 14:21:29 +01:00
bresch 9b172d36a2 matrix: allow casting float<->double 2024-11-29 14:21:29 +01:00
bresch 85bc8ef885 SIH-plane: fix actuator mapping 2024-11-27 11:14:56 -05:00
bresch 8a9bac29a2 SIH-FW: allow pitching up during takeoff 
Otherwise difficult to get lift
 2024-11-27 11:14:56 -05:00
bresch 7236ef2d17 SIH-FW: fix aileron and elevator signs 
This broken when changing from mixer files to the control allocation module.
 2024-11-27 11:14:56 -05:00
bresch 1dad25b763 SIH: do not assume being a tailsitter when creating airspeed measurement 2024-11-27 11:14:56 -05:00
bresch 14fe6c2167 ekf2: resetHeightTo -> resetAltitudeTo 
The vertical position state is now an altitude, not just a local height
 2024-11-22 15:13:47 +01:00
bresch 93c690f133 ekf2: do not inline getPosition 2024-11-22 15:13:47 +01:00
bresch a224d38e52 lla: move implementation to cpp file 
This reduces flash usage
 2024-11-22 15:13:47 +01:00
bresch aeb182a8ed ekf2: rename resetAltitudeTo to initialiseAltitudeTo 
This is to better show that the altitude is also used to set the origin.
 2024-11-22 15:13:47 +01:00
bresch 30d98885b7 update change indicator 2024-11-22 15:13:47 +01:00
bresch 814a2706f5 ekf2: compensate for coriolis and transport rate accelerations 2024-11-22 15:13:47 +01:00
bresch 842212df6c ekf2: update earth rate also without GNSS 2024-11-22 15:13:47 +01:00
bresch 6becccb217 update change indicator 2024-11-22 15:13:47 +01:00
bresch a492457322 ekf2-derivation: build state struct based on type 2024-11-22 15:13:47 +01:00
bresch 13c5bd4412 ekf-flow: do not reset position when resetting to flow 
Flow only provides velocity information
 2024-11-22 15:13:47 +01:00
bresch b19a6ee3b5 ekf2: store position state as lat/lon/alt 
The position error state is still defined in a body-fixed NED frame but the
position state itself is latitude-longitude-altitude.
 2024-11-22 15:13:47 +01:00
bresch ea1a6a327c flow: add scale factor parameter 2024-11-13 10:28:29 -05:00
bresch 31bef770aa replay: do not fail on first ignored topic 2024-11-12 21:22:25 -05:00
bresch 651675fed0 ekf-yaw-est: do not store full S_inverse 2024-11-07 11:37:56 -05:00
bresch 7b04ea0dca ekf-yaw-est: wrap_pi when updating yaw 2024-11-07 11:37:56 -05:00
bresch 3ad4b57315 ekf-yaw-est: do not reset when all weights collapse 
The likelyhood can be really small when the innovation is suddenly
large. This can occur during a GNSS velocity glitch and shouldn't reset
the filter as it would bring it back to a really vulnerable state.
 2024-11-07 11:37:56 -05:00
bresch aa5fdd3bb3 ekf2-rng-kin: allow invalidating at any vertical speed 
The issue is that when the HAGL is low, the drone will usually
decelerate and then the check would run anymore. If the low HAGL
estimate is due to bad sensor readings (e.g.: reflections), it will be
stuck in that state.
 2024-11-05 16:51:49 +01:00
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