Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: combine z position control into single method

Browse Source
This commit is contained in:
Leonard Hall 2017-11-12 23:20:19 +10:30 committed by Randy Mackay
parent 993e638752
commit 85b7f06554
2 changed files with 20 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -482,12 +482,12 @@ void AC_PosControl::update_z_controller()
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
pos_to_rate_z();
// call z-axis position controller
run_z_controller();
}
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
/// called by update_z_controller if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z) {
@ -497,10 +497,11 @@ void AC_PosControl::calc_leash_length_z()
}
}
// pos_to_rate_z - position to rate controller for Z axis
// run position control for Z axis
// target altitude should be set with one of these functions: set_alt_target, set_target_to_stopping_point_z, init_takeoff
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
void AC_PosControl::run_z_controller()
{
float curr_alt = _inav.get_altitude();
@ -544,17 +545,11 @@ void AC_PosControl::pos_to_rate_z()
_vel_target.z += _vel_desired.z;
}
// call rate based throttle controller which will update accel based throttle controller targets
rate_to_accel_z();
}
// the following section calculates acceleration required to achieve the velocity target
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
// calculates desired acceleration and calls accel throttle controller
void AC_PosControl::rate_to_accel_z()
{
const Vector3f& curr_vel = _inav.get_velocity();
float p; // used to capture pid values for logging
// TODO: remove velocity derivative calculation
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
@ -586,20 +581,12 @@ void AC_PosControl::rate_to_accel_z()
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
}
// calculate p
p = _p_vel_z.kP() * _vel_error.z;
_accel_target.z = _p_vel_z.get_p(_vel_error.z);
// consolidate and constrain target acceleration
_accel_target.z = _accel_desired.z + p;
_accel_target.z += _accel_desired.z;
// set target for accel based throttle controller
accel_to_throttle(_accel_target.z);
}
// accel_to_throttle - alt hold's acceleration controller
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl::accel_to_throttle(float accel_target_z)
{
// the following section calculates a desired throttle needed to achieve the acceleration target
float z_accel_meas; // actual acceleration
float p,i,d; // used to capture pid values for logging
@ -613,12 +600,12 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
_flags.reset_accel_to_throttle = false;
} else {
// calculate accel error
_accel_error.z = accel_target_z - z_accel_meas;
_accel_error.z = _accel_target.z - z_accel_meas;
}
// set input to PID
_pid_accel_z.set_input_filter_all(_accel_error.z);
_pid_accel_z.set_desired_rate(accel_target_z);
_pid_accel_z.set_desired_rate(_accel_target.z);
// separately calculate p, i, d values for logging
p = _pid_accel_z.get_p();
@ -647,7 +634,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
}
///
/// position controller
/// lateral position controller
///
/// set_accel_xy - set horizontal acceleration in cm/s/s
@ -1000,7 +987,7 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
// this section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
@ -1057,7 +1044,7 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
_accel_target.x += _accel_desired.x;
_accel_target.y += _accel_desired.y;
// This section converts desired accelerations provided in lat/lon frame to roll/pitch angles
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
float accel_right, accel_forward;

14
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -87,7 +87,7 @@ public:
float get_accel_z() const { return _accel_z_cms; }
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
/// called by update_z_controller if z-axis speed or accelerations are changed
void calc_leash_length_z();
/// set_alt_target - set altitude target in cm above home
@ -317,18 +317,12 @@ protected:
/// z controller private methods
///
// pos_to_rate_z - position to rate controller for Z axis
// target altitude should be placed into _pos_target.z using or set with one of these functions
// run position control for Z axis
// target altitude should be set with one of these functions
// set_alt_target
// set_target_to_stopping_point_z
// init_takeoff
void pos_to_rate_z();
// rate_to_accel_z - calculates desired accel required to achieve the velocity target
void rate_to_accel_z();
// accel_to_throttle - alt hold's acceleration controller
void accel_to_throttle(float accel_target_z);
void run_z_controller();
///
/// xy controller private methods