// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
// this function updates the _yaw_error_cd value
void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed)
{
// record system time of call
last_steer_to_wp_ms = AP_HAL::millis();
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
rover.nav_controller->set_reverse(reversed);
rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius);
float desired_lat_accel = rover.nav_controller->lateral_acceleration();
float desired_heading = rover.nav_controller->target_bearing_cd();
if (reversed) {
desired_heading = wrap_360_cd(desired_heading + 18000);
desired_lat_accel *= -1.0f;
}
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
if (rover.sailboat_use_indirect_route(desired_heading)) {
// sailboats use heading controller when tacking upwind
desired_heading = rover.sailboat_calc_heading(desired_heading);
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else if (rover.use_pivot_steering(_yaw_error_cd)) {
// for pivot turns use heading controller
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else {
// call lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
}
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(_desired_speed, false, true);
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
} else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
}
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// note: copied from calc_reduced_speed_for_turn_or_distance
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, false, true);
// mark us as in_reverse when using a negative throttle
// To-Do: only in reverse if vehicle is actually travelling backwards?
rover.set_reverse(_desired_speed < 0);
}
void ModeSimple::update()
{
float desired_heading_cd, desired_speed;
// get pilot input
get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
// rotate heading around based on initial heading
if (g2.simple_type == Simple_InitialHeading) {
desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);
}
// if sticks in middle, use previous desired heading (important when vehicle is slowing down)
if (!is_positive(desired_speed)) {
desired_heading_cd = _desired_heading_cd;
} else {
// record desired heading for next iteration
_desired_heading_cd = desired_heading_cd;
}
// run throttle and steering controllers
calc_steering_to_heading(desired_heading_cd);
calc_throttle(desired_speed, true);
}
void ModeGuided::update()
{
switch (_guided_mode) {
case Guided_WP:
{
_distance_to_destination = get_distance(rover.current_loc, _destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else {
stop_vehicle();
}
break;
}
case Guided_HeadingAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else {
stop_vehicle();
}
break;
}
case Guided_TurnRateAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true);
} else {
stop_vehicle();
}
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}
