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

try adding hover?

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 20:57:35 +02:00
parent 0c543614a9
commit 799ef4237e
1 changed files with 17 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,6 +49,7 @@ private:
double start_time_; double start_time_;
bool has_sent_status = false; bool has_sent_status = false;
bool flying = false; bool flying = false;
bool has_swithed = false;
int setpoint_count = 0; int setpoint_count = 0;
float thrust = 0.5; float thrust = 0.5;
@@ -84,7 +85,7 @@ private:
// result quaternion // result quaternion
std::array<float, 4> q = {0, 0, 0, 0}; std::array<float, 4> q = {0, 0, 0, 0};
if (this->get_clock()->now().seconds() - start_time_ < 30) if (this->get_clock()->now().seconds() - start_time_ < 15)
{ {
// move up? // move up?
msg.thrust_body[0] = 0; // north msg.thrust_body[0] = 0; // north
@@ -94,7 +95,21 @@ private:
calculate_quaternion(q, 0, 0, 0); calculate_quaternion(q, 0, 0, 0);
} }
else else if (this->get_clock()->now().seconds() - start_time_ < 30)
{
if (!has_swithed)
{
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[2] = -0.5; // down, 100% thrust up
calculate_quaternion(q, 0, 0, 0);
}
else
{ {
if (flying) if (flying)
{ {
@@ -103,20 +118,6 @@ private:
RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds"); RCLCPP_INFO(this->get_logger(), "Disarm command sent after 30 seconds");
} }
} }
// else
// {
// if (!has_sent_status)
// {
// has_sent_status = true;
// RCLCPP_INFO(this->get_logger(), "changing down thrust to 0.5 and east to 1");
// }
// // no thrust
// msg.thrust_body[0] = 0; // north
// msg.thrust_body[1] = 1; // east
// msg.thrust_body[2] = 0.5; // down
// calculate_quaternion(q, 0, degrees_to_radians(10), 0);
// }
msg.q_d[0] = q.at(0); msg.q_d[0] = q.at(0);
msg.q_d[1] = q.at(1); msg.q_d[1] = q.at(1);
@@ -128,28 +129,9 @@ private:
msg.fw_control_yaw_wheel = false; msg.fw_control_yaw_wheel = false;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
// send heartbeat together with attitude setpoint
// send_heartbeat();
vehicle_setpoint_publisher_->publish(msg); vehicle_setpoint_publisher_->publish(msg);
} }
// void send_heartbeat()
// {
// // set message to enable attitude
// auto msg = px4_msgs::msg::OffboardControlMode();
// msg.position = false;
// msg.velocity = false;
// msg.acceleration = false;
// msg.attitude = true;
// msg.body_rate = false;
// msg.actuator = false;
// // get timestamp and publish message
// msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
// offboard_control_mode_publisher_->publish(msg);
// // RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
// }
/** /**
* @brief Publish vehicle commands * @brief Publish vehicle commands
* @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes) * @param command Command code (matches VehicleCommand and MAVLink MAV_CMD codes)