diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps index 0c77ab9aa6d..5273d6aec06 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps @@ -1,9 +1,6 @@ #!/bin/sh # Standard apps for a ackermann drive rover. -# Start the attitude and position estimator. -ekf2 start & - # Start rover ackermann drive controller. rover_ackermann start diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a3df406a9ca..65ec54d8b01 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -180,6 +180,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg + RoverAckermannStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg index 3c34b63c63f..69dec269420 100644 --- a/msg/RoverAckermannGuidanceStatus.msg +++ b/msg/RoverAckermannGuidanceStatus.msg @@ -1,10 +1,9 @@ uint64 timestamp # time since system start (microseconds) -float32 actual_speed # [m/s] Rover ground speed -float32 desired_speed # [m/s] Rover desired ground speed -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [deg] Heading error of the pure pursuit controller -float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions -float32 crosstrack_error # [m] Shortest distance from the vehicle to the path +float32 desired_speed # [m/s] Rover desired ground speed +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error # [deg] Heading error of the pure pursuit controller +float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions +float32 crosstrack_error # [m] Shortest distance from the vehicle to the path # TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg new file mode 100644 index 00000000000..bf0556b9453 --- /dev/null +++ b/msg/RoverAckermannStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 throttle_setpoint # [-1, 1] Normalized throttle setpoint +float32 steering_setpoint # [-1, 1] Normalized steering setpoint +float32 actual_speed # [m/s] Rover ground speed + +# TOPICS rover_ackermann_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 7dd0a43e940..fc187a954d0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -103,7 +103,8 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_topic("rover_ackermann_guidance_status", 100); + add_optional_topic("rover_ackermann_guidance_status", 100); + add_optional_topic("rover_ackermann_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 249a4748768..805e20b73f7 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_module( DEPENDS RoverAckermannGuidance px4_work_queue + SlewRate MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 2c6b8c30d47..e65eb11442e 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -40,6 +40,7 @@ RoverAckermann::RoverAckermann() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_ackermann_status_pub.advertise(); updateParams(); } @@ -52,6 +53,16 @@ bool RoverAckermann::init() 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()); + } + + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) / + _param_ra_max_steer_angle.get()); + } } void RoverAckermann::Run() @@ -59,6 +70,7 @@ void RoverAckermann::Run() if (should_exit()) { ScheduleClear(); exit_and_cleanup(); + return; } // uORB subscriber updates @@ -70,44 +82,98 @@ 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; } - // Navigation modes - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + _actual_speed = rover_velocity.norm(); + } - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _motor_setpoint.steering = manual_control_setpoint.roll; - _motor_setpoint.throttle = manual_control_setpoint.throttle; - } + // Timestamps + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - } break; + if (_armed) { + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _motor_setpoint = _ackermann_guidance.purePursuit(); - break; + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _motor_setpoint.steering = manual_control_setpoint.roll; + _motor_setpoint.throttle = manual_control_setpoint.throttle; + } - default: // Unimplemented nav states will stop the rover + } 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; + } + + // Sanitize actuator commands + if (!PX4_ISFINITE(_motor_setpoint.steering)) { + _motor_setpoint.steering = 0.f; + } + + if (!PX4_ISFINITE(_motor_setpoint.throttle)) { + _motor_setpoint.throttle = 0.f; + } + + // Acceleration slew rate + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON + && 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); + } + + // Steering slew rate + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.update(_motor_setpoint.steering, dt); + + } else { + _steering_with_rate_limit.setForcedValue(_motor_setpoint.steering); + } + + // Publish rover ackermann status (logging) + rover_ackermann_status_s rover_ackermann_status{}; + rover_ackermann_status.timestamp = _timestamp; + rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; + rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; + rover_ackermann_status.actual_speed = _actual_speed; + _rover_ackermann_status_pub.publish(rover_ackermann_status); + + // Publish to motor + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + // Publish to servo + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); + + } else { // Reset on disarm _motor_setpoint.steering = 0.f; _motor_setpoint.throttle = 0.f; - break; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); } - - hrt_abstime now = hrt_absolute_time(); - - // Publish to wheel motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = _motor_setpoint.throttle; - actuator_motors.timestamp = now; - _actuator_motors_pub.publish(actuator_motors); - - // Publish to servo - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = _motor_setpoint.steering; - actuator_servos.timestamp = now; - _actuator_servos_pub.publish(actuator_servos); } int RoverAckermann::task_spawn(int argc, char *argv[]) @@ -147,7 +213,7 @@ int RoverAckermann::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover state machine. +Rover ackermann module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_ackermann", "controller"); diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 101647dd4e2..cea9300a1b3 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -39,6 +39,7 @@ #include #include #include +#include // uORB includes #include @@ -49,6 +50,9 @@ #include #include #include +#include +#include + // Standard library includes #include @@ -89,10 +93,12 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; // Instances RoverAckermannGuidance _ackermann_guidance{this}; @@ -100,9 +106,18 @@ private: // Variables int _nav_state{0}; RoverAckermannGuidance::motor_setpoint _motor_setpoint; + hrt_abstime _timestamp{0}; + float _actual_speed{0.f}; + SlewRate _steering_with_rate_limit{0.f}; + SlewRate _throttle_with_accel_limit{0.f}; + bool _armed{false}; // Parameters DEFINE_PARAMETERS( - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_max_steer_angle, + (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_ra_max_accel, + (ParamFloat) _param_ra_max_steering_rate ) }; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index d718df0ad49..19cdf5cf1aa 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -40,6 +40,7 @@ using namespace time_literals; RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) { + _rover_ackermann_guidance_status_pub.advertise(); updateParams(); pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); } @@ -55,13 +56,14 @@ void RoverAckermannGuidance::updateParams() 1); // Output limit } -RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() +RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const int nav_state) { // Initializations float desired_speed{0.f}; float desired_steering{0.f}; float vehicle_yaw{0.f}; float actual_speed{0.f}; + bool mission_finished{false}; // uORB subscriber updates if (_vehicle_global_position_sub.updated()) { @@ -84,6 +86,12 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() 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(); } @@ -98,21 +106,58 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() if (_mission_result_sub.updated()) { mission_result_s mission_result{}; _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; + mission_finished = mission_result.finished; + } + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0), + _next_wp(1)) < _acceptance_radius) { // Return to launch + mission_finished = true; } // Guidance logic - if (!_mission_finished) { + if (!mission_finished) { // Calculate desired speed const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _prev_wp(0), _prev_wp(1)); + const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), + _curr_wp(1)); - if (distance_to_prev_wp <= _prev_acc_rad) { // Cornering speed - const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad; - 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 { // Default mission speed + } 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 { + desired_speed = _param_ra_miss_vel_def.get(); + } + } + + } else { desired_speed = _param_ra_miss_vel_def.get(); } @@ -124,9 +169,9 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() } // 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) { @@ -144,8 +189,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit() } // Publish ackermann controller status (logging) - _rover_ackermann_guidance_status.timestamp = now; - _rover_ackermann_guidance_status.actual_speed = actual_speed; + _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); @@ -165,7 +209,12 @@ void RoverAckermannGuidance::updateWaypoints() _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + if (position_setpoint_triplet.current.valid) { + _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + _curr_wp = Vector2d(0, 0); + } if (position_setpoint_triplet.previous.valid) { _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); @@ -178,7 +227,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 @@ -187,9 +236,15 @@ void RoverAckermannGuidance::updateWaypoints() _next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1)); // Update acceptance radius - _prev_acc_rad = _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()); + _prev_acceptance_radius = _acceptance_radius; + + 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, diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index f6446cdacb8..a618f830e59 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -43,10 +43,11 @@ #include #include #include -#include #include +#include #include #include +#include // Standard library includes #include @@ -78,8 +79,9 @@ public: /** * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. + * @param nav_state Vehicle navigation state */ - motor_setpoint purePursuit(); + motor_setpoint purePursuit(const int nav_state); /** * @brief Update global/local waypoint coordinates and acceptance radius @@ -143,6 +145,7 @@ private: 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_pub{ORB_ID(rover_ackermann_guidance_status)}; @@ -156,18 +159,18 @@ private: 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{}; float _acceptance_radius{0.5f}; - float _prev_acc_rad{0.f}; - bool _mission_finished{false}; + float _prev_acceptance_radius{0.5f}; // Parameters DEFINE_PARAMETERS( @@ -177,7 +180,6 @@ private: (ParamFloat) _param_ra_lookahd_max, (ParamFloat) _param_ra_lookahd_min, (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_def, (ParamFloat) _param_ra_acc_rad_gain, (ParamFloat) _param_ra_miss_vel_def, (ParamFloat) _param_ra_miss_vel_min, @@ -185,6 +187,8 @@ private: (ParamFloat) _param_ra_p_speed, (ParamFloat) _param_ra_i_speed, (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_ra_max_jerk, + (ParamFloat) _param_ra_max_accel, (ParamFloat) _param_nav_acc_rad ) }; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index a3152b9e77b..d8cb8883167 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -66,31 +66,20 @@ parameters: RA_ACC_RAD_MAX: description: - short: Maximum acceptance radius + short: Maximum acceptance radius for the waypoints long: | The controller scales the acceptance radius based on the angle between - the previous, current and next waypoint. Used as tuning parameter. + the previous, current and next waypoint. Higher value -> smoother trajectory at the cost of how close the rover gets - to the waypoint (Set equal to RA_ACC_RAD_DEF to disable corner cutting). + to the waypoint (Set to -1 to disable corner cutting). type: float unit: m - min: 0.1 + min: -1 max: 100 increment: 0.01 decimal: 2 default: 3 - RA_ACC_RAD_DEF: - description: - short: Default acceptance radius - type: float - unit: m - min: 0.1 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - RA_ACC_RAD_GAIN: description: short: Tuning parameter for corner cutting @@ -110,21 +99,21 @@ parameters: short: Default rover velocity during a mission type: float unit: m/s - min: 0.1 + min: 0 max: 100 increment: 0.01 decimal: 2 - default: 3 + default: 2 RA_MISS_VEL_MIN: description: short: Minimum rover velocity during a mission long: | The velocity off the rover is reduced based on the corner it has to take - to smooth the trajectory (To disable this feature set it equal to RA_MISSION_VEL_DEF) + to smooth the trajectory (Set to -1 to disable) type: float unit: m/s - min: 0.1 + min: -1 max: 100 increment: 0.01 decimal: 2 @@ -133,9 +122,12 @@ parameters: RA_MISS_VEL_GAIN: description: short: Tuning parameter for the velocity reduction during cornering - long: Lower value -> More velocity reduction during cornering + long: | + The cornering speed is equal to the inverse of the acceptance radius + of the WP multiplied with this factor. + Lower value -> More velocity reduction during cornering. type: float - min: 0.1 + min: 0.05 max: 100 increment: 0.01 decimal: 2 @@ -175,3 +167,46 @@ parameters: increment: 0.01 decimal: 2 default: -1 + + RA_MAX_ACCEL: + description: + short: Maximum acceleration for the rover + long: | + This is used for the acceleration slew rate, the feed-forward term + for the speed controller during missions and the corner slow down effect. + Note: For the corner slow down effect RA_MAX_JERK, RA_MISS_VEL_GAIN and + RA_MISS_VEL_MIN also have to be set. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RA_MAX_JERK: + description: + short: Maximum jerk + long: | + Limit for forwards acc/deceleration change. + This is used for the corner slow down effect. + Note: RA_MAX_ACCEL, RA_MISS_VEL_GAIN and RA_MISS_VEL_MIN also have to be set + for this to be enabled. + type: float + unit: m/s^3 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RA_MAX_STR_RATE: + description: + short: Maximum steering rate for the rover + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1