From 3412fe75c1e048768cf7875a5e71464c0055109b Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 15 May 2023 18:22:24 +0200
Subject: [PATCH] comment

---
 src/px4_connection/src/px4_controller.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 4e4cb08d..0b1bfdd1 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -415,6 +415,13 @@ private:
 
     void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)
     {
+        // set start values to current position
+        if (!user_in_control)
+        {
+            // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf
+            rho_0 = pow(msg->x,2) + pow(msg->y,2);
+            theta_0 = atan2(msg->y, msg->x);
+        }
         X = msg->x;
         Y = msg->y;
         float Z = msg->z;