MAVLink: Improve message handling / tracking

[LorenzMeier]

The message handling was not obeying action focused messages and high-rate messages properly before. With this change update rates track the desired rates closely. Critical high-rate messages such as ADS-B are queued additionally to guarantee that all received packets are being correctly forwarded.

[mhkabir]

Something isn't right here. I tested with a VIO/camera trigger setup, and this branch kills the CAMERA_TRIGGER message (it never gets sent from Firmware side.)

[LorenzMeier]

A quick inspection did not reveal why this should not work (there are relevant changes in those code sections of course).

[LorenzMeier]

@mhkabir Found and fixed - could you re-test?

Planned rates:

configure_stream("SYS_STATUS", 1.0f);
		configure_stream("EXTENDED_SYS_STATE", 2.0f);
		configure_stream("HIGHRES_IMU", 50.0f);
		configure_stream("ATTITUDE", 50.0f);
		configure_stream("ATTITUDE_QUATERNION", 50.0f);
		configure_stream("RC_CHANNELS", 10.0f);
		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
		configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
		configure_stream("ALTITUDE", 10.0f);
		configure_stream("GPS_RAW_INT", 10.0f);
		configure_stream("ADSB_VEHICLE");
		configure_stream("COLLISION", 20.0f);
		configure_stream("DISTANCE_SENSOR", 10.0f);
		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
		configure_stream("VISION_POSITION_ESTIMATE", 10.0f);
		configure_stream("ESTIMATOR_STATUS", 5.0f);
		configure_stream("NAV_CONTROLLER_OUTPUT", 10.0f);
		configure_stream("GLOBAL_POSITION_INT", 10.0f);
		configure_stream("LOCAL_POSITION_NED", 30.0f);
		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
		configure_stream("ATTITUDE_TARGET", 8.0f);
		configure_stream("HOME_POSITION", 0.5f);
		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
		configure_stream("VFR_HUD", 20.0f);
		configure_stream("WIND_COV", 10.0f);
		configure_stream("CAMERA_TRIGGER");
		configure_stream("CAMERA_IMAGE_CAPTURED");
		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
		configure_stream("MANUAL_CONTROL", 5.0f);

[LorenzMeier]

Difference:

• 40 Hz instead of 50: ("SERVO_OUTPUT_RAW_0", 20.0f);
• 5 Hz instead of 10 (GPS delivers only 5 Hz): ("GPS_RAW_INT", 10.0f);

Much better compared to previously, following up on the servo output right now.

[mhkabir]

Checking this in a minute, thanks 👍

[LorenzMeier]

System load before last set of commits:

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE 
   0 Idle Task                   91657 35.789   604/  748   0 (  0)  READY 
   1 hpwork                       9944  4.385   888/ 1780 192 (192)  w:sig 
   2 lpwork                        515  0.175   640/ 1780  50 ( 50)  w:sig 
   3 init                        23830  0.000  2008/ 2484 100 (100)  w:sem 
 337 mavlink_shell                   0  0.000   880/ 2020 100 (100)  w:sem 
  94 gps                           560  0.263  1064/ 1580 220 (220)  w:sem 
 102 dataman                       305  0.000   704/ 1180  90 ( 90)  w:sem 
 154 sensors                     18629  5.877  1464/ 1964 249 (249)  w:sem 
 156 commander                    5355  2.280  2624/ 3652 140 (140)  w:sig 
 157 commander_low_prio              7  0.000   592/ 2996  50 ( 50)  w:sem 
 170 px4io                        7293  3.245   992/ 1484 240 (240)  w:sem 
 178 mavlink_if0                  6179  2.807  1712/ 2428 100 (100)  w:sig 
 179 mavlink_rcv_if0               959  0.438  1416/ 2140 175 (175)  w:sem 
 196 mavlink_if1                  3099  1.403  1712/ 2388 100 (100)  READY 
 197 mavlink_rcv_if1              1272  0.438  1592/ 2140 175 (175)  w:sem 
 244 log_writer_file                 0  0.000   344/ 1068  60 ( 60)  w:sem 
 223 mavlink_if2                 25970 11.842  1712/ 2388 100 (100)  w:sig 
 231 mavlink_rcv_if2               976  0.438  1480/ 2140 175 (175)  w:sem 
 240 logger                       1300  0.614  1232/ 3540 245 (245)  w:sem 
 341 top                            54  1.578  1288/ 1684 100 (100)  R 299 ekf2                        41645 18.846  5080/ 5876 250 (250)  w:sem 
 301 mc_att_control               9762  4.302  1176/ 1676 250 (250)  w:sem 
 303 mc_pos_control               7898  3.528  1152/ 1876 250 (250)  w:sem 
 311 navigator                     422  0.258   888/ 1772 105 (105)  w:sem 

Processes: 24 total, 2 running, 22 sleeping
CPU usage: 62.31% tasks, 1.38% sched, 36.32% idle
DMA Memory: 5120 total, 1024 used 1024 peak
Uptime: 232.021s total, 91.249s idle

[mhkabir]

Trigger messages come through now, but there are still lots of drops :

I also get a high txerr on the link, which is probably why it's dropping so many messages :

I will need to check why this happens. CPU load also seems to be higher (as expected), but I haven't checked the load without this patchset.

[LorenzMeier]

What rate (-r param) did you start the link at? Try increasing that one. Overall you're trying to send more than the link is configured for, so dropped messages are non-surprising.

[mhkabir]

It is at standard companion rate (80K): https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/rcS#L694

[LorenzMeier]

Assuming you have that on Telem2: Increase the TX buffer size in nuttx-configs/px4fmu-v3_default/nsh/defconfig:

Change CONFIG_USART3_TXBUFSIZE=300 to CONFIG_USART3_TXBUFSIZE=1100

And please re-test.

[mhkabir]

Already did increase buffers. Testing now.

[mhkabir]

Hang on. I increased them for USART2. My bad. Is TELEM2 actually USART3 ?

I will be able test to this again in a few hours time. Gotta run now.

[LorenzMeier]

Telem 2 is USART3, yes. And USART2 already had large buffers.

[mhkabir]

We should fix the outdated hardware docs then (good time to move it to the Devguide maybe?) : https://pixhawk.org/users/wiring

Then page indicates that ttyS2 is USART2.

[LorenzMeier]

That was wrong all along it seems. I've fixed that page.