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

change to use _body msg params

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-02 16:39:13 +02:00
parent 0a18a68f5c
commit 09b6c6110e
1 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -21,7 +21,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
#include <drone_services/srv/set_attitude.hpp> #include <drone_services/srv/set_attitude.hpp>
// #include <px4_msgs/msg/offboard_control_mode.hpp> // #include <px4_msgs/msg/offboard_control_mode.hpp>
#define D_SPEED(x) -x - 9.81 #define D_SPEED(x) -x - 9.81
@@ -69,6 +68,8 @@ private:
float last_setpoint[3] = {0, 0, 0}; float last_setpoint[3] = {0, 0, 0};
float last_angle = 0; float last_angle = 0;
float last_thrust = 0; float last_thrust = 0;
// result quaternion
std::array<float, 4> q = {0, 0, 0, 0};
void set_setpoint( void set_setpoint(
const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<rmw_request_id_t> request_header,
@@ -122,11 +123,12 @@ private:
// set message to enable attitude // set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
// result quaternion
std::array<float, 4> q = {0, 0, 0, 0};
if (this->get_clock()->now().seconds() - start_time_ < 30) // if (this->get_clock()->now().seconds() - start_time_ < 30)
{ // {
msg.yaw_body = last_setpoint[0];
msg.pitch_body = last_setpoint[1];
msg.roll_body = last_setpoint[2];
// 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
@@ -135,8 +137,7 @@ private:
RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); RCLCPP_INFO(this->get_logger(), "yaw:%f pitch:%f roll:%f thrust:%f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]); calculate_quaternion(q, last_setpoint[0], last_setpoint[1], last_setpoint[2]);
// }
}
// else // else
// { // {
// if (flying) // if (flying)
@@ -147,7 +148,6 @@ private:
// } // }
// } // }
// set quaternion // set quaternion
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);