Browse Source

AC_AttitudeControl: use M_PI_F instead of (float)M_PI

mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
84ac721340
  1. 6
      libraries/AC_AttitudeControl/AC_PosControl.cpp

6
libraries/AC_AttitudeControl/AC_PosControl.cpp

@ -876,9 +876,9 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) @@ -876,9 +876,9 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/(float)M_PI),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*(float)M_PI/18000);
_roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/(float)M_PI), -lean_angle_max, lean_angle_max);
_pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000);
_roll_target = constrain_float(fast_atan(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max);
}
// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s

Loading…
Cancel
Save