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

add comments to heartbeat

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-05 11:52:29 +02:00
parent b93a616f58
commit 615190ca06
1 changed files with 6 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,18 +1,8 @@
/*
We need to send attitude setpoints to be able to arm the drone:
https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET
We need attitude setpoints because we don't have a GPS:
https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9
*/
#include <chrono> #include <chrono>
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <px4_msgs/msg/offboard_control_mode.hpp> #include <px4_msgs/msg/offboard_control_mode.hpp>
// #include <px4_msgs/msg/timesync.hpp>
using namespace std::chrono_literals; using namespace std::chrono_literals;
@@ -30,6 +20,12 @@ public:
} }
private: private:
// publisher for offboard control mode messages
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
// timer for sending the heartbeat
rclcpp::TimerBase::SharedPtr timer_;
// start time of node in seconds
double start_time;
/** /**
* @brief Publish offboard control mode messages as a heartbeat. * @brief Publish offboard control mode messages as a heartbeat.
@@ -50,20 +46,8 @@ 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!");
// check if 5 seconds have elapsed
// if (this->get_clock()->now().seconds() - start_time > 5)
// {
// RCLCPP_INFO(this->get_logger(), "5 seconds elapsed!");
// }
} }
rclcpp::Publisher<px4_msgs::msg::OffboardControlMode>::SharedPtr offboard_control_mode_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
double start_time;
}; };
int main(int argc, char *argv[]) int main(int argc, char *argv[])