Copter: use the right constrain() type in Attitude code

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>

ArduCopter/Attitude.pde

@@ -7,14 +7,14 @@ get_stabilize_roll(int32_t target_angle)
     target_angle            = wrap_180_cd(target_angle - ahrs.roll_sensor);
 
     // limit the error we're feeding to the PID
-    target_angle            = constrain(target_angle, -4500, 4500);
+    target_angle            = constrain_int32(target_angle, -4500, 4500);
 
         // convert to desired Rate:
     int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
 
     int16_t i_stab;
     if(labs(ahrs.roll_sensor) < 500) {
-        target_angle            = constrain(target_angle, -500, 500);
+        target_angle            = constrain_int32(target_angle, -500, 500);
         i_stab                          = g.pi_stabilize_roll.get_i(target_angle, G_Dt);
     }else{
         i_stab                          = g.pi_stabilize_roll.get_integrator();
@@ -31,14 +31,14 @@ get_stabilize_pitch(int32_t target_angle)
     target_angle            = wrap_180_cd(target_angle - ahrs.pitch_sensor);
 
     // limit the error we're feeding to the PID
-    target_angle            = constrain(target_angle, -4500, 4500);
+    target_angle            = constrain_int32(target_angle, -4500, 4500);
 
     // convert to desired Rate:
     int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle);
 
     int16_t i_stab;
     if(labs(ahrs.pitch_sensor) < 500) {
-        target_angle            = constrain(target_angle, -500, 500);
+        target_angle            = constrain_int32(target_angle, -500, 500);
         i_stab                          = g.pi_stabilize_pitch.get_i(target_angle, G_Dt);
     }else{
         i_stab                          = g.pi_stabilize_pitch.get_integrator();
@@ -60,7 +60,7 @@ get_stabilize_yaw(int32_t target_angle)
 
     // limit the error we're feeding to the PID
 
-    angle_error             = constrain(angle_error, -4500, 4500);
+    angle_error             = constrain_int32(angle_error, -4500, 4500);
 
     // convert angle error to desired Rate:
     target_rate = g.pi_stabilize_yaw.get_p(angle_error);
@@ -69,7 +69,7 @@ get_stabilize_yaw(int32_t target_angle)
     // do not use rate controllers for helicotpers with external gyros
 #if FRAME_CONFIG == HELI_FRAME
     if(motors.ext_gyro_enabled) {
-        g.rc_4.servo_out = constrain((target_rate + i_term), -4500, 4500);
+        g.rc_4.servo_out = constrain_int32((target_rate + i_term), -4500, 4500);
     }
 #endif
 
@@ -130,12 +130,12 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
 
     // ensure that we don't reach gimbal lock
     if (labs(roll_axis) > 4500 && g.acro_trainer_enabled) {
-        roll_axis	= constrain(roll_axis, -4500, 4500);
+        roll_axis	= constrain_int32(roll_axis, -4500, 4500);
         angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
     } else {
         // angle error with maximum of +- max_angle_overshoot
         angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
-        angle_error	= constrain(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
+        angle_error	= constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
     }
 
     if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
@@ -166,12 +166,12 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
 
     // ensure that we don't reach gimbal lock
     if (labs(pitch_axis) > 4500) {
-        pitch_axis	= constrain(pitch_axis, -4500, 4500);
+        pitch_axis	= constrain_int32(pitch_axis, -4500, 4500);
         angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
     } else {
         // angle error with maximum of +- max_angle_overshoot
         angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
-        angle_error	= constrain(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
+        angle_error	= constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
     }
 
     if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
@@ -203,7 +203,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
     angle_error = wrap_180_cd(nav_yaw - ahrs.yaw_sensor);
 
     // limit the maximum overshoot
-    angle_error	= constrain(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
+    angle_error	= constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
 
     if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
     	angle_error = 0;
@@ -329,7 +329,7 @@ get_heli_rate_roll(int32_t target_rate)
     output = p + i + d + ff;
 
     // constrain output
-    output = constrain(output, -4500, 4500);
+    output = constrain_int32(output, -4500, 4500);
 
 #if LOGGING_ENABLED == ENABLED
     // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -381,7 +381,7 @@ get_heli_rate_pitch(int32_t target_rate)
     output = p + i + d + ff;
 
     // constrain output
-    output = constrain(output, -4500, 4500);
+    output = constrain_int32(output, -4500, 4500);
 
 #if LOGGING_ENABLED == ENABLED
     // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -469,7 +469,7 @@ get_rate_roll(int32_t target_rate)
     output = p + i + d;
 
     // constrain output
-    output = constrain(output, -5000, 5000);
+    output = constrain_int32(output, -5000, 5000);
 
 #if LOGGING_ENABLED == ENABLED
     // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -510,7 +510,7 @@ get_rate_pitch(int32_t target_rate)
     output = p + i + d;
 
     // constrain output
-    output = constrain(output, -5000, 5000);
+    output = constrain_int32(output, -5000, 5000);
 
 #if LOGGING_ENABLED == ENABLED
     // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -546,7 +546,7 @@ get_rate_yaw(int32_t target_rate)
     d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
 
     output  = p+i+d;
-    output = constrain(output, -4500, 4500);
+    output = constrain_int32(output, -4500, 4500);
 
 #if LOGGING_ENABLED == ENABLED
     // log output if PID loggins is on and we are tuning the yaw
@@ -567,7 +567,7 @@ get_rate_yaw(int32_t target_rate)
     int16_t yaw_limit = 2200 + abs(g.rc_4.control_in);
 
     // smoother Yaw control:
-    return constrain(output, -yaw_limit, yaw_limit);
+    return constrain_int32(output, -yaw_limit, yaw_limit);
 #endif // TRI_FRAME
 }
 #endif // !HELI_FRAME
@@ -603,7 +603,7 @@ get_of_roll(int32_t input_roll)
             d = 0;
         }
         // limit amount of change and maximum angle
-        of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
+        of_roll = constrain_int32(new_roll, (of_roll-20), (of_roll+20));
 
  #if LOGGING_ENABLED == ENABLED
         // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -618,7 +618,7 @@ get_of_roll(int32_t input_roll)
     }
 
     // limit max angle
-    of_roll = constrain(of_roll, -1000, 1000);
+    of_roll = constrain_int32(of_roll, -1000, 1000);
 
     return input_roll+of_roll;
 #else
@@ -657,7 +657,7 @@ get_of_pitch(int32_t input_pitch)
         }
 
         // limit amount of change
