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

add print of q_d

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-04 14:49:07 +02:00
parent 3baad7c7bd
commit 5bd2885ad8
1 changed files with 9 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -216,6 +216,8 @@ private:
msg.q_d[2] = base_q[2] + q.at(2); msg.q_d[2] = base_q[2] + q.at(2);
msg.q_d[3] = base_q[3] + q.at(3); msg.q_d[3] = base_q[3] + q.at(3);
RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]);
msg.yaw_sp_move_rate = 0; msg.yaw_sp_move_rate = 0;
msg.reset_integral = false; msg.reset_integral = false;
msg.fw_control_yaw_wheel = false; msg.fw_control_yaw_wheel = false;
@@ -255,6 +257,13 @@ private:
void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
{ {
/*
average q:
- 0.7313786745071411
- 0.005042835604399443
- 0.0002370707516092807
- 0.6819528937339783
*/
if (!armed) if (!armed)
{ {
if (base_q_amount > 2) if (base_q_amount > 2)