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

assign angle to starting angle while not in user control

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 19:07:39 +02:00
parent 4a5a8eb25e
commit 4fa2387c85
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

@@ -194,7 +194,7 @@ private:
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]);
} }
last_angle = request->yaw; last_angle += request->yaw;
new_setpoint = true; new_setpoint = true;
RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle);
response->success = true; response->success = true;
@@ -422,6 +422,7 @@ private:
// https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf
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;
} }
X = msg->x; X = msg->x;
Y = msg->y; Y = msg->y;