From 6a29220bc9a21edacf1ed898a756f84639dbad9f Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 5 May 2023 12:57:48 +0200 Subject: [PATCH] add comments --- src/height/src/height_reader.cpp | 122 ++++++++++++---------- src/px4_connection/src/heartbeat.cpp | 17 +-- src/px4_connection/src/px4_controller.cpp | 17 ++- 3 files changed, 86 insertions(+), 70 deletions(-) diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 92a652fa..1df63c2d 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -1,5 +1,10 @@ +/** + * @file height_reader.cpp + * @author Sem van der Hoeven (sem.hoeven@gmail.com) + * @brief Uses the Terabee API to read the Terabee Evo Mini + * height sensor data and publishes it on a /drone/height topic + */ #include -#include #include "rclcpp/rclcpp.hpp" #include "height/msg/height_data.hpp" @@ -16,70 +21,81 @@ using terabee::ITerarangerEvoMini; class HeightReader : public rclcpp::Node { public: - HeightReader() : rclcpp::Node("height_reader") - { - rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; - descriptor.description = "serial port for the USB port of the height sensor"; - - this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); - - factory = terabee::ITerarangerFactory::getFactory(); - evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); - - if (!evo_mini) + HeightReader() : rclcpp::Node("height_reader") { - RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); - return; + rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{}; + descriptor.description = "serial port for the USB port of the height sensor"; + + this->declare_parameter("height_serial_port", "/dev/ttyACM0", descriptor); + + factory = terabee::ITerarangerFactory::getFactory(); + evo_mini = factory->createTerarangerEvoMini(this->get_parameter("height_serial_port").as_string()); + + if (!evo_mini) + { + RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!"); + return; + } + + evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); + + if (!evo_mini->initialize()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); + + timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); + publisher_ = this->create_publisher("drone/height", 10); } - evo_mini->setPixelMode(ITerarangerEvoMini::Px4Mode); - - if (!evo_mini->initialize()) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize evo mini!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); - - timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); - publisher_ = this->create_publisher("drone/height", 10); - } - private: - void read_height() - { - auto msg = height::msg::HeightData(); - - float min = 255; - terabee::DistanceData distance_data = evo_mini->getDistance(); - for (size_t i = 0; i < distance_data.size(); i++) + /** + * @brief reads the height value from the Terabee Evo Mini sensor and publishes a + * + */ + void read_height() { - msg.heights[i] = distance_data.distance[i]; - if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) - { - min = distance_data.distance[i]; - } + auto msg = height::msg::HeightData(); + + // high initial minimal value + float min = 255; + terabee::DistanceData distance_data = evo_mini->getDistance(); + // add readings and calculate mimimum value + for (size_t i = 0; i < distance_data.size(); i++) + { + msg.heights[i] = distance_data.distance[i]; + if (distance_data.distance[i] < min && distance_data.distance[i] >= 0) + { + min = distance_data.distance[i]; + } + } + // add minimum value and publish message + msg.min_height = min; + publisher_->publish(msg); + + RCLCPP_INFO(this->get_logger(), "publishing message with min distance %f", msg.min_height); } - msg.min_height = min; - publisher_->publish(msg); - RCLCPP_INFO(this->get_logger(),"publishing message with min distance %f",msg.min_height); - } + // timer for publishing height readings + rclcpp::TimerBase::SharedPtr timer_; + // publisher for height readings + rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - - std::unique_ptr factory; - std::unique_ptr evo_mini; + // factory for height sensor + std::unique_ptr factory; + // height sensor object pointer + std::unique_ptr evo_mini; }; int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); - return 0; + return 0; } diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index fafd2c2c..dd733c46 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -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 #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); - } }; diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 8c8e1d1f..9f28763c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -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 #include #include "rclcpp/rclcpp.hpp" -// #include "attitude_msg_code.hpp" #include #include @@ -23,8 +20,6 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied- #include -// #include - #define D_SPEED(x) -x - 9.81 using namespace std::chrono_literals;