// 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); } }