Tuning Speed and Throttle

This page decribes how a Rover’s speed and throttle controls can be tuned. In general it is best to tune this controller before moving on to the steering controllers.

../_images/rover-throttle-and-speed1.png

Cruise Throttle and Cruise Speed

The CRUISE_THROTTLE and CRUISE_SPEED parameters set the baseline throttle output used by the Desired-Speed-to-Throttle controller. It is important that these two values are reasonable and consistent with each other meaning that the CRUISE_THROTTLE value (expressed as a percentage) should be close to the throttle required to achieve the CRUISE_SPEED (expressed in m/s).

One way to set the value is to:

  • set the Auxiliary Function Switch to “Learn Cruise Speed”
  • arm the vehicle and switch to Manual
  • drive the vehicle at a speed close to the speed you would normally drive it at (in Auto or other modes)
  • move the aux function switch to the high position for a few seconds and then back down to the low position
  • connect with a ground station and check that the CRUISE_SPEED and CRUISE_THROTTLE values have been updated

The CRUISE_SPEED is also used as the default speed in Auto, Guided, RTL and SmartRTL modes although these can be overridden using the WP_SPEED and/or RTL_SPEED parameters.

Desired Speed to Throttle PID Tuning

The Desired-Speed-to-Throttle controller attempts to achieve the desired speed (set by the pilot or autopilot) using a relatively standard PID controller. All modes except Hold and Manual use this controller.

The Feed Forward, P, I and D gains for this controller are held in the ATC_SPEED_FF, ATC_SPEED_P, ATC_SPEED_I and ATC_SPEED_D parameters respectively.

Recommended steps for tuning this controller are:

  • connect the ground station to the vehicle using a telemetry radio
  • drive the vehicle in Acro or Steering mode
  • the FF gain should be left at zero because the CRUISE_THROTTLE and CRUISE_SPEED are used to calculate a base throttle output which removes the need for feed-forward.
  • the P gain is the most important and should be tuned first. If the vehicle’s speed is jerky and unstable then this parameter should be reduced. If the vehicle is slow to get up to speed, this parameter should be increased.
  • the I gain corrects for long-term error. If the vehicle never achieves the desired speed, then this parameter should be increased. If the vehicle’s speed is slowly oscillating between too fast and too slow, this parameter should be reduced.
  • the D gain is meant to stablize the output by fighting against short-term changes in speed. This gain can normally be left at zero.

The desired speed vs achieved speed, along with the individual contributions to the throttle output from P, I and D gains can be seen in real-time by doing the following:

  • connect the Mission Planner to the vehicle’s flight controller using a telemetry radio
  • set the GCS_PID_MASK to 2 (Throttle)
  • on the Flight Data screen, check the “Tuning” checkbox (bottom middle) and select “pidachieved”, “piddesired”
  • increase and decrease the desired speed of the vehicle and see how well the “achieved” follows the “desired”
../_images/rover-throttle-and-speed2.png

Acceleration Maximum

Although less important than other tuning values, the ATC_ACCEL_MAX should be set the vehicle’s maximum acceleration or deceleration value (whichever is lower) in m/s/s. This will stop the Desired-Speed-to-Throttle controller from attempting impossible accelerations and reduce overshoot.

Throttle Slew

The MOT_SLEWRATE parameter can be used to limit how quickly the throttle output can change.

  • a value of 100 allows the throttle output to change over its full range in one second
  • a value of zero disables the limit