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

Copter: guided mode applies acceleration limits to velocity controller

Browse Source
This commit is contained in:
Randy Mackay 2016-07-20 20:45:11 +09:00
parent 866487608e
commit dc52f3b2df
2 changed files with 45 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -812,6 +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_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();

48
ArduCopter/control_guided.cpp
View File

@ -236,11 +236,10 @@ void Copter::guided_set_velocity(const Vector3f& velocity)
guided_vel_control_start();
}
// record velocity target
guided_vel_target_cms = velocity;
vel_update_time_ms = millis();
// set position controller velocity target
pos_control.set_desired_velocity(velocity);
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity);
}
@ -440,7 +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()) {
pos_control.set_desired_velocity(Vector3f(0,0,0));
guided_set_desired_velocity_with_accel_limits(Vector3f(0.0f,0.0f,0.0f));
} else {
guided_set_desired_velocity_with_accel_limits(guided_vel_target_cms);
}
// call velocity controller which includes z axis controller
@ -586,6 +587,45 @@ void Copter::guided_angle_control_run()
pos_control.update_z_controller();
}
// 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)
{
// get current desired velocity
Vector3f curr_vel_des = pos_control.get_desired_velocity();
// exit immediately if already equal
if (curr_vel_des == vel_des) {
return;
}
// get change in desired velocity
Vector3f vel_delta = vel_des - curr_vel_des;
// limit xy change
float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y));
float vel_delta_xy_max = G_Dt * pos_control.get_accel_xy();
float ratio_xy = 1.0f;
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
ratio_xy = vel_delta_xy_max / vel_delta_xy;
}
// 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);
// update position controller with new target
pos_control.set_desired_velocity(curr_vel_des);
}
// Guided Limit code
// guided_limit_clear - clear/turn off guided limits