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

change vehicle attitude setpoint to trajectory setpoint for velocity

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 12:45:27 +02:00
parent 4ed6dde80e
commit 6bdb10dd5c
2 changed files with 14 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/px4_connection/src/heartbeat.cpp
View File

@@ -40,9 +40,9 @@ private:
// set message to enable attitude // set message to enable attitude
auto msg = px4_msgs::msg::OffboardControlMode(); auto msg = px4_msgs::msg::OffboardControlMode();
msg.position = false; msg.position = false;
msg.velocity = false; msg.velocity = true;
msg.acceleration = false; msg.acceleration = false;
msg.attitude = true; msg.attitude = false;
msg.body_rate = false; msg.body_rate = false;
msg.actuator = false; msg.actuator = false;

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

@@ -8,10 +8,12 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
*/ */
#include <chrono> #include <chrono>
#include <math.h>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp> #include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#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>
@@ -26,6 +28,7 @@ public:
// create a publisher on the vehicle attitude setpoint topic // 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_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);
// set to offboard mode // set to offboard mode
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6); this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
@@ -41,6 +44,7 @@ public:
private: 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::VehicleCommand>::SharedPtr vehicle_command_publisher_; rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr vehicle_command_publisher_;
rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_;
@@ -51,30 +55,20 @@ private:
void send_setpoint() void send_setpoint()
{ {
// set message to enable attitude // set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); auto msg = px4_msgs::msg::TrajectorySetpoint();
// get timestamp and publish message // 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/TrajectorySetpoint.msg
msg.roll_body = 1; msg.velocity[0] = 0; // north
msg.pitch_body = 1; msg.velocity[1] = 0; // east
msg.yaw_body = 1; msg.velocity[2] = 1.0; // down (1 m/s -> TODO test if this accounts for 9.81 m/s earth gravity)
msg.yaw_sp_move_rate = 1; msg.yaw = (0f * M_PI) / 180f; // 0 degrees rotation of yaw
msg.yawspeed = 0; // 0 rotation speed
for (int i = 0; i < 4; i++)
{
msg.q_d[i] = 1;
}
msg.thrust_body[0] = 0;
msg.thrust_body[1] = 0;
msg.thrust_body[2] = -10; // negative throttle amount
msg.reset_integral = false;
msg.fw_control_yaw_wheel = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_setpoint_publisher_->publish(msg); trajectory_setpoint_publisher->publish(msg);
RCLCPP_INFO(this->get_logger(), "published message"); RCLCPP_INFO(this->get_logger(), "published message");
} }