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

remove heartbeat from px4 controller

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 20:52:02 +02:00
parent 37b5f14f72
commit c4e9e3bb10
1 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -17,7 +17,7 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
// #include <px4_msgs/msg/timesync.hpp> // #include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp> #include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp> #include <px4_msgs/msg/vehicle_control_mode.hpp>
#include <px4_msgs/msg/offboard_control_mode.hpp> // #include <px4_msgs/msg/offboard_control_mode.hpp>
using namespace std::chrono_literals; using namespace std::chrono_literals;
@@ -30,7 +30,7 @@ public:
vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10); vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10); trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10); // offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
// create timer to send vehicle attitude setpoints every second // create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this));
@@ -43,7 +43,7 @@ private:
rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
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::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_; // rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
double start_time_; double start_time_;
@@ -61,10 +61,10 @@ private:
setpoint_count++; setpoint_count++;
// if (setpoint_count % 20 == 0 && thrust <= 1) { if (setpoint_count % 20 == 0 && thrust <= 1) {
// thrust += 0.1; thrust += 0.1;
// RCLCPP_INFO(this->get_logger(), "increasing thrust"); RCLCPP_INFO(this->get_logger(), "increasing thrust to %d", thrust);
// } }
if (setpoint_count == 20) if (setpoint_count == 20)
{ {
@@ -89,9 +89,9 @@ private:
// 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] = -0.6; // down, 100% thrust up msg.thrust_body[2] = -thrust; // down, 100% thrust up
calculate_quaternion(q, 0, degrees_to_radians(20), 0); calculate_quaternion(q, 0, 0, 0);
} }
else else
@@ -129,7 +129,7 @@ private:
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
// send heartbeat together with attitude setpoint // send heartbeat together with attitude setpoint
send_heartbeat(); // send_heartbeat();
vehicle_setpoint_publisher_->publish(msg); vehicle_setpoint_publisher_->publish(msg);
} }