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

test move up

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 17:25:26 +02:00
parent 8acd7f2c73
commit 52132684d2
1 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,6 +40,8 @@ public:
// create timer to send vehicle attitude setpoints every second // create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
start_time_ = this->get_clock()->now().seconds();
} }
private: private:
@@ -47,6 +49,7 @@ private:
rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher; rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr trajectory_setpoint_publisher;
rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
double start_time_;
/** /**
* @brief Only the attitude is enabled, because that is how the drone will be controlled. * @brief Only the attitude is enabled, because that is how the drone will be controlled.
@@ -56,20 +59,27 @@ private:
{ {
// set message to enable attitude // set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
if (this->get_clock()->now().seconds() - start_time_ < 5)
{
// 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] = 0.5; // down, 50% thrust up
std::array<float, 4> array = {0, 0, 0, 0}; std::array<float, 4> array = {0, 0, 0, 0};
calculate_quaternion(array, 0, degrees_to_radians(20), 0); calculate_quaternion(array, degrees_to_radians(40), 0, 0);
std::string elements = "";
for (float f : array)
{
elements += std::to_string(f) + " ";
} }
RCLCPP_INFO(this->get_logger(), "q_d: %s", elements); else
{
// no thrust
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = 0; // down
std::array<float, 4> array = {0, 0, 0, 0};
calculate_quaternion(array, degrees_to_radians(270), 0, 0);
}
msg.q_d[0] = array[0]; msg.q_d[0] = array[0];
msg.q_d[1] = array[1]; msg.q_d[1] = array[1];
msg.q_d[2] = array[2]; msg.q_d[2] = array[2];
@@ -102,7 +112,6 @@ private:
vehicle_command_publisher_->publish(msg); vehicle_command_publisher_->publish(msg);
} }
/** /**
* @brief calculates a quaternion based on the given input values * @brief calculates a quaternion based on the given input values
* *