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

add try sending trajectory setpoints

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-01 10:19:45 +02:00
parent 19d1987484
commit 3fe7b60374
2 changed files with 53 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@@ -52,34 +52,31 @@ private:
bool has_swithed = false;
int setpoint_count = 0;
float thrust = 0.5;
bool ready_to_fly = setpoint_count == 20;
/**
* @brief Only the attitude is enabled, because that is how the drone will be controlled.
*
*/
void send_setpoint()
void send_trajectory_setpoint()
{
auto msg = px4_msgs::msg::TrajectorySetpoint();
setpoint_count++;
msg.velocity[0] = 2;
msg.velocity[1] = 0;
msg.velocity[2] = 0.5;
if (setpoint_count % 20 == 0 && thrust <= 1) {
msg.yaw = -3.14;
msg.yawspeed = 0;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher->publush(msg);
}
void send_attitude_setpoint()
{
if (setpoint_count % 20 == 0 && thrust <= 1)
{
thrust += 0.1;
RCLCPP_INFO(this->get_logger(), "increasing thrust to %f", thrust);
}
if (setpoint_count == 20)
{
// switch to offboard mode and arm
// 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
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
RCLCPP_INFO(this->get_logger(), "Arm command sent");
flying = true;
}
// set message to enable attitude
auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
// result quaternion
@@ -88,8 +85,8 @@ private:
if (this->get_clock()->now().seconds() - start_time_ < 10)
{
// move up?
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = -0.8; // down, 100% thrust up
calculate_quaternion(q, 0, 0, 0);
@@ -97,19 +94,19 @@ private:
else if (this->get_clock()->now().seconds() - start_time_ < 30)
{
if (!has_swithed)
if (!has_swithed)
{
RCLCPP_INFO(this->get_logger(),"changing to 0.5 thrust");
RCLCPP_INFO(this->get_logger(), "changing to 0.5 thrust");
has_swithed = true;
}
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east
msg.thrust_body[2] = -0.5; // down, 100% thrust up
calculate_quaternion(q, 0, 0, 0);
}
else
else
{
if (flying)
{
@@ -132,6 +129,33 @@ private:
vehicle_setpoint_publisher_->publish(msg);
}
/**
* @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed.
*
*/
void send_setpoint()
{
// increase amount of setpoints sent
setpoint_count++;
// after 20 setpoints, send arm command to make drone actually fly
if (ready_to_fly)
{
// switch to offboard mode and arm
// 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
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
RCLCPP_INFO(this->get_logger(), "Arm command sent");
flying = true;
}
send_trajectory_setpoint();
}
/**
* @brief Publish vehicle commands
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)