Rework NXP UWB driver #21124

NXPLoic · 2023-02-14T12:41:30Z

UWB sr150 driver rework

This is a rework for the NXP UWB SR150 driver.
Enabling precision navigation task (Preclanding) based on the NXP UWB SR150 chip.

The PR includes the enablement of the UWB driver for single anchor ranging and modification of the Landing Target Estimator for precision landing.

UWB driver rework uses 2 UWB boards - 1 as a Controller (Initiator) and one as the Controllee (Anchor).
This is an improvement over the previous UWB system requiring at least 4 Anchors.
With this new system, a Angle of Arrival from three antennas on the UWB module can be used to transfer a spherical position into the Drones cartesian local position (in NED).

Graphic showing the coordinate system that is measured by the UWB system (Antennas facing up):

UWB driver module changes:

Moved all navigation related code out of the UWB driver
Changed driver to take advantage of the workqueue
Added support for anlge of arrival measurements (AoA)
Updated sensor offset and added orientation parametr to represent the physical mounting
Removed deprecated code from code.

Landing_target_estimator module changes:

LTE produces position data based on SensorUwb measurements
Translated landing_target_estimator_params.c into module.yaml
Added new corrected position conversion algorithm using uwb spherical coordinates to kartesian
Fixed POV orientation of conversion algorithm
Added Offset and Orientation from UWB to the LTE position calculation.

Uorb topics:

UwbDistance Renamed to SensorUwb.msg to match current PX4 naming scheme for messages.
SensorUwb: Changed message data tot reflect measurements send by UWB system
SensorUwb: Added metadata to UWB Uorb message
SensorUwb: Added UWB Aoa figure of merit to Uorb message
SensorUwb: Added UWB sensor mounting configuration to Uorb message
UWBgrid: removed message because it is deprecated

For multiple NXP boards

default.px4board: Enabled uwb_sr150 driver configuration. I.e. fmuk66-e, fmurt1062-v1, and ucans32k146.

Major new features:

Autostart via workqueue
Parameters for offset and orientation propagated via Uorb
Relative position calculation via a single ground anchor
Preclanding with higher quality and easier setup

Test coverage

Screenshot showing Landing_target_pose publishing a position based on UWB:

Screenshot of data from the UWB driver:

Screenshot of data from the landing target estimator driver:

Flying tests 11.02.2023:
For all tests the Anchor is placed on the ground pointing north.
Parameters as listed below will be tweaked.

Test 1

LTEST_ACC_UNC = 20.0
LTEST_MEAS_UNC = 0.0005
LTEST_MODE = Moving
MPC_LAND_SPEED = 0.1 m/s
5m alt

Observations
https://logs.px4.io/plot_app?log=21814395-8676-4fff-ba6a-3fd865b73b36
Drone landed 15cm W of anchor.
Precision is higher. But wide swings left right during descend.

Test 2

LTEST_ACC_UNC = 20.0
LTEST_MEAS_UNC = 0.0005
LTEST_MODE = Moving
MPC_LAND_SPEED = 0.1 m/s
10m alt

Observations
https://logs.px4.io/plot_app?log=6bb7eee2-3fe0-47c6-82e9-cb138a58f6f3
Drone landed 20cm SW of anchor.
Precision is higher. But wide swings left right during descend and "Toilet bowl" at static search.

Test 3

LTEST_ACC_UNC = 20.0
LTEST_MEAS_UNC = 0.0005
LTEST_MODE = Stationary
MPC_LAND_SPEED = 0.1 m/s
10m alt

Observations
https://logs.px4.io/plot_app?log=bd795998-d257-4325-9ed1-056355670e37
Drone landed 30cm SE of anchor.
Precision lower than with LTEST_MODE = Moving. "Toiled bowl" at descend and very slow.

Future

Preclanding needs to be improved:
- Faster to react (drone reacts quite slowly to changes)
- More accurate (UWB system is able to achieve <10cm accuracy)
- more reliable
Specific flights modes for following and preclanding
Follow me for drones
Indoor follow me for rovers/drones

Context

This is a rework of #14474

Videos and Picture will follow.

@dk7xe @NXPBrianna @dagar @bkueng @junwoo091400 @igalloway @potaito

