sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

comment

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 18:22:24 +02:00
parent c95191af5f
commit 3412fe75c1
1 changed files with 7 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/px4_connection/src/px4_controller.cpp
View File

@@ -415,6 +415,13 @@ private:
void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) 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; X = msg->x;
Y = msg->y; Y = msg->y;
float Z = msg->z; float Z = msg->z;