Open rmackay9 opened 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.
Perhaps we could include better motion and trajectory control with better acc and deceleration phase calculated from the next waypoint.
Please, what is the equivalent of this configuration in the case of Copter ??
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: