Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-07 16:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity
44,965 Commits 71 Branches 510 Tags 770 MiB
0529ddcd67
Commit Graph

452 Commits

Author SHA1 Message Date
Andrew Tridgell
6fe05a9ab6 AP_NavEKF3: restore XKTV message 2020-11-10 16:15:45 +11:00
Andrew Tridgell
f5275fd0ad AP_NavEKF3: fixed use of pointers in ringbuffers 
these don't work with AP_DAL
 2020-11-10 16:15:45 +11:00
Andrew Tridgell
eaa8474563 AP_NavEKF3: cope with INS_MAX_INSTANCES below 3 2020-11-10 16:15:45 +11:00
Andrew Tridgell
9b81c5a1e0 AP_NavEKF3: use dal reference in EKF backends 
saves a bit of flash space
 2020-11-10 16:15:45 +11:00
Andrew Tridgell
1bdf6173ec AP_NavEKF3: removed have_ekf_logging 2020-11-10 16:15:45 +11:00
Andrew Tridgell
21dfdc2192 AP_NavEKF3: use DAL for body frame odometry 2020-11-10 16:15:45 +11:00
Andrew Tridgell
a0de6c620f AP_NavEKF3: use selected GPS for gps_yaw_deg() 
more than one GPS could provide yaw
 2020-11-10 16:15:45 +11:00
Andrew Tridgell
f4c3d7286c AP_NavEKF3: support replay with wheel odomotry 2020-11-10 16:15:45 +11:00
Andrew Tridgell
e87f98066f AP_NavEKF3: support replay with external navigation data 2020-11-10 16:15:45 +11:00
Andrew Tridgell
7bbbbd314c AP_NavEKF3: added optflow support for AP_DAL 2020-11-10 16:15:45 +11:00
Andrew Tridgell
31091d6307 AP_NavEKF3: use ekf_low_time_remaining for core scheduling 2020-11-10 16:15:45 +11:00
Andrew Tridgell
f3c3778169 prevent duplicate EKF logging 
vehicle code sometimes tries to log the same data twice
 2020-11-10 16:15:45 +11:00
Andrew Tridgell
096aab9388 AP_NavEKF3: convert to use AP_DAL for new replay structure 
Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
 2020-11-10 16:15:45 +11:00
Randy Mackay
2681d3afe2 AP_NavEKF3: simplify alignYawAngle 2020-11-03 11:00:01 +11:00
Peter Barker
fc49b55b85 AP_NavEKF3: make coreBetterScore const 2020-11-03 10:39:35 +11:00
Randy Mackay
b726630ef4 AP_NavEKF3: pos timeout or glitch does not reset vel if fusing successfully 
vel is not reset if GPS velocity, optical flow, visual odometry or wheel
encoders are successfully being fused
 2020-10-28 19:06:53 +11:00
Randy Mackay
3b38aa6d7b AP_NavEKF3: GPS change pos reset uses ResetPositonNE and ResetPositionD 
Using these existing functions removes some code duplicate
non-functional change
 2020-10-28 19:03:34 +11:00
Randy Mackay
7b898ee001 AP_NavEKF3: minor comment and format fixes 2020-10-28 12:17:40 +09:00
Paul Riseborough
f2af2b3458 AP_NavEKF3: Initialise tiltErrorVariance class variable 2020-10-26 11:03:50 +11:00
Paul Riseborough
43386a8eea AP_NAvEKF3: Use a const reference 2020-10-26 11:03:50 +11:00
Paul Riseborough
64a3d8fe4e AP_NavEKF3: Update documentation 2020-10-26 11:03:50 +11:00
Paul Riseborough
6c7820dd8d AP_NavEKF3: Add verification of tiltErrorVariance() method in SITL 2020-10-26 11:03:50 +11:00
Paul Riseborough
90e928c32a AP_NavEKF3: Re-tune tilt alignment check 
New tilt error variance estimate is more accurate and larger than before.
 2020-10-26 11:03:50 +11:00
Paul Riseborough
16ae75a681 AP_NavEKF3: Consolidate and log tilt error variance calculation 2020-10-26 11:03:50 +11:00
Paul Riseborough
8a88dd23a2 AP_NavEKF3: Add missing generated code file for yaw fusion 2020-10-26 11:03:50 +11:00
Paul Riseborough
69632336e1 AP_NavEKF3: Remove singularity in yaw fusion at +-90deg yaw 
Uses sympy derivation
 2020-10-26 11:03:50 +11:00
Paul Riseborough
4e41e9f5f1 AP_NavEKF3: Ensure consistent use of 312 or 321 rotation order 
Prevents possibility of a differnt sequence being used to calculate the yaw and application of the yaw to the quaternions
 2020-10-26 11:03:50 +11:00
