// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// set origin to last destination if waypoint controller active
if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
calc_stopping_location(_origin);
}
_destination = destination;
// initialise distance
_distance_to_destination = get_distance(_origin, _destination);
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (is_zero(turn_angle_cd)) {
// if not turning can continue at full speed
_desired_speed_final = _desired_speed;
} else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
// pivoting so we will stop
_desired_speed_final = 0.0f;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
bool ModeGuided::_enter()
{
// initialise waypoint speed
set_desired_speed_to_default();
// set desired location to reasonable stopping point
calc_stopping_location(_destination);
set_desired_location(_destination);
return true;
}
bool ModeLoiter::_enter()
{
// set _destination to reasonable stopping point
calc_stopping_location(_destination);
// initialise desired speed to current speed
if (!attitude_control.get_forward_speed(_desired_speed)) {
_desired_speed = 0.0f;
}
// initialise heading to current heading
_desired_yaw_cd = ahrs.yaw_sensor;
_yaw_error_cd = 0.0f;
return true;
}
