From 2fd4150b389b0bac3812d577be0db70b3360587f Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Mon, 16 Sep 2024 12:09:51 +0200 Subject: [PATCH] differential: Add stabilized and position mode (#23669) * differential: add position and stabilized mode * differential: add hardcoded stick input deadzones --- .../init.d-posix/airframes/4009_gz_r1_rover | 1 + .../airframes/50001_aion_robotics_r1_rover | 1 + src/lib/pure_pursuit/PurePursuit.cpp | 16 ++-- src/lib/pure_pursuit/PurePursuit.hpp | 6 +- .../rover_differential/RoverDifferential.cpp | 92 ++++++++++++++++++- .../rover_differential/RoverDifferential.hpp | 15 ++- .../RoverDifferentialGuidance.cpp | 2 +- .../RoverDifferentialGuidance.hpp | 1 + src/modules/rover_differential/module.yaml | 16 +++- 9 files changed, 136 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 4bf8815520af..b41d61355c35 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -21,6 +21,7 @@ param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_SPEED 5 param set-default RD_MAX_THR_SPD 7 param set-default RD_SPEED_P 1 param set-default RD_SPEED_I 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index d00063c573c3..901127f09bd2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -31,6 +31,7 @@ param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_SPEED 1.8 param set-default RD_MAX_THR_SPD 2 param set-default RD_SPEED_P 0.5 param set-default RD_SPEED_I 0.1 diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp index 324a504959ba..e28e7b0924a8 100644 --- a/src/lib/pure_pursuit/PurePursuit.cpp +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2 const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned; const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); - const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * - prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp - const Vector2f curr_pos_to_path = distance_on_line_segment - - prev_wp_to_curr_pos; // Shortest vector from the current position to the path + _distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u; + _curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos; - if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point - return atan2f(curr_pos_to_path(1), curr_pos_to_path(0)); + if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point + return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0)); } - const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), - 2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point - const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension * + const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(), + 2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point + const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension * prev_wp_to_curr_wp_u; const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos; return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0)); diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 9de498c2e1b1..5740d0184c7a 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -103,6 +103,8 @@ class PurePursuit : public ModuleParams float vehicle_speed); float getLookaheadDistance() {return _lookahead_distance;}; + float getCrosstrackError() {return _curr_pos_to_path.norm();}; + float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();}; protected: /** @@ -122,5 +124,7 @@ class PurePursuit : public ModuleParams float lookahead_min{1.f}; } _params{}; private: - float _lookahead_distance{0.f}; + float _lookahead_distance{0.f}; // Radius of the circle around the vehicle + Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp + Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path }; diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 411c5c4b5559..a7a9e82d0217 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -99,12 +99,94 @@ void RoverDifferential::Run() } break; + case vehicle_status_s::NAVIGATION_STATE_STAB: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_yaw_ctl) { + _stab_desired_yaw = _vehicle_yaw; + _yaw_ctl = true; + } + + rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + + if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _yaw_ctl = false; + + + } else { // Course control if the yaw rate input is zero (keep driving on a straight line) + if (!_yaw_ctl) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _yaw_ctl = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( + rover_differential_setpoint.forward_speed_setpoint) * + vector_scaling * _pos_ctl_course_direction; + // Calculate yaw setpoint + const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, + _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); + rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? + yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards + rover_differential_setpoint.yaw_rate_setpoint = NAN; + + } + + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); + } + + } break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); break; default: // Unimplemented nav states will stop the rover + _rover_differential_control.resetControllers(); + _yaw_ctl = false; rover_differential_setpoint_s rover_differential_setpoint{}; rover_differential_setpoint.forward_speed_setpoint = NAN; rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; @@ -115,8 +197,9 @@ void RoverDifferential::Run() break; } - if (!_armed) { // Reset on disarm + if (!_armed) { // Reset when disarmed _rover_differential_control.resetControllers(); + _yaw_ctl = false; } _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); @@ -135,6 +218,12 @@ void RoverDifferential::updateSubscriptions() if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status{}; _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state != _nav_state) { // Reset on mode change + _rover_differential_control.resetControllers(); + _yaw_ctl = false; + } + _nav_state = vehicle_status.nav_state; _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; } @@ -155,6 +244,7 @@ void RoverDifferential::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); + _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_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index eee464ede127..ad238aa428dc 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -39,6 +39,7 @@ #include #include #include +#include // uORB includes #include @@ -103,8 +104,10 @@ class RoverDifferential : public ModuleBase, public ModulePar // Instances RoverDifferentialGuidance _rover_differential_guidance{this}; RoverDifferentialControl _rover_differential_control{this}; + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library // Variables + Vector2f _curr_pos_ned{}; matrix::Quatf _vehicle_attitude_quaternion{}; float _vehicle_yaw_rate{0.f}; float _vehicle_forward_speed{0.f}; @@ -112,6 +115,10 @@ class RoverDifferential : public ModuleBase, public ModulePar float _max_yaw_rate{0.f}; int _nav_state{0}; bool _armed{false}; + bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode + float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode + Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode + Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode // Thresholds to avoid moving at rest due to measurement noise static constexpr float YAW_RATE_THRESHOLD = @@ -119,8 +126,14 @@ class RoverDifferential : public ModuleBase, public ModulePar static constexpr float SPEED_THRESHOLD = 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero + // Stick input deadzone + static constexpr float STICK_DEADZONE = + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value + DEFINE_PARAMETERS( (ParamFloat) _param_rd_man_yaw_scale, - (ParamFloat) _param_rd_max_yaw_rate + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index c56830ad08a2..848a7f513acf 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints() // Waypoint cruising speed if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = position_setpoint_triplet.current.cruising_speed; + _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); } else { _max_forward_speed = _param_rd_miss_spd_def.get(); diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index d2b9862b2cc0..4b3815247e59 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -143,6 +143,7 @@ class RoverDifferentialGuidance : public ModuleParams (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_max_speed, (ParamFloat) _param_rd_miss_spd_def, (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 8ef9f62f5e30..3bad343f2e8b 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -113,6 +113,19 @@ parameters: decimal: 2 default: 0.5 + RD_MAX_SPEED: + description: + short: Maximum speed setpoint + long: | + This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + RD_MAX_THR_SPD: description: short: Speed the rover drives at maximum throttle @@ -133,7 +146,8 @@ parameters: description: short: Maximum allowed yaw rate for the rover long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates + in Acro,Stabilized and Position mode. type: float unit: deg/s min: 0.01