Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

dirksavage88 · 2024-10-16T16:33:26Z

Solved Problem

Adds support for dual RTK heading via AP Periph, which publishes heading as a distinct message (RelPosHeading) instead of via the ecef position fields.

Fixes #22520

Solution

Add sub callback for relposheading within GNSS sensor uavcan driver
Process heading information and validity in process fixx method

Changelog Entry

For release notes:

Feature: Add support for AP Periph (dronecan) dual GPS heading setups
New parameter: EKF2_GPS_YAW_OFF
Documentation:  Updates merged in PX4 user guide https://github.com/PX4/PX4-user_guide/pull/3406

Alternatives

Create a sub to the sensor gnss relative uORB topic directly in EKF2

Test coverage

Tested using dual Holybro ZED-F9P dronecan nodes and pixhawk 6xrt

Context

~~TODO: post log~~
~~Pending doc update PR: PX4/PX4-user_guide#3406~~

dirksavage88 · 2024-10-17T09:09:02Z

@vincentpoont2

vincentpoont2 · 2024-10-21T05:07:13Z

@dirksavage88 Anything we can assist on our end?

dirksavage88 · 2024-10-21T19:24:20Z

@vincentpoont2 I am having issues getting the rover RTK F9P to get Fix type of fixed (6). It only reports fix type of 3 or 4 even after sitting for a while. (Similar issue to this post: https://discuss.ardupilot.org/t/heading-informations-via-moving-baseline-configuration-gnss-antenna-setup/113674/8)

Other than this issue, I have some questions for the maintainers on how to integrate the gps yaw offset correctly, since it is handled differently in PX4 than what ardupilot is doing.

DronecodeBot · 2024-10-23T15:50:38Z

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-sync-q-a-oct-23-2024/42053/1

dirksavage88 · 2024-10-23T20:36:34Z

@vincentpoont2 I was able to get the dual F9P working with the correct heading offset. The offset had to be applied manually since it only occurs in the serial GPS driver and not in PX4.

The rest is just architecture changes from maintainer feedback.

src/modules/ekf2/EKF/common.h

src/modules/ekf2/EKF2.hpp

src/modules/ekf2/EKF2.cpp

src/modules/ekf2/module.yaml

src/modules/ekf2/EKF2.cpp

dagar · 2024-10-30T16:13:32Z

@bresch FYI this adds an optional EKF2_GPS_YAW_OFF that we can add if there's incoming GPS yaw data from UAVCAN (or any source) that doesn't have the correct offset.

DronecodeBot · 2024-10-30T17:02:29Z

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-sync-q-a-oct-30-2024/42184/1

dirksavage88 · 2024-10-30T19:32:15Z

~~@dagar could we revert the param changes? It was working within EKF2.cpp where the offset was applied. With the param changes the offset no longer is applied and the estimator was complaining~~

E.g. commit d61c069 works, but estimator errors on 74b1f1d

Looks like it had to do with the conditional, fixed it and tested.

Signed-off-by: dirksavage88 <[email protected]>

…ed for non px4 cannode gps's Signed-off-by: dirksavage88 <[email protected]>

Signed-off-by: dirksavage88 <[email protected]>

dakejahl · 2024-10-30T22:50:37Z

Do we really need to check the fix_type to decide if we include the heading? I would leave that out, and always include the heading if it's being published.

…usion Signed-off-by: dirksavage88 <[email protected]>

…ing inclusion" This reverts commit 17ed0c9.

dirksavage88 · 2024-10-31T00:23:38Z

Fix type on the left, heading on the right (rads)

dakejahl · 2024-10-31T00:27:26Z

Tested this on a frame with 2 ARK RTK GPS. Working as expected.

dakejahl

I'm going to make a few changes to simplify the logic. I'll re-test it.

dakejahl · 2024-10-31T01:12:42Z

One of my GPS antennas was loose, looks good now.

@dirksavage88 @dagar can you guys do one more pass and if it looks good hit the merge button

dirksavage88 · 2024-10-31T01:23:20Z

Looks good to me

dakejahl · 2024-11-02T17:45:19Z

src/drivers/uavcan/sensors/gnss.cpp

-	report.heading_offset = heading_offset;
-	report.heading_accuracy = heading_accuracy;
+	// Use heading from RelPosHeading message if available and we have RTK Fixed solution.
+	if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) {


I wonder if we should also have a timestamp associated with the rel_heading. I'm not sure if this can happen, but I'm imagining a case where we have heading from moving base, but then it goes away and comes back as an RTK fix but without the heading...

src/modules/ekf2/EKF2.cpp

Signed-off-by: dirksavage88 <[email protected]>

src/modules/ekf2/EKF2.cpp

Co-authored-by: Mathieu Bresciani <[email protected]>

DronecodeBot · 2024-11-06T16:52:35Z

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-sync-q-a-nov-6-2024/42264/1

dagar · 2024-11-06T16:53:23Z

src/drivers/uavcan/sensors/gnss.cpp

+{
+	_rel_heading_valid = msg.reported_heading_acc_available;
+	_rel_heading = matrix::wrap_pi(math::radians(msg.reported_heading_deg));
+	_rel_heading_accuracy = matrix::wrap_2pi(math::radians(msg.reported_heading_acc_deg));


Suggested change

_rel_heading_accuracy = matrix::wrap_2pi(math::radians(msg.reported_heading_acc_deg));

_rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg);

dagar · 2024-11-06T16:53:50Z

src/drivers/uavcan/sensors/gnss.cpp

+		uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg)
+{
+	_rel_heading_valid = msg.reported_heading_acc_available;
+	_rel_heading = matrix::wrap_pi(math::radians(msg.reported_heading_deg));


Suggested change

_rel_heading = matrix::wrap_pi(math::radians(msg.reported_heading_deg));

_rel_heading = math::radians(msg.reported_heading_deg);

dagar · 2024-11-06T16:56:09Z

src/drivers/uavcan/sensors/gnss.cpp

+	if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) {
+		report.heading = _rel_heading;
+		report.heading_offset = NAN;
+		report.heading_accuracy = _rel_heading_accuracy;


Suggested change

report.heading_accuracy = _rel_heading_accuracy;

report.heading_accuracy = _rel_heading_accuracy;

_rel_heading = NAN;

_rel_heading_accuracy = NAN;

_rel_heading_valid = false;

Signed-off-by: dirksavage88 <[email protected]>

dirksavage88 · 2024-11-06T19:26:09Z

@dagar

dirksavage88 marked this pull request as ready for review October 23, 2024 20:39

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF/common.h Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF2.hpp Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/module.yaml Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

dagar reviewed Oct 30, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

dirksavage88 force-pushed the pr-dronecan_rtk_yaw branch from e2bcd53 to 656dfc7 Compare October 30, 2024 20:47

dirksavage88 added 12 commits October 30, 2024 16:50

add relposheading in process fix uavcan driver

2f73080

Signed-off-by: dirksavage88 <[email protected]>

switch to debug for rtk fix type

592f8ed

Signed-off-by: dirksavage88 <[email protected]>

convert heading offset to rads, wrap heading

417fbaf

Signed-off-by: dirksavage88 <[email protected]>

implement ekf2 yaw offset param

51a7a9f

Signed-off-by: dirksavage88 <[email protected]>

working dual heading with manual offset applied in uavcan driver

4babbd5

Signed-off-by: dirksavage88 <[email protected]>

add conditional for gps fix type

fbf51f6

Signed-off-by: dirksavage88 <[email protected]>

add back in default heading_offset

53e7e91

Signed-off-by: dirksavage88 <[email protected]>

remove param from uavcan side, add in ekf2; ekf2 heading offset appli…

8d13a17

…ed for non px4 cannode gps's Signed-off-by: dirksavage88 <[email protected]>

add back in fix type conditional

bf7a096

Signed-off-by: dirksavage88 <[email protected]>

fixes

fd9321e

Signed-off-by: dirksavage88 <[email protected]>

move ekf2 yaw param to gnss yaml, fixes

268c272

Signed-off-by: dirksavage88 <[email protected]>

fixes to conditional

9d25e2c

Signed-off-by: dirksavage88 <[email protected]>

dirksavage88 force-pushed the pr-dronecan_rtk_yaw branch from 656dfc7 to 9d25e2c Compare October 30, 2024 20:51

dirksavage88 added 2 commits October 30, 2024 19:03

change from rtk fixed to differential as conditional for heading incl…

17ed0c9

…usion Signed-off-by: dirksavage88 <[email protected]>

Revert "change from rtk fixed to differential as conditional for head…

6f09912

…ing inclusion" This reverts commit 17ed0c9.

remove unused variables

bfce742

dakejahl approved these changes Oct 31, 2024

View reviewed changes

dakejahl requested changes Oct 31, 2024

View reviewed changes

simplified logic, cleaned up comments

835c2ca

dakejahl previously approved these changes Oct 31, 2024

View reviewed changes

hamishwillee mentioned this pull request Oct 31, 2024

Dronecan Holybro F9P Param change fix PX4/PX4-user_guide#3425

Draft

1 task

dakejahl reviewed Nov 2, 2024

View reviewed changes

bresch reviewed Nov 2, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

bresch requested changes Nov 2, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

remove convert to radians comment

116711d

Signed-off-by: dirksavage88 <[email protected]>

dirksavage88 dismissed dakejahl’s stale review via 116711d November 4, 2024 17:27

bresch requested changes Nov 5, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

bresch requested changes Nov 5, 2024

View reviewed changes

src/modules/ekf2/EKF2.cpp Outdated Show resolved Hide resolved

dirksavage88 and others added 2 commits November 5, 2024 11:10

Update src/modules/ekf2/EKF2.cpp

1b2a27b

Co-authored-by: Mathieu Bresciani <[email protected]>

Update src/modules/ekf2/EKF2.cpp

3f084c9

Co-authored-by: Mathieu Bresciani <[email protected]>

bresch previously approved these changes Nov 6, 2024

View reviewed changes

dagar reviewed Nov 6, 2024

View reviewed changes

fixes to uavcan heading accuracy

2c2a760

Signed-off-by: dirksavage88 <[email protected]>

dirksavage88 dismissed bresch’s stale review via 2c2a760 November 6, 2024 18:53

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

dirksavage88 commented Oct 16, 2024 •

edited

Loading

dirksavage88 commented Oct 17, 2024

vincentpoont2 commented Oct 21, 2024

dirksavage88 commented Oct 21, 2024

DronecodeBot commented Oct 23, 2024

dirksavage88 commented Oct 23, 2024

dagar commented Oct 30, 2024

DronecodeBot commented Oct 30, 2024

dirksavage88 commented Oct 30, 2024 •

edited

Loading

dakejahl commented Oct 30, 2024

dirksavage88 commented Oct 31, 2024 •

edited

Loading

dakejahl commented Oct 31, 2024

dakejahl left a comment

dakejahl commented Oct 31, 2024

dirksavage88 commented Oct 31, 2024

dakejahl Nov 2, 2024

DronecodeBot commented Nov 6, 2024

dagar Nov 6, 2024

dirksavage88 Nov 6, 2024

dagar Nov 6, 2024

dirksavage88 Nov 6, 2024

dagar Nov 6, 2024

dirksavage88 Nov 6, 2024

dirksavage88 commented Nov 6, 2024

	_rel_heading_accuracy = matrix::wrap_2pi(math::radians(msg.reported_heading_acc_deg));
	_rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg);

	_rel_heading = matrix::wrap_pi(math::radians(msg.reported_heading_deg));
	_rel_heading = math::radians(msg.reported_heading_deg);

