diff --git a/src/modules/boat/Boat.cpp b/src/modules/boat/Boat.cpp index 2882bd67ad..e931d4ac6e 100644 --- a/src/modules/boat/Boat.cpp +++ b/src/modules/boat/Boat.cpp @@ -50,17 +50,13 @@ void Boat::updateParams() { ModuleParams::updateParams(); - // _boat_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - _max_speed = _param_bt_spd_max.get(); _boat_guidance.setMaxSpeed(_max_speed); _boat_kinematics.setMaxSpeed(_max_speed); - printf("max speed: %f\n", (double)_max_speed); _max_angular_velocity = _param_bt_ang_max.get(); _boat_guidance.setMaxAngularVelocity(_max_angular_velocity); _boat_kinematics.setMaxAngularVelocity(_max_angular_velocity); - printf("max angular velocity: %f\n", (double)_max_angular_velocity); } void Boat::Run() @@ -102,7 +98,7 @@ void Boat::Run() if (_manual_driving) { // Manual mode - // directly produce setpoints from the manual control setpoint (joystick) + // Directly produce setpoints from the manual control setpoint (joystick) if (_manual_control_setpoint_sub.updated()) { manual_control_setpoint_s manual_control_setpoint{}; @@ -129,13 +125,13 @@ void Boat::Run() } else if (_mission_driving) { // Mission mode - // directly receive setpoints from the guidance library - printf("I'm in mission mode\n"); + // Directly receive setpoints from the guidance library _boat_guidance.computeGuidance( _boat_control.getVehicleYaw(), _boat_control.getLocalPosition(), dt ); + // _boat_guidance_pp.purePursuit(); } _boat_control.control(dt); diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.cpp b/src/modules/boat/BoatGuidance/BoatGuidance.cpp index cbd7fbdbdf..476c9be2a7 100644 --- a/src/modules/boat/BoatGuidance/BoatGuidance.cpp +++ b/src/modules/boat/BoatGuidance/BoatGuidance.cpp @@ -67,72 +67,88 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l vehicle_local_position.timestamp); } - const Vector2f current_waypoint_local_position = _global_local_proj_ref.project(current_waypoint(0), + const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(current_waypoint(0), current_waypoint(1)); - const Vector2f previous_waypoint_local_position = _global_local_proj_ref.project(previous_waypoint(0), + const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(previous_waypoint(0), previous_waypoint(1)); - const Vector2f local_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - const Vector2f local_velocity = Vector2f(vehicle_local_position.vx, vehicle_local_position.vy); + const Vector2f local_frame_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - const float distance_to_next_wp = get_distance_to_next_waypoint(local_position(0), local_position(1), - current_waypoint_local_position(0), - current_waypoint_local_position(1)); + const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + current_waypoint(0), current_waypoint(1)); - float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), next_waypoint(0), - next_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); + float heading_error = 0.f; + float speed_interpolation = 0.f; + float heading_to_next_waypoint = 0.f; + float heading_error_to_next_waypoint = 0.f; + float desired_speed = _param_bt_spd_cruise.get(); + // Go back to driving, when the waypoint has been reached if (_current_waypoint != current_waypoint) { _currentState = GuidanceState::DRIVING; } - // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. + // Make boat stop when it arrives at the last waypoint if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { _currentState = GuidanceState::GOAL_REACHED; } - float speed_interpolation = 0.f; - - if (PX4_ISFINITE(heading_error)) { - speed_interpolation = math::interpolate(abs(heading_error), _param_bt_min_heading_error.get() * M_PI_F / 180.f, - _param_bt_max_heading_error.get() * M_PI_F / 180.f, _param_bt_spd_cruise.get(), - _param_bt_spd_min.get()); - } - - float desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed); - switch (_currentState) { case GuidanceState::DRIVING: { if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) { - _l1_guidance.navigate_waypoints(previous_waypoint_local_position, current_waypoint_local_position, local_position, - local_velocity); + + float look_ahead_distance = getLookAheadDistance(current_waypoint_local_frame, previous_waypoint_local_frame, + local_frame_position); + float desired_heading = calcDesiredHeading(current_waypoint_local_frame, previous_waypoint_local_frame, + local_frame_position, look_ahead_distance); + + heading_error = matrix::wrap_pi(desired_heading - yaw); + _desired_angular_velocity = heading_error; + + heading_to_next_waypoint = get_bearing_to_next_waypoint(previous_waypoint(0), previous_waypoint(1), current_waypoint(0), + current_waypoint(1)); } else { - _previous_local_position = local_position; + _previous_local_position = local_frame_position; + _previous_position = global_position; _currentState = GuidanceState::DRIVING_TO_POINT; } break; } - case GuidanceState::DRIVING_TO_POINT: + // Control logic if there is no previous waypoint + case GuidanceState::DRIVING_TO_POINT: { - _l1_guidance.navigate_waypoints(_previous_local_position, current_waypoint_local_position, local_position, - local_velocity); - desired_speed = _param_bt_spd_cruise.get(); - break; + float look_ahead_distance = getLookAheadDistance(current_waypoint_local_frame, _previous_local_position, + local_frame_position); + float desired_heading = calcDesiredHeading(current_waypoint_local_frame, _previous_local_position, local_frame_position, + look_ahead_distance); + heading_error = matrix::wrap_pi(desired_heading - yaw); + _desired_angular_velocity = heading_error; + heading_to_next_waypoint = get_bearing_to_next_waypoint(_previous_position(0), _previous_position(1), + current_waypoint(0), + current_waypoint(1)); + break; + } case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle desired_speed = 0.f; heading_error = 0.f; _desired_angular_velocity = 0.f; break; } - _desired_angular_velocity = math::constrain(_l1_guidance.nav_lateral_acceleration_demand(), -_max_angular_velocity, - _max_angular_velocity); + heading_error_to_next_waypoint = matrix::wrap_pi(heading_to_next_waypoint - yaw); + + // Interpolate the speed based on the heading error + if (PX4_ISFINITE(heading_error_to_next_waypoint) && desired_speed > 0.1f) { + speed_interpolation = math::interpolate(abs(heading_error_to_next_waypoint), + _param_bt_min_heading_error.get() * M_PI_F / 180.f, + _param_bt_max_heading_error.get() * M_PI_F / 180.f, _param_bt_spd_cruise.get(), + _param_bt_spd_min.get()); + desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed); + } boat_setpoint_s output{}; output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); @@ -145,10 +161,73 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l _current_waypoint = current_waypoint; } +float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &curr_pos_local) +{ + // Calculate crosstrack error + const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local; + + if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen) + return 0.f; + } + + const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local; + const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) / + prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized(); + const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local; + + if (crosstrack_error.length() < _param_look_ahead_distance.get()) { + return _param_look_ahead_distance.get(); + + } else { + return crosstrack_error.length(); + } +} + +float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + Vector2f const &curr_pos_local, + float const &lookahead_distance) +{ + // Setup variables + const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0)); + const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local( + 0) - prev_wp_local(0)); + const float a = -line_segment_slope; + const float c = -line_segment_rover_offset; + const float r = lookahead_distance; + const float x0 = -a * c / (a * a + 1.0f); + const float y0 = -c / (a * a + 1.0f); + + // Calculate intersection points + if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist + return 0.f; + + } else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists + return atan2f(y0, x0); + + } else { // Two intersetion points exist + const float d = r * r - c * c / (a * a + 1.0f); + const float mult = sqrt(d / (a * a + 1.0f)); + const float ax = x0 + mult; + const float bx = x0 - mult; + const float ay = y0 - a * mult; + const float by = y0 + a * mult; + const Vector2f point1(ax, ay); + const Vector2f point2(bx, by); + const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1; + const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2; + + // Return intersection point closer to current waypoint + if (distance1.norm_squared() < distance2.norm_squared()) { + return atan2f(ay, ax); + + } else { + return atan2f(by, bx); + } + } +} + void BoatGuidance::updateParams() { ModuleParams::updateParams(); - - _l1_guidance.set_l1_damping(_param_bt_l1_damping.get()); - _l1_guidance.set_l1_period(_param_bt_l1_period.get()); } diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.hpp b/src/modules/boat/BoatGuidance/BoatGuidance.hpp index 3833e19da4..55cfb18439 100644 --- a/src/modules/boat/BoatGuidance/BoatGuidance.hpp +++ b/src/modules/boat/BoatGuidance/BoatGuidance.hpp @@ -104,6 +104,12 @@ public: */ float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } + float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, + const float &lookahead_distance); + + float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &curr_pos_local); + protected: /** * @brief Update the parameters of the module. @@ -115,41 +121,32 @@ private: uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Publication _boat_setpoint_pub{ORB_ID(boat_setpoint)}; - position_setpoint_triplet_s _position_setpoint_triplet{}; vehicle_global_position_s _vehicle_global_position{}; GuidanceState _currentState; - float _desired_angular_velocity; - - float _max_speed; - float _max_angular_velocity; - - matrix::Vector2d _current_waypoint; - - VelocitySmoothing _forwards_velocity_smoothing; - PositionSmoothing _position_smoothing; - - ECL_L1_Pos_Controller _l1_guidance; - - Vector2f _previous_local_position{}; + float _desired_angular_velocity{}; + float _max_angular_velocity{}; + float _look_ahead_distance{}; + float _max_speed{}; + VelocitySmoothing _forwards_velocity_smoothing{}; + PositionSmoothing _position_smoothing{}; MapProjection _global_local_proj_ref{}; + matrix::Vector2d _current_waypoint{}; + ECL_L1_Pos_Controller _l1_guidance{}; + Vector2f _previous_local_position{}; + Vector2d _previous_position{}; DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - - (ParamFloat) _param_bt_l1_period, - (ParamFloat) _param_bt_l1_damping, - (ParamFloat) _param_bt_max_heading_error, (ParamFloat) _param_bt_min_heading_error, - - (ParamFloat) _param_bt_spd_max, - (ParamFloat) _param_bt_spd_min, - + (ParamFloat) _param_look_ahead_distance, + (ParamFloat) _param_nav_loiter_rad, (ParamFloat) _param_bt_spd_cruise, - (ParamFloat) _param_nav_loiter_rad + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_bt_spd_max, + (ParamFloat) _param_bt_spd_min ) }; diff --git a/src/modules/boat/BoatGuidance/CMakeLists.txt b/src/modules/boat/BoatGuidance/CMakeLists.txt index d5957c39ca..0075a930df 100644 --- a/src/modules/boat/BoatGuidance/CMakeLists.txt +++ b/src/modules/boat/BoatGuidance/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_library(BoatGuidance BoatGuidance.cpp + BoatGuidance.hpp ) target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat/module.yaml b/src/modules/boat/module.yaml index a7ec26aac0..9f1211e9b9 100644 --- a/src/modules/boat/module.yaml +++ b/src/modules/boat/module.yaml @@ -21,32 +21,6 @@ parameters: increment: 0.01 decimal: 2 default: 1 - BT_LACC_LIM: - description: - short: Lateral acceleration limit - type: float - unit: m/s^2 - min: 0.0 - max: 6.0 - decimal: 1 - default: 2.0 - BT_L1_PERIOD: - description: - short: L1 period - type: float - unit: s - min: 0.5 - max: 50.0 - decimal: 1 - default: 25.0 - BT_L1_DAMPING: - description: - short: L1 damping - type: float - min: 0.6 - max: 0.9 - decimal: 2 - default: 0.9 BT_SPD_CRUISE: description: short: Default cruise speed @@ -165,37 +139,11 @@ parameters: min: 0.0 decimal: 2 default: 30.0 - BT_MISSION_THR: + BT_LOOKAHEAD: description: - short: Mission throttle setpoint - type: float - min: -1.0 - decimal: 2 - default: 0.8 - BT_MIN_THR: - description: - short: Mission minimum slowdown parameter + short: Lookahead distance for heading error type: float + unit: m min: 0.0 decimal: 2 - default: 0.5 - BT_MAX_JERK: - description: - short: Maximum jerk - type: float - unit: m/s^3 - min: 0.0 - max: 10.0 - decimal: 1 - default: 5.0 - BT_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 + default: 10.0