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

add saving of local position

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-16 15:48:58 +02:00
parent 40ea450b78
commit 9b915ee698
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -117,6 +117,9 @@ private:
float X; float X;
float Y; float Y;
float local_x = 0;
float local_y = 0;
uint32_t discrete_time_index = 0; uint32_t discrete_time_index = 0;
// result quaternion // result quaternion
@@ -308,7 +311,8 @@ private:
auto msg = px4_msgs::msg::TrajectorySetpoint(); auto msg = px4_msgs::msg::TrajectorySetpoint();
RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
msg.position = {des_x, des_y, des_z}; RCLCPP_INFO(this->get_logger("local position: %f %f", local_x, local_y));
msg.position = {local_x, local_y, des_z};
msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.velocity = {dot_des_x, dot_des_y, 0.0};
msg.yaw = gamma; //-3.14; // [-PI:PI] msg.yaw = gamma; //-3.14; // [-PI:PI]
@@ -423,6 +427,9 @@ private:
rho_0 = pow(msg->x,2) + pow(msg->y,2); rho_0 = pow(msg->x,2) + pow(msg->y,2);
theta_0 = atan2(msg->y, msg->x); theta_0 = atan2(msg->y, msg->x);
last_angle = msg->heading; last_angle = msg->heading;
local_x = msg->x;
local_y = msg->y;
} }
X = msg->x; X = msg->x;
Y = msg->y; Y = msg->y;