Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use fast_atan2 for bearing calcs

Browse Source
This commit is contained in:
Randy Mackay 2014-06-06 17:55:31 +09:00
parent 083f2898a9
commit f23e94707c
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

@ -822,7 +822,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
_yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
@ -861,7 +861,7 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector
// To-Do: move this to math library
float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const
{
float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
float bearing = 9000 + fast_atan2(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
if (bearing < 0) {
bearing += 36000;
}