diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d9e32a807835..8378577c82f3 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -104,7 +104,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("px4io_status"); add_topic("radio_status"); add_topic("rover_ackermann_guidance_status", 100); - add_topic("rover_ackermann_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/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index a7d8d3b5bf33..5686ba875601 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -53,12 +53,13 @@ void RoverAckermann::updateParams() { ModuleParams::updateParams(); - if (_param_ra_max_accel.get() > 0 && _param_ra_max_speed.get() > 0) { + 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() > 0 && _param_ra_max_steer_angle.get() > 0) { - _steering_with_rate_limit.setSlewRate(_param_ra_max_steering_rate.get() / _param_ra_max_steer_angle.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()); } } @@ -111,22 +112,24 @@ void RoverAckermann::Run() break; } - // Slew rates + // Timestamps hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); const float dt = (_timestamp - timestamp_prev) * 10e-6f; - if (_param_ra_max_accel.get() > 0 && _param_ra_max_speed.get() > 0 + // 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())) { // Acceleration slew rate + && 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); } - if (_param_ra_max_steering_rate.get() > 0 && _param_ra_max_steer_angle.get() > 0 - && _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO) { // Steering slew rate + // 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 { @@ -141,7 +144,7 @@ void RoverAckermann::Run() rover_ackermann_status.actual_speed = _actual_speed; _rover_ackermann_status_pub.publish(rover_ackermann_status); - // Publish to wheel motors + // Publish to motor actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); actuator_motors.control[0] = _throttle_with_accel_limit.getState(); diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index f050a2a3c026..47fa560d5343 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -102,7 +102,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const mission_finished = mission_result.finished; } - if (nav_state == 5 + 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 mission_finished = true; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index c7277ccedc47..12d19b1f5baa 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -56,6 +56,7 @@ #include using namespace matrix; +static constexpr uint8_t NAVIGATION_STATE_AUTO_RTL = 5; /** * @brief Class for ackermann drive guidance. diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index da742d58ad86..479977552a8f 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -133,7 +133,9 @@ 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 max: 100 @@ -143,12 +145,12 @@ parameters: RA_MISS_VEL_RED: description: - short: Scaling factor on the distance to the WP at which the cornering slow down effect starts + short: Tuning parameter for the start of the velocity reduction during cornering long: | - This is a multiplicative factor on the acceptance radius of the waypoints. - A value higher than one means that the rover will start to slow down before it reaches the - acceptance radius of the waypoint. This means the rover could already reach the reduced velocity - before it enters the corner, which can help reduce the risk of the rover rolling over. + The rover will start slowing down at a distance of acceptance_radius * RA_MISS_VEL_RED + to the WP s.t. it could reach the reduced velocity before it enters the corner. + This can help to reduce the risk of the rover rolling over during corners. + Higher value -> Rover starts the velocity reduction further from the waypoint. type: float min: 1.0 max: 100 @@ -206,9 +208,9 @@ parameters: description: short: Maximum steering rate for the rover type: float - unit: rad/s + unit: deg/s min: -1 - max: 100 + max: 360 increment: 0.01 decimal: 2 default: -1