From b727ce86a09ca78e63e21629ef1a46e0dc84de8a Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 29 Apr 2025 15:11:41 +0200 Subject: [PATCH] rover: clean up speed terminology --- msg/RoverThrottleSetpoint.msg | 1 - msg/RoverVelocityStatus.msg | 8 ++-- .../AckermannPosControl.cpp | 48 +++++++++---------- .../AckermannVelControl.cpp | 37 +++++++------- .../AckermannVelControl.hpp | 3 +- .../DifferentialPosControl.cpp | 43 +++++++++-------- .../DifferentialPosControl.hpp | 4 +- .../DifferentialVelControl.cpp | 36 +++++++------- .../DifferentialVelControl.hpp | 3 +- 9 files changed, 92 insertions(+), 91 deletions(-) diff --git a/msg/RoverThrottleSetpoint.msg b/msg/RoverThrottleSetpoint.msg index 9924f35585..0452303ec7 100644 --- a/msg/RoverThrottleSetpoint.msg +++ b/msg/RoverThrottleSetpoint.msg @@ -2,5 +2,4 @@ uint64 timestamp # time since system start (microseconds) float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] (Positiv = forwards, Negativ = backwards) - 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 490c2c4a62..5de9409f7a 100644 --- a/msg/RoverVelocityStatus.msg +++ b/msg/RoverVelocityStatus.msg @@ -1,10 +1,8 @@ uint64 timestamp # time since system start (microseconds) float32 measured_speed_body_x # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 speed_body_x_setpoint # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards -float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) -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 +float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left (Mecanum only) +float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Mecanum only) +float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction (Mecanum only) diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 65c7ca3079..64ec81d94b 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -158,21 +158,21 @@ void AckermannPosControl::manualPositionMode() manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - 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 speed_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_delta) > FLT_EPSILON - || fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + || fabsf(speed_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.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = speed_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) @@ -185,19 +185,19 @@ void AckermannPosControl::manualPositionMode() // 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) * + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_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, _pos_ctl_start_position_ned, - _curr_pos_ned, fabsf(speed_body_x_setpoint)); + _curr_pos_ned, fabsf(speed_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 = speed_body_x_setpoint < -FLT_EPSILON; + ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); + ackermann_velocity_setpoint.backwards = speed_setpoint < -FLT_EPSILON; _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); } } @@ -232,20 +232,20 @@ void AckermannPosControl::autoPositionMode() _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); } else { // Regular guidance algorithm - 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()); + const float speed_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_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.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); ackermann_velocity_setpoint.backwards = false; _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); @@ -360,22 +360,22 @@ void AckermannPosControl::goToPositionMode() 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); + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ? _rover_position_setpoint.cruising_speed : _param_ro_speed_limit.get(); - const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed); + speed_setpoint = math::min(speed_setpoint, max_speed); 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)); + _curr_pos_ned, fabsf(speed_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.velocity_ned[0] = fabsf(speed_setpoint) * cosf(yaw_setpoint); + ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_setpoint) * sinf(yaw_setpoint); ackermann_velocity_setpoint.backwards = false; _ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint); diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp index cfa6cfbd61..46b86fbf96 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp @@ -80,11 +80,11 @@ void AckermannVelControl::updateVelControl() // 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.measured_speed_body_x = _vehicle_speed; 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.measured_speed_body_y = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; rover_velocity_status.pid_throttle_body_y_integral = NAN; _rover_velocity_status_pub.publish(rover_velocity_status); } @@ -106,10 +106,10 @@ void AckermannVelControl::updateSubscriptions() 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; + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } } @@ -143,9 +143,16 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint() _ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint); } + const Vector2f velocity_ned = Vector2f(_ackermann_velocity_setpoint.velocity_ned[0], + _ackermann_velocity_setpoint.velocity_ned[1]); + + // Santitize input + if (!velocity_ned.isAllFinite()) { + return; + } + // Attitude Setpoint - if (fabsf(_ackermann_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON - && fabsf(_ackermann_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) { + if (velocity_ned.norm() < FLT_EPSILON) { rover_attitude_setpoint_s rover_attitude_setpoint{}; rover_attitude_setpoint.timestamp = _timestamp; rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; @@ -154,23 +161,21 @@ void AckermannVelControl::generateAttitudeAndThrottleSetpoint() } 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]); + const float yaw_setpoint = atan2f(velocity_ned(1), 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; + const float speed_magnitude = math::min(velocity_ned.norm(), _param_ro_speed_limit.get()); + const float speed_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(), + speed_setpoint, _vehicle_speed, _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.throttle_body_y = NAN; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp index dd7cdde780..deac0edd62 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp @@ -126,8 +126,7 @@ private: // 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_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards float _vehicle_yaw{0.f}; float _dt{0.f}; bool _prev_param_check_passed{true}; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index 954068ab3f..446bdcb9d0 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -97,9 +97,10 @@ void DifferentialPosControl::updateSubscriptions() } _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - const 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; + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } } @@ -148,14 +149,14 @@ void DifferentialPosControl::manualPositionMode() manual_control_setpoint_s manual_control_setpoint{}; _manual_control_setpoint_sub.copy(&manual_control_setpoint); - 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 speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); const float bearing_scaling = math::min(_max_yaw_rate / _param_ro_yaw_p.get(), _param_rd_trans_drv_trn.get() - FLT_EPSILON); const float bearing_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling); - if (fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Turn on spot + if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot _course_control = false; const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); differential_velocity_setpoint_s differential_velocity_setpoint{}; @@ -169,7 +170,7 @@ void DifferentialPosControl::manualPositionMode() const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.speed = speed_setpoint; differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); @@ -183,18 +184,18 @@ void DifferentialPosControl::manualPositionMode() // 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) * + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) * vector_scaling * _pos_ctl_course_direction; pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; const float bearing_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)); + _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_body_x_setpoint; - differential_velocity_setpoint.bearing = speed_body_x_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( + differential_velocity_setpoint.speed = speed_setpoint; + differential_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( bearing_setpoint + M_PI_F); _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } @@ -238,18 +239,18 @@ void DifferentialPosControl::autoPositionMode() _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } else { - const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), - _param_rd_miss_spd_gain.get(), _curr_wp_type); + const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get(), _curr_wp_type); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; const float bearing_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_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.speed = speed_setpoint; differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); } @@ -289,21 +290,21 @@ void DifferentialPosControl::goToPositionMode() 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); + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ? _rover_position_setpoint.cruising_speed : _param_ro_speed_limit.get(); - const float speed_body_x_setpoint = math::min(speed_setpoint, max_speed); + speed_setpoint = math::min(speed_setpoint, max_speed); pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = _timestamp; const float bearing_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)); + _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); differential_velocity_setpoint_s differential_velocity_setpoint{}; differential_velocity_setpoint.timestamp = _timestamp; - differential_velocity_setpoint.speed = speed_body_x_setpoint; + differential_velocity_setpoint.speed = speed_setpoint; differential_velocity_setpoint.bearing = bearing_setpoint; _differential_velocity_setpoint_pub.publish(differential_velocity_setpoint); diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 51f6ccc855..05ac34c7f7 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -163,7 +163,7 @@ private: uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; uORB::Publication _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables hrt_abstime _timestamp{0}; @@ -171,7 +171,7 @@ 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{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; float _dt{0.f}; diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp index cf9376c70b..0ce9113bc0 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp @@ -76,14 +76,14 @@ void DifferentialVelControl::updateVelControl() _speed_setpoint.setForcedValue(0.f); } - // Publish position controller status (logging only) + // Publish velocity 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.measured_speed_body_x = _vehicle_speed; 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.measured_speed_body_y = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; rover_velocity_status.pid_throttle_body_y_integral = NAN; _rover_velocity_status_pub.publish(rover_velocity_status); } @@ -103,10 +103,10 @@ void DifferentialVelControl::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); - const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - const 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; + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } } @@ -155,31 +155,31 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() _current_state = DrivingState::DRIVING; } - float speed_body_x_setpoint = 0.f; + float speed_setpoint = 0.f; if (_current_state == DrivingState::DRIVING) { - speed_body_x_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(), - _param_ro_speed_limit.get()); + speed_setpoint = math::constrain(_differential_velocity_setpoint.speed, -_param_ro_speed_limit.get(), + _param_ro_speed_limit.get()); - const float speed_body_x_setpoint_normalized = math::interpolate(speed_body_x_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + const float speed_setpoint_normalized = math::interpolate(speed_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); if (_rover_steering_setpoint_sub.updated()) { _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); } - if (fabsf(speed_body_x_setpoint_normalized) > 1.f - fabsf( + if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( _rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - speed_body_x_setpoint = math::interpolate(sign(speed_body_x_setpoint_normalized) * (1.f - fabsf( - _rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f, - - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf( + _rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f, + - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); } } 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(), + speed_setpoint, _vehicle_speed, _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); diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp index fcdcfbba7e..42150a49a4 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp @@ -137,8 +137,7 @@ private: // 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_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards float _vehicle_yaw{0.f}; float _dt{0.f}; bool _prev_param_check_passed{false};