aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: use fast tan for accel to lean angle calcs

Browse Source 
Committed on behalf of Leonard
This commit is contained in:
Randy Mackay 2013-05-30 18:25:04 +09:00
parent d81b7b507d
commit a1926441da
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -585,8 +585,8 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc
accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain_float((accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
_desired_pitch = constrain_float((-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
_desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
}
// get_bearing_cd - return bearing in centi-degrees between two positions