Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: rename p_alt_pos to p_pos_z 

Also pid_alt_accel to pid_accel_z
This commit is contained in:
Leonard Hall 2015-01-31 15:50:32 +09:00 committed by Randy Mackay
parent 349f1aeceb
commit 9866eaded1
2 changed files with 23 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -22,15 +22,15 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = {
//
AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control,
AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel,
AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z,
AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy) :
_ahrs(ahrs),
_inav(inav),
_motors(motors),
_attitude_control(attitude_control),
_p_alt_pos(p_alt_pos),
_p_alt_rate(p_alt_rate),
_pid_alt_accel(pid_alt_accel),
_p_pos_z(p_pos_z),
_p_vel_z(p_vel_z),
_pid_accel_z(pid_accel_z),
_p_pos_xy(p_pos_xy),
_pi_vel_xy(pi_vel_xy),
_dt(POSCONTROL_DT_10HZ),
@ -77,7 +77,7 @@ void AC_PosControl::set_dt(float delta_sec)
_dt = delta_sec;
// update rate controller's dt
_pid_alt_accel.set_dt(_dt);
_pid_accel_z.set_dt(_dt);
// update rate z-axis velocity error and accel error filters
_vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ);
@ -187,13 +187,13 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = _accel_z_cms/_p_alt_pos.kP();
linear_velocity = _accel_z_cms/_p_pos_z.kP();
if ((float)fabs(curr_vel_z) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
stopping_point.z = curr_pos_z + curr_vel_z/_p_alt_pos.kP();
stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();
} else {
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
if (curr_vel_z > 0){
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
} else {
@ -214,7 +214,7 @@ void AC_PosControl::init_takeoff()
freeze_ff_z();
// shift difference between last motor out and hover throttle into accelerometer I
_pid_alt_accel.set_integrator(_motors.get_throttle_out()-_throttle_hover);
_pid_accel_z.set_integrator(_motors.get_throttle_out()-_throttle_hover);
}
// is_active_z - returns true if the z-axis position controller has been run very recently
@ -246,8 +246,8 @@ void AC_PosControl::update_z_controller()
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z) {
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_alt_pos.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_alt_pos.kP());
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());
_flags.recalc_leash_z = false;
}
}
@ -279,7 +279,7 @@ void AC_PosControl::pos_to_rate_z()
}
// calculate _vel_target.z using from _pos_error.z using sqrt controller
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_alt_pos.kP(), _accel_z_cms);
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms);
// call rate based throttle controller which will update accel based throttle controller targets
rate_to_accel_z();
@ -338,7 +338,7 @@ void AC_PosControl::rate_to_accel_z()
}
// calculate p
p = _p_alt_rate.kP() * _vel_error.z;
p = _p_vel_z.kP() * _vel_error.z;
// consolidate and constrain target acceleration
desired_accel = _accel_feedforward.z + p;
@ -370,22 +370,22 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
}
// set input to PID
_pid_alt_accel.set_input_filter_d(_accel_error.z);
_pid_accel_z.set_input_filter_d(_accel_error.z);
// separately calculate p, i, d values for logging
p = _pid_alt_accel.get_p();
p = _pid_accel_z.get_p();
// get i term
i = _pid_alt_accel.get_integrator();
i = _pid_accel_z.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
// To-Do: should this be replaced with limits check from attitude_controller?
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {
i = _pid_alt_accel.get_i();
i = _pid_accel_z.get_i();
}
// get d term
d = _pid_alt_accel.get_d();
d = _pid_accel_z.get_d();
// send throttle to attitude controller with angle boost
_attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);

13
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -48,7 +48,7 @@ public:
/// Constructor
AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control,
AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel,
AC_P& p_pos_z, AC_P& p_vel_z, AC_PID& pid_accel_z,
AC_P& p_pos_xy, AC_PI_2D& pi_vel_xy);
// xy_mode - specifies behavior of xy position controller
@ -150,9 +150,6 @@ public:
float get_leash_down_z() const { return _leash_down_z; }
float get_leash_up_z() const { return _leash_up_z; }
/// althold_kP - returns altitude hold position control PID's kP gain
float althold_kP() const { return _p_alt_pos.kP(); }
///
/// xy position controller
///
@ -332,10 +329,10 @@ private:
const AP_Motors& _motors;
AC_AttitudeControl& _attitude_control;
// references to pid controllers and motors
AC_P& _p_alt_pos;
AC_P& _p_alt_rate;
AC_PID& _pid_alt_accel;
// references to pid controllers
AC_P& _p_pos_z;
AC_P& _p_vel_z;
AC_PID& _pid_accel_z;
AC_P& _p_pos_xy;
AC_PI_2D& _pi_vel_xy;