add comments
This commit is contained in:
@@ -1,3 +1,9 @@
|
||||
/**
|
||||
* @file heartbeat.cpp
|
||||
* @author Sem van der Hoeven (sem.hoeven@gmail.com)
|
||||
* @brief Heartbeat node that keeps the connection between the flight computer
|
||||
* and PX4 flight controller alive by sending OffboardControl messages
|
||||
*/
|
||||
#include <chrono>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
@@ -27,11 +33,11 @@ private:
|
||||
// start time of node in seconds
|
||||
double start_time;
|
||||
|
||||
/**
|
||||
* @brief Publish offboard control mode messages as a heartbeat.
|
||||
* Only the attitude is enabled, because that is how the drone will be controlled.
|
||||
*
|
||||
*/
|
||||
/**
|
||||
* @brief Publish offboard control mode messages as a heartbeat.
|
||||
* Only the attitude is enabled, because that is how the drone will be controlled.
|
||||
*
|
||||
*/
|
||||
void send_heartbeat()
|
||||
{
|
||||
// set message to enable attitude
|
||||
@@ -46,7 +52,6 @@ private:
|
||||
// get timestamp and publish message
|
||||
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
|
||||
offboard_control_mode_publisher_->publish(msg);
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -1,17 +1,14 @@
|
||||
/*
|
||||
|
||||
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
|
||||
|
||||
*/
|
||||
/**
|
||||
* @file px4_controller.cpp
|
||||
* @author Sem van der Hoeven (sem.hoeven@gmail.com)
|
||||
* @brief Controller node to contol the PX4 using attitude or trajectory setpoints.
|
||||
* It subscribes to the /drone/set_attitude topic to receive control commands
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <math.h>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
// #include "attitude_msg_code.hpp"
|
||||
|
||||
#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
|
||||
#include <px4_msgs/msg/trajectory_setpoint.hpp>
|
||||
@@ -23,8 +20,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
|
||||
|
||||
#include <std_srvs/srv/empty.hpp>
|
||||
|
||||
// #include <px4_msgs/msg/offboard_control_mode.hpp>
|
||||
|
||||
#define D_SPEED(x) -x - 9.81
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
Reference in New Issue
Block a user