From 3450ccb2b238bb7cea07b500f488158816734bb7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 6 Nov 2017 18:05:30 +0100 Subject: [PATCH] FlightTaskOrbit: Fix zero vector resulting in NaN output and ignored setpoint --- src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 7539f9575a..81e77b385b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -94,11 +94,12 @@ public: /* xy velocity to go around in a circle */ matrix::Vector2 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));