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

Copter: guided mode's velocity controller stops before fence

Browse Source
This commit is contained in:
Randy Mackay 2016-07-25 11:47:44 +09:00
parent 7a6e0a981b
commit 49674abb21
2 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -812,7 +812,7 @@ private:
void guided_vel_control_run();
void guided_posvel_control_run();
void guided_angle_control_run();
void guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des);
void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des);
void guided_limit_clear();
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
void guided_limit_init_time_and_pos();

15
ArduCopter/control_guided.cpp
View File

@ -439,9 +439,9 @@ void Copter::guided_vel_control_run()
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
guided_set_desired_velocity_with_accel_limits(Vector3f(0.0f,0.0f,0.0f));
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f));
} else {
guided_set_desired_velocity_with_accel_limits(guided_vel_target_cms);
guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
}
// call velocity controller which includes z axis controller
@ -588,7 +588,7 @@ void Copter::guided_angle_control_run()
}
// helper function to update position controller's desired velocity while respecting acceleration limits
void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des)
void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
{
// get current desired velocity
Vector3f curr_vel_des = pos_control.get_desired_velocity();
@ -608,15 +608,16 @@ void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_d
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
ratio_xy = vel_delta_xy_max / vel_delta_xy;
}
curr_vel_des.x += (vel_delta.x * ratio_xy);
curr_vel_des.y += (vel_delta.y * ratio_xy);
// limit z change
float vel_delta_z_max = G_Dt * pos_control.get_accel_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 += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);
// limit the velocity to prevent fence violations
avoid.adjust_velocity(pos_control.get_pos_xy_kP(), pos_control.get_accel_xy(), curr_vel_des);
// update position controller with new target
pos_control.set_desired_velocity(curr_vel_des);
}