L1 controller tuning for rover

I am testing the new added R1 Rover with Gazebo along with PX4 firmware (master). I know that a lot of effort have been put on the L1 controller for the rover.

What I did is to send mavros/setpoint_raw/local with MAVROS. The idea is to simulate indoor movement so waypoints are few meters away from each other.

I modified the GND POS CONTROL parameters:

  • mainly GND_L1_PERIOD
  • I do not know how GND_L1_DIST is used but as I want to reach the waypoint I set a small value

After arming and offboard mode set, I send mavros/setpoint_raw/local with a pose=(3,0,0). The behavior I go from the rover is described by the red line on the picture below:

The rover move backward first, then in dagonal and backward again to reach (3.45, -1.57, 0).
I tried different values with the same outcome.

Does anyone can tell me where is the problem?