diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 0eafcaa9f8..05b93a1328 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -215,8 +215,7 @@ float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate } void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, - position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, - MapProjection &global_ned_proj_ref) + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, MapProjection &global_ned_proj_ref) { if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { @@ -241,8 +240,7 @@ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); } else { - next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN, - NAN); // Enables corner slow down with RTL + next_wp_ned = Vector2f(NAN, NAN); } } diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 1196e04650..125a8a382a 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -118,11 +118,10 @@ float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate * @param next_wp_ned Next waypoint in NED frame (Updated by this function) * @param position_setpoint_triplet Position Setpoint Triplet * @param curr_pos Current position of the rover in global frame - * @param home_pos Home position in global frame * @param global_ned_proj_ref Global to ned projection */ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, - position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, MapProjection &global_ned_proj_ref); /** diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp index b5f2e8b96e..f082f6d8bc 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp @@ -131,12 +131,6 @@ void AckermannPosVelControl::updateSubscriptions() _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - } - } void AckermannPosVelControl::generateAttitudeSetpoint() @@ -254,7 +248,9 @@ void AckermannPosVelControl::offboardVelocityMode() void AckermannPosVelControl::autoPositionMode() { - updateAutoSubscriptions(); + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } // Distances to waypoints const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), @@ -262,17 +258,25 @@ void AckermannPosVelControl::autoPositionMode() const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + // Check stopping conditions + bool auto_stop{false}; + + 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 + auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); } - if (_mission_finished) { + 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); } 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(), _nav_state, + _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; @@ -284,25 +288,7 @@ void AckermannPosVelControl::autoPositionMode() rover_attitude_setpoint.timestamp = _timestamp; rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } -} -void AckermannPosVelControl::updateAutoSubscriptions() -{ - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypointsAndAcceptanceRadius(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; } } @@ -310,9 +296,10 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + _curr_wp_type = position_setpoint_triplet.current.type; RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, - _curr_pos_ned, _home_position, _global_ned_proj_ref); + _curr_pos_ned, _global_ned_proj_ref); _prev_waypoint_transition_angle = _waypoint_transition_angle; _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); @@ -339,13 +326,16 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi { // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment float acceptance_radius = default_acceptance_radius; - const float theta = waypoint_transition_angle / 2.f; - const float min_turning_radius = wheel_base / sinf(max_steer_angle); - const float acceptance_radius_temp = min_turning_radius / tanf(theta); - const float acceptance_radius_temp_scaled = acceptance_radius_gain * - acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects - acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, - acceptance_radius_max); + + if (PX4_ISFINITE(_waypoint_transition_angle)) { + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + } // Publish updated acceptance radius position_controller_status_s pos_ctrl_status{}; @@ -357,7 +347,7 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi float AckermannPosVelControl::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 nav_state, + 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) { // Catch improper values @@ -365,13 +355,24 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons return cruising_speed; } + // Upcoming stop + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + return math::min(straight_line_speed, cruising_speed); + } + // Cornering slow down effect - if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) { const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); const float cornering_speed = _max_yaw_rate * turning_circle; return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + } + + if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) { const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); const float cornering_speed = _max_yaw_rate * turning_circle; return math::constrain(cornering_speed, miss_speed_min, cruising_speed); @@ -380,26 +381,17 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons // Straight line speed if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { - float straight_line_speed{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - - } else { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - float cornering_speed = _max_yaw_rate * turning_circle; - cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp - acc_rad, cornering_speed); - } - + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + float cornering_speed = _max_yaw_rate * turning_circle; + cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); + const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp - acc_rad, cornering_speed); return math::min(straight_line_speed, cruising_speed); - } else { - return cruising_speed; } + return cruising_speed; // Fallthrough + } bool AckermannPosVelControl::runSanityChecks() diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp index d731b897b1..a04d372ba1 100644 --- a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp @@ -58,12 +58,10 @@ #include #include #include +#include #include #include -#include #include -#include -#include #include using namespace matrix; @@ -125,11 +123,6 @@ private: */ void autoPositionMode(); - /** - * @brief Update uORB subscriptions used for auto modes. - */ - void updateAutoSubscriptions(); - /** * @brief Update global/NED waypoint coordinates and acceptance radius. */ @@ -161,14 +154,14 @@ private: * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. * @param max_decel Maximum allowed deceleration [m/s^2]. * @param max_jerk Maximum allowed jerk [m/s^3]. - * @param nav_state Current nav_state of the rover. + * @param curr_wp_type Type of the current waypoint. * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] * @param max_speed Maximum speed setpoint [m/s] * @return Speed setpoint [m/s]. */ float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int curr_wp_type, float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); /** @@ -185,10 +178,6 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; @@ -213,13 +202,11 @@ private: 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 _nav_state{0}; + int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. - bool _mission_finished{false}; bool _prev_param_check_passed{true}; // Waypoint variables - Vector2d _home_position{}; Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp index 98a715b7b9..6554c52cc7 100644 --- a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -145,12 +145,6 @@ void DifferentialPosVelControl::updateSubscriptions() _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - } - } void DifferentialPosVelControl::generateAttitudeSetpoint() @@ -268,14 +262,32 @@ void DifferentialPosVelControl::offboardVelocityMode() void DifferentialPosVelControl::autoPositionMode() { - updateAutoSubscriptions(); + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + _curr_wp_type = position_setpoint_triplet.current.type; + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + } // Distances to waypoints const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + // Check stopping conditions + bool auto_stop{false}; + + 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 + auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); } // State machine @@ -287,7 +299,7 @@ void DifferentialPosVelControl::autoPositionMode() _pure_pursuit_status_pub.publish(pure_pursuit_status); const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + if (!auto_stop) { if (_currentState == GuidanceState::STOPPED) { _currentState = GuidanceState::DRIVING; } @@ -309,80 +321,66 @@ void DifferentialPosVelControl::autoPositionMode() // Calculate desired speed in body x direction _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()); + _param_rd_miss_spd_gain.get(), _curr_wp_type); + 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); } break; - case GuidanceState::SPOT_TURNING: - if (fabsf(_vehicle_speed_body_x) > 0.f) { - yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + case GuidanceState::SPOT_TURNING: { + _speed_body_x_setpoint = 0.f; - } + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop - _speed_body_x_setpoint = 0.f; - break; + } + + 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); + } break; case GuidanceState::STOPPED: default: - yaw_setpoint = _vehicle_yaw; _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); break; } - 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); -} -void DifferentialPosVelControl::updateAutoSubscriptions() -{ - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - if (_position_setpoint_triplet_sub.updated()) { - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, - _curr_pos_ned, _home_position, _global_ned_proj_ref); - - _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); - - // Waypoint cruising speed - _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } } float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, - const float trans_drv_trn, const float miss_spd_gain) + const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type) { - float speed_body_x_setpoint = cruising_speed; - - if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { - speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); - - } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON - && miss_spd_gain > FLT_EPSILON) { - const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, - 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, - max_speed * (1.f - speed_reduction)); + // Upcoming stop + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) + || _waypoint_transition_angle < M_PI_F - trans_drv_trn || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + return math::min(straight_line_speed, cruising_speed); } - return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + // Straight line speed + if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, + distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + return math::min(straight_line_speed, cruising_speed); + } + + return cruising_speed; // Fallthrough } diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp index 58f57b1c07..c534608646 100644 --- a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -59,12 +59,9 @@ #include #include #include +#include #include #include -#include -#include -#include -#include using namespace matrix; @@ -134,11 +131,6 @@ private: */ void autoPositionMode(); - /** - * @brief Update uORB subscriptions used for auto modes. - */ - void updateAutoSubscriptions(); - /** * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). @@ -152,10 +144,11 @@ private: * @param max_speed Maximum speed setpoint [m/s] * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @param curr_wp_type Type of the current waypoint. * @return Speed setpoint [m/s]. */ float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, - float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain, int curr_wp_type); /** @@ -172,10 +165,6 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; @@ -186,7 +175,6 @@ private: 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)}; // Variables @@ -201,12 +189,10 @@ private: float _max_yaw_rate{0.f}; float _speed_body_x_setpoint{0.f}; float _dt{0.f}; - int _nav_state{0}; + int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. - bool _mission_finished{false}; // Waypoint variables - Vector2d _home_position{}; Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp index 2ad95e8bcf..5139ef8e20 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp @@ -165,12 +165,6 @@ void MecanumPosVelControl::updateSubscriptions() _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - } - } void MecanumPosVelControl::generateAttitudeSetpoint() @@ -313,48 +307,13 @@ void MecanumPosVelControl::offboardVelocityMode() void MecanumPosVelControl::autoPositionMode() { - updateAutoSubscriptions(); - - const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), - 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - - if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), - _nav_state); - 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, - velocity_magnitude); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); - Vector2f desired_velocity(0.f, 0.f); - _speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame); - _speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _auto_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); -} - -void MecanumPosVelControl::updateAutoSubscriptions() -{ - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - if (_position_setpoint_triplet_sub.updated()) { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + _curr_wp_type = position_setpoint_triplet.current.type; RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, - _curr_pos_ned, _home_position, _global_ned_proj_ref); + _curr_pos_ned, _global_ned_proj_ref); _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); @@ -371,38 +330,72 @@ void MecanumPosVelControl::updateAutoSubscriptions() } } - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + // Check stopping conditions + bool auto_stop{false}; + + 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 + auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + if (auto_stop) { + _speed_body_x_setpoint = 0.f; + _speed_body_y_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); + + } else { // Regular guidance algorithm + const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_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, + velocity_magnitude); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + Vector2f desired_velocity(0.f, 0.f); + _speed_body_x_setpoint = velocity_magnitude * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = velocity_magnitude * sinf(bearing_setpoint_body_frame); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _auto_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } } float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp, const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, - const float miss_spd_gain, const int nav_state) + const float miss_spd_gain, const int curr_wp_type) { - float velocity_magnitude{auto_speed}; - - if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON - && miss_spd_gain > FLT_EPSILON) { - float max_velocity_magnitude = velocity_magnitude; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - - } else if (PX4_ISFINITE(waypoint_transition_angle)) { - const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, - max_speed * (1.f - speed_reduction)); - } - - velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + // Upcoming stop + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); } - return velocity_magnitude; + // Straight line speed + if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, + distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + + return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + } + + return auto_speed; // Fallthrough } bool MecanumPosVelControl::runSanityChecks() diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp index 089bc79b1a..c4339798dc 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp @@ -59,12 +59,10 @@ #include #include #include +#include #include #include -#include -#include -#include -#include +#include using namespace matrix; @@ -125,11 +123,6 @@ private: */ void autoPositionMode(); - /** - * @brief Update uORB subscriptions used for auto modes. - */ - void updateAutoSubscriptions(); - /** * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). @@ -142,11 +135,11 @@ private: * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] * @param max_speed Maximum velocity magnitude setpoint [m/s] * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. - * @param nav_state Vehicle navigation state + * @param curr_wp_type Type of the current waypoint. * @return Velocity magnitude setpoint [m/s]. */ float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk, - float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state); + float waypoint_transition_angle, float max_speed, float miss_spd_gain, int curr_wp_type); /** * @brief Check if the necessary parameters are set. @@ -162,10 +155,6 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; @@ -176,19 +165,14 @@ private: 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)}; // Variables hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; - Vector2d _home_position{}; Vector2f _curr_pos_ned{}; Vector2f _pos_ctl_course_direction{}; Vector2f _pos_ctl_start_position_ned{}; - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; float _vehicle_speed_body_x{0.f}; float _vehicle_speed_body_y{0.f}; float _vehicle_yaw{0.f}; @@ -199,11 +183,15 @@ private: float _dt{0.f}; float _auto_speed{0.f}; float _auto_yaw{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - int _nav_state{0}; - bool _mission_finished{false}; + int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; bool _prev_param_check_passed{true}; + // Waypoint variables + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + // Controllers PID _pid_speed_x; PID _pid_speed_y;