fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 20:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

formatting

Browse Source
This commit is contained in:
PerFrivik
2024-06-12 14:07:51 +02:00
parent 9f74849f3f
commit bb4648130f
8 changed files with 105 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/boat/Boat.cpp
-1
View File
@@ -131,7 +131,6 @@ void Boat::Run()
_boat_control.getLocalPosition(),
dt
);
// _boat_guidance_pp.purePursuit();
}
_boat_control.control(dt);
src/modules/boat/Boat.hpp
+8 -6
View File
@@ -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_s> _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<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
(ParamFloat<px4::params::BT_ANG_MAX>) _param_bt_ang_max,
(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
(ParamFloat<px4::params::BT_ANG_VEL_SCALE>) _param_bt_ang_vel_scale,
(ParamFloat<px4::params::BT_SPD_SCALE>) _param_bt_spd_scale
(ParamFloat<px4::params::BT_SPD_SCALE>) _param_bt_spd_scale,
(ParamFloat<px4::params::BT_ANG_MAX>) _param_bt_ang_max,
(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max
)
};
src/modules/boat/BoatGuidance/BoatGuidance.cpp
+62 -31
View File
@@ -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<float>(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));
src/modules/boat/BoatGuidance/BoatGuidance.hpp
+26 -8
View File
@@ -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_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
position_setpoint_triplet_s _position_setpoint_triplet{};
vehicle_global_position_s _vehicle_global_position{};
src/modules/boat/BoatKinematics/BoatKinematics.cpp
+2
View File
@@ -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);
src/modules/boat/BoatKinematics/BoatKinematics.hpp
+1
View File
@@ -94,6 +94,7 @@ private:
uORB::PublicationMulti<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
boat_setpoint_s _boat_control_output{};
bool _armed = false;
float _wheel_base{0.f};
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
+4 -4
View File
@@ -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;
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
+2 -2
View File
@@ -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.
};
/**