// 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);
}
}
// 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);
float desired_lat_accel = rover.nav_controller->lateral_acceleration();
if (reversed) {
_yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor + 18000);
} else {
_yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor);
}
if (rover.use_pivot_steering(_yaw_error_cd)) {
if (_yaw_error_cd >= 0.0f) {
desired_lat_accel = g.turn_max_g * GRAVITY_MSS;
} else {
desired_lat_accel = -g.turn_max_g * GRAVITY_MSS;
}
}
// call lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
}
void ModeSteering::update()
{
float desired_steering, desired_throttle;
get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
// convert pilot throttle input to desired speed (up to twice the cruise speed)
const float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so stop
g2.motors.set_throttle(0.0f);
g2.motors.set_steering(0.0f);
_desired_lat_accel = 0.0f;
return;
}
// determine if pilot is requesting pivot turn
bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
// In steering mode we control lateral acceleration directly.
// For pivot steering vehicles we use the TURN_MAX_G parameter
// For regular steering vehicles we use the maximum lateral acceleration at full steering lock for this speed: V^2/R where R is the radius of turn.
float max_g_force;
if (is_pivot_turning) {
max_g_force = g.turn_max_g * GRAVITY_MSS;
} else {
max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
}
// constrain to user set TURN_MAX_G
max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);
// convert pilot steering input to desired lateral acceleration
_desired_lat_accel = max_g_force * (desired_steering / 4500.0f);
// reverse target lateral acceleration if backing up
bool reversed = false;
if (is_negative(target_speed)) {
reversed = true;
_desired_lat_accel = -_desired_lat_accel;
}
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run speed to throttle output controller
if (is_zero(target_speed) && !is_pivot_turning) {
stop_vehicle();
} else {
// run lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(false);
calc_throttle(target_speed, false);
}
}
