High latency between set_target_attitude and servo_output_raw

I am attempting to run a relatively high frequency control loop offboard with communication happening over mavlink. One metric I would like to minimize is the time it takes between the offboard computer sending a SET_TARGET_ATTITUDE and the flight controller modifying the servo outputs accordingly.

I have measured the mavlink round trip latency using TIMESYNC messages at around 7 ms.

One test I have run is sending SET_SERVO messages and listening to SERVO_OUTPUT_RAW at 100 hz. In this test, it typically takes about 15 ms between the offboard computer sending the message, and the offboard computer receiving a message with the updated servo value. More specifically, it takes a minimum of 10 ms and maximum of about 25 ms. The first mavlink SERVO_OUTPUT_RAW message never has the updated value, but the second one typically does, and sometimes it takes a third one. If I increase SERVER_OUTPUT_RAW frequency to 200 hz, I still get round trip time of about 10 ms - 25 ms.

There is almost nothing else on the mavlink channel. I have all other messages turned off and i’m sending vision messages at 30 hz.

After reading through the code quite a bit, I am not seeing why it should take more than 2 of the 400 hz cycles through fast_loop, update_send, and update_receive. It seems that this should only take approximately 1 cycle longer than a round trip TIMESYNC, though it is taking about 3 cycles longer, and often more. Still, this is workable, even if it would be nice to get further improvements.

However, the results are significantly worse when sending SET_ATTITUDE_TARGET messages. In that case, I am sending (with blades detached) 0 thrust messages at 25 hz, except for once a second 3x 0.2 thrust requests are made. It takes an average of 48 ms before the servo messages reflect the change in thrust (standard deviation: 15 ms, minimum: 15 ms, maximum: 71 ms). This is the exact same code as used above to measure SET_SERVO latencies. Does anyone know why there is such a delay? I have ATC_INPUT_TC set to 0. Are there any other parameters I should be checking? I have traced through the code and don’t see any reason for such a large delay. Does anyone have any ideas about what could be causing this, or how to fix it?