diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index af56b501266e6..5d42d08f624b0 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -806,7 +806,7 @@ void AC_PosControl::init_z() /// The vel is projected forwards in time based on a time step of dt and acceleration accel. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The function alters the vel to be the kinematic path based on accel -void AC_PosControl::input_accel_z(const float accel) +void AC_PosControl::input_accel_z(float accel) { // calculated increased maximum acceleration if over speed float accel_z_cmss = _accel_max_z_cmss; @@ -827,7 +827,7 @@ void AC_PosControl::input_accel_z(const float accel) /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. -void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit, bool limit_output) +void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool ignore_descent_limit, bool limit_output) { if (ignore_descent_limit) { // turn off limits in the negative z direction @@ -857,7 +857,7 @@ void AC_PosControl::input_vel_accel_z(float &vel, const float accel, bool ignore /// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// The zero target altitude is varied to follow pos_offset_z -void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel) +void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel) { // remove terrain offsets for flat earth assumption _pos_target.z -= _pos_offset_z; @@ -879,10 +879,9 @@ void AC_PosControl::set_pos_target_z_from_climb_rate_cm(const float vel) /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing. -void AC_PosControl::land_at_climb_rate_cm(const float vel, bool ignore_descent_limit) +void AC_PosControl::land_at_climb_rate_cm(float vel, bool ignore_descent_limit) { - float vel_temp = vel; - input_vel_accel_z(vel_temp, 0, ignore_descent_limit); + input_vel_accel_z(vel, 0, ignore_descent_limit); } /// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. @@ -890,7 +889,7 @@ void AC_PosControl::land_at_climb_rate_cm(const float vel, bool ignore_descent_l /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The function alters the pos and vel to be the kinematic path based on accel /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. -void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float accel, bool limit_output) +void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, float accel, bool limit_output) { // calculated increased maximum acceleration if over speed float accel_z_cmss = _accel_max_z_cmss; @@ -917,11 +916,10 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, const float ac /// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm /// using the default position control kinematic path. -void AC_PosControl::set_alt_target_with_slew(const float& pos) +void AC_PosControl::set_alt_target_with_slew(float pos) { - float posf = pos; float zero = 0; - input_pos_vel_accel_z(posf, zero, 0); + input_pos_vel_accel_z(pos, zero, 0); } /// update_pos_offset_z - updates the vertical offsets used by terrain following diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index c4cab730ae042..53c69ec33aa11 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -187,24 +187,24 @@ class AC_PosControl /// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. - virtual void input_accel_z(const float accel); + virtual void input_accel_z(float accel); /// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. /// The function alters the vel to be the kinematic path based on accel /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. - virtual void input_vel_accel_z(float &vel, const float accel, bool ignore_descent_limit, bool limit_output = true); + virtual void input_vel_accel_z(float &vel, float accel, bool ignore_descent_limit, bool limit_output = true); /// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// The zero target altitude is varied to follow pos_offset_z - void set_pos_target_z_from_climb_rate_cm(const float vel); + void set_pos_target_z_from_climb_rate_cm(float vel); /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing. - void land_at_climb_rate_cm(const float vel, bool ignore_descent_limit); + void land_at_climb_rate_cm(float vel, bool ignore_descent_limit); /// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. @@ -214,7 +214,7 @@ class AC_PosControl /// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm /// using the default position control kinematic path. - void set_alt_target_with_slew(const float& pos); + void set_alt_target_with_slew(float pos); /// update_pos_offset_z - updates the vertical offsets used by terrain following void update_pos_offset_z(float pos_offset);