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

AC_PosControl: update some comments re leash lengths

Browse Source
This commit is contained in:
Randy Mackay 2014-03-17 09:40:11 +09:00
parent 345115fddd
commit dd45647626
2 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -141,10 +141,10 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
const float curr_pos_z = _inav.get_altitude();
const float curr_vel_z = _inav.get_velocity_z();
float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
float linear_velocity; // the velocity we swap between linear and sqrt
// calculate the velocity at which we switch from calculating the stopping point using a linear funcction to a sqrt function
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = POSCONTROL_ALT_HOLD_ACCEL_MAX/_p_alt_pos.kP();
if (fabs(curr_vel_z) < linear_velocity) {
@ -338,7 +338,6 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
d = _pid_alt_accel.get_d(_accel_error.z, _dt);
// To-Do: pull min/max throttle from motors
// To-Do: where to get hover throttle?
// To-Do: we had a contraint here but it's now removed, is this ok? with the motors library handle it ok?
_attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);
@ -612,7 +611,7 @@ void AC_PosControl::rate_to_accel_xy(float dt)
_vel_error.x = _vel_target.x - vel_curr.x;
_vel_error.y = _vel_target.y - vel_curr.y;
// combine feed foward accel with PID output from velocity error
// combine feed forward accel with PID output from velocity error
// To-Do: check accel limit flag before adding I term
_accel_target.x += _pid_rate_lat.get_pid(_vel_error.x, dt);
_accel_target.y += _pid_rate_lon.get_pid(_vel_error.y, dt);

8
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -64,8 +64,8 @@ public:
/// set_speed_z - sets maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
/// calc_leash_length_z should be called afterwards
/// speed_down should be a negative number
/// leash length will be recalculated the next time update_z_controller() is called
void set_speed_z(float speed_down, float speed_up);
/// get_speed_up - accessor for current up speed in cm/s
@ -75,7 +75,7 @@ public:
float get_speed_down() { return _speed_down_cms; }
/// set_accel_z - set vertical acceleration in cm/s/s
/// calc_leash_length_z should be called afterwards
/// leash length will be recalculated the next time update_z_controller() is called
void set_accel_z(float accel_cmss);
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
@ -131,12 +131,12 @@ public:
///
/// set_accel_xy - set horizontal acceleration in cm/s/s
/// calc_leash_length_xy should be called afterwards
/// leash length will be recalculated the next time update_pos_controller() is called
void set_accel_xy(float accel_cmss);
float get_accel_xy() const { return _accel_cms; }
/// set_speed_xy - set horizontal speed maximum in cm/s
/// calc_leash_length_xy should be called afterwards
/// leash length will be recalculated the next time update_pos_controller() is called
void set_speed_xy(float speed_cms);
float get_speed_xy() const { return _speed_cms; }