Skip to content

AC_AttitudeControl use const where possible #30165

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
May 26, 2025
Merged

AC_AttitudeControl use const where possible #30165

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 30 additions & 30 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -346,8 +346,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_cd(float eu
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds * 0.01f);
const float euler_pitch_angle_rad = radians(euler_pitch_angle_cd * 0.01f);
const float euler_yaw_rate_rads = radians(euler_yaw_rate_cds * 0.01f);

// update attitude target
update_attitude_target();
Expand Down Expand Up @@ -400,8 +400,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_cd(float euler_roll_an
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd * 0.01f);
const float euler_pitch_angle_rad = radians(euler_pitch_angle_cd * 0.01f);
const float euler_yaw_angle_rad = radians(euler_yaw_angle_cd * 0.01f);

// update attitude target
update_attitude_target();
Expand Down Expand Up @@ -461,9 +461,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_cd(float euler_roll_an
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_cds(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate_rads = radians(euler_roll_rate_cds * 0.01f);
float euler_pitch_rate_rads = radians(euler_pitch_rate_cds * 0.01f);
float euler_yaw_rate_rads = radians(euler_yaw_rate_cds * 0.01f);
const float euler_roll_rate_rads = radians(euler_roll_rate_cds * 0.01f);
const float euler_pitch_rate_rads = radians(euler_pitch_rate_cds * 0.01f);
const float euler_yaw_rate_rads = radians(euler_yaw_rate_cds * 0.01f);

// update attitude target
update_attitude_target();
Expand Down Expand Up @@ -507,9 +507,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_cds(float euler_roll_ra
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);
const float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
const float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
const float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);

// update attitude target
update_attitude_target();
Expand Down Expand Up @@ -548,9 +548,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_cds(float roll_rate_bf_cds
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);
const float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
const float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
const float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);

// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
Expand All @@ -574,9 +574,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_cds(float roll_rate_bf_c
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3_cds(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);
const float roll_rate_bf_rads = radians(roll_rate_bf_cds * 0.01f);
const float pitch_rate_bf_rads = radians(pitch_rate_bf_cds * 0.01f);
const float yaw_rate_bf_rads = radians(yaw_rate_bf_cds * 0.01f);

// Update attitude error
Vector3f attitude_error;
Expand Down Expand Up @@ -657,9 +657,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_no_shaping_cds(float roll_
void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw_cd(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd)
{
// Convert from centidegrees on public interface to radians
float roll_angle_step_bf_rad = radians(roll_angle_step_bf_cd * 0.01f);
float pitch_angle_step_bf_rad = radians(pitch_angle_step_bf_cd * 0.01f);
float yaw_angle_step_bf_rad = radians(yaw_angle_step_bf_cd * 0.01f);
const float roll_angle_step_bf_rad = radians(roll_angle_step_bf_cd * 0.01f);
const float pitch_angle_step_bf_rad = radians(pitch_angle_step_bf_cd * 0.01f);
const float yaw_angle_step_bf_rad = radians(yaw_angle_step_bf_cd * 0.01f);

// rotate attitude target by desired step
Quaternion attitude_target_update;
Expand Down Expand Up @@ -937,10 +937,10 @@ void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitud
// attitude_target and attitude_body are passive rotations from target / body frames to the NED frame

// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
const Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector

// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up; // current thrust vector
const Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up; // current thrust vector

// the dot product is used to calculate the current lean angle for use of external functions
thrust_angle_rad = acosf(constrain_float(thrust_vector_up * att_body_thrust_vec,-1.0f,1.0f));
Expand Down Expand Up @@ -1044,7 +1044,7 @@ void AC_AttitudeControl::ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_m
euler_rad.y = constrain_float(euler_rad.y, -ang_vel_pitch_max_rads, ang_vel_pitch_max_rads);
}
} else {
Vector2f thrust_vector_ang_vel(euler_rad.x / ang_vel_roll_max_rads, euler_rad.y / ang_vel_pitch_max_rads);
const Vector2f thrust_vector_ang_vel(euler_rad.x / ang_vel_roll_max_rads, euler_rad.y / ang_vel_pitch_max_rads);
float thrust_vector_length = thrust_vector_ang_vel.length();
if (thrust_vector_length > 1.0f) {
euler_rad.x = thrust_vector_ang_vel.x * ang_vel_roll_max_rads / thrust_vector_length;
Expand Down Expand Up @@ -1246,11 +1246,11 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float dt_average = AP::scheduler().get_filtered_loop_time();
float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(dt_average), get_rate_pitch_pid().get_filt_D_alpha(dt_average));
float alpha_remaining = 1 - alpha;
const float dt_average = AP::scheduler().get_filtered_loop_time();
const float alpha = MIN(get_rate_pitch_pid().get_filt_E_alpha(dt_average), get_rate_pitch_pid().get_filt_D_alpha(dt_average));
const float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
const float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid().kD()) / _dt + get_rate_pitch_pid().kP());
if (is_positive(_ang_vel_pitch_max_degs)) {
rate_max = MIN(rate_max, get_ang_vel_pitch_max_rads());
Expand All @@ -1261,11 +1261,11 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float dt_average = AP::scheduler().get_filtered_loop_time();
float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(dt_average), get_rate_yaw_pid().get_filt_D_alpha(dt_average));
float alpha_remaining = 1 - alpha;
const float dt_average = AP::scheduler().get_filtered_loop_time();
const float alpha = MIN(get_rate_yaw_pid().get_filt_E_alpha(dt_average), get_rate_yaw_pid().get_filt_D_alpha(dt_average));
const float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
const float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid().kD()) / _dt + get_rate_yaw_pid().kP());
if (is_positive(_ang_vel_yaw_max_degs)) {
rate_max = MIN(rate_max, get_ang_vel_yaw_max_rads());
Expand Down
Loading