comment

2023-05-15 18:22:24 +02:00
parent c95191af5f
commit 3412fe75c1
1 changed files with 7 additions and 0 deletions
--- 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;