-        of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
+        of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20));
 
  #if LOGGING_ENABLED == ENABLED
         // log output if PID logging is on and we are tuning the rate P, I or D gains
@@ -670,7 +670,7 @@ get_of_pitch(int32_t input_pitch)
     }
 
     // limit max angle
-    of_pitch = constrain(of_pitch, -1000, 1000);
+    of_pitch = constrain_int32(of_pitch, -1000, 1000);
 
     return input_pitch+of_pitch;
 #else
@@ -895,8 +895,8 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control)
     }
 
     // ensure reasonable throttle values
-    throttle_control = constrain(throttle_control,0,1000);
-    g.throttle_mid = constrain(g.throttle_mid,300,700);
+    throttle_control = constrain_int16(throttle_control,0,1000);
+    g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
 
     // check throttle is above, below or in the deadband
     if (throttle_control < THROTTLE_IN_MIDDLE) {
@@ -929,7 +929,7 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
     }
 
     // ensure a reasonable throttle value
-    throttle_control = constrain(throttle_control,0,1000);
+    throttle_control = constrain_int16(throttle_control,0,1000);
 
     // check throttle is above, below or in the deadband
     if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
@@ -969,7 +969,7 @@ get_initial_alt_hold( int32_t alt_cm, int16_t climb_rate_cms)
             target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) );
         }
     }
-    return constrain(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT);
+    return constrain_int32(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT);
 }
 
 // get_throttle_rate - calculates desired accel required to achieve desired z_target_speed