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

AC_AttitudeControl: Rename several set_speed and set_accel functions

Browse Source
This commit is contained in:
Michael du Breuil 2018-09-19 22:43:48 -07:00 committed by WickedShell
parent b6efc8a20b
commit 1d13aff711
2 changed files with 29 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -197,20 +197,14 @@ AC_PosControl::AC_PosControl(const AP_AHRS_View& ahrs, const AP_InertialNav& ina
_p_pos_xy(POSCONTROL_POS_XY_P), _p_pos_xy(POSCONTROL_POS_XY_P),
_pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_50HZ), _pid_vel_xy(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ, POSCONTROL_DT_50HZ),
_dt(POSCONTROL_DT_400HZ), _dt(POSCONTROL_DT_400HZ),
_last_update_xy_ms(0),
_last_update_z_ms(0),
_speed_down_cms(POSCONTROL_SPEED_DOWN), _speed_down_cms(POSCONTROL_SPEED_DOWN),
_speed_up_cms(POSCONTROL_SPEED_UP), _speed_up_cms(POSCONTROL_SPEED_UP),
_speed_cms(POSCONTROL_SPEED), _speed_cms(POSCONTROL_SPEED),
_accel_z_cms(POSCONTROL_ACCEL_Z), _accel_z_cms(POSCONTROL_ACCEL_Z),
_accel_last_z_cms(0.0f),
_accel_cms(POSCONTROL_ACCEL_XY), _accel_cms(POSCONTROL_ACCEL_XY),
_leash(POSCONTROL_LEASH_LENGTH_MIN), _leash(POSCONTROL_LEASH_LENGTH_MIN),
_leash_down_z(POSCONTROL_LEASH_LENGTH_MIN), _leash_down_z(POSCONTROL_LEASH_LENGTH_MIN),
_leash_up_z(POSCONTROL_LEASH_LENGTH_MIN), _leash_up_z(POSCONTROL_LEASH_LENGTH_MIN),
_roll_target(0.0f),
_pitch_target(0.0f),
_distance_to_target(0.0f),
_accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ) _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -249,11 +243,9 @@ void AC_PosControl::set_dt(float delta_sec)
_vel_error_filter.set_cutoff_frequency(POSCONTROL_VEL_ERROR_CUTOFF_FREQ); _vel_error_filter.set_cutoff_frequency(POSCONTROL_VEL_ERROR_CUTOFF_FREQ);
} }
/// set_speed_z - sets maximum climb and descent rates /// set_max_speed_z - set the maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation /// To-Do: call this in the main code as part of flight mode initialisation
/// calc_leash_length_z should be called afterwards void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
/// speed_down should be a negative number
void AC_PosControl::set_speed_z(float speed_down, float speed_up)
{ {
// ensure speed_down is always negative // ensure speed_down is always negative
speed_down = -fabsf(speed_down); speed_down = -fabsf(speed_down);
@ -266,8 +258,8 @@ void AC_PosControl::set_speed_z(float speed_down, float speed_up)
} }
} }
/// set_accel_z - set vertical acceleration in cm/s/s /// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
void AC_PosControl::set_accel_z(float accel_cmss) void AC_PosControl::set_max_accel_z(float accel_cmss)
{ {
if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) { if (fabsf(_accel_z_cms-accel_cmss) > 1.0f) {
_accel_z_cms = accel_cmss; _accel_z_cms = accel_cmss;
@ -640,8 +632,8 @@ void AC_PosControl::run_z_controller()
/// lateral position controller /// lateral position controller
/// ///
/// set_accel_xy - set horizontal acceleration in cm/s/s /// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
void AC_PosControl::set_accel_xy(float accel_cmss) void AC_PosControl::set_max_accel_xy(float accel_cmss)
{ {
if (fabsf(_accel_cms-accel_cmss) > 1.0f) { if (fabsf(_accel_cms-accel_cmss) > 1.0f) {
_accel_cms = accel_cmss; _accel_cms = accel_cmss;
@ -650,8 +642,8 @@ void AC_PosControl::set_accel_xy(float accel_cmss)
} }
} }
/// set_speed_xy - set horizontal speed maximum in cm/s /// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
void AC_PosControl::set_speed_xy(float speed_cms) void AC_PosControl::set_max_speed_xy(float speed_cms)
{ {
if (fabsf(_speed_cms-speed_cms) > 1.0f) { if (fabsf(_speed_cms-speed_cms) > 1.0f) {
_speed_cms = speed_cms; _speed_cms = speed_cms;
@ -700,7 +692,7 @@ void AC_PosControl::set_target_to_stopping_point_xy()
/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
/// distance_max allows limiting distance to stopping point /// distance_max allows limiting distance to stopping point
/// results placed in stopping_position vector /// results placed in stopping_position vector
/// set_accel_xy() should be called before this method to set vehicle acceleration /// set_max_accel_xy() should be called before this method to set vehicle acceleration
/// set_leash_length() should have been called before this method /// set_leash_length() should have been called before this method
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{ {

40
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -61,26 +61,26 @@ public:
/// z position controller /// z position controller
/// ///
/// set_speed_z - sets maximum climb and descent rates /// set_max_speed_z - sets maximum climb and descent rates
/// speed_down can be positive or negative but will always be interpreted as a descent speed /// speed_down can be positive or negative but will always be interpreted as a descent speed
/// leash length will be recalculated the next time update_z_controller() is called /// leash length will be recalculated
void set_speed_z(float speed_down, float speed_up); void set_max_speed_z(float speed_down, float speed_up);
/// get_speed_up - accessor for current up speed in cm/s /// get_max_speed_up - accessor for current maximum up speed in cm/s
float get_speed_up() const { return _speed_up_cms; } float get_max_speed_up() const { return _speed_up_cms; }
/// get_speed_down - accessors for current down speed in cm/s. Will be a negative number /// get_max_speed_down - accessors for current maximum down speed in cm/s. Will be a negative number
float get_speed_down() const { return _speed_down_cms; } float get_max_speed_down() const { return _speed_down_cms; }
/// get_vel_target_z - returns current vertical speed in cm/s /// get_vel_target_z - returns current vertical speed in cm/s
float get_vel_target_z() const { return _vel_target.z; } float get_vel_target_z() const { return _vel_target.z; }
/// set_accel_z - set vertical acceleration in cm/s/s /// set_max_accel_z - set the maximum vertical acceleration in cm/s/s
/// leash length will be recalculated the next time update_z_controller() is called /// leash length will be recalculated
void set_accel_z(float accel_cmss); void set_max_accel_z(float accel_cmss);
/// get_accel_z - returns current vertical acceleration in cm/s/s /// get_max_accel_z - returns current maximum vertical acceleration in cm/s/s
float get_accel_z() const { return _accel_z_cms; } float get_max_accel_z() const { return _accel_z_cms; }
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration /// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by update_z_controller if z-axis speed or accelerations are changed /// called by update_z_controller if z-axis speed or accelerations are changed
@ -165,15 +165,15 @@ public:
/// this does not update the xy target /// this does not update the xy target
void init_xy_controller(); void init_xy_controller();
/// set_accel_xy - set horizontal acceleration in cm/s/s /// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
/// leash length will be recalculated the next time update_xy_controller() is called /// leash length will be recalculated
void set_accel_xy(float accel_cmss); void set_max_accel_xy(float accel_cmss);
float get_accel_xy() const { return _accel_cms; } float get_max_accel_xy() const { return _accel_cms; }
/// set_speed_xy - set horizontal speed maximum in cm/s /// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
/// leash length will be recalculated the next time update_xy_controller() is called /// leash length will be recalculated
void set_speed_xy(float speed_cms); void set_max_speed_xy(float speed_cms);
float get_speed_xy() const { return _speed_cms; } float get_max_speed_xy() const { return _speed_cms; }
/// set_limit_accel_xy - mark that accel has been limited /// set_limit_accel_xy - mark that accel has been limited
/// this prevents integrator buildup /// this prevents integrator buildup