AP_NavEKF3: add source selection to allow switching between GPS and T265 #14803

rmackay9 · 2020-07-13T06:14:11Z

This PR adds support for manually switching the EKF3 between sensors sources used for position, velocity and yaw. It also includes a lua script to automatically switch between GPS and NonGPS sources.

New AP_NavEKF_Source class is relatively simple with 10 new parameters that allow the user to set to sources (primary and secondary) for position (_POSXY, POSXY2, POSZ, POSZ2), velocity (_VELXY, _VELXY2, _VELZ, _VELZ2) and yaw (_YAW, _YAW2). This class allows the currently active source for each axis to be get or set using methods like getPosXYSource().
RC_Channel option "89" allows the user to change all active sources between the primary and secondary
EKF3 is enhanced to check the active source when deciding what sensor data to fuse. It also checks for a change in source and performs a reset.

This PR also includes a couple of additional, mostly unrelated fixes:

EKF3's ExtNav support enhanced to allow using non-ExtNav altitude (i.e. baro, rangefinder, etc)
VisualOdometry T265 driver ignores sensor input for 1 second after a reset (in testing we've found dozens of resets happen one after the other)
AHRS's PreArm check method is made consistent with other subsystems

This has been tested in SITL and on a real vehicle. I've put the logs for two tests below:

To test using GPS + Vicon in SITL the following parameters should be set:

AHRS_EKF_TYPE = 3
EK2_ENABLE = 0
EK3_ENABLE = 1
EK3_SRC1_POSXY = 3 (GPS)
EK3_SRC2_POSXY = 6 (External Nav)
EK3_SRC1_POSZ = 1 (Baro)
EK3_SRC2_POSZ = 6 (External Nav)
EK3_SRC1_VELXY = 3 (GPS)
EK3_SRC2_VELXY = 6 (External Nav)
EK3_SRC1_VELZ = 3 (GPS)
EK3_SRC2_VELZ = 6 (External Nav)
EKF3_SRC1_YAW = 1 (Compass)
EK3_SRC2_YAW = 2 (External)
VISO_TYPE = 1
SERIAL5_PROTOCOL = 2 (MAVLink)
RC9_OPTION = 90 (EKF Pos Source)

Then start SITL from the ArduCopter directory with this command

../Tools/autotest/sim_vehicle.py --map --console -A "--uartF=sim:vicon:"

To switch between sources:

rc 9 1000 (switches to GPS)
rc 9 1500 (switches to vicon)

To add offsets in position and orientation to the SITL vehicle:

param set SIM_VICON_YAW 45 (to rotate the Vicon system by 45 degrees)
param set SIM_VICON_GLIT_Y 50 (to add 50m of offset in the Y axis to the Vicon system)

These are the known issues and questions:

PreArm check is perhaps too strict because it checks the EK3_SRC_xxx2 parameters which many users may not use
The fall back from GPS->Optical Flow doesn't work anymore but in practice I don't think it worked before either.
Source changes of Z-axis velocity alone will not trigger a velocity reset. This is mostly because of existing limitations in the EKF.
When the Yaw source changes to the compass, the vehicle's yaw takes 1 to 2 seconds to rotate around to the correct heading. I'm unsure why it is so slow because resets to ExtNav yaw occur immediately

This is the additional testing we should do before merging:

Yaw resets from all possible sources (Compass, GSF, ExtNav)

All feedback is greatly appreciated!

rmackay9 · 2020-07-13T08:58:58Z

This is failing semaphore because the Matek405-wing has run out of space to fit the firmware.

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

libraries/AP_NavEKF/AP_NavEKF_Source.h

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

libraries/AP_NavEKF3/AP_NavEKF3.cpp

rmackay9 · 2020-07-14T00:19:40Z

Actions for me:

test yaw including resets for all combinations
test the heck out of this to ensure there are no dangerous issues
create a list of combinations of param values that aren't actually supported
setup a meeting with Tridge & Paul to go over this

peterbarker

This is conflicting horribly already.

libraries/AP_NavEKF/AP_NavEKF_Source.cpp

mcelhennyi · 2020-10-21T14:41:55Z

Is there a way for an offboard computer to make this switch?

Or maybe a way to make it compare GPS to VIO confidence and have it automatically swap?

also integrate useVelZSource

…ations

switches automatically between gps, external nav and optical flow

if we get to this point we must be using the compass fallback logic, and should do the reset

MAG_CAL param description include deprecated values Pre-arm check of MAG_CAL using deprecated values effective_magCal interprets 5 (was EXTERNAL_YAW) as Never, 6 (was EXTERNAL_YAW_FALLBACK) as WhenFlying Update comments in param conversion from MAG_CAL to EK3_SRC1_YAW

shorten param conversion config error if gps and optical flow are enabled we default SRC2_VELXY to optflow convert_params run from InitialiseFilter ensure param conversion only run once

rmackay9 · 2020-11-20T07:43:56Z

Thanks for all the feedback and reviews, merged!

tridge · 2020-11-20T10:15:31Z

yay!

rmackay9 added Enhancement EKF labels Jul 13, 2020

rmackay9 requested review from tridge and priseborough July 13, 2020 06:14

rmackay9 added the DevCallTopic label Jul 13, 2020

rmackay9 commented Jul 14, 2020

View reviewed changes

libraries/AP_NavEKF/AP_NavEKF_Source.cpp Outdated Show resolved Hide resolved

tridge previously requested changes Jul 14, 2020

View reviewed changes

CraigElder removed the DevCallTopic label Jul 14, 2020

rmackay9 force-pushed the ekf-pos-source2 branch 5 times, most recently from f4140a9 to ce16d6b Compare August 11, 2020 05:18

rmackay9 force-pushed the ekf-pos-source2 branch 2 times, most recently from d90d748 to a0e16e7 Compare August 17, 2020 03:45

rmackay9 added the WIP label Aug 20, 2020

Hwurzburg added the WikiNeeded needs wiki update label Aug 20, 2020

rmackay9 mentioned this pull request Aug 24, 2020

AP_AHRS: remove Rover's fall back to DCM even if GPS is enabled #15148

Closed

rmackay9 force-pushed the ekf-pos-source2 branch from a0e16e7 to 1a7385e Compare September 29, 2020 05:03

rmackay9 requested a review from tridge September 29, 2020 05:10

rmackay9 mentioned this pull request Oct 5, 2020

AP_AHRS/EKF: replace prearm_healthy() and prearm_failure_reason() with pre_arm_check() #15503

Merged

peterbarker reviewed Oct 17, 2020

View reviewed changes

libraries/AP_NavEKF/AP_NavEKF_Source.cpp Outdated Show resolved Hide resolved

rmackay9 mentioned this pull request Oct 19, 2020

EKF3: allow alt from other sources when using ExtNav #15602

Merged

rmackay9 force-pushed the ekf-pos-source2 branch 4 times, most recently from 5fe2573 to 5c057b7 Compare October 21, 2020 04:55

rmackay9 and others added 19 commits November 20, 2020 15:19

AP_NavEKF3: integrate AP_NavEKF_Source::useVelXYSource

fcf637e

also integrate useVelZSource

AP_NavEKF3: minor indenting fix

e5f322f

AP_NavEKF3: rename _sources to sources

eef36cc

AP_NavEKF3: move sources to 2nd bank of parameters

7112636

AP_NavEKF3: update comments

bd6a381

AP_NavEKF3: use_compass minor order fixup

c93ba04

AP_NavEKF3: rename posxy_source_last and posxy_source_reset

b783fe6

AP_NavEKF3: rename setPosVelYawSourceSet

0322242

AP_AHRS: add set_posvelyaw_source_set

a29a020

AP_AHRS: add get_vel_innovations_and_variances_for_source

c3013c3

RC_Channel: add aux switch to change the EKF's sensor sources

276450e

AP_VisualOdom: add align_position_to_ahrs

b932138

Tools: autotest for beacon sets EK3_SRC params

d1f83a6

AP_Scripting: add bindings for set_posvelyaw_source and get_vel_innov…

b866416

…ations

AP_Scripting: ahrs-source example script

13e5ab0

switches automatically between gps, external nav and optical flow

Tools: add test.Copter.GPSViconSwitching to CopterTests1d

342899b

AP_NavEKF3: handle compass fallback in yaw source reset

1b852a5

if we get to this point we must be using the compass fallback logic, and should do the reset

AP_NavEKF_Source: default OPTIONS to 1 (fuse all velocities)

2e4cdba

rmackay9 force-pushed the ekf-pos-source2 branch 2 times, most recently from 4dd5b57 to 79d0043 Compare November 20, 2020 06:21

rmackay9 added 2 commits November 20, 2020 15:37

AP_NavEKF_Source: add mark_configured_in_storage

6566dd9

AP_NavEKF3: fixup source param conversion

cb8830c

shorten param conversion config error if gps and optical flow are enabled we default SRC2_VELXY to optflow convert_params run from InitialiseFilter ensure param conversion only run once

rmackay9 force-pushed the ekf-pos-source2 branch from 79d0043 to cb8830c Compare November 20, 2020 06:38

rmackay9 merged commit d1983b0 into ArduPilot:master Nov 20, 2020

rmackay9 mentioned this pull request Nov 20, 2020

EKF3: improve source switching (GPS<->NonGPS) #15859

Open

16 tasks

rmackay9 deleted the ekf-pos-source2 branch April 12, 2021 23:34

Hwurzburg removed the WikiNeeded needs wiki update label Apr 2, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_NavEKF3: add source selection to allow switching between GPS and T265 #14803

AP_NavEKF3: add source selection to allow switching between GPS and T265 #14803

rmackay9 commented Jul 13, 2020 •

edited

Loading

rmackay9 commented Jul 13, 2020

rmackay9 commented Jul 14, 2020 •

edited

Loading

peterbarker left a comment

mcelhennyi commented Oct 21, 2020

rmackay9 commented Nov 20, 2020

tridge commented Nov 20, 2020

AP_NavEKF3: add source selection to allow switching between GPS and T265 #14803

AP_NavEKF3: add source selection to allow switching between GPS and T265 #14803

Conversation

rmackay9 commented Jul 13, 2020 • edited Loading

rmackay9 commented Jul 13, 2020

rmackay9 commented Jul 14, 2020 • edited Loading

peterbarker left a comment

Choose a reason for hiding this comment

mcelhennyi commented Oct 21, 2020

rmackay9 commented Nov 20, 2020

tridge commented Nov 20, 2020

rmackay9 commented Jul 13, 2020 •

edited

Loading

rmackay9 commented Jul 14, 2020 •

edited

Loading