Is tthe radius_calc_denom the sin(turn_angle_rad)?

whiani · November 20, 2020, 9:38am

Hi,everyone.How does this equation come from?

ArduPilot/ardupilot/blob/Rover-4.0/libraries/AR_WPNav/AR_WPNav.cpp#L400


// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(_oa_origin.get_bearing_to(_oa_destination) - heading_cd);
const float dist_from_line = fabsf(_cross_track_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error);
const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;


// calculate radius of circle that touches vehicle's current position and heading and target position and heading
float radius_m = 999.0f;
const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
if (!is_zero(radius_calc_denom)) {
    radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom;
}


// calculate and limit speed to allow vehicle to stay on circle
const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m));
des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);


// ensure speed does not fall below minimum
apply_speed_min(des_speed_lim);


// limit speed based on distance to waypoint and max acceleration/deceleration

Is tthe radius_calc_denom the sin(turn_angle_rad)?
I can’t see this equation. Can anyone help me? Thank you.