uORB::Subscription subscribe directly to uORB device node object

[dagar]

Continuation of #11176.

This is a minimal version of #11176 with the core uORB changes. The larger changes to mavlink, logger, flighttasks, etc can follow incrementally.

Changes

• existing uORB::Subscription moved to uORB::SubscriptionPolled
  • uORB::SubscriptionBase -> uORB::SubscriptionBasePolled
  • uORB::SubscriptionNode -> uORB::SubscriptionPolledNode
  • uORB::Subscription -> uORB::SubscriptionPolled

The new uORB::Subscription subscribes directly to the underlying uORB::DeviceNode, bypasses file descriptors. It's faster, uses less memory, and gets us past NuttX file descriptor limits.

Longer term uORB::SubscriptionPolled will be phased out entirely.

[dannyfpv]

Indoor test on Pixhawk 4 v5

Modes Tested

• Takeoff: Good
• Stabilized Mode: Good

Flight Test:
✓ Arm and Take off in the stabilized mode no issue
✓ Throttle Test no issue
✓ Pitch, Roll, Yaw no issue
✓ Land no issue

Notes:
Armed and took off in stabilized mode

Flight Log
https://review.px4.io/plot_app?log=aa6502c1-4f65-4622-a0f8-562795936e10

[jorge789]

Indoor flight test on PixRacer V4

Modes Tested
Takeoff: Good
Stabilized Mode: Good.

Flight Test:
✓ Arm and Take off in the stabilized mode
✓ Throttle Test
✓ Pitch, Roll, Yaw
✓ Land

Notes:
Took off in stabilized mode for approximately 5 min, No issues noted good flight in general. The vehicle behaved as expected.

Flight log: 1
https://review.px4.io/plot_app?log=3d557ab4-1ca7-4bee-a095-8f72371533cf

Flight log: 2
https://review.px4.io/plot_app?log=637bfdf6-7aa9-4ef5-bf97-f9f1ffca0d52

Indoor flight test on Pixhawk 2 Cube V3

Modes Tested
Takeoff: Good
Stabilized Mode: Good

Flight Test
✓ Arm and Take off in the stabilized mode
✓ Throttle Test
✓ Pitch, Roll, Yaw
✓ Land

Notes:
Took off in stabilized mode for approximately 5 min, No issues noted good flight in general. The vehicle behaved as expected.

Flight log: 1
https://review.px4.io/plot_app?log=25566deb-76b0-460b-b6b1-5b6109f7d975

Flight log : 2
https://review.px4.io/plot_app?log=19995186-fdae-4faa-b52f-7bc7ab64cffb

[dagar]

