mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 04:28:30 -04:00
AC_WPNav: Compiler warnings: nuke fast_atan2()
per Randy's suggestion, fast_atan2() is no longer necessary over atan2() because only copter uses it and copter is no longer supported on future builds of APM
ccd578664f (commitcomment-11025083)
This commit is contained in:
parent
4ec2fb3a9c
commit
7fd285f483
@ -247,7 +247,7 @@ void AC_Circle::init_start_angle(bool use_heading)
|
||||
_angle = wrap_PI(_ahrs.yaw-PI);
|
||||
} else {
|
||||
// get bearing from circle center to vehicle in radians
|
||||
float bearing_rad = fast_atan2(curr_pos.y-_center.y,curr_pos.x-_center.x);
|
||||
float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
|
||||
_angle = wrap_PI(bearing_rad);
|
||||
}
|
||||
}
|
||||
|
@ -962,7 +962,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
|
||||
_pos_control.set_pos_target(target_pos);
|
||||
|
||||
// update the yaw
|
||||
_yaw = RadiansToCentiDegrees(fast_atan2(target_vel.y,target_vel.x));
|
||||
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
|
||||
|
||||
// advance spline time to next step
|
||||
_spline_time += _spline_time_scale*dt;
|
||||
@ -1001,7 +1001,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 + fast_atan2(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
|
||||
float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f;
|
||||
if (bearing < 0) {
|
||||
bearing += 36000;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user