VOXL2 slow in responding

[jocacace13]

Hi to all,

I am addressing a strange problem operating my voxl2 system with Visual Inertial Odometry. In particular, the system is very slow in responding to the vision input and suddenly rejects the position control.
In my architecture, I used qvio and px4 to communicate with the mavlink server. To get info from the autopilot I use both MAVROS and QgroundControl. In both cases, the output of the local position NED is very slow, preventing the system from being controlled in position.

For example, when I launch the MAVROS server along the qvio, asking for the local pose publication rate I have the following time:

rostopic hz /mavros/local_position/pose
subscribed to [/mavros/local_position/pose]
no new messages
no new messages
average rate: 2398.527
	min: 0.000s max: 0.009s std dev: 0.00097s window: 81
no new messages
no new messages
no new messages
average rate: 51.832
	min: 0.000s max: 3.038s std dev: 0.23866s window: 162
no new messages
no new messages
average rate: 38.837

This behaviour is also clear from the mavlink inspector from QgroundControl. Finally, I have another lag strange behaviour. The RC sticks are read with a very huge lag. For instance, the change in the control mode takes 4-5 seconds to be effective (also if I want to change from altitude to stabilized).

Have you some hint on how to solve this problem?