ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
11.01k stars 17.55k forks source link

Rover: combine SPEED_TURN_GAIN, PIVOT_TURN_ANGLE and WP_OVERSHOOT #7386

Open rmackay9 opened 6 years ago

rmackay9 commented 6 years ago

Rover has two separate methods for reducing speed to improve cornering in Auto, Guide and RTL. We should consolidate these methods somehow.

The WP_OVERSHOOT parameter is set by the user and the vehicle should slowdown as it reaches a waypoint so that the vehicle will not overshoot the waypoint by more than this distance (in meters). The calculations for the _desired_final_speed value can be found in mode.cpp's Mode::set_desired_location method (https://github.com/ArduPilot/ardupilot/blob/master/APMrover2/mode.cpp#L128)

The SPEED_TURN_GAIN parameter allows the user to set a minimum speed (expressed as a percentage of the desired speed (which normally comes from CRUISE_SPEED, WP_SPEED or RTL_SPEED) used during turns (in Auto, Guided, RTL). The final speed used during cornering uses linear interpolation between this minimum speed (i.e. SPEED_TURN_GAIN) and the desired speed (i.e. CRUISE_SPEED or WP_SPEED) based on the desired lateral acceleration (from the L1 controller) as a percentage of the maximum lateral acceleration (i.e. TURN_MAX_G) and the heading error between the vehicle's current heading and the heading to the next waypoint. Code can be found in mode.cpp's Mode::calc_reduced_speed_for_turn_or_distance. https://github.com/ArduPilot/ardupilot/blob/master/APMrover2/mode.cpp#L128

I have two rough ideas:

rmackay9 commented 6 years ago

After a ping from @peterbarker, I should add that there is another parameter involved for cases involving pivot turn vehicles, PIVOT_TURN_ANGLE. So if the heading error (i.e. difference between the vehicle's current heading and the heading to the next waypoint) is larger than this parameter value the vehicle reduces it's speed to zero. This logic is also in the Mode::calc_reduced_speed_for_turn_or_distance function.

khancyr commented 6 years ago

Perhaps we could include better motion and trajectory control with better acc and deceleration phase calculated from the next waypoint.

Oussalvatore commented 6 years ago

Please, what is the equivalent of this configuration in the case of Copter ??