px4io causing inner loop latency

[dongwoook]

The inner loop of PX4 involves several tasks, such as sensors, ekf2, mc_att_control, mc_pos_control, and px4io.
And in this loop, px4io is expected to read values from actuator_controls_0 topic and update PWM registers every 4ms.

However, px4io does several other things consuming CPU.
Every 20ms, it reads and publishes raw RC input and update arming state.
And every 200ms, it does DSM paring and parameter update handling.

Thus px4io sometimes does some or all of these jobs before updating PWM registers
and consequently increases inner loop latency, time elapsed from when sensors task reading gyro to when px4io task updating PWM

And another source of the latency is that px4io's priority (245) is lower than those of other tasks involved the inner loop (250).

We can find out this in "perf" result showing "io latency" meaning elapsed time between beginning of ekf2's main loop and px4io's PWM update

as shown in below perf result, maximum "io latency" is 7427us while average is 4775us indicating the inner loop suffers from significant latency

io latency: 61066 events, 291478904us elapsed, 4773us avg, min 3956us max 7108us 1007967.375us rms

As a temporary solution, I boosted px4io's priority from 240 to 250 right after mc_acc_control publishes to actuator_controls_0 topic and restore the priority right after px4io completes PWM update with the published data.

Below is the perf result with modified PX4.
Max "io latency" decreases to 4937us and avg decreases to 3787us.

io latency: 164447 events, 622789254us elapsed, 3787us avg, min 3221us max 4949us 718777.500us rms

This seems to remove one of the latency sources, px4io's lower priority.
But the another source, px4io's lower frequency jobs, is still remaining.

Thus I think below is one better solution.

1. divide px4io's original jobs into three groups according to their periods
2. create three tasks instead of px4io
3. assigns each job group to each task
4. give different and appropriate priorities to them
   (For example, 250 to px4io_4ms, 245 to px4io_20ms, and 240 to px4io_200ms)

[dagar]

Which version of PX4 are you using?
Many of the priorities were already reviewed and adjusted a few months ago (after the last stable release). https://github.com/PX4/Firmware/blob/master/src/platforms/px4_tasks.h#L116

Before splitting the px4io into multiple threads (which requires a lot of additional memory) I'd recommend profiling the other routines (raw RC input, arming state, DSM, etc)

Here's the io update perf counter (surrounds everything you mentioned) from a random log (https://logs.px4.io/plot_app?log=167834f7-6d3f-4eb7-be9c-3d9116bc1e89).

io update: 128950 events, 74825278us elapsed, 580us avg, min 262us max 2294us 913.672us rms

The very first thing the loop does is send the actuator controls to the physical px4io (a separate processor), so I don't know that you're actually going to be able to decrease latency by splitting it.

One minor optimization is that we could probably skip things like param update and DSM pairing after arming. Param updates will cause significant jitter and in some cases aren't safe while flying.

[dagar]

A found a bigger problem. The MC attitude controller one of the highest priority tasks and updates when there's new gyro data, but the timestamp sample is still being set from the vehicle_attitude message (published by the estimator).

gyro -> rate controller -> output

io latency: 73243 events, 44045138us elapsed, 601us avg, min 162us max 1922us 410.388us rms

#8415

[dagar]

I believe we can consider this resolved by #8415.