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

bruh

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-12 15:46:22 +02:00
parent 3b41f4f81d
commit 46d32066a2
2 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -56,13 +56,20 @@ private:
{ {
// set message to enable attitude based on control mode variable // set message to enable attitude based on control mode variable
auto msg = px4_msgs::msg::OffboardControlMode(); auto msg = px4_msgs::msg::OffboardControlMode();
/*
msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false;
msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false;
msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false;
msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false;
msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false;
msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false;
*/
msg.position = false;
msg.velocity = false;
msg.acceleration = false;
msg.attitude = true;
msg.body_rate = false;
msg.actuator = false;
// 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);

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

@@ -269,6 +269,8 @@ private:
armed = true; armed = true;
} }
send_attitude_setpoint();
/*
RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
if (!flying) if (!flying)
{ {
@@ -293,6 +295,7 @@ private:
send_position_setpoint(); send_position_setpoint();
} }
} }
*/
} }
void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)