Paul Riseborough
fa10d114e4 AP_NavEKF3: Fix yaw drift after yaw reset at +-90 deg pitch 
Use existing covariance prediction code to set quaternion state covariances.
Assumes tilt error is 3 deg 1-sigma.
 2020-10-26 11:03:50 +11:00
Paul Riseborough
73d5ca5ad3 AP_NavEKF3: Fix yaw drift after yaw reset at +-90 deg pitch 
Use existing covariance prediction code to set quaternion state covariances.
Assumes tilt error is 3 deg 1-sigma.
TODO derive and add function that calculates tilt error variance.
 2020-10-26 11:03:50 +11:00
Paul Riseborough
a97d030e4b AP_NavEKF3: Reset earth field variances when starting mag field estimation 2020-10-26 11:03:50 +11:00
Paul Riseborough
9168d0cd19 AP_NavEKF3: Initialise mag field variances to non zero values 2020-10-26 11:03:50 +11:00
Paul Riseborough
0b96f046c2 AP_NavEKF3: Unblock initial yaw alignment message 2020-10-26 11:03:50 +11:00
Paul Riseborough
f81abd73d6 AP_NavEKF3: Update covariance prediction to yse SymPy generated equations 
Saves flash and variable space.
 2020-10-26 11:03:50 +11:00
Andrew Tridgell
a4716fb9bf AP_NavEKF3: fixed multi-core yaw reset 
thanks to Alex Burka for finding this
 2020-10-24 19:35:30 +11:00
Andrew Tridgell
fcccdcc936 AP_NavEKF3: fixed bug in external yaw for fixed wing 
when we are on the ground we should not chang to AID_NONE if we have
an external yaw source

this fixes an EKF3 error loop on the ground found by Michael
 2020-10-24 13:50:33 +11:00
Randy Mackay
c3fd82b6d7 AP_NavEKF3: minor spelling fix 2020-10-24 12:35:58 +11:00
Randy Mackay
2f1ae29a9c AP_NavEKF3: extnav data gets corrected boolean 
CorrectExtNavForSensorOffset and CorrectExtNavVelForSensorOffset use
corrected boolean to avoid correcting twice
 2020-10-24 12:35:58 +11:00
Randy Mackay
a16de76f57 AP_NavEKF3: correct extnav position as it is recalled from the buffer 
This makes extnav position correction consistent with the velocity correction
 2020-10-24 12:35:58 +11:00
Randy Mackay
71487e9c10 AP_NavEKF3: gps_elements gets corrected flag 
CorrectGPSForAntennaOffset uses this flag to ensure position and velocity are only corrected once
 2020-10-24 08:49:33 +09:00
Randy Mackay
3d4e1cd5c5 AP_NavEKF3: CorrectGPSForAntennaOffset always corrects vel 2020-10-24 08:49:33 +09:00
Randy Mackay
e25579cc00 AP_NavEKF3: correct GPS for position as it is recalled from buffer 2020-10-24 08:49:33 +09:00
Peter Barker
0496ecc4f6 AP_NavEKF2: remove unused state variables 2020-10-22 08:47:08 +11:00
Peter Barker
a1e7c91f8e AP_NaveEKF3: reduce code duplication in setup_core 
Less code.

Saves 40 bytes on PixRacer
 2020-10-22 08:47:08 +11:00
Randy Mackay
e4056086e0 AP_NavEKF3: minor format and spelling fixes 2020-10-21 13:54:22 +09:00
Randy Mackay
231a958e51 AP_NavEKF3: minor comment fix 2020-10-21 11:40:35 +09:00
Randy Mackay
e4ccd21dd7 AP_NavEKF3: consolidate extNav height source timeouts 2020-10-20 17:11:26 +09:00
Randy Mackay
6989a28bfc AP_NavEKF3: allow alt from other sources when using ExtNav 
also allows using ExtNav alt when not using it for XY position
removes redundant and incorrect setting of activeHgtSource from ExtNav
which was always being overwritten by the correct value later in selectHeightForFusion
 2020-10-20 17:11:26 +09:00
Randy Mackay
0cebde0632 AP_NavEKF3: minor restructure of how fusePosData and fuseVelData are set 
this non-functional change slightly reduces the number of places we set these variables
 2020-10-20 17:11:26 +09:00
