fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 10:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskOrbit: Fix zero vector resulting in NaN output and ignored setpoint

Browse Source
This commit is contained in:
Matthias Grob
2017-11-06 18:05:30 +01:00
committed by Beat Küng
parent a570390dfb
commit 3450ccb2b2
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+3 -2
View File
@@ -94,11 +94,12 @@ public:
/* xy velocity to go around in a circle */
matrix::Vector2<float> velocity_xy = matrix::Vector2f(center_to_position(1), -center_to_position(0));
velocity_xy.normalize();
velocity_xy = velocity_xy.unit_or_zero();
velocity_xy *= v;
/* xy velocity adjustment to stay on the radius distance */
velocity_xy += (r - center_to_position.norm()) * center_to_position.normalized();
velocity_xy += (r - center_to_position.norm()) * center_to_position.unit_or_zero();
float yaw = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
_set_position_setpoint(matrix::Vector3f(NAN, NAN, z));