diff --git a/src/modules/boat/Boat.cpp b/src/modules/boat/Boat.cpp index e931d4ac6e..b2f65e6179 100644 --- a/src/modules/boat/Boat.cpp +++ b/src/modules/boat/Boat.cpp @@ -131,7 +131,6 @@ void Boat::Run() _boat_control.getLocalPosition(), dt ); - // _boat_guidance_pp.purePursuit(); } _boat_control.control(dt); diff --git a/src/modules/boat/Boat.hpp b/src/modules/boat/Boat.hpp index cf206f5266..5419fb0157 100644 --- a/src/modules/boat/Boat.hpp +++ b/src/modules/boat/Boat.hpp @@ -79,16 +79,16 @@ protected: private: void Run() override; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Publication _boat_setpoint_pub{ORB_ID(boat_setpoint)}; bool _manual_driving = false; bool _mission_driving = false; bool _acro_driving = false; - hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + BoatGuidance _boat_guidance{this}; BoatControl _boat_control{this}; @@ -97,11 +97,13 @@ private: float _max_speed{0.f}; float _max_angular_velocity{0.f}; + hrt_abstime _time_stamp_last{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_com_spoolup_time, - (ParamFloat) _param_bt_ang_max, - (ParamFloat) _param_bt_spd_max, (ParamFloat) _param_bt_ang_vel_scale, - (ParamFloat) _param_bt_spd_scale + (ParamFloat) _param_bt_spd_scale, + (ParamFloat) _param_bt_ang_max, + (ParamFloat) _param_bt_spd_max ) }; diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.cpp b/src/modules/boat/BoatGuidance/BoatGuidance.cpp index 476c9be2a7..6752ddbd6a 100644 --- a/src/modules/boat/BoatGuidance/BoatGuidance.cpp +++ b/src/modules/boat/BoatGuidance/BoatGuidance.cpp @@ -41,7 +41,7 @@ BoatGuidance::BoatGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _currentState = GuidanceState::DRIVING; + _currentState = GuidanceState::kDriving; } void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, @@ -67,14 +67,19 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l vehicle_local_position.timestamp); } - const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(current_waypoint(0), - current_waypoint(1)); - const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(previous_waypoint(0), - previous_waypoint(1)); + const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project( + current_waypoint(0), + current_waypoint(1)); + const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project( + previous_waypoint(0), + previous_waypoint(1)); 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(global_position(0), global_position(1), - current_waypoint(0), current_waypoint(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 heading_error = 0.f; float speed_interpolation = 0.f; @@ -84,55 +89,78 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l // Go back to driving, when the waypoint has been reached if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::DRIVING; + _currentState = GuidanceState::kDriving; } // 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; + _currentState = GuidanceState::kGoalReached; } switch (_currentState) { - case GuidanceState::DRIVING: { - + case GuidanceState::kDriving: { if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) { - 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); + 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)); + 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_frame_position; _previous_position = global_position; - _currentState = GuidanceState::DRIVING_TO_POINT; + _currentState = GuidanceState::kDrivingToAPoint; } break; } // Control logic if there is no previous waypoint - case GuidanceState::DRIVING_TO_POINT: { + case GuidanceState::kDrivingToAPoint: { + 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 + ); - 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)); + + 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: + case GuidanceState::kGoalReached: desired_speed = 0.f; heading_error = 0.f; _desired_angular_velocity = 0.f; @@ -143,17 +171,21 @@ void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_l // 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_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); output.yaw_rate = math::constrain(_desired_angular_velocity, -_max_angular_velocity, _max_angular_velocity); - output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; + output.closed_loop_speed_control = true; + output.closed_loop_yaw_rate_control = true; output.timestamp = hrt_absolute_time(); _boat_setpoint_pub.publish(output); @@ -185,8 +217,7 @@ float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Ve } float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, - Vector2f const &curr_pos_local, - float const &lookahead_distance) + 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)); diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.hpp b/src/modules/boat/BoatGuidance/BoatGuidance.hpp index 55cfb18439..92305b7353 100644 --- a/src/modules/boat/BoatGuidance/BoatGuidance.hpp +++ b/src/modules/boat/BoatGuidance/BoatGuidance.hpp @@ -59,9 +59,9 @@ using namespace matrix; * @brief Enum class for the different states of guidance. */ enum class GuidanceState { - DRIVING, ///< The vehicle is currently driving straight. - DRIVING_TO_POINT, ///< The vehicle is currently driving to the next waypoint. - GOAL_REACHED ///< The vehicle has reached its goal. + kDriving, ///< The vehicle is currently driving straight. + kDrivingToAPoint, ///< The vehicle is currently driving to the next waypoint. + kGoalReached ///< The vehicle has reached its goal. }; /** @@ -89,6 +89,27 @@ public: */ void computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, float dt); + /** + * @brief Calculate the lookahead distance based on the current and previous waypoints and the current position. + * @param curr_wp_local The current waypoint in local coordinates. + * @param prev_wp_local The previous waypoint in local coordinates. + * @param curr_pos_local The current position in local coordinates. + * @return The calculated lookahead distance. + */ + float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, + const Vector2f &curr_pos_local); + + /** + * @brief Calculate the desired heading based on the current and previous waypoints, current position, and lookahead distance. + * @param curr_wp_local The current waypoint in local coordinates. + * @param prev_wp_local The previous waypoint in local coordinates. + * @param curr_pos_local The current position in local coordinates. + * @param lookahead_distance The lookahead distance. + * @return The calculated desired heading. + */ + float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, + const float &lookahead_distance); + /** * @brief Set the maximum speed for the vehicle. * @param max_speed The maximum speed in m/s. @@ -98,17 +119,13 @@ public: /** - * @brief Set the maximum angular velocity for the vehicle. + * @brief Set the maximum angular velocity for the boat. * @param max_angular_velocity The maximum angular velocity in rad/s. * @return The set maximum angular velocity in rad/s. */ 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: /** @@ -121,6 +138,7 @@ 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{}; diff --git a/src/modules/boat/BoatKinematics/BoatKinematics.cpp b/src/modules/boat/BoatKinematics/BoatKinematics.cpp index 6de9147d14..c4cedf3540 100644 --- a/src/modules/boat/BoatKinematics/BoatKinematics.cpp +++ b/src/modules/boat/BoatKinematics/BoatKinematics.cpp @@ -79,6 +79,8 @@ matrix::Vector2f BoatKinematics::computeInverseKinematics(float linear_velocity_ return Vector2f(); } + // Room for more advanced dynamics, if required + linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); diff --git a/src/modules/boat/BoatKinematics/BoatKinematics.hpp b/src/modules/boat/BoatKinematics/BoatKinematics.hpp index f12c46f928..9e75b3aad3 100644 --- a/src/modules/boat/BoatKinematics/BoatKinematics.hpp +++ b/src/modules/boat/BoatKinematics/BoatKinematics.hpp @@ -94,6 +94,7 @@ private: uORB::PublicationMulti _actuator_servos_pub{ORB_ID(actuator_servos)}; boat_setpoint_s _boat_control_output{}; + bool _armed = false; float _wheel_base{0.f}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp index df453ff8b5..b019599555 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp @@ -72,7 +72,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { - _currentState = GuidanceState::GOAL_REACHED; + _currentState = GuidanceState::kGoalReached; } float desired_speed = 0.f; @@ -82,12 +82,12 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit desired_speed = 0.f; if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; + _currentState = GuidanceState::kDriving; } break; - case GuidanceState::DRIVING: { + case GuidanceState::kDriving: { const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); _forwards_velocity_smoothing.updateDurations(max_velocity); @@ -97,7 +97,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit break; } - case GuidanceState::GOAL_REACHED: + case GuidanceState::kGoalReached: // temporary till I find a better way to stop the vehicle desired_speed = 0.f; heading_error = 0.f; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp index 0a5a29bbca..379282fa5d 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp @@ -58,8 +58,8 @@ */ enum class GuidanceState { TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. + kDriving, ///< The vehicle is currently driving straight. + kGoalReached ///< The vehicle has reached its goal. }; /**