This page provides an overview of Rover’s navigation feature including the L1 controller which is also used in Plane. The L1 controller is based on this paper by Sanghyuk Park, John Deyst and Jonathan P How of MIT.

Overview

• on every iteration of the main loop (50hz) a call is made to the active mode’s update method (here is RTL’s update function). While in Auto, Guide, RTL and SmartRTL mode, the update calls into the Mode class’s calc_steering_to_waypoint method. Mode’s calc_steering_to_waypoint then call’s the AP_L1_controller library’s update_waypoint method providing it the location that the rover should drive towards.
• The AP_L1_controller’s update_waypoint method returns a desired lateral acceleration which is passed into Mode’s calc_steering_from_lateral_acceleration
• Mode’s calc_steering_from_lateral_acceleration sends the desired acceleration to APM_Control/AR_AttitudeControl’s get_steering_out_lat_accel which uses a PID controller to calculate a steering output
• The steering output is sent into the AP_MotorsUGV library using the set_steering method

L1 Controller

The final output of the L1 controller’s update_waypoint method is a desired lateral acceleration (shown as “latAccDem” in red below) which should bring the vehicle back to the line between the origin and destination.

The formulas used are also shown below. damping is from the NAVL1_DAMPING parameter. period is from the NAVL1_PERIOD parameter.