Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: simplify guided mode's velocity controller's accel limit calcs

Browse Source
This commit is contained in:
Randy Mackay 2016-07-25 11:40:08 +09:00
parent f27cf8d388
commit 7a6e0a981b
1 changed files with 1 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/control_guided.cpp
View File

@ -610,17 +610,12 @@ void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_d
}
// limit z change
float vel_delta_z = fabsf(vel_delta.z);
float vel_delta_z_max = G_Dt * pos_control.get_accel_z();
float ratio_z = 1.0f;
if (!is_zero(vel_delta_z) && (vel_delta_z > vel_delta_z_max)) {
ratio_z = vel_delta_z_max / vel_delta_z;
}
// new target
curr_vel_des.x += (vel_delta.x * ratio_xy);
curr_vel_des.y += (vel_delta.y * ratio_xy);
curr_vel_des.z += (vel_delta.z * ratio_z);
curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);
// update position controller with new target
pos_control.set_desired_velocity(curr_vel_des);