Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: Excluded target velocity from slowdown calculations 

The slowdown calculations should apply only to the relative velocity,
not the absolute one. Thus the target baseline velocity should be added
afterwards.
Naturally the absolute velocity limits should be applied afterwards.
This commit is contained in:
George Zogopoulos 2024-01-21 20:12:47 +01:00 committed by Randy Mackay
parent c54529aaf8
commit b2898ca1d3
1 changed files with 13 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/mode_follow.cpp
View File

@ -66,30 +66,9 @@ void ModeFollow::run()
// convert dist_vec_offs to cm in NEU
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
// calculate desired velocity vector in cm/s in NEU
// calculate desired relative velocity vector in cm/s in NEU
const float kp = g2.follow.get_pos_p().kP();
desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy_cms())) {
const float scalar_xy = pos_control->get_max_speed_xy_cms() / desired_speed_xy;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_max_speed_xy_cms();
}
// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down_cms()), pos_control->get_max_speed_up_cms());
// unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu;
const float dir_to_target_neu_len = dir_to_target_neu.length();
if (!is_zero(dir_to_target_neu_len)) {
dir_to_target_neu /= dir_to_target_neu_len;
}
desired_velocity_neu_cms = dist_vec_offs_neu * kp;
// create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
@ -104,13 +83,22 @@ void ModeFollow::run()
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity_2D(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
desired_velocity_neu_cms.xy() = desired_velocity_xy_cms;
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// Add the target velocity baseline.
desired_velocity_neu_cms.xy() += vel_of_target.xy() * 100.0f;
desired_velocity_neu_cms.z += -vel_of_target.z * 100.0f;
// scale desired velocity to stay within horizontal speed limit
desired_velocity_neu_cms.xy().limit_length(pos_control->get_max_speed_xy_cms());
// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down_cms()), pos_control->get_max_speed_up_cms());
// limit the velocity for obstacle/fence avoidance
copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss(), G_Dt);