From 9fe98b0724227c28076432ac07e8f0038cc81d77 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 31 Mar 2025 09:13:53 +0200 Subject: [PATCH] ackermann: seperate velocity control --- msg/AckermannVelocitySetpoint.msg | 4 + msg/CMakeLists.txt | 1 + msg/PurePursuitStatus.msg | 2 - msg/RoverAttitudeSetpoint.msg | 2 - msg/RoverAttitudeStatus.msg | 2 - msg/RoverRateSetpoint.msg | 2 - msg/RoverRateStatus.msg | 2 - msg/RoverSteeringSetpoint.msg | 5 +- msg/RoverThrottleSetpoint.msg | 5 +- msg/RoverVelocityStatus.msg | 2 - src/modules/logger/logged_topics.cpp | 2 + .../AckermannPosControl.cpp} | 282 ++++++++---------- .../AckermannPosControl.hpp} | 60 ++-- .../CMakeLists.txt | 8 +- .../AckermannVelControl.cpp | 198 ++++++++++++ .../AckermannVelControl.hpp | 149 +++++++++ .../AckermannVelControl/CMakeLists.txt | 39 +++ src/modules/rover_ackermann/CMakeLists.txt | 6 +- .../rover_ackermann/RoverAckermann.cpp | 7 +- .../rover_ackermann/RoverAckermann.hpp | 12 +- 20 files changed, 555 insertions(+), 235 deletions(-) create mode 100644 msg/AckermannVelocitySetpoint.msg rename src/modules/rover_ackermann/{AckermannPosVelControl/AckermannPosVelControl.cpp => AckermannPosControl/AckermannPosControl.cpp} (54%) rename src/modules/rover_ackermann/{AckermannPosVelControl/AckermannPosVelControl.hpp => AckermannPosControl/AckermannPosControl.hpp} (81%) rename src/modules/rover_ackermann/{AckermannPosVelControl => AckermannPosControl}/CMakeLists.txt (89%) create mode 100644 src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt diff --git a/msg/AckermannVelocitySetpoint.msg b/msg/AckermannVelocitySetpoint.msg new file mode 100644 index 0000000000..4567255782 --- /dev/null +++ b/msg/AckermannVelocitySetpoint.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32[2] velocity_ned # 2-dimensional velocity setpoint in NED frame [m/s] +bool backwards # Flag for backwards driving diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6c8e9e3ce3..938a3513d0 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW) include(px4_list_make_absolute) set(msg_files + AckermannVelocitySetpoint.msg ActionRequest.msg ActuatorArmed.msg ActuatorControlsStatus.msg diff --git a/msg/PurePursuitStatus.msg b/msg/PurePursuitStatus.msg index ef55854ed3..85c6fd8dfa 100644 --- a/msg/PurePursuitStatus.msg +++ b/msg/PurePursuitStatus.msg @@ -5,5 +5,3 @@ float32 target_bearing # [rad] Target bearing calculated by the pure pursu float32 crosstrack_error # [m] Shortest distance from the vehicle to the path (Positiv: Vehicle is on the right hand side with respect to the oriented path vector, Negativ: Left of the path) float32 distance_to_waypoint # [m] Distance from the vehicle to the current waypoint float32 bearing_to_waypoint # [rad] Bearing towards current waypoint - -# TOPICS pure_pursuit_status diff --git a/msg/RoverAttitudeSetpoint.msg b/msg/RoverAttitudeSetpoint.msg index 0ea9a7ea17..0a37838440 100644 --- a/msg/RoverAttitudeSetpoint.msg +++ b/msg/RoverAttitudeSetpoint.msg @@ -1,5 +1,3 @@ uint64 timestamp # time since system start (microseconds) float32 yaw_setpoint # [rad] Expressed in NED frame - -# TOPICS rover_attitude_setpoint diff --git a/msg/RoverAttitudeStatus.msg b/msg/RoverAttitudeStatus.msg index 197d31c2a7..bc750d0995 100644 --- a/msg/RoverAttitudeStatus.msg +++ b/msg/RoverAttitudeStatus.msg @@ -2,5 +2,3 @@ uint64 timestamp # time since system start (microseconds) float32 measured_yaw # [rad/s] Measured yaw rate float32 adjusted_yaw_setpoint # [rad/s] Yaw setpoint that is being tracked (Applied slew rates) - -# TOPICS rover_attitude_status diff --git a/msg/RoverRateSetpoint.msg b/msg/RoverRateSetpoint.msg index 88054f9dd6..ed96012b30 100644 --- a/msg/RoverRateSetpoint.msg +++ b/msg/RoverRateSetpoint.msg @@ -1,5 +1,3 @@ uint64 timestamp # time since system start (microseconds) float32 yaw_rate_setpoint # [rad/s] Expressed in NED frame - -# TOPICS rover_rate_setpoint diff --git a/msg/RoverRateStatus.msg b/msg/RoverRateStatus.msg index 4e82eb30fa..46a6fe9e4a 100644 --- a/msg/RoverRateStatus.msg +++ b/msg/RoverRateStatus.msg @@ -3,5 +3,3 @@ uint64 timestamp # time since system start (microseconds) float32 measured_yaw_rate # [rad/s] Measured yaw rate float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint that is being tracked (Applied slew rates) float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller - -# TOPICS rover_rate_status diff --git a/msg/RoverSteeringSetpoint.msg b/msg/RoverSteeringSetpoint.msg index 252215ee8a..4876d3e9b7 100644 --- a/msg/RoverSteeringSetpoint.msg +++ b/msg/RoverSteeringSetpoint.msg @@ -1,6 +1,5 @@ uint64 timestamp # time since system start (microseconds) -float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Only for Ackermann-steered rovers) -float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Only for Differential/Mecanum rovers) +float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Ackermann only, Positiv: Steer right, Negativ: Steer left) -# TOPICS rover_steering_setpoint +float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Differential/Mecanum only, Positiv = Turn right, Negativ: Turn left) diff --git a/msg/RoverThrottleSetpoint.msg b/msg/RoverThrottleSetpoint.msg index 3b6d7047b3..9924f35585 100644 --- a/msg/RoverThrottleSetpoint.msg +++ b/msg/RoverThrottleSetpoint.msg @@ -1,7 +1,6 @@ uint64 timestamp # time since system start (microseconds) -float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] -float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] +float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] (Positiv = forwards, Negativ = backwards) -# TOPICS rover_throttle_setpoint +float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] (Mecanum only, Positiv = right, Negativ = left) diff --git a/msg/RoverVelocityStatus.msg b/msg/RoverVelocityStatus.msg index f16de0f6b7..490c2c4a62 100644 --- a/msg/RoverVelocityStatus.msg +++ b/msg/RoverVelocityStatus.msg @@ -8,5 +8,3 @@ float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y directio float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) float32 pid_throttle_body_x_integral # Integral of the PID for the closed loop controller of the speed in body x direction float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction - -# TOPICS rover_velocity_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 7949b95509..8851cc55be 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -45,6 +45,7 @@ using namespace px4::logger; void LoggedTopics::add_default_topics() { + add_optional_topic("ackermann_velocity_setpoint", 100); add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300); @@ -107,6 +108,7 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_optional_topic("rover_attitude_setpoint", 100); add_optional_topic("rover_attitude_status", 100); + add_optional_topic("rover_position_setpoint", 100); add_optional_topic("rover_rate_setpoint", 100); add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp similarity index 54% rename from src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp rename to src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index f082f6d8bc..3e8b8cabbf 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -31,39 +31,29 @@ * ****************************************************************************/ -#include "AckermannPosVelControl.hpp" +#include "AckermannPosControl.hpp" using namespace time_literals; -AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent) +AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_rate_setpoint_pub.advertise(); - _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_status_pub.advertise(); _pure_pursuit_status_pub.advertise(); + _ackermann_velocity_setpoint_pub.advertise(); updateParams(); } -void AckermannPosVelControl::updateParams() +void AckermannPosControl::updateParams() { ModuleParams::updateParams(); - _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); - _pid_speed.setIntegralLimit(1.f); - _pid_speed.setOutputLimit(1.f); _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); - } - if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); } } -void AckermannPosVelControl::updatePosVelControl() +void AckermannPosControl::updatePosControl() { const hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); @@ -71,37 +61,14 @@ void AckermannPosVelControl::updatePosVelControl() updateSubscriptions(); - if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) - && _vehicle_control_mode.flag_armed && runSanityChecks()) { - generateAttitudeSetpoint(); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, - _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateVelocitySetpoint(); - } else { // Reset controller and slew rate when position control is not active - _pid_speed.resetIntegral(); - _speed_setpoint.setForcedValue(0.f); } - // Publish position controller status (logging only) - rover_velocity_status_s rover_velocity_status; - rover_velocity_status.timestamp = _timestamp; - rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; - rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; - rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); - rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; - rover_velocity_status.speed_body_y_setpoint = NAN; - rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; - rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); - rover_velocity_status.pid_throttle_body_y_integral = NAN; - _rover_velocity_status_pub.publish(rover_velocity_status); } -void AckermannPosVelControl::updateSubscriptions() +void AckermannPosControl::updateSubscriptions() { if (_vehicle_control_mode_sub.updated()) { _vehicle_control_mode_sub.copy(&_vehicle_control_mode); @@ -125,128 +92,81 @@ void AckermannPosVelControl::updateSubscriptions() } _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; - _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } } -void AckermannPosVelControl::generateAttitudeSetpoint() +void AckermannPosControl::generateVelocitySetpoint() { - if (_vehicle_control_mode.flag_control_manual_enabled - && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + // Manual Position Mode + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) { manualPositionMode(); - - } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Position Control - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - if (_offboard_control_mode.position) { - offboardPositionMode(); - - } else if (_offboard_control_mode.velocity) { - offboardVelocityMode(); - } - - } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode - autoPositionMode(); + return; } + + // Auto Mode + if (_vehicle_control_mode.flag_control_auto_enabled) { + autoPositionMode(); + return; + } + + // Offboard Position Mode + if (_offboard_control_mode_sub.copy(&_offboard_control_mode) && _offboard_control_mode.position) { + goToPositionMode(); + } + } -void AckermannPosVelControl::manualPositionMode() +void AckermannPosControl::manualPositionMode() { manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(), + _max_yaw_rate / _param_ro_yaw_p.get()); - if (fabsf(yaw_rate_setpoint) > FLT_EPSILON - || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _course_control = false; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_vehicle_speed_body_x) * yaw_rate_setpoint; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + if (fabsf(yaw_delta) > FLT_EPSILON + || fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta); + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); - } else { // Course control if the steering input is zero (keep driving on a straight line) - if (!_course_control) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _course_control = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; - const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * - vector_scaling * _pos_ctl_course_direction; - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, fabsf(_speed_body_x_setpoint)); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; } - } -} -void AckermannPosVelControl::offboardPositionMode() -{ - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - // Translate trajectory setpoint to rover setpoints - const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); - const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); - - if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { - const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, 0.f); - _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; + const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, - _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(speed_body_x_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - } else { - _speed_body_x_setpoint = 0.f; + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); } } -void AckermannPosVelControl::offboardVelocityMode() -{ - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - - if (velocity_in_local_frame.isAllFinite()) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - _speed_body_x_setpoint = velocity_in_local_frame.norm(); - } -} - -void AckermannPosVelControl::autoPositionMode() +void AckermannPosControl::autoPositionMode() { if (_position_setpoint_triplet_sub.updated()) { updateWaypointsAndAcceptanceRadius(); @@ -263,36 +183,40 @@ void AckermannPosVelControl::autoPositionMode() if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE - || !_next_wp_ned.isAllFinite()) { // Check stopping conditions + || !_next_wp_ned.isAllFinite()) { auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); } if (auto_stop) { - _speed_body_x_setpoint = 0.f; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = 0.f; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = 0.f; + ackermann_velocity_setpoint.velocity_ned[1] = 0.f; + ackermann_velocity_setpoint.backwards = false; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); } else { // Regular guidance algorithm - _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, - _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _curr_wp_type, - _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get()); + const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, + distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle, + _param_ro_speed_limit.get()); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - fabsf(_speed_body_x_setpoint)); + fabsf(speed_body_x_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = false; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); } } -void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() +void AckermannPosControl::updateWaypointsAndAcceptanceRadius() { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); @@ -320,7 +244,7 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); } -float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transition_angle, +float AckermannPosControl::updateAcceptanceRadius(const float waypoint_transition_angle, const float default_acceptance_radius, const float acceptance_radius_gain, const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) { @@ -345,7 +269,7 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi return acceptance_radius; } -float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, +float AckermannPosControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad, const float max_decel, const float max_jerk, const int curr_wp_type, const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) @@ -394,7 +318,41 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons } -bool AckermannPosVelControl::runSanityChecks() +void AckermannPosControl::goToPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + const float speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = _timestamp; + const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(speed_body_x_setpoint)); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = false; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); + + } else { + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = 0.f; + ackermann_velocity_setpoint.velocity_ned[1] = 0.f; + ackermann_velocity_setpoint.backwards = false; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); + } +} + +bool AckermannPosControl::runSanityChecks() { bool ret = true; @@ -412,12 +370,10 @@ bool AckermannPosVelControl::runSanityChecks() if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; + } - if (_prev_param_check_passed) { - events::send(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get()); - } - + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; } _prev_param_check_passed = ret; diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp similarity index 81% rename from src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp rename to src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index a04d372ba1..96385075bd 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -49,10 +49,7 @@ // uORB includes #include #include -#include -#include -#include -#include +#include #include #include #include @@ -69,20 +66,20 @@ using namespace matrix; /** * @brief Class for ackermann position control. */ -class AckermannPosVelControl : public ModuleParams +class AckermannPosControl : public ModuleParams { public: /** - * @brief Constructor for AckermannPosVelControl. + * @brief Constructor for AckermannPosControl. * @param parent The parent ModuleParams object. */ - AckermannPosVelControl(ModuleParams *parent); - ~AckermannPosVelControl() = default; + AckermannPosControl(ModuleParams *parent); + ~AckermannPosControl() = default; /** * @brief Update position controller. */ - void updatePosVelControl(); + void updatePosControl(); protected: /** @@ -97,32 +94,26 @@ private: void updateSubscriptions(); /** - * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or - * trajcetorySetpoint (Offboard position or velocity control) or - * positionSetpointTriplet (Auto Mode). + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or + * positionSetpointTriplet (Auto Mode) or roverPositionSetpoint. */ - void generateAttitudeSetpoint(); + void generateVelocitySetpoint(); /** - * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. */ void manualPositionMode(); /** - * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. - */ - void offboardPositionMode(); - - /** - * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. - */ - void offboardVelocityMode(); - - /** - * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. */ void autoPositionMode(); + /** + * @brief Generate and publish roverVelocitySetpoint from trajectorySetpoint. + */ + void goToPositionMode(); + /** * @brief Update global/NED waypoint coordinates and acceptance radius. */ @@ -182,12 +173,9 @@ private: offboard_control_mode_s _offboard_control_mode{}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables hrt_abstime _timestamp{0}; @@ -195,11 +183,8 @@ private: Vector2f _curr_pos_ned{}; Vector2f _pos_ctl_course_direction{}; Vector2f _pos_ctl_start_position_ned{}; - float _vehicle_speed_body_x{0.f}; - float _vehicle_speed_body_y{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; - float _speed_body_x_setpoint{0.f}; float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. float _dt{0.f}; int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; @@ -216,17 +201,11 @@ private: float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - // Controllers - PID _pid_speed; - SlewRate _speed_setpoint; - // Class Instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates DEFINE_PARAMETERS( (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_p, - (ParamFloat) _param_ro_speed_i, (ParamFloat) _param_ro_yaw_stick_dz, (ParamFloat) _param_ro_accel_limit, (ParamFloat) _param_ro_decel_limit, @@ -237,6 +216,7 @@ private: (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, (ParamFloat) _param_ra_acc_rad_max, (ParamFloat) _param_ra_acc_rad_gain, (ParamFloat) _param_ra_max_str_ang, diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannPosControl/CMakeLists.txt similarity index 89% rename from src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt rename to src/modules/rover_ackermann/AckermannPosControl/CMakeLists.txt index caa8fb9d5e..e5195b75dd 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/AckermannPosControl/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(AckermannPosVelControl - AckermannPosVelControl.cpp +px4_add_library(AckermannPosControl + AckermannPosControl.cpp ) -target_link_libraries(AckermannPosVelControl PUBLIC PID) -target_include_directories(AckermannPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(AckermannPosControl PUBLIC PID) +target_include_directories(AckermannPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp new file mode 100644 index 0000000000..cfa6cfbd61 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "AckermannVelControl.hpp" + +using namespace time_literals; + +AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_throttle_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + _ackermann_velocity_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + updateParams(); +} + +void AckermannVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } + +} + +void AckermannVelControl::updateVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control + generateVelocitySetpoint(); + } + + generateAttitudeAndThrottleSetpoint(); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void AckermannVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + +} + +void AckermannVelControl::generateVelocitySetpoint() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position; + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { + ackermann_velocity_setpoint_s ackermann_velocity_setpoint{}; + ackermann_velocity_setpoint.timestamp = _timestamp; + ackermann_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0); + ackermann_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1); + ackermann_velocity_setpoint.backwards = false; + _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); + } +} + +void AckermannVelControl::generateAttitudeAndThrottleSetpoint() +{ + if (_ackermann_velocity_setpoint_sub.updated()) { + _ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint); + } + + // Attitude Setpoint + if (fabsf(_ackermann_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON + && fabsf(_ackermann_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + const float yaw_setpoint = atan2f(_ackermann_velocity_setpoint.velocity_ned[1], + _ackermann_velocity_setpoint.velocity_ned[0]); + rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi( + yaw_setpoint + M_PI_F) : yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + // Throttle Setpoint + const float speed_magnitude = math::min(sqrtf(powf(_ackermann_velocity_setpoint.velocity_ned[0], + 2) + powf(_ackermann_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get()); + const float speed_body_x_setpoint = _ackermann_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude; + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + +} + +bool AckermannVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp new file mode 100644 index 0000000000..dd7cdde780 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann position control. + */ +class AckermannVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannVelControl. + * @param parent The parent ModuleParams object. + */ + AckermannVelControl(ModuleParams *parent); + ~AckermannVelControl() = default; + + /** + * @brief Update velocity controller. + */ + void updateVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in velocity controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint. + */ + void generateVelocitySetpoint(); + + /** + * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint + * from roverVelocitySetpoint. + */ + void generateAttitudeAndThrottleSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _ackermann_velocity_setpoint_sub{ORB_ID(ackermann_velocity_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)}; + ackermann_velocity_setpoint_s _ackermann_velocity_setpoint{}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th + ) +}; diff --git a/src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt new file mode 100644 index 0000000000..bcf45619c3 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(AckermannVelControl + AckermannVelControl.cpp +) + +target_link_libraries(AckermannVelControl PUBLIC PID) +target_include_directories(AckermannVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index a68bbf9e72..957f56d74b 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -33,7 +33,8 @@ add_subdirectory(AckermannRateControl) add_subdirectory(AckermannAttControl) -add_subdirectory(AckermannPosVelControl) +add_subdirectory(AckermannVelControl) +add_subdirectory(AckermannPosControl) px4_add_module( MODULE modules__rover_ackermann @@ -44,7 +45,8 @@ px4_add_module( DEPENDS AckermannRateControl AckermannAttControl - AckermannPosVelControl + AckermannVelControl + AckermannPosControl px4_work_queue rover_control pure_pursuit diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index c76bbf9ebb..c586741e25 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -73,7 +73,8 @@ void RoverAckermann::Run() _timestamp = hrt_absolute_time(); _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - _ackermann_pos_vel_control.updatePosVelControl(); + _ackermann_pos_control.updatePosControl(); + _ackermann_vel_control.updateVelControl(); _ackermann_att_control.updateAttControl(); _ackermann_rate_control.updateRateControl(); @@ -86,14 +87,14 @@ void RoverAckermann::Run() && !_vehicle_control_mode.flag_control_rates_enabled; if (full_manual_mode_enabled) { // Manual mode - generateSteeringSetpoint(); + generateSteeringAndThrottleSetpoint(); } generateActuatorSetpoint(); } -void RoverAckermann::generateSteeringSetpoint() +void RoverAckermann::generateSteeringAndThrottleSetpoint() { manual_control_setpoint_s manual_control_setpoint{}; diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 635abff3f8..1a1cd0c987 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -60,7 +60,8 @@ // Local includes #include "AckermannRateControl/AckermannRateControl.hpp" #include "AckermannAttControl/AckermannAttControl.hpp" -#include "AckermannPosVelControl/AckermannPosVelControl.hpp" +#include "AckermannVelControl/AckermannVelControl.hpp" +#include "AckermannPosControl/AckermannPosControl.hpp" class RoverAckermann : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -93,9 +94,9 @@ private: void Run() override; /** - * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). + * @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode). */ - void generateSteeringSetpoint(); + void generateSteeringAndThrottleSetpoint(); /** * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. @@ -122,8 +123,9 @@ private: // Class instances AckermannRateControl _ackermann_rate_control{this}; - AckermannAttControl _ackermann_att_control{this}; - AckermannPosVelControl _ackermann_pos_vel_control{this}; + AckermannAttControl _ackermann_att_control{this}; + AckermannVelControl _ackermann_vel_control{this}; + AckermannPosControl _ackermann_pos_control{this}; // Variables hrt_abstime _timestamp{0};