aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: follow mode renames and comment improvements

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2018-02-15 20:16:43 +01:00 committed by Randy Mackay
parent 8ae4047a00
commit 35a4748c06
1 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/mode_follow.cpp
View File

@ -48,7 +48,7 @@ void Copter::ModeFollow::run()
// position and velocity requests will be ignored while the vehicle is not in guided mode // position and velocity requests will be ignored while the vehicle is not in guided mode
// variables to be sent to velocity controller // variables to be sent to velocity controller
Vector3f desired_velocity; Vector3f desired_velocity_neu_cms;
bool use_yaw = false; bool use_yaw = false;
float yaw_cd = 0.0f; float yaw_cd = 0.0f;
@ -69,21 +69,21 @@ void Copter::ModeFollow::run()
// calculate desired velocity vector in cm/s in NEU // calculate desired velocity vector in cm/s in NEU
const float kp = g2.follow.get_pos_p().kP(); const float kp = g2.follow.get_pos_p().kP();
desired_velocity.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp); desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp); desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * 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 // scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y)); 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_speed_xy())) { if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_speed_xy())) {
const float scalar_xy = pos_control->get_speed_xy() / desired_speed_xy; const float scalar_xy = pos_control->get_speed_xy() / desired_speed_xy;
desired_velocity.x *= scalar_xy; desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity.y *= scalar_xy; desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_speed_xy(); desired_speed_xy = pos_control->get_speed_xy();
} }
// limit desired velocity to be between maximum climb and descent rates // limit desired velocity to be between maximum climb and descent rates
desired_velocity.z = constrain_float(desired_velocity.z, -fabsf(pos_control->get_speed_down()), pos_control->get_speed_up()); desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_speed_down()), pos_control->get_speed_up());
// unit vector towards target position (i.e. vector to lead vehicle + offset) // unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu; Vector3f dir_to_target_neu = dist_vec_offs_neu;
@ -93,31 +93,31 @@ void Copter::ModeFollow::run()
} }
// create horizontal desired velocity vector (required for slow down calculations) // create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy(desired_velocity.x, desired_velocity.y); Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
// create horizontal unit vector towards target (required for slow down calculations) // create horizontal unit vector towards target (required for slow down calculations)
Vector2f dir_to_target_xy(desired_velocity_xy.x, desired_velocity_xy.y); Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
if (!dir_to_target_xy.is_zero()) { if (!dir_to_target_xy.is_zero()) {
dir_to_target_xy.normalize(); dir_to_target_xy.normalize();
} }
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down) // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length(); const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() * 0.5f, desired_velocity_xy, dir_to_target_xy, dist_to_target_xy, copter.G_Dt); copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// limit the horizontal velocity to prevent fence violations // limit the horizontal velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy, G_Dt); copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy_cms, G_Dt);
// copy horizontal velocity limits back to 3d vector // copy horizontal velocity limits back to 3d vector
desired_velocity.x = desired_velocity_xy.x; desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity.y = desired_velocity_xy.y; desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
// limit vertical desired_velocity to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down) // 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_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt); const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity.z = constrain_float(desired_velocity.z, -des_vel_z_max, des_vel_z_max); desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate // get avoidance adjusted climb rate
desired_velocity.z = get_avoidance_adjusted_climbrate(desired_velocity.z); desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z);
// calculate vehicle heading // calculate vehicle heading
switch (g2.follow.get_yaw_behave()) { switch (g2.follow.get_yaw_behave()) {
@ -140,7 +140,7 @@ void Copter::ModeFollow::run()
} }
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
const Vector3f vel_vec(desired_velocity.x, desired_velocity.y, 0.0f); const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
if (vel_vec.length() > 100.0f) { if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec); yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true; use_yaw = true;
@ -164,7 +164,7 @@ void Copter::ModeFollow::run()
last_log_ms = now; last_log_ms = now;
} }
// re-use guided mode's velocity controller (takes NEU) // re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd, false, 0.0f, false, log_request); Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run(); Copter::ModeGuided::run();
} }