dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskOrbit: further simplify circling yaw setpoint calculation 

The whole angle adding and pi wrap was just necessary because the
correct atan2 arguments were never completely thought through.
This commit is contained in:
Matthias Grob 2020-12-16 14:58:35 +01:00 committed by Julian Kent
parent f435bea57c
commit 44606ca872
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -250,7 +250,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
_yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + (sign(_v) * M_PI_F / 2.f));
_yaw_setpoint = atan2f(sign(_v) * center_to_position(0), -sign(_v) * center_to_position(1));
_yawspeed_setpoint = _v / _r;
break;
@ -260,7 +260,7 @@ void FlightTaskOrbit::generate_circle_yaw_setpoints(const Vector2f &center_to_po
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
default:
_yaw_setpoint = wrap_pi(atan2f(center_to_position(1), center_to_position(0)) + M_PI_F);
_yaw_setpoint = atan2f(-center_to_position(1), -center_to_position(0));
// yawspeed feed-forward because we know the necessary angular rate
_yawspeed_setpoint = _v / _r;
break;