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

change 0.5 thrust to 0.8

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-04-28 17:44:22 +02:00
parent 7a2f19d311
commit 5a7a54fc53
2 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,6 +26,7 @@ public:
// create timer to send heartbeat messages (offboard control) every 100ms // create timer to send heartbeat messages (offboard control) every 100ms
timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this));
start_time = this->get_clock()->now().seconds(); start_time = this->get_clock()->now().seconds();
RCLCPP_INFO(this->get_logger(), "done initializing after %d seconds. Sending heartbeat...", start_time);
} }
private: private:
@@ -49,13 +50,13 @@ private:
// get timestamp and publish message // get timestamp and publish message
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
offboard_control_mode_publisher_->publish(msg); offboard_control_mode_publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!"); // RCLCPP_INFO(this->get_logger(), "sent offboard control mode message!");
// check if 5 seconds have elapsed // check if 5 seconds have elapsed
if (this->get_clock()->now().seconds() - start_time > 5) // if (this->get_clock()->now().seconds() - start_time > 5)
{ // {
RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!"); // RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
} // }

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

@@ -42,6 +42,7 @@ public:
timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this)); timer_ = this->create_wall_timer(1000ms, std::bind(&PX4Controller::send_setpoint, this));
start_time_ = this->get_clock()->now().seconds(); start_time_ = this->get_clock()->now().seconds();
RCLCPP_INFO(this->get_logger(), "finished initializing after %d seconds.", start_time_);
} }
private: private:
@@ -50,6 +51,7 @@ private:
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_;
double start_time_; double start_time_;
bool has_sent_status = false;
/** /**
* @brief 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.
@@ -67,12 +69,17 @@ 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.5; // down, 50% thrust up msg.thrust_body[2] = 0.8; // down, 80% thrust up
calculate_quaternion(q, degrees_to_radians(40), 0, 0); calculate_quaternion(q, degrees_to_radians(40), 0, 0);
} }
else else
{ {
if (!has_sent_status)
{
has_sent_status = true;
RCLCPP_INFO(this->get_logger(), "changing down thrust to 0");
}
// no thrust // no thrust
msg.thrust_body[0] = 0; // north msg.thrust_body[0] = 0; // north
msg.thrust_body[1] = 0; // east msg.thrust_body[1] = 0; // east
@@ -88,8 +95,6 @@ private:
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
vehicle_setpoint_publisher_->publish(msg); vehicle_setpoint_publisher_->publish(msg);
RCLCPP_INFO(this->get_logger(), "published message");
} }
/** /**