Peter Barker
ef505df78a AP_NavEKF3: remove unused quat2Tbn method 2020-10-20 11:40:54 +11:00
Peter Barker
0eafe4b662 AP_NavEKF3: remove dangling method declarations 2020-10-20 11:40:54 +11:00
Peter Barker
83d784c22d AP_NavEKF3: fix includes 2020-10-20 11:40:54 +11:00
Peter Barker
3dc67f09fd AP_NavEKF3: remove unused innovationIncrement and lastInnovation 2020-10-20 11:24:34 +11:00
Randy Mackay
eeb1a1846c AP_NavEKF3: replace prearm_healthy with pre_arm_check 2020-10-07 11:53:07 +11:00
Randy Mackay
f5a04fd1a3 AP_NavEKF3: shorten buffer size send_text message length 2020-09-29 08:57:52 +09:00
Randy Mackay
7398529f6b AP_NavEKF3: minor format fix 2020-09-28 20:57:39 +09:00
Andrew Tridgell
8a4b0f858a AP_NavEKF3: reset body mag variances at key points 
we need to reset the body mag variances if we change sensors or if we
are starting 3D fusion. When not doing 3D fusion we zero the
variances, so they must be initialised again when we restart
fusion. This fixes a bug in handling the variances on a 2nd flight
 2020-09-20 09:25:57 +10:00
Andrew Tridgell
e9185f526b AP_NavEKF3: add comments on state indexes 
make it easier to check indexes
 2020-09-20 09:25:57 +10:00
Andrew Tridgell
1f21d72c79 AP_NavEKF3: fixed init of earth state mag variances 
we were starting with zero variance for X and Y earth states, it
should start with sq(magNoise)
 2020-09-20 09:25:57 +10:00
Andrew Tridgell
2bc7d17e7a AP_NavEKF3: improved core comparison to check alignment 
this adds coreBetterScore() which takes account of tilt and yaw
alignment when comparing cores for lane switching

this ensures we don't switch to a lane that is unaligned due to the
zero error score for unaligned lanes
 2020-09-14 20:29:04 +10:00
Andrew Tridgell
c03c43e52c AP_NavEKF3: use same mag switch pattern as EKF2 
based on Pauls suggested change
 2020-09-08 11:01:14 +10:00
Andrew Tridgell
3836b59041 AP_NavEKF3: fixed handling of failed compass 
when a compass goes unhealthy due to sensor failure we should try
another compass after 10s if another compass is available
 2020-09-08 11:01:14 +10:00
Andrew Tridgell
458ade86f5 AP_NavEKF3: fixed segfault when IMU mask covers more IMUs than GSF mask 2020-08-28 10:17:00 +10:00
Harshit Kumar Sankhla
edc3709653 AP_NavEKF3: implement sensor affinity using EK3_AFFINITY parameter 
this allows the EKF core index to be used to select a GPS/baro/mag
instance. This is an alternative to GPS blending that allows EKF lane
switching to be used to select the right combination of GPS and IMU
add logging to XKFS message
 2020-08-27 20:20:51 +10:00
Harshit Kumar Sankhla
d7edc946b6 AP_NavEKF3: Relative Error based Lane-Switching 
Improvments to the lane selection logic, we accumulate error for each
EKF lane relative to the primary for a more robust core selection
 2020-08-27 20:20:51 +10:00
Randy Mackay
46a7e96c48 AP_NavEKF3: readRngBcnData avoids array overflow from beacon count 2020-08-25 09:38:10 +09:00
Randy Mackay
b314195f6e AP_NavEKF3: minor spelling fix 2020-08-25 09:38:10 +09:00
Randy Mackay
b86e16e927 AP_NavEKF3: readRngBcnData gets minor formatting fix 2020-08-25 09:38:10 +09:00
Randy Mackay
384029d278 AP_NavEKF3: rngBcnDataNew made local 2020-08-25 09:38:10 +09:00
Randy Mackay
8c877f02af AP_NavEKF3: shrink beacon arrays and add assert 2020-08-25 09:38:10 +09:00
Randy Mackay
d292ba028a AP_NavEKF3: increase beacon buffer to imu + 1 2020-08-25 09:38:10 +09:00
Randy Mackay
886a7d44a7 AP_NavEKF3: allow switch to ExtNav from optflow 2020-08-24 16:52:34 +09:00
Randy Mackay
6bf2057712 AP_NavEKF3: minor formatting fixes 2020-08-21 13:18:58 +09:00
Randy Mackay
4c40d2ac3f AP_NavEKF3: readyToUseBodyOdm uses delayed imu and wheel encoder timestamps 2020-08-21 13:18:58 +09:00
Randy Mackay
056a2de260 AP_NavEKF3: wheelOdmDataNew member made local 
this variable is never used outside the writeWheelOdom method
 2020-08-21 13:18:58 +09:00
