add comments

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 <chrono>
-#include <iostream>
 
 #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")
+    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)
     {
-      RCLCPP_ERROR(this->get_logger(), "Failed to create Evo Mini!");
+        rcl_interfaces::msg::ParameterDescriptor descriptor = rcl_interfaces::msg::ParameterDescriptor{};
-      return;
+        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<height::msg::HeightData>("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<height::msg::HeightData>("drone/height", 10);
-  }
-
 private:
-  void read_height()
+    /**
-  {
+     * @brief reads the height value from the Terabee Evo Mini sensor and publishes a
-    auto msg = height::msg::HeightData();
+     *
-
+     */
-    float min = 255;
+    void read_height()
-    terabee::DistanceData distance_data = evo_mini->getDistance();
-    for (size_t i = 0; i < distance_data.size(); i++)
     {
-      msg.heights[i] = distance_data.distance[i];
+        auto msg = height::msg::HeightData();
-      if (distance_data.distance[i] < min && distance_data.distance[i] >= 0)
+
-      {
+        // high initial minimal value
-        min = distance_data.distance[i];
+        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<height::msg::HeightData>::SharedPtr publisher_;
 
-  rclcpp::TimerBase::SharedPtr timer_;
+    // factory for height sensor
-  rclcpp::Publisher<height::msg::HeightData>::SharedPtr publisher_;
+    std::unique_ptr<terabee::ITerarangerFactory> factory;
-
+    // height sensor object pointer
-  std::unique_ptr<terabee::ITerarangerFactory> factory;
+    std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
-  std::unique_ptr<terabee::ITerarangerEvoMini> evo_mini;
 };
 
 int main(int argc, char **argv)
 {
 
-  rclcpp::init(argc, argv);
+    rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<HeightReader>());
+    rclcpp::spin(std::make_shared<HeightReader>());
-  rclcpp::shutdown();
+    rclcpp::shutdown();
 
-  return 0;
+    return 0;
 }

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 <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.
+     * @brief Publish offboard control mode messages as a heartbeat.
- *        Only the attitude is enabled, because that is how the drone will be controlled.
+     *        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);
-
     }
 };
 

src/px4_connection/src/px4_controller.cpp

@@ -1,17 +1,14 @@
-/*
+/**
-
+ * @file px4_controller.cpp
-We need to send attitude setpoints to be able to arm the drone:
+ * @author Sem van der Hoeven (sem.hoeven@gmail.com)
-https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET
+ * @brief Controller node to contol the PX4 using attitude or trajectory setpoints. 
-We need attitude setpoints because we don't have a GPS:
+ * It subscribes to the /drone/set_attitude topic to receive control commands
-https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-manual-control-lost/31565/9
+ */
-
-*/
 
 #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;