AC_AttitudeControl: Add const where possible

Author: lthall

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -345,9 +345,9 @@ void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vec
 void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
 {
     // 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_roll_angle_rad = euler_roll_angle_cd * 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);
     input_euler_angle_roll_pitch_euler_rate_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_rate_rads);
 }
 
@@ -403,9 +403,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_rad(float e
 void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
 {
     // 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_roll_angle_rad = euler_roll_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);
 
     input_euler_angle_roll_pitch_yaw_rad(euler_roll_angle_rad, euler_pitch_angle_rad, euler_yaw_angle_rad, slew_yaw);
 }
@@ -470,9 +470,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_rad(float euler_roll_a
 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);
 
     input_euler_rate_roll_pitch_yaw_rads(euler_roll_rate_rads, euler_pitch_rate_rads, euler_yaw_rate_rads);
 }
@@ -521,9 +521,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw_rads(float euler_roll_r
 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);
 
     input_rate_bf_roll_pitch_yaw_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
 }
@@ -567,9 +567,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_rads(float roll_rate_bf_ra
 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);
 
     input_rate_bf_roll_pitch_yaw_2_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
 }
@@ -598,9 +598,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2_rads(float roll_rate_bf_
 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);
 
     input_rate_bf_roll_pitch_yaw_3_rads(roll_rate_bf_rads, pitch_rate_bf_rads, yaw_rate_bf_rads);
 }
@@ -692,9 +692,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_no_shaping_rads(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);
 
     input_angle_step_bf_roll_pitch_yaw_rad(roll_angle_step_bf_rad, pitch_angle_step_bf_rad, yaw_angle_step_bf_rad);
 }
@@ -724,9 +724,9 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw_rad(float roll_angle
 void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw_cds(float roll_rate_step_bf_cds, float pitch_rate_step_bf_cds, float yaw_rate_step_bf_cds)
 {
     // Convert from centidegrees on public interface to radians
-    float roll_rate_step_bf_rads = roll_rate_step_bf_cds * radians(0.01f);
-    float pitch_rate_step_bf_rads = pitch_rate_step_bf_cds * radians(0.01f);
-    float yaw_rate_step_bf_rads = yaw_rate_step_bf_cds * radians(0.01f);
+    const float roll_rate_step_bf_rads = roll_rate_step_bf_cds * radians(0.01f);
+    const float pitch_rate_step_bf_rads = pitch_rate_step_bf_cds * radians(0.01f);
+    const float yaw_rate_step_bf_rads = yaw_rate_step_bf_cds * radians(0.01f);
 
     input_rate_step_bf_roll_pitch_yaw_rads(roll_rate_step_bf_rads, pitch_rate_step_bf_rads, yaw_rate_step_bf_rads);
 }
@@ -750,7 +750,7 @@ void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw_rads(float roll_rate_
 void AC_AttitudeControl::input_thrust_vector_rate_heading_cds(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
 {
     // Convert from centidegrees on public interface to radians
-    float heading_rate_rads = heading_rate_cds * radians(0.01f);
+    const float heading_rate_rads = heading_rate_cds * radians(0.01f);
 
     input_thrust_vector_rate_heading_rads(thrust_vector, heading_rate_rads, slew_yaw);
 }
@@ -812,8 +812,8 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading_rads(const Vector3f& t
 void AC_AttitudeControl::input_thrust_vector_heading_cd(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
 {
     // Convert from centidegrees on public interface to radians
-    float heading_rate_rads = heading_rate_cds * radians(0.01f);
-    float heading_angle_rad = heading_angle_cd * radians(0.01f);
+    const float heading_rate_rads = heading_rate_cds * radians(0.01f);
+    const float heading_angle_rad = heading_angle_cd * radians(0.01f);
 
     input_thrust_vector_heading_rad(thrust_vector, heading_angle_rad, heading_rate_rads);
 }
@@ -1000,10 +1000,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));
@@ -1107,7 +1107,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;
@@ -1309,11 +1309,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());
@@ -1324,11 +1324,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());