Wrong MAVLink output frequency.

[xuhao1]

Describe the bug

I am using a Kakute H7 board and using the stable branch of the code. I found the _main_loop_delay in src/modules/mavlink/mavlink_main.cpp controls the max output frequency of MAVlink. And the frequency looks incorrect.

To Reproduce

I change the IMU_GYRO_RATEMAX=2000Hz, so the output frequency of HIGHRES_IMU can reach 2kHz ideally. I am trying to output 1kHZ IMU data. When _main_loop_delay is 1500us (the smallest in default), the maxium output frequency of MAVLink message is 333Hz (HIGHRES_IMU, I set it to unlimited_rate). I am using USB the the bandwidth is enough:

instance #1:
    GCS heartbeat valid
    mavlink chan: #1
    type:        USB CDC
    flow control: OFF
    rates:
      tx: 32786.4 B/s
      txerr: 0.0 B/s
      tx rate mult: 1.000
      tx rate max: 100000 B/s
      rx: 46.9 B/s
      rx loss: 0.0%
    Received Messages:
      sysid:255, compid:  0, Total: 36 (lost: 0)
      sysid:255, compid:190, Total: 327 (lost: 0)
        msgid:  126, Rate:  0.2 Hz, last 0.02s ago
        msgid:    4, Rate:  1.0 Hz, last 0.54s ago
        msgid:    0, Rate:  1.0 Hz, last 0.54s ago
    FTP enabled: YES, TX enabled: YES
    mode: Config
    MAVLink version: 2
    transport protocol: serial (/dev/ttyACM0 @2000000)
    ping statistics:
      last: 0.33 ms
      mean: 0.34 ms
      max: 463.30 ms
      min: 0.17 ms
      dropped packets: 0

The cpu is also OK in the test

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                   54431 57.760   272/  512   0 (  0)  READY  3
   1 hpwork                          0  0.000   332/ 1264 249 (249)  w:sig  3
   2 lpwork                          0  0.000   892/ 1616  50 ( 50)  w:sig  3
   3 init                            0  0.000  2168/ 2928 100 (100)  w:sem  3
   4 wq:manager                      0  0.000   412/ 1256 255 (255)  w:sem  4
1552 mavlink_shell                   0  0.000  1052/ 2024 100 (100)  w:sem  3
 101 wq:hp_default                   8  4.267  1180/ 1904 237 (237)  w:sem  4
 111 dataman                         0  0.000   916/ 1208  90 ( 90)  w:sem  4
 119 wq:lp_default                   0  0.194   956/ 1920 205 (205)  w:sem  4
 395 wq:SPI4                        19  9.538  1528/ 2336 250 (250)  w:sem  4
 397 wq:I2C1                         0  0.047   776/ 2336 246 (246)  w:sem  4
 505 wq:nav_and_controllers          3  1.961  1220/ 2240 242 (242)  w:sem  4
 506 wq:rate_ctrl                   21 10.726  1532/ 3152 255 (255)  w:sem  4
 508 wq:INS0                         2  1.301   972/ 6000 241 (241)  w:sem  4
 518 commander                       1  0.581  1452/ 3224 140 (140)  w:sig  5
 984 gps                             0  0.036  1188/ 1680 205 (205)  w:sem  4
1439 mavlink_if0                     9  4.589  1852/ 2728 100 (100)  w:sig  4
1447 mavlink_rcv_if0                 0  0.165  1316/ 4712 175 (175)  w:sem  4
1460 wq:ttyS4                        0  0.254   788/ 1632 230 (230)  w:sem  4
1480 navigator                       0  0.012  1068/ 1920 105 (105)  w:sem  6
1523 logger                          0  0.130  2636/ 3648 230 (230)  w:sem  3
1525 log_writer_file                 0  0.000   372/ 1176  60 ( 60)  w:sem  3
1546 mavlink_if1                    14  7.005  1844/ 2824 100 (100)  READY  6
1548 mavlink_rcv_if1                 0  0.166  1660/ 4712 175 (175)  w:sem  6
1553 top                             0  0.000  1964/ 4080 237 (237)  RUN    3

Processes: 25 total, 3 running, 22 sleeping
CPU usage: 40.98% tasks, 1.26% sched, 57.76% idle
DMA Memory: 5120 total, 1024 used 1536 peak
Uptime: 91.559s total, 54.431s idle

When I change _main_loop_delay in the mavlink_main.cpp to 1000us. The output frequency is 500Hz. When I change _main_loop_delay to 2000us, the output frequency is 333Hz.

Here I draw a table of relationship of _main_loop_delay and real output

_main_loop_delay (us)	Output HIGHRES_IMU freq (Hz)
1500	333
2000	333
5000	167
1000	500
500	500
200	500

Expected behavior

I would like to output 1kHz IMU data.

Log Files and Screenshots

My SD card also not works correctly, so currently no LOG.

Add screenshots to help explain your problem.

Drone (please complete the following information):

None

Additional context

Is px4_usleep only sleep 2ms or 3ms 6ms?