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

change heartbeat hz to 100

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-26 23:44:49 +02:00
parent 90f3b55dc9
commit fe4ab53c87
4 changed files with 18 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -29,7 +29,7 @@ public:
// create a publisher on the offboard control mode topic
offboard_control_mode_publisher_ = this->create_publisher<px4_msgs::msg::OffboardControlMode>("/fmu/in/offboard_control_mode", 10);
// 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(10ms, std::bind(&HeartBeat::send_heartbeat, this));
start_time = this->get_clock()->now().seconds();
RCLCPP_INFO(this->get_logger(), "done initializing at %d seconds. Sending heartbeat...", start_time);
}

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

@@ -108,8 +108,7 @@ private:
float rho_0 = 0; // initial rho of polar coordinate
float theta_0 = 0; // initial theta of polar coordinate
float p0_z = -0.0; // initial height
float des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position
float dot_des_x = 0.0, dot_des_y = 0.0; // desired velocity
float des_x = 0.0, des_y = 0.0, des_z = p0_z; // desired position
float gamma = M_PI_4; // desired heading direction
float local_x = 0; // local position x
@@ -258,7 +257,11 @@ private:
const std::shared_ptr<drone_services::srv::ArmDrone::Response> response)
{
RCLCPP_INFO(this->get_logger(), "Got arm request...");
if (this->failsafe_enabled)
{
response->success = false;
return;
}
if (!armed)
{
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
@@ -343,7 +346,6 @@ private:
RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z);
RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y);
msg.position = {local_x, local_y, des_z};
msg.velocity = {dot_des_x, dot_des_y, 0.0};
msg.yaw = gamma; //-3.14; // [-PI:PI]
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
@@ -391,13 +393,6 @@ private:
des_y = rho * sin(theta);
des_z = position[2]; // the z position can be set to the received height
// velocity computation
float dot_rho = K * omega;
dot_des_x = dot_rho * cos(theta) - rho * sin(theta) * omega;
dot_des_y = dot_rho * sin(theta) + rho * cos(theta) * omega;
// desired heading direction
gamma = atan2(dot_des_y, dot_des_x);
if (!user_in_control)
{
// RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
@@ -491,6 +486,11 @@ private:
RCLCPP_INFO(this->get_logger(), "Control mode set to %d", current_control_mode);
}
/**
* @brief Callback function for receiving failsafe messages
*
* @param msg the message indicating that the failsafe was enabled
*/
void on_failsafe_received(const drone_services::msg::FailsafeMsg::SharedPtr msg)
{
if (msg->enabled)