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

add setting to offboard mode and arming

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-25 16:53:40 +02:00
parent 4540a24d85
commit 455c495685
1 changed files with 41 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,6 +13,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
#include <px4_msgs/msg/timesync.hpp>
#include <px4_msgs/msg/vehicle_command.hpp>
#include <px4_msgs/msg/vehicle_control_mode.hpp>
using namespace std::chrono_literals;
@@ -23,17 +25,29 @@ public:
{
// create a publisher on the vehicle attitude setpoint topic
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);
// set to offboard mode
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
// arm the drone
publish_vehicle_command(VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0);
RCLCPP_INFO(this->get_logger(), "Arm command sent");
// create timer to send vehicle attitude setpoints every second
timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
}
private:
rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
/**
* @brief Publish offboard control mode messages as a heartbeat.
* 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.
*
*/
void send_setpoint()
{
// set message to enable attitude
@@ -41,7 +55,7 @@ private:
// get timestamp and publish message
//https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg
// https://github.com/PX4/px4_msgs/blob/main/msg/VehicleAttitudeSetpoint.msg
msg.roll_body = 1;
msg.pitch_body = 1;
msg.yaw_body = 1;
@@ -64,8 +78,27 @@ private:
RCLCPP_INFO(this->get_logger(), "published message");
}
rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr vehicle_setpoint_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
/**
* @brief Publish vehicle commands
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)
* @param param1 Command parameter 1
* @param param2 Command parameter 2
*/
void publish_vehicle_command(uint16_t command, float param1, float param2)
{
px4_msgs::msg::VehicleCommand msg{};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 1;
msg.target_component = 1;
msg.source_system = 1;
msg.source_component = 1;
msg.from_external = true;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_command_publisher_->publish(msg);
}
};
int main(int argc, char *argv[])