fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: reduce speed based on course error

Browse Source
This commit is contained in:
chfriedrich98
2025-06-18 08:35:48 +02:00
committed by chfriedrich98
parent 5a430f0ba6
commit eed966a1c6
21 changed files with 123 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp
+29 -70
View File
@@ -54,49 +54,35 @@ void AutoMode::updateParams()
void AutoMode::autoControl()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_position_setpoint_triplet_sub.updated()) {
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
}
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
if (_position_setpoint_triplet_sub.updated()) {
updateWaypointsAndAcceptanceRadius();
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = _cruising_speed;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
// Distances to waypoints
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
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));
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = cruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
_prev_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
void AutoMode::updateWaypointsAndAcceptanceRadius()
@@ -108,12 +94,9 @@ void AutoMode::updateWaypointsAndAcceptanceRadius()
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_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);
// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
@@ -152,7 +135,7 @@ float AutoMode::updateAcceptanceRadius(const float waypoint_transition_angle,
return acceptance_radius;
}
float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
float AutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad,
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
{
if (!PX4_ISFINITE(waypoint_transition_angle)
@@ -160,37 +143,13 @@ float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
return 0.f; // Stop at the waypoint
} else {
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); // Slow down for cornering
}
}
float AutoMode::cruisingSpeed(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 waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
{
// Catch improper values
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
return cruising_speed;
}
// Cornering slow down effect
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);
}
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);
} else if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(
M_PI_F - waypoint_transition_angle,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
return math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), min_speed,
cruising_speed); // Slow down for cornering
}
return cruising_speed; // Fallthrough
}