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

add z in calculation

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 17:15:58 +02:00
parent 1c0e6ade6d
commit e7e06cf6d7
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -189,7 +189,7 @@ private:
{ {
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++)
{ {
position[i] = request->values[i]; position[i] += request->values[i];
} }
RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
} }
@@ -338,6 +338,7 @@ private:
// from polar to cartesian coordinates // from polar to cartesian coordinates
des_x = rho * cos(theta); des_x = rho * cos(theta);
des_y = rho * sin(theta); des_y = rho * sin(theta);
des_z = position[3]; // the z position can be set to the received height
// velocity computation // velocity computation
float dot_rho = K * omega; float dot_rho = K * omega;