diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index 3cc6dfc3c2b9..2862bcf18199 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -125,47 +125,54 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c _curr_wp(0), _curr_wp(1)); - if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() - && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect - if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { // Straight line speed - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON - && _acceptance_radius > FLT_EPSILON) { - float max_velocity{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp, 0.f); + if (distance_to_curr_wp > _acceptance_radius) { + if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() + && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect + if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { + const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius; + desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); + + } else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) { + const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; + desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); + + } else { // Straight line speed + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON + && _acceptance_radius > FLT_EPSILON) { + float max_velocity{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), + _param_ra_max_accel.get(), distance_to_curr_wp, 0.f); + + } else { + const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; + max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), + _param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed); + } + + desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); } else { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed); + desired_speed = _param_ra_miss_vel_def.get(); } - - desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { - desired_speed = _param_ra_miss_vel_def.get(); } + + } else { + desired_speed = _param_ra_miss_vel_def.get(); } - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } + // Calculate desired steering + desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(), + _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw); + desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get()); + _prev_desired_steering = desired_steering; - // Calculate desired steering - desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(), - _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw); - desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); + } else { // Catch delay command + desired_steering = _prev_desired_steering; // Avoid steering on the spot + desired_speed = 0.f; + } } // Throttle PID diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 090f37b15e97..0a73abceb553 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -151,6 +151,7 @@ class RoverAckermannGuidance : public ModuleParams Vector2f _curr_pos_ned{}; PID_t _pid_throttle; hrt_abstime _timestamp{0}; + float _prev_desired_steering{0.f}; // Waypoint variables Vector2d _curr_wp{};