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

fix

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

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

@@ -59,6 +59,8 @@ private:
{
// set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
std::array<float, 4> q = {0, 0, 0, 0};
if (this->get_clock()->now().seconds() - start_time_ < 5)
{
// move up?
@@ -66,7 +68,6 @@ private:
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = 0.5; // down, 50% thrust up
std::array<float, 4> array = {0, 0, 0, 0};
calculate_quaternion(array, degrees_to_radians(40), 0, 0);
}
else
@@ -76,14 +77,13 @@ private:
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = 0; // down
std::array<float, 4> q = {0, 0, 0, 0};
calculate_quaternion(q, degrees_to_radians(270), 0, 0);
}
msg.q_d[0] = q[0];
msg.q_d[1] = q[1];
msg.q_d[2] = q[2];
msg.q_d[3] = q[3];
msg.q_d[0] = q.at(0);
msg.q_d[1] = q.at(1);
msg.q_d[2] = q.at(2);
msg.q_d[3] = q.at(3);
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_setpoint_publisher_->publish(msg);