Randy Mackay
3d5161f364 AP_NavEKF3: remove unused usingWheelSensors variable 
This variable was set but never consumed
 2020-08-21 13:18:58 +09:00
Randy Mackay
c4b7a1c41a AP_NavEKF3: flowDataToFuse moved to local variable 
This variable is never used outside the SelectFlowFusion() method
This variable is always updated at the top of the function meaning subsequent calls to the function will always overwrite its previous value
 2020-08-21 13:12:10 +09:00
Randy Mackay
41acf555bf AP_NavEKF3: fix skipping of optflow fusion if mag fusion performed 2020-08-21 13:12:10 +09:00
Andrew Tridgell
44d5a923cc AP_NavEKF3: apply min GPS accuracy at measurement point 
this fixes an issue a RTK GPS gives 1cm horizontal and vertical
accuracy and that causes the variances to get too small
 2020-08-21 07:40:13 +10:00
chobits
c722367c6c AP_NavEKF3: move extNavVelNew to local variable 2020-08-12 15:40:38 +09:00
chobits
7ad9e3548f AP_NavEKF3: make reset source variable local 2020-08-10 09:43:28 +09:00
Peter Barker
6b701ae3b3 AP_NavEKF3: correct includes 2020-08-07 19:20:07 +10:00
chobits
4b8be2e535 AP_NavEKF3: fix comments 2020-08-04 18:16:18 +10:00
Paul Riseborough
c8bbbd2720 AP_NavEKF3: Fix bug preventing setting of mag field states 2020-07-25 20:46:04 +10:00
Randy Mackay
d8f2d0cf8d AP_NavEKF3: max delay includes visual odometry 2020-07-21 09:31:52 +10:00
chobits
564244ce9a AP_NavEKF3: fix extnav vel reset 2020-07-17 14:13:06 +09:00
Andy Piper
e35458cedf AP_NavEKF3: INS get_sample_rate() renamed to get_loop_rate_hz() 2020-07-10 16:45:29 +10:00
Paul Riseborough
24fccd5d87 AP_NavEKF3: Don't delay GPS use unnecessarily 
Fixed wing should not wait for bias state convergence after in-flight yaw alignment
 2020-07-07 15:33:58 +10:00
Paul Riseborough
9a4108f55e AP_NavEKF3: Simplify setting EK3_MAG_CAL 
Don't require user to separately set EK3_MAG_CAL to fly without a magnetomer
 2020-07-07 15:33:58 +10:00
Paul Riseborough
0dc598a6e1 AP_NavEKF3: Fix param description and comment 2020-07-07 15:33:58 +10:00
Paul Riseborough
21ea5d5039 AP_NavEKF3: Require GSF yaw history for reset when not using a yaw sensor 2020-07-07 15:33:58 +10:00
Paul Riseborough
f86c2e1db6 AP_NavEKF3: Don't fuse EKF-GSF yaw for FW flight 2020-07-07 15:33:58 +10:00
Paul Riseborough
354b551ef0 AP_NavEKF3: Handle repeated FW flight without magnetometer 
The EKF can build up large yaw errors on ground so it is safer to stop using GPS and re-align after launch as per first launch.
 2020-07-07 15:33:58 +10:00
Paul Riseborough
8ff6780323 AP_NavEKF3: Clarify distinct use cases for 'takeoff expected' 2020-07-07 15:33:58 +10:00
Paul Riseborough
d1a0c2eb30 AP_NavEKF3: Start EKF-GSF yaw estimator before fixed wing takeoff 2020-07-07 15:33:58 +10:00
Paul Riseborough
b469a80633 AP_NavEKF3: Reset all co-variances when performing a yaw reset 2020-07-07 15:33:58 +10:00
Paul Riseborough
d8c2096ad8 AP_NavEKF3: improve angle accuracy during no mag launch 2020-07-07 15:33:58 +10:00
Paul Riseborough
f7c8ee807c AP_NavEKF3: Reset yaw estimator bias states at start of flight 
Enables yaw bias to be learned when sitting stationary on ground.
 2020-07-07 15:33:58 +10:00
Paul Riseborough
bcd23e1063 AP_NavEKF3: Fix one frame delay in processing yaw estimator velocity data 2020-07-07 15:33:58 +10:00
Peter Barker
2e55584417 AP_NavEKF3: correct comments on getFilterFaults and getFilterTimeouts methods 2020-07-07 10:50:41 +10:00
Randy Mackay
ec5f84c926 AP_NavEKF3: remove unused terrainHgtStableSet_ms 2020-06-24 21:16:50 +09:00