…or), one as Controllee (Anchor) uwb_sr150: template for new UWB driver with modifications uwb_sr150: included pr_nxp_uwb_driver & add. to new driver uwb_sr150: updated files to make UWB demo functional. Added actuator_controls_3 again uwb_sr150: Navigation elements localization and precision navigation removed from UWB driver to LandingTargetEstimator. rc.sensor: Added uwb to rc.sensor list for auto-start uwb_sr150driver: Deleted experimental uwb files uwb_sr150: Removed parameters from uwb module.yaml uwb_sr150: Cleaned up the code uwb_sr150: Removed the navigation functions to LandingTargetEstimator LTE: translated landing_target_estimator_params.c into module.yaml LTE: added UWB params from uwb_sr150 module.yaml UwbDistance.msg-> SensorUwb.msg: renamed UwbDistance.msg to SensorUwb.msg SensorUwb.msg: renamed everything to correspond with the renaming of the message uwb_sr150: Added orientation param, but need to check with Loic. uwb_sr150: Removed gps & pos struct. uwb_sr150: Cleaned up driver of unnecessary code. uwb_sr150: Edited UWB board orientation param. uwb_sr150: Readded position. uwb_sr150: Added orientation param to module.yaml uwb_sr150: Started making changes to UWB driver to be work queue task uwb_sr150: Added getter for orientation param uwb_sr150: Restructured header file to match workqueue example uwb_sr150: Reworked to work with workqueue and remove unnecessary items uwb_sr150: Revert "Added getter for orientation param". This reverts commit a4ccdf4. uwb_sr150: Revert "updated files to make UWB demo functional. Added actuator_controls_3 again". This reverts commit a7db41f. Removed Yaw and Pitch related items. TBD with Daniel uwb_sr150: Updated copyright years & readded *buffer pointer to collectData() rc.sensor: UWB added to wq, so not needed Added UWB SR150 to board config uwb_sr150: Removed no longer relevant code: uwb_sr150: position (moved to landing_target_estimator). uwb_sr150: uwb_pos_debug is removed completely Added uwb_sr150 driver config to NXP boards LTE: added new corrected position conversion algorithm from uwb spherical coordinates to kartesian LTE: removed deprecated code LTE: Fixed compile issues LTE: LTE produces data upon UWB sensor_uwb publish produced data is corrupt LTE: Fixed LTE position not bein calculated on non corrupt data LTE: Fixed POV orientation of conversion algorithm SensorUwb.msg: Added metadata to UWB Uorb message SensorUwb.msg: Added UWB Aoa figure of merit to Uorb message SensorUwb.msg: Added UWB sensor mounting configuration to Uorb message LTE: Added Offset and Orientation from UWB LTE: Added Offset and orientation calculation to the LTE position calculation LTE: Changed double precision to float precision LTE: Fixed formating uwb_sr150: Fixed formating

Added uwb sensor plausability check

dk7xe · 2023-02-14T13:44:03Z

Picture of landing place - Test 1

Video final approach - Test 2
https://user-images.githubusercontent.com/3907200/218755546-02d30640-8bc7-41c2-9d85-a911cf75713c.mp4

…loat identifier.

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

src/modules/landing_target_estimator/LandingTargetEstimator.h

msg/SensorUwb.msg

Removed Deg2Rad and used PX4 function Changed FOV limit of AOA UWB measurements to const

NXPLoic · 2023-02-17T08:54:55Z

@potaito I commited your requested changes
We are preparing some more documentation on UWB and this driver.

dk7xe · 2023-02-28T08:07:18Z

dk7xe · 2023-04-13T08:50:41Z

@NXPBrianna i guess this reflects UWB MW v4 orientation?

Why has the offset Y been removed (replaced) ? There could be situations where the sensor is not mounted in the centerline of the vehicle. BTW i would not use the term "drone" in the code since we are vehicle type independent.

…for UWB targets

Remove unnecessary changes

NXPLoic · 2023-06-23T09:00:27Z

I removed the Landing Target Estimator integration of the UWB driver.
This PR now only contains the UWB driver and removal of deprecated UWB interaction.

The driver is running and receives Data from the UWB module.
@potaito @dagar

I resolved all issues and would like to request a review.
@dk7xe

potaito · 2023-06-24T05:17:05Z

I will take a look on Monday/Tuesday 🙂

potaito · 2023-06-27T08:24:30Z

@NXPLoic what's your plan for using the sensor_uwb_s message now that it was removed from Landing Target Estimator?

I don't see an issue with merging it the way it is now. Just curious what will be next, because now there is no subscriber to sensor_uwb_s and the message is also not logged.

dk7xe · 2023-06-27T14:49:48Z

Hi @potaito Allesandro, Loic will do the precision landing part soon that will use that message. We just liked to have the driver portion done before moving forward. It also gives us the ability to send the topic to ROS via dds bridge.

potaito · 2023-06-28T13:21:03Z

Hi @potaito Allesandro, Loic will do the precision landing part soon that will use that message. We just liked to have the driver portion done before moving forward. It also gives us the ability to send the topic to ROS via dds bridge.

Understood, thank you. Makes sense 👍

potaito · 2023-06-28T13:22:03Z

src/drivers/uwb/uwb_sr150/module.yaml

+                  Default position is the antannaes downward facing, UWB board parallel with body frame.
+          type: enum
+          values:
+            0: ROTATION_NONE


@bresch is there a way to re-use the rotation value definitions that already exist for other sensors?

Could we get an answer here?

Removed whitespace for formater

NXPLoic · 2023-07-05T09:21:18Z

Removing the changes freed up some space (Atleast from the px4_fmu-v5 build)

181 bytes vs 693 bytes

The failing checks from this commit are also present on main.

@potaito

dk7xe · 2023-07-11T07:13:47Z

