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

try different values

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 18:29:20 +02:00
parent 1224735954
commit f767ee2583
1 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,16 +79,17 @@ private:
// result quaternion // result quaternion
std::array<float, 4> q = {0, 0, 0, 0}; std::array<float, 4> q = {0, 0, 0, 0};
if (this->get_clock()->now().seconds() - start_time_ < 10) if (this->get_clock()->now().seconds() - start_time_ < 20)
{ {
// move up? // move up?
msg.thrust_body[0] = 0; // north msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = 1; // down, 100% thrust up msg.thrust_body[2] = 1; // down, 100% thrust up
calculate_quaternion(q, 0, 1, 1); calculate_quaternion(q, 0, degrees_to_radians(20), 0);
} }
else if (this->get_clock()->now().seconds() - start_time_ > 20)
else
{ {
if (flying) if (flying)
{ {
@@ -97,20 +98,20 @@ private:
RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds"); RCLCPP_INFO(this->get_logger(), "Disarm command sent after 20 seconds");
} }
} }
else // else
{ // {
if (!has_sent_status) // if (!has_sent_status)
{ // {
has_sent_status = true; // has_sent_status = true;
RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1"); // RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1");
} // }
// no thrust // // no thrust
msg.thrust_body[0] = 0; // north // msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 1; // east // msg.thrust_body[1] = 1; // east
msg.thrust_body[2] = 0.5; // down // msg.thrust_body[2] = 0.5; // down
calculate_quaternion(q, 0, degrees_to_radians(10), 0); // calculate_quaternion(q, 0, degrees_to_radians(10), 0);
} // }
msg.q_d[0] = q.at(0); msg.q_d[0] = q.at(0);
msg.q_d[1] = q.at(1); msg.q_d[1] = q.at(1);