AC_AttitudeControl: Add const where possible

Author: lthall

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -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 = euler_roll_angle_cd * radians(0.01f);
-    float euler_pitch_angle_rad = euler_pitch_angle_cd * radians(0.01f);
-    float euler_yaw_rate_rads = euler_yaw_rate_cds * radians(0.01f);
+    const float euler_pitch_angle_rad = euler_pitch_angle_cd * radians(0.01f);
+    const float euler_yaw_rate_rads = euler_yaw_rate_cds * radians(0.01f);
 
     // update attitude target
     update_attitude_target();
@@ -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 = euler_roll_angle_cd * radians(0.01f);
-    float euler_pitch_angle_rad = euler_pitch_angle_cd * radians(0.01f);
-    float euler_yaw_angle_rad = euler_yaw_angle_cd * radians(0.01f);
+    const float euler_pitch_angle_rad = euler_pitch_angle_cd * radians(0.01f);
+    const float euler_yaw_angle_rad = euler_yaw_angle_cd * radians(0.01f);
 
     // update attitude target
     update_attitude_target();
@@ -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 = euler_roll_rate_cds * radians(0.01f);
-    float euler_pitch_rate_rads = euler_pitch_rate_cds * radians(0.01f);
-    float euler_yaw_rate_rads = euler_yaw_rate_cds * radians(0.01f);
+    const float euler_roll_rate_rads = euler_roll_rate_cds * radians(0.01f);
+    const float euler_pitch_rate_rads = euler_pitch_rate_cds * radians(0.01f);
+    const float euler_yaw_rate_rads = euler_yaw_rate_cds * radians(0.01f);
 
     // update attitude target
     update_attitude_target();
@@ -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 = roll_rate_bf_cds * radians(0.01f);
-    float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
-    float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
+    const float roll_rate_bf_rads = roll_rate_bf_cds * radians(0.01f);
+    const float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
+    const float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
 
     // update attitude target
     update_attitude_target();
@@ -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 = roll_rate_bf_cds * radians(0.01f);
-    float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
-    float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
+    const float roll_rate_bf_rads = roll_rate_bf_cds * radians(0.01f);
+    const float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
+    const float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
 
     // Compute acceleration-limited body frame rates
     // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
@@ -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 = roll_rate_bf_cds * radians(0.01f);
-    float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
-    float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
+    const float roll_rate_bf_rads = roll_rate_bf_cds * radians(0.01f);
+    const float pitch_rate_bf_rads = pitch_rate_bf_cds * radians(0.01f);
+    const float yaw_rate_bf_rads = yaw_rate_bf_cds * radians(0.01f);
 
     // Update attitude error
     Vector3f attitude_error;
@@ -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 = roll_angle_step_bf_cd * radians(0.01f);
-    float pitch_angle_step_bf_rad = pitch_angle_step_bf_cd * radians(0.01f);
-    float yaw_angle_step_bf_rad = yaw_angle_step_bf_cd * radians(0.01f);
+    const float roll_angle_step_bf_rad = roll_angle_step_bf_cd * radians(0.01f);
+    const float pitch_angle_step_bf_rad = pitch_angle_step_bf_cd * radians(0.01f);
+    const float yaw_angle_step_bf_rad = yaw_angle_step_bf_cd * radians(0.01f);
 
     // rotate attitude target by desired step
     Quaternion attitude_target_update;
@@ -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));
@@ -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;
@@ -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());
@@ -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());