Skip to content

Commit

Permalink
ackermann: improvements
Browse files Browse the repository at this point in the history 
improved cornering slow down and made it work with RTL, fixed slew rate bug, updated ackermann parameters
  • Loading branch information
@chfriedrich98
chfriedrich98 committed Jul 4, 2024
1 parent 5aa314d commit 65187f6
Show file tree
Hide file tree
Showing 5 changed files with 131 additions and 96 deletions.
88 changes: 49 additions & 39 deletions src/modules/rover_ackermann/RoverAckermann.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ void RoverAckermann::updateParams()
{
ModuleParams::updateParams();

// Update slew rates
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
_throttle_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get());
}
Expand All @@ -80,6 +81,7 @@ void RoverAckermann::Run()
vehicle_status_s vehicle_status;
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
_armed = vehicle_status.arming_state == 2;
}

if (_local_position_sub.updated()) {
Expand All @@ -89,52 +91,60 @@ void RoverAckermann::Run()
_actual_speed = rover_velocity.norm();
}

// Navigation modes
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
manual_control_setpoint_s manual_control_setpoint{};

if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_motor_setpoint.steering = manual_control_setpoint.roll;
_motor_setpoint.throttle = manual_control_setpoint.throttle;
}

} break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state);
break;

default: // Unimplemented nav states will stop the rover
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
break;
}

// Timestamps
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = (_timestamp - timestamp_prev) * 1e-6f;
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;

if (_armed) {
// Navigation modes
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
manual_control_setpoint_s manual_control_setpoint{};

if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
_motor_setpoint.steering = manual_control_setpoint.roll;
_motor_setpoint.throttle = manual_control_setpoint.throttle;
}

} break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state);
break;

default: // Unimplemented nav states will stop the rover
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
break;
}

// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO
&& fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);
// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO
&& fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);

} else {
_throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle);
}
} else {
_throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle);
}

// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON
&& _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO) {
_steering_with_rate_limit.update(_motor_setpoint.steering, dt);
// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON
&& _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO) {
_steering_with_rate_limit.update(_motor_setpoint.steering, dt);

} else {
_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
} else {
_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
}

} else { // Reset on disarm
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
}

// Publish rover ackermann status (logging)
Expand Down
1 change: 1 addition & 0 deletions src/modules/rover_ackermann/RoverAckermann.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,7 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
float _actual_speed{0.f};
SlewRate<float> _steering_with_rate_limit{0.f};
SlewRate<float> _throttle_with_accel_limit{0.f};
bool _armed{false};

// Parameters
DEFINE_PARAMETERS(
Expand Down
61 changes: 43 additions & 18 deletions src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,12 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
actual_speed = rover_velocity.norm();
}

if (_home_position_sub.updated()) {
home_position_s home_position{};
_home_position_sub.copy(&home_position);
_home_position = Vector2d(home_position.lat, home_position.lon);
}

if (_position_setpoint_triplet_sub.updated()) {
updateWaypoints();
}
Expand All @@ -105,7 +111,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const

if (nav_state == NAVIGATION_STATE_AUTO_RTL
&& get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0),
_next_wp(1)) < _param_ra_acc_rad_def.get()) { // Return to launch
_next_wp(1)) < _acceptance_radius) { // Return to launch
mission_finished = true;
}

Expand All @@ -119,17 +125,30 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
_curr_wp(0),
_curr_wp(1));

if (distance_to_prev_wp <= _prev_acceptance_radius * _param_ra_miss_vel_red.get()
&& _prev_acceptance_radius > FLT_EPSILON) { // Cornering speed
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());
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) {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
const float 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 {
desired_speed = _param_ra_miss_vel_def.get();
}
}

} else if (distance_to_curr_wp <= _acceptance_radius * _param_ra_miss_vel_red.get()
&& _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 { // Default mission speed
} else {
desired_speed = _param_ra_miss_vel_def.get();
}

Expand All @@ -141,9 +160,9 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
}

// Throttle PID
hrt_abstime now = hrt_absolute_time();
const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
_time_stamp_last = now;
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
float throttle = 0.f;

if (desired_speed < FLT_EPSILON) {
Expand All @@ -161,7 +180,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
}

// Publish ackermann controller status (logging)
_rover_ackermann_guidance_status.timestamp = now;
_rover_ackermann_guidance_status.timestamp = _timestamp;
_rover_ackermann_guidance_status.desired_speed = desired_speed;
_rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral;
_rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status);
Expand Down Expand Up @@ -194,7 +213,7 @@ void RoverAckermannGuidance::updateWaypoints()
_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);

} else {
_next_wp = _curr_wp;
_next_wp = _home_position; // Enables corner slow down with RTL
}

// Local waypoint coordinates
Expand All @@ -204,8 +223,14 @@ void RoverAckermannGuidance::updateWaypoints()

// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_ra_acc_rad_def.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());

if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_nav_acc_rad.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());

} else {
_acceptance_radius = _param_nav_acc_rad.get();
}
}

float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
Expand Down
9 changes: 6 additions & 3 deletions src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/home_position.h>

// Standard library includes
#include <matrix/matrix/math.hpp>
Expand Down Expand Up @@ -144,6 +145,7 @@ class RoverAckermannGuidance : public ModuleParams
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
uORB::Subscription _home_position_sub{ORB_ID(home_position)};

// uORB publications
uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};
Expand All @@ -157,12 +159,13 @@ class RoverAckermannGuidance : public ModuleParams
Vector2d _curr_pos{};
Vector2f _curr_pos_local{};
PID_t _pid_throttle;
hrt_abstime _time_stamp_last{0};
hrt_abstime _timestamp{0};

// Waypoint variables
Vector2d _curr_wp{};
Vector2d _next_wp{};
Vector2d _prev_wp{};
Vector2d _home_position{};
Vector2f _curr_wp_local{};
Vector2f _prev_wp_local{};
Vector2f _next_wp_local{};
Expand All @@ -177,15 +180,15 @@ class RoverAckermannGuidance : public ModuleParams
(ParamFloat<px4::params::RA_LOOKAHD_MAX>) _param_ra_lookahd_max,
(ParamFloat<px4::params::RA_LOOKAHD_MIN>) _param_ra_lookahd_min,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_DEF>) _param_ra_acc_rad_def,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
(ParamFloat<px4::params::RA_MISS_VEL_MIN>) _param_ra_miss_vel_min,
(ParamFloat<px4::params::RA_MISS_VEL_GAIN>) _param_ra_miss_vel_gain,
(ParamFloat<px4::params::RA_MISS_VEL_RED>) _param_ra_miss_vel_red,
(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_p_speed,
(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
(ParamFloat<px4::params::RA_MAX_JERK>) _param_ra_max_jerk,
(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
};
Loading

0 comments on commit 65187f6

Please sign in to comment.