The main motivation for this change is saving memory (see #11176 (comment) for details), but the performance improvements in uORB are also going to be worthwhile overall.

Here's a comparison of the common check and copy operations.

C API (orb_check(), orb_copy(), etc)

orb_check vehicle_local_position: 1000 events, 2746us elapsed, 2.75us avg, min 2us max 4us 0.438us rms
orb_stat vehicle_local_position: 1000 events, 2319us elapsed, 2.32us avg, min 2us max 3us 0.466us rms
orb_copy vehicle_local_position: 1000 events, 3928us elapsed, 3.93us avg, min 3us max 5us 0.324us rms

C++ API

uORB::Subscription orb_check vehicle_local_position: 1000 events, 907us elapsed, 0.91us avg, min 1us max 2us 0.301us rms
uORB::Subscription orb_stat vehicle_local_position: 1000 events, 1092us elapsed, 1.09us avg, min 1us max 2us 0.289us rms
uORB::Subscription orb_copy vehicle_local_position: 1000 events, 2530us elapsed, 2.53us avg, min 2us max 3us 0.499us rms

On a typical setup these operations are called tens of thousands of times every second, so shaving off microseconds from each orb check and copy will actually be a considerable cpu usage improvement (in aggregate).

[LorenzMeier]

Awesome! Just for the sake of curiosity: Coukd you share the output of top for one representative setup?

[dagar]

Awesome! Just for the sake of curiosity: Coukd you share the output of top for one representative setup?

We'll need to move the high rate modules and logger to uORB::Subscription to see most of the benefit.

Here's the current comparison below where the main difference is a small decrease in cpu usage for each Mavlink instance (notice the FDs per task). This isn't a direct comparison though, because for safety I also restored the original MavlinkOrbSubscription semantics where the data is always copied even if there's no timestamp update (the PR is doing more orb copy work yet still faster).

https://github.com/PX4/Firmware/blob/88127380e7b61df030a270765d6570f80d01afaa/src/modules/mavlink/mavlink_orb_subscription.h#L61

Memory-wise the huge win will be after logger and a few other heavy subscribers are updated. That's when we'll be able to drop the NuttX file descriptors significantly and overall have an extra 15-20 kB of memory to play with.

Master

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                   20693 16.990   284/  748   0 (  0)  READY  3
   1 hpwork                        328  0.404   800/ 1780 249 (249)  READY 12
   2 lpwork                        104  0.161   648/ 1780  50 ( 50)  w:sig  8
   3 init                         1129  0.000  2040/ 2604 100 (100)  w:sem  3
   4 wq:manager                      2  0.000   760/ 1172 244 (244)  w:sem  7
1561 top                           481  3.883  1256/ 1684 255 (255)  RUN    3
  17 dataman                        84  0.000   752/ 1180  90 ( 90)  w:sem  4
  21 wq:I2C1                       708  0.728   880/ 1196 248 (248)  READY  7
  24 wq:lp_default                 100  0.161   496/ 1196 205 (205)  w:sem  7
  27 wq:hp_default                  19  0.000   432/ 1196 244 (244)  w:sem  7
 138 wq:SPI1                     25778 26.213   696/ 1196 254 (254)  READY  7
 143 wq:I2C2                         0  0.000   360/ 1196 247 (247)  w:sem  7
 148 wq:I2C3                       492  0.485   704/ 1196 246 (246)  READY  7
 151 wq:SPI5                         0  0.000   360/ 1196 250 (250)  w:sem  7
 165 wq:SPI4                       405  0.404   528/ 1196 251 (251)  READY  7
 182 sensors                      3767  3.964  1352/ 1964 238 (238)  w:sem 17
 186 commander                     844  0.809  1480/ 3212 140 (140)  READY 29
 188 commander_low_prio              2  0.000   576/ 2996  50 ( 50)  w:sem 29
 200 mavlink_if0                  6749  6.877  1760/ 2532 100 (100)  READY 34
 201 mavlink_rcv_if0               512  0.404  1544/ 2836 175 (175)  w:sem 34
 307 gps                           250  0.242   992/ 1516 209 (209)  w:sem  5
 363 mavlink_if1                  5137  5.339  1768/ 2484 100 (100)  READY 36
 364 mavlink_rcv_if1               393  0.404  1176/ 2836 175 (175)  w:sem 36
 382 mavlink_if2                  5273  5.339  1632/ 2492 100 (100)  READY 36
 383 mavlink_rcv_if2               460  0.485  1120/ 2836 175 (175)  w:sem 36
 422 px4io                        3674  3.802   976/ 1484 241 (241)  w:sem 11
 434 fmu                           520  0.566   760/ 1324 241 (241)  w:sem  7
 848 ekf2                        11185 11.974  4576/ 6572 239 (239)  READY 20
 861 mc_att_control               4707  5.177   928/ 1660 240 (240)  w:sem 18
 877 mc_pos_control                365  0.404   904/ 1860 239 (239)  w:sem 10
 910 navigator                     116  0.080   896/ 1764 105 (105)  w:sem 16

Processes: 31 total, 12 running, 19 sleeping, max FDs: 54
CPU usage: 78.32% tasks, 4.69% sched, 16.99% idle
DMA Memory: 5120 total, 1024 used 1024 peak
Uptime: 100.517s total, 20.693s idle

PR

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                   26888 19.174   284/  748   0 (  0)  READY  3
   1 hpwork                        403  0.323   768/ 1780 249 (249)  w:sig 12
   2 lpwork                        126  0.161   656/ 1780  50 ( 50)  w:sig  8
   3 init                         1120  0.000  2048/ 2604 100 (100)  w:sem  3
   4 wq:manager                      2  0.000   792/ 1172 244 (244)  w:sem  7
1319 top                          2960  3.883  1264/ 1684 255 (255)  RUN    3
  17 dataman                        84  0.000   752/ 1180  90 ( 90)  w:sem  4
  21 wq:I2C1                       888  0.728   872/ 1196 248 (248)  READY  7
  24 wq:lp_default                 126  0.161   496/ 1196 205 (205)  w:sem  7
  27 wq:hp_default                  25  0.000   432/ 1196 244 (244)  w:sem  7
 138 wq:SPI1                     31942 26.375   696/ 1196 254 (254)  READY  7
 143 wq:I2C2                         0  0.000   360/ 1196 247 (247)  w:sem  7
 148 wq:I2C3                       619  0.485   704/ 1196 246 (246)  READY  7
 151 wq:SPI5                         0  0.000   360/ 1196 250 (250)  w:sem  7
 165 wq:SPI4                       510  0.404   528/ 1196 251 (251)  READY  7
 182 sensors                      4593  3.883  1320/ 1964 238 (238)  w:sem 17
 186 commander                    1008  0.809  1472/ 3212 140 (140)  READY 21
 188 commander_low_prio              3  0.000   656/ 2996  50 ( 50)  READY 21
 200 mavlink_if0                  5782  6.229  1776/ 2532 100 (100)  READY  4
 201 mavlink_rcv_if0               545  0.404  1528/ 2836 175 (175)  w:sem  4
 307 gps                           293  0.242   992/ 1516 209 (209)  w:sem  5
 363 mavlink_if1                  6100  4.611  1648/ 2484 100 (100)  READY  4
 364 mavlink_rcv_if1               532  0.404  1424/ 2836 175 (175)  w:sem  4
 382 mavlink_if2                  6303  4.773  1648/ 2492 100 (100)  w:sig  4
 383 mavlink_rcv_if2               592  0.485  1424/ 2836 175 (175)  w:sem  4
 422 px4io                        4319  3.721   936/ 1484 241 (241)  w:sem 11
 434 fmu                           677  0.566   760/ 1324 241 (241)  w:sem  7
 848 ekf2                        14018 11.893  4576/ 6572 239 (239)  w:sem 20
 861 mc_att_control               6119  5.339   928/ 1660 240 (240)  READY 18
 877 mc_pos_control                461  0.404   936/ 1860 239 (239)  w:sem 10
 910 navigator                     188  0.080   896/ 1764 105 (105)  w:sem 14

Processes: 31 total, 11 running, 20 sleeping, max FDs: 54
CPU usage: 76.38% tasks, 4.45% sched, 19.17% idle
DMA Memory: 5120 total, 1024 used 1024 peak
Uptime: 124.074s total, 26.889s idle

[bkueng]

Nice, this makes Subscription much more usable.
Some general remarks:

• The class API docs don't make it clear when to use which class. Can you clarify this? Especially that Subscription should generally be used over the Polled classes.
• Can you rename SubscriptionPolled to SubscriptionPollable? I find Polled confusing, since there is no poll involved.
• Did you check the need for virtual for the Subscription classes? If not required, we should remove that overhead early on.

[dagar]

• The class API docs don't make it clear when to use which class. Can you clarify this? Especially that Subscription should generally be used over the Polled classes.
• Can you rename SubscriptionPolled to SubscriptionPollable? I find Polled confusing, since there is no poll involved.

The naming is better, but I'd actually like to kill this off entirely as soon as possible (likely once FlightTasks is migrated). Then it's only LPE using it for polling, which we can either work around with the C API, or add a blocking update method to the new uORB::Subscription.

• Did you check the need for virtual for the Subscription classes? If not required, we should remove that overhead early on.

uORB::SubscriptionInterval overrides update, but let's revisit after logger has been migrated and see what we really need. I'd still like to get to a template uORB::Subscription (without the internal data).

[dannyfpv]

@dagar after the pr we upload master and it worked without issues

[dagar]

@dagar after the pr we upload master and it worked without issues

Can you share the details of that setup and the parameters? I'm unable to reproduce locally on a Pixhawk 4 or 4 mini.

[dagar]

In the last refactor I consolidated the copy logic to be shared by read(), copy(), and copy_and_get_timestamp(). The common code was missing a null check here (data isn't allocated until the first publication).

https://github.com/PX4/Firmware/pull/12040/files#diff-6933463aec16725fb6cb34d9b949df62R169

@dannyfpv please try again.

[dannyfpv]

Tested on Pixhawk 4 v5 (Update)

Modes Tested:

• Position Mode: no issue
• Altitude Mode: no issue
• Stabilized Mode: no issue
• Mission mode: no issue

Notes:
No issues were noted, good flight in general.

Logs:

• https://review.px4.io/plot_app?log=16115d93-31e0-47de-a0de-012414627bb0
• https://review.px4.io/plot_app?log=7138dc8d-f164-4e5b-b2ec-5afd344c3fc1

Note:
Same results on Pixhawk 4mini v5.

[Junkim3DR]

Tested on Pixhawk 4mini v5

Modes Tested

• Position Mode: Good.
• Altitude Mode: Good.
• Stabilized Mode: Good.
• Mission Plan Mode (Automated): Good.
• Failsafe RTL (Return To Land): Not returning after RC shutdown.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint turn off RC to trigger Failsafe RTL and see landing behavior.

Notes
No issues noted, good flight in general.

Log

• https://review.px4.io/plot_app?log=53d27f98-e7e1-4c59-9ffb-a7acf7ed1cef

[dagar]

As discussed on the dev call this week, let's give this another round of thorough testing, including a setup with heavy mavlink usage and logging over mavlink. The majority of the new uORB::Subscription usage is in the mavlink module.

[dagar]

Rebased on master to resolve minor FlightTasks merge conflict.

[bkueng]

Looks correct, but let's do the heavy load test first before merging.

[bkueng]

Using an early return would reduce the level of indentation:

Suggested change

	if ((dst != nullptr) && (_data != nullptr)) {
	if (!dst || !_data) {
	return false;
	}

[jkflying]

Edit: --- IGNORE THIS COMMENT ---

Commit before this PR:

This PR:

[jkflying]

Ok, there was a bug with the system, this is updated info

Commit before this PR:

This PR:

Mavlink status during testing:

[dagar]

Testing looks good, let's get this into master and keep going. There's a lot more to be done.

Thanks everyone!