@dagar @potaito @mrpollo i'd like to bring this to the developer call this week since we are stuck it seems.
SW was extensively tested. Even on AEPX4. See https://www.linkedin.com/posts/nxp-semiconductors_drones-uwb-drones-activity-7081622429617254400-FhKv/?utm_source=share&utm_medium=member_desktop

potaito · 2023-07-11T08:22:47Z

Apologies, I'm on vacation right now. But last I checked the PR looked good to me.

mrpollo · 2023-07-11T23:30:57Z

We can discuss on this weeks Q&A if someone from your team can join @dk7xe

DronecodeBot · 2023-07-12T15:15:37Z

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-july-12-2023/33086/5

mrpollo · 2023-07-12T15:45:21Z

Thanks for joining the call @dk7xe, glad to see this merged

* UWB driver rework that uses 2 UWB MKBoards - 1 as Controller (Initiator), one as Controllee (Anchor) Co-authored-by: NXPBrianna <108274268+NXPBrianna@users.noreply.github.com>

NXPBrianna and others added 4 commits February 10, 2023 20:35

Merge branch 'PX4:main' into pr_nxp_rework_uwb_driver

90f043b

Removed UWBgrid uorb message

2af7945

Added uwb sensor plausability check

Merge branch 'PX4:main' into pr_nxp_rework_uwb_driver

8b34e6c

NXPLoic marked this pull request as ready for review February 14, 2023 13:15

Fixed Landing_Target_Esitmator param default values having an f for f…

7de4396

…loat identifier.

potaito requested changes Feb 16, 2023

View reviewed changes

Named LTEST_SENS_ROT back to sensor_yaw.

e9adf79

Removed Deg2Rad and used PX4 function Changed FOV limit of AOA UWB measurements to const

NXPLoic requested a review from potaito March 1, 2023 08:59

junwoo091400 added Drivers 🔧 Sensors, Actuators, etc Admin: Enhancement (improvement) 💡 labels Mar 24, 2023

Modified to correct axises and angles

dfc8e4f

NXPLoic and others added 3 commits June 9, 2023 10:47

Fixed the POV of the Spherical - Kartesian conversion inside the LTE …

bb4d214

…for UWB targets

Removed UWB precision landing changes from Landing Target Estimator.

d73536f

Update settings.json

91dd6bf

Remove unnecessary changes

potaito reviewed Jun 28, 2023

View reviewed changes

NXPLoic added 2 commits July 4, 2023 09:20

Merge branch 'main' into pr_nxp_rework_uwb_driver

ce5991d

Update LandingTargetEstimator.cpp

70c11ae

Removed whitespace for formater

dagar approved these changes Jul 12, 2023

View reviewed changes

dagar merged commit f8c9be0 into PX4:main Jul 12, 2023
81 of 85 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Rework NXP UWB driver #21124

Rework NXP UWB driver #21124

NXPLoic commented Feb 14, 2023 •

edited

dk7xe commented Feb 14, 2023

NXPLoic commented Feb 17, 2023 •

edited

dk7xe commented Feb 28, 2023

dk7xe commented Apr 13, 2023

NXPLoic commented Jun 23, 2023

potaito commented Jun 24, 2023

potaito commented Jun 27, 2023

dk7xe commented Jun 27, 2023 •

edited

potaito commented Jun 28, 2023

potaito Jun 28, 2023

NXPLoic Jul 4, 2023

NXPLoic commented Jul 5, 2023 •

edited

dk7xe commented Jul 11, 2023 •

edited

potaito commented Jul 11, 2023

mrpollo commented Jul 11, 2023

DronecodeBot commented Jul 12, 2023

mrpollo commented Jul 12, 2023

Rework NXP UWB driver #21124

Rework NXP UWB driver #21124

Conversation

NXPLoic commented Feb 14, 2023 • edited

UWB sr150 driver rework

UWB driver module changes:

Landing_target_estimator module changes:

Uorb topics:

Major new features:

Test coverage

Test 1

Test 2

Test 3

Future

Context

dk7xe commented Feb 14, 2023

NXPLoic commented Feb 17, 2023 • edited

dk7xe commented Feb 28, 2023

dk7xe commented Apr 13, 2023

NXPLoic commented Jun 23, 2023

potaito commented Jun 24, 2023

potaito commented Jun 27, 2023

dk7xe commented Jun 27, 2023 • edited

potaito commented Jun 28, 2023

potaito Jun 28, 2023

Choose a reason for hiding this comment

NXPLoic Jul 4, 2023

Choose a reason for hiding this comment

NXPLoic commented Jul 5, 2023 • edited

dk7xe commented Jul 11, 2023 • edited

potaito commented Jul 11, 2023

mrpollo commented Jul 11, 2023

DronecodeBot commented Jul 12, 2023

mrpollo commented Jul 12, 2023

NXPLoic commented Feb 14, 2023 •

edited

NXPLoic commented Feb 17, 2023 •

edited

dk7xe commented Jun 27, 2023 •

edited

NXPLoic commented Jul 5, 2023 •

edited

dk7xe commented Jul 11, 2023 •

edited