-		report.heading_accuracy = _rel_heading_accuracy;
+		report.heading_accuracy = _rel_heading_accuracy;
+		_rel_heading = NAN;
+		_rel_heading_accuracy = NAN;
+		_rel_heading_valid = false;

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

Are you sure you want to change the base?

Add relposheading in process fix uavcan driver (AP Periph Dronecan Dual RTK support) #23821

Conversation

dirksavage88 commented Oct 16, 2024 • edited Loading

Solved Problem

Solution

Changelog Entry

Alternatives

Test coverage

Context

dirksavage88 commented Oct 17, 2024

vincentpoont2 commented Oct 21, 2024

dirksavage88 commented Oct 21, 2024

DronecodeBot commented Oct 23, 2024

dirksavage88 commented Oct 23, 2024

dagar commented Oct 30, 2024

DronecodeBot commented Oct 30, 2024

dirksavage88 commented Oct 30, 2024 • edited Loading

dakejahl commented Oct 30, 2024

dirksavage88 commented Oct 31, 2024 • edited Loading

dakejahl commented Oct 31, 2024

dakejahl left a comment

Choose a reason for hiding this comment

dakejahl commented Oct 31, 2024

dirksavage88 commented Oct 31, 2024

dakejahl Nov 2, 2024

Choose a reason for hiding this comment

DronecodeBot commented Nov 6, 2024

dagar Nov 6, 2024

Choose a reason for hiding this comment

dirksavage88 Nov 6, 2024

Choose a reason for hiding this comment

dagar Nov 6, 2024

Choose a reason for hiding this comment

dirksavage88 Nov 6, 2024

Choose a reason for hiding this comment

dagar Nov 6, 2024

Choose a reason for hiding this comment

dirksavage88 Nov 6, 2024

Choose a reason for hiding this comment

dirksavage88 commented Nov 6, 2024

dirksavage88 commented Oct 16, 2024 •

edited

Loading

dirksavage88 commented Oct 30, 2024 •

edited

Loading

dirksavage88 commented Oct 31, 2024 •

edited

Loading