├── multiwii2_node
    ├── .gitignore
    ├── src
    │   ├── multiwii_node.cpp
    │   └── MultiWiiNode.cpp
    ├── package.xml
    ├── CMakeLists.txt
    └── include
    │   └── MultiWiiNode.hpp
├── multiwii_msgs
    ├── msg
    │   └── MSPRawMessage.msg
    ├── srv
    │   ├── SendMSPRawMessage.srv
    │   └── ReceiveMSPRawMessage.srv
    ├── package.xml
    └── CMakeLists.txt
└── LICENSE


/multiwii2_node/.gitignore:
--------------------------------------------------------------------------------
1 | CMakeLists.txt.user
2 | 


--------------------------------------------------------------------------------
/multiwii_msgs/msg/MSPRawMessage.msg:
--------------------------------------------------------------------------------
1 | uint8 id
2 | uint8[] data
3 | 


--------------------------------------------------------------------------------
/multiwii_msgs/srv/SendMSPRawMessage.srv:
--------------------------------------------------------------------------------
1 | MSPRawMessage msg
2 | ---
3 | 


--------------------------------------------------------------------------------
/multiwii_msgs/srv/ReceiveMSPRawMessage.srv:
--------------------------------------------------------------------------------
1 | uint8 id
2 | ---
3 | MSPRawMessage msg
4 | 


--------------------------------------------------------------------------------
/multiwii2_node/src/multiwii_node.cpp:
--------------------------------------------------------------------------------
1 | #include <MultiWiiNode.hpp>
2 | 
3 | int main(int argc, char **argv) {
4 |     rclcpp::init(argc, argv);
5 |     rclcpp::spin(std::make_shared<MultiWiiNode>());
6 |     rclcpp::shutdown();
7 |     return 0;
8 | }
9 | 


--------------------------------------------------------------------------------
/multiwii_msgs/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>multiwii_msgs</name>
 5 |   <version>0.0.0</version>
 6 |   <description>MultiWii message definitions.</description>
 7 |   <maintainer email="Rauch.Christian@gmx.de">Christian Rauch</maintainer>
 8 |   <license>Apache License 2.0</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <buildtool_depend>rosidl_default_generators</buildtool_depend>
13 | 
14 |   <exec_depend>rosidl_default_runtime</exec_depend>
15 | 
16 |   <member_of_group>rosidl_interface_packages</member_of_group>
17 | 
18 |   <export>
19 |     <build_type>ament_cmake</build_type>
20 |   </export>
21 | </package>
22 | 


--------------------------------------------------------------------------------
/multiwii_msgs/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | 
 3 | project(multiwii_msgs)
 4 | 
 5 | # Default to C++14
 6 | if(NOT CMAKE_CXX_STANDARD)
 7 |   set(CMAKE_CXX_STANDARD 14)
 8 | endif()
 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10 |   # we dont use add_compile_options with pedantic in message packages
11 |   # because the Python C extensions dont comply with it
12 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
13 | endif()
14 | 
15 | find_package(ament_cmake REQUIRED)
16 | find_package(rosidl_default_generators REQUIRED)
17 | 
18 | set(msg_files
19 |     "msg/MSPRawMessage.msg"
20 | )
21 | set(srv_files
22 |     "srv/ReceiveMSPRawMessage.srv"
23 |     "srv/SendMSPRawMessage.srv"
24 | )
25 | 
26 | rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files})
27 | 
28 | ament_export_dependencies(rosidl_default_runtime)
29 | 
30 | ament_package()
31 | 


--------------------------------------------------------------------------------
/multiwii2_node/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>multiwii_node</name>
 4 |   <version>0.0.0</version>
 5 |   <description>MultiWii node</description>
 6 | 
 7 |   <maintainer email="Rauch.Christian@gmx.de">Christian Rauch</maintainer>
 8 | 
 9 |   <license>BSD</license>
10 | 
11 |   <author email="Rauch.Christian@gmx.de">Christian Rauch</author>
12 | 
13 | 
14 |   <buildtool_depend>ament_cmake</buildtool_depend>
15 | 
16 |   <depend>rclcpp</depend>
17 |   <depend>rclcpp_components</depend>
18 |   <depend>msp</depend>
19 |   <depend>std_msgs</depend>
20 |   <depend>geometry_msgs</depend>
21 |   <depend>sensor_msgs</depend>
22 |   <depend>mavros_msgs</depend>
23 |   <depend>multiwii_msgs</depend>
24 |   <depend>tf2_ros</depend>
25 | 
26 |   <build_depend>eigen</build_depend>
27 | 
28 |   <!-- The export tag contains other, unspecified, tags -->
29 |   <export>
30 |     <!-- Other tools can request additional information be placed here -->
31 |     <build_type>ament_cmake</build_type>
32 |   </export>
33 | </package>
34 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2017 Christian Rauch
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/multiwii2_node/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.5)
 2 | 
 3 | project(multiwii_node)
 4 | 
 5 | # Default to C++14
 6 | if(NOT CMAKE_CXX_STANDARD)
 7 |   set(CMAKE_CXX_STANDARD 14)
 8 | endif()
 9 | 
10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
11 |   add_compile_options(-Wall -Wextra -Wpedantic)
12 | endif()
13 | 
14 | find_package(ament_cmake REQUIRED)
15 | find_package(rclcpp REQUIRED)
16 | find_package(rclcpp_components REQUIRED)
17 | find_package(sensor_msgs REQUIRED)
18 | find_package(std_msgs REQUIRED)
19 | find_package(geometry_msgs REQUIRED)
20 | find_package(mavros_msgs REQUIRED)
21 | find_package(multiwii_msgs REQUIRED)
22 | find_package(class_loader REQUIRED)
23 | find_package(tf2_ros REQUIRED)
24 | 
25 | find_package(Eigen3 REQUIRED)
26 | include_directories(${Eigen3_INCLUDE_DIR})
27 | 
28 | find_package(PkgConfig)
29 | pkg_check_modules(msp REQUIRED msp)
30 | include_directories(${msp_INCLUDE_DIRS})
31 | link_directories(${msp_LIBRARY_DIRS})
32 | 
33 | include_directories(include)
34 | 
35 | add_library(MultiWiiNode SHARED src/MultiWiiNode.cpp)
36 | ament_target_dependencies(MultiWiiNode
37 |   rclcpp
38 |   rclcpp_components
39 |   sensor_msgs
40 |   std_msgs
41 |   geometry_msgs
42 |   multiwii_msgs
43 |   mavros_msgs
44 |   class_loader
45 |   tf2_ros
46 | )
47 | target_link_libraries(MultiWiiNode ${msp_LIBRARIES})
48 | rclcpp_components_register_nodes(MultiWiiNode "MultiWiiNode")
49 | 
50 | add_executable(multiwii_node src/multiwii_node.cpp)
51 | target_link_libraries(multiwii_node MultiWiiNode)
52 | 
53 | ament_environment_hooks(${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH})
54 | 
55 | install(TARGETS MultiWiiNode multiwii_node
56 |     ARCHIVE DESTINATION lib
57 |     LIBRARY DESTINATION lib
58 |     RUNTIME DESTINATION lib/${PROJECT_NAME})
59 | 
60 | ament_package()
61 | 


--------------------------------------------------------------------------------
/multiwii2_node/include/MultiWiiNode.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef MULTIWIINODE_HPP
 2 | #define MULTIWIINODE_HPP
 3 | 
 4 | #include <rclcpp/rclcpp.hpp>
 5 | #include <tf2_ros/transform_broadcaster.h>
 6 | 
 7 | #include <multiwii_msgs/srv/receive_msp_raw_message.hpp>
 8 | #include <multiwii_msgs/srv/send_msp_raw_message.hpp>
 9 | 
10 | #include <std_msgs/msg/u_int16.hpp>
11 | #include <std_msgs/msg/u_int32.hpp>
12 | #include <std_msgs/msg/u_int8_multi_array.hpp>
13 | #include <std_msgs/msg/bool.hpp>
14 | #include <std_msgs/msg/float64.hpp>
15 | 
16 | #include <geometry_msgs/msg/pose_stamped.hpp>
17 | #include <geometry_msgs/msg/vector3.hpp>
18 | 
19 | #include <sensor_msgs/msg/battery_state.hpp>
20 | #include <sensor_msgs/msg/imu.hpp>
21 | #include <sensor_msgs/msg/magnetic_field.hpp>
22 | 
23 | #include <mavros_msgs/msg/rc_in.hpp>
24 | #include <mavros_msgs/msg/rc_out.hpp>
25 | #include <mavros_msgs/msg/override_rc_in.hpp>
26 | #include <mavros_msgs/msg/actuator_control.hpp>
27 | #include <mavros_msgs/srv/command_bool.hpp>
28 | #include <mavros_msgs/msg/state.hpp>
29 | 
30 | #include <eigen3/Eigen/Geometry>
31 | 
32 | #include <msp/FlightController.hpp>
33 | 
34 | class MultiWiiNode : public rclcpp::Node {
35 | public:
36 |     MultiWiiNode();
37 | 
38 | private:
39 |     std::unique_ptr<fcu::FlightController> fcu;
40 | 
41 |     rclcpp::Clock clk;
42 | 
43 |     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_raw;
44 |     rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr pub_imu_mag;
45 |     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose;
46 |     rclcpp::Publisher<mavros_msgs::msg::RCIn>::SharedPtr pub_rc_in;
47 |     rclcpp::Publisher<mavros_msgs::msg::RCOut>::SharedPtr pub_motors;
48 |     rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_battery;
49 |     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_altitude;
50 |     rclcpp::Publisher<mavros_msgs::msg::State>::SharedPtr pub_state;
51 | 
52 |     rclcpp::Subscription<mavros_msgs::msg::OverrideRCIn>::SharedPtr sub_rc_in;
53 |     rclcpp::Subscription<mavros_msgs::msg::OverrideRCIn>::SharedPtr sub_rc_in_raw;
54 | 
55 |     OnSetParametersCallbackHandle::SharedPtr param_cb_hndl;
56 | 
57 |     tf2_ros::TransformBroadcaster tf_broadcaster;
58 | 
59 |     rclcpp::TimerBase::SharedPtr timer_state;
60 | 
61 |     static double deg2rad(const double deg) {
62 |         return deg/180.0 * M_PI;
63 |     }
64 | 
65 |     static double rad2deg(const double rad) {
66 |         return rad/M_PI * 180.0;
67 |     }
68 | 
69 |     rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
70 | 
71 |     bool subscribe(const std::string &topic, const double period);
72 | 
73 |     void onImu(const msp::msg::RawImu &imu);
74 | 
75 |     void onAttitude(const msp::msg::Attitude &attitude);
76 | 
77 |     void onRc(const msp::msg::Rc &rc);
78 | 
79 |     void onMotor(const msp::msg::Motor &motor);
80 | 
81 |     void onAnalog(const msp::msg::Analog &analog);
82 | 
83 |     void onAltitude(const msp::msg::Altitude &altitude);
84 | 
85 |     void onVoltage(const msp::msg::VoltageMeters &voltage_meters);
86 | 
87 |     void onCurrent(const msp::msg::CurrentMeters &current_meters);
88 | 
89 |     void onBattery(const msp::msg::BatteryState &battery_state);
90 | 
91 |     void onState();
92 | 
93 |     void rc_override_AERT1234(const mavros_msgs::msg::OverrideRCIn::SharedPtr rc);
94 | 
95 |     void rc_override_raw(const mavros_msgs::msg::OverrideRCIn::SharedPtr rc);
96 | };
97 | 
98 | #endif // MULTIWIINODE_HPP
99 | 


--------------------------------------------------------------------------------
/multiwii2_node/src/MultiWiiNode.cpp:
--------------------------------------------------------------------------------
  1 | #include <MultiWiiNode.hpp>
  2 | #include <class_loader/register_macro.hpp>
  3 | 
  4 | #include <geometry_msgs/msg/transform_stamped.hpp>
  5 | 
  6 | using namespace std::chrono_literals;
  7 | 
  8 | static const std::set<std::string> sub_params = {
  9 |     "imu", "motor", "rc", "attitude", "altitude", "analog", "voltage", "current", "battery"
 10 | };
 11 | 
 12 | MultiWiiNode::MultiWiiNode() : Node("multiwii"), tf_broadcaster(this)
 13 | {
 14 |     declare_parameter("device_path", "/dev/ttyUSB0");
 15 |     declare_parameter("baud", 115200);
 16 | 
 17 |     for(const std::string& sub_param : sub_params) {
 18 |         declare_parameter<float>("sub/"+sub_param);
 19 |     }
 20 | 
 21 |     const std::string device_path = get_parameter("device_path").as_string();
 22 |     const size_t baud = get_parameter("baud").as_int();
 23 | 
 24 |     std::cout << "open " << device_path << "@" << std::to_string(baud) << std::endl;
 25 | 
 26 |     fcu = std::make_unique<fcu::FlightController>();
 27 |     fcu->connect(device_path, baud);
 28 | 
 29 |     pub_imu_raw = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", rclcpp::SensorDataQoS());
 30 |     pub_imu_mag = create_publisher<sensor_msgs::msg::MagneticField>("imu/mag", rclcpp::SensorDataQoS());
 31 |     pub_pose = create_publisher<geometry_msgs::msg::PoseStamped>("local_position/pose", rclcpp::SensorDataQoS());
 32 |     pub_rc_in = create_publisher<mavros_msgs::msg::RCIn>("rc/in", rclcpp::SensorDataQoS());
 33 |     pub_motors = create_publisher<mavros_msgs::msg::RCOut>("motors", rclcpp::SensorDataQoS());
 34 |     pub_battery = create_publisher<sensor_msgs::msg::BatteryState>("battery", rclcpp::SensorDataQoS());
 35 |     pub_altitude = create_publisher<std_msgs::msg::Float64>("global_position/rel_alt", rclcpp::SensorDataQoS());
 36 |     pub_state = create_publisher<mavros_msgs::msg::State>("state", rclcpp::SensorDataQoS());
 37 | 
 38 |     param_cb_hndl = add_on_set_parameters_callback(std::bind(&MultiWiiNode::onParameterChange, this, std::placeholders::_1));
 39 | 
 40 |     sub_rc_in = this->create_subscription<mavros_msgs::msg::OverrideRCIn>(
 41 |                 "rc/override", rclcpp::SystemDefaultsQoS(),
 42 |                 std::bind(&MultiWiiNode::rc_override_AERT1234, this, std::placeholders::_1));
 43 |     sub_rc_in_raw = this->create_subscription<mavros_msgs::msg::OverrideRCIn>(
 44 |                 "rc/override/raw", rclcpp::SystemDefaultsQoS(),
 45 |                 std::bind(&MultiWiiNode::rc_override_raw, this, std::placeholders::_1));
 46 | 
 47 |     timer_state = create_wall_timer(100ms, std::bind(&MultiWiiNode::onState, this));
 48 | 
 49 |     // subscribe with default period
 50 |     set_parameter({"sub/imu", 0.01});       // 102
 51 |     set_parameter({"sub/motor", 0.1});      // 104
 52 |     set_parameter({"sub/rc", 0.1});         // 105
 53 |     set_parameter({"sub/attitude", 0.1});   // 108
 54 |     set_parameter({"sub/altitude", 0.1});   // 109
 55 |     set_parameter({"sub/analog", 0.1});     // 110
 56 |     set_parameter({"sub/voltage", 1});      // 128
 57 |     set_parameter({"sub/current", 1});      // 129
 58 |     set_parameter({"sub/battery", 1});      // 130
 59 | }
 60 | 
 61 | bool MultiWiiNode::subscribe(const std::string &topic, const double period) {
 62 |     if (topic=="imu")
 63 |         fcu->subscribe(&MultiWiiNode::onImu, this, period);
 64 |     else if (topic=="attitude")
 65 |         fcu->subscribe(&MultiWiiNode::onAttitude, this, period);
 66 |     else if (topic=="rc")
 67 |         fcu->subscribe(&MultiWiiNode::onRc, this, period);
 68 |     else if (topic=="motor")
 69 |         fcu->subscribe(&MultiWiiNode::onMotor, this, period);
 70 |     else if (topic=="analog")
 71 |         fcu->subscribe(&MultiWiiNode::onAnalog, this, period);
 72 |     else if (topic=="altitude")
 73 |         fcu->subscribe(&MultiWiiNode::onAltitude, this, period);
 74 |     else if (topic=="voltage")
 75 |         fcu->subscribe(&MultiWiiNode::onVoltage, this, period);
 76 |     else if (topic=="current")
 77 |         fcu->subscribe(&MultiWiiNode::onCurrent, this, period);
 78 |     else if (topic=="battery")
 79 |         fcu->subscribe(&MultiWiiNode::onBattery, this, period);
 80 |     else
 81 |         return false;
 82 | 
 83 |     return true;
 84 | }
 85 | 
 86 | rcl_interfaces::msg::SetParametersResult MultiWiiNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
 87 |     rcl_interfaces::msg::SetParametersResult result;
 88 |     result.successful = true;
 89 |     for (const auto & parameter : parameters) {
 90 |         const std::string name = parameter.get_name();
 91 |         static const std::string prefix = "sub/";
 92 |         const std::string sub_name = name.substr(prefix.length(), name.length());
 93 |         if (name.substr(0, prefix.length()) == prefix &&
 94 |             sub_params.count(sub_name) &&
 95 |             parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
 96 |         {
 97 |             if(!subscribe(sub_name, parameter.as_double())) {
 98 |                 result.successful = false;
 99 |                 result.reason = "unknown subscriber";
100 |             }
101 |         }
102 |         else {
103 |             result.successful = false;
104 |             result.reason = "unknown or invalid parameter";
105 |         }
106 |     }
107 |     return result;
108 | }
109 | 
110 | void MultiWiiNode::onImu(const msp::msg::RawImu &imu) {
111 |     const msp::msg::ImuSI imu_si(imu, 512.0, 1/4.096, 0.092, 9.80665);
112 | 
113 |     std_msgs::msg::Header hdr;
114 |     hdr.stamp = clk.now();
115 |     hdr.frame_id = "multiwii";
116 | 
117 |     // raw imu data without orientation
118 |     sensor_msgs::msg::Imu imu_msg;
119 |     imu_msg.header = hdr;
120 |     imu_msg.linear_acceleration.x = imu_si.acc[0];
121 |     imu_msg.linear_acceleration.y = imu_si.acc[1];
122 |     imu_msg.linear_acceleration.z = imu_si.acc[2];
123 |     imu_msg.angular_velocity.x = deg2rad(imu_si.gyro[0]);
124 |     imu_msg.angular_velocity.y = deg2rad(imu_si.gyro[1]);
125 |     imu_msg.angular_velocity.z = deg2rad(imu_si.gyro[2]);
126 |     pub_imu_raw->publish(imu_msg);
127 | 
128 |     // magnetic field vector
129 |     sensor_msgs::msg::MagneticField mag_msg;
130 |     mag_msg.header = hdr;
131 |     mag_msg.magnetic_field.x = imu_si.mag[0] * 1e-6;
132 |     mag_msg.magnetic_field.y = imu_si.mag[1] * 1e-6;
133 |     mag_msg.magnetic_field.z = imu_si.mag[2] * 1e-6;
134 |     pub_imu_mag->publish(mag_msg);
135 | }
136 | 
137 | void MultiWiiNode::onAttitude(const msp::msg::Attitude &attitude) {
138 |     // r,p,y to rotation matrix
139 |     Eigen::Matrix3f rot;
140 |     rot = Eigen::AngleAxisf(deg2rad(attitude.roll), Eigen::Vector3f::UnitX())
141 |         * Eigen::AngleAxisf(deg2rad(attitude.pitch),  Eigen::Vector3f::UnitY())
142 |         * Eigen::AngleAxisf(deg2rad(attitude.yaw), Eigen::Vector3f::UnitZ());
143 | 
144 |     const Eigen::Quaternionf quat(rot);
145 | 
146 |     geometry_msgs::msg::PoseStamped pose_stamped;
147 |     pose_stamped.header.stamp = clk.now();
148 |     pose_stamped.header.frame_id = "multiwii";
149 |     pose_stamped.pose.orientation.x = quat.x();
150 |     pose_stamped.pose.orientation.y = quat.y();
151 |     pose_stamped.pose.orientation.z = quat.z();
152 |     pose_stamped.pose.orientation.w = quat.w();
153 | 
154 |     geometry_msgs::msg::TransformStamped tf;
155 |     tf.header = pose_stamped.header;
156 |     tf.child_frame_id = "attitude";
157 |     tf.transform.rotation = pose_stamped.pose.orientation;
158 |     tf_broadcaster.sendTransform(tf);
159 | 
160 |     pub_pose->publish(pose_stamped);
161 | }
162 | 
163 | void MultiWiiNode::onRc(const msp::msg::Rc &rc) {
164 |     mavros_msgs::msg::RCIn rc_msg;
165 |     rc_msg.header.stamp = clk.now();
166 |     rc_msg.header.frame_id = "multiwii";
167 |     rc_msg.channels = rc.channels;
168 | 
169 |     pub_rc_in->publish(rc_msg);
170 | }
171 | 
172 | void MultiWiiNode::onMotor(const msp::msg::Motor &motor) {
173 |     mavros_msgs::msg::RCOut motor_out;
174 |     motor_out.header.stamp = clk.now();
175 |     motor_out.header.frame_id = "multiwii";
176 |     for(const uint16_t m : motor.motor) {
177 |         motor_out.channels.push_back(m);
178 |     }
179 |     pub_motors->publish(motor_out);
180 | }
181 | 
182 | void MultiWiiNode::onAnalog(const msp::msg::Analog &analog) {
183 |     sensor_msgs::msg::BatteryState battery;
184 |     battery.header.stamp = clk.now();
185 |     battery.header.frame_id = "multiwii";
186 |     battery.voltage = analog.vbat;
187 |     battery.current = analog.amperage;
188 | 
189 |     pub_battery->publish(battery);
190 | }
191 | 
192 | void MultiWiiNode::onAltitude(const msp::msg::Altitude &altitude) {
193 |     std_msgs::msg::Float64 alt; // altitude in meter
194 |     alt.data = altitude.altitude;
195 | 
196 |     geometry_msgs::msg::TransformStamped tf;
197 |     tf.header.stamp = clk.now();
198 |     tf.header.frame_id = "multiwii";
199 |     tf.child_frame_id = "altitude";
200 |     tf.transform.translation.z = altitude.altitude;
201 |     tf_broadcaster.sendTransform(tf);
202 | 
203 |     pub_altitude->publish(alt);
204 | }
205 | 
206 | void MultiWiiNode::onVoltage(const msp::msg::VoltageMeters &voltage_meters) {
207 | //    std::cout << voltage_meters << std::endl;
208 |     (void)voltage_meters;
209 | }
210 | 
211 | void MultiWiiNode::onCurrent(const msp::msg::CurrentMeters &current_meters) {
212 | //    std::cout << current_meters << std::endl;
213 |     (void)current_meters;
214 | }
215 | 
216 | void MultiWiiNode::onBattery(const msp::msg::BatteryState &battery_state) {
217 | //    std::cout << battery_state << std::endl;
218 |     (void)battery_state;
219 | }
220 | 
221 | void MultiWiiNode::onState() {
222 |     mavros_msgs::msg::State state;
223 |     state.armed = fcu->isArmed();
224 |     pub_state->publish(state);
225 | }
226 | 
227 | void MultiWiiNode::rc_override_AERT1234(const mavros_msgs::msg::OverrideRCIn::SharedPtr rc) {
228 |     // set channels in order of
229 |     // AERT (Aileron, Elevator, Rudder, Throttle), or
230 |     // RPYT (Roll, Pitch, Yaw, Throttle) respectively
231 |     fcu->setRc(rc->channels[0], rc->channels[1], rc->channels[2], rc->channels[3],
232 |                rc->channels[4], rc->channels[5], rc->channels[6], rc->channels[7]);
233 | }
234 | 
235 | void MultiWiiNode::rc_override_raw(const mavros_msgs::msg::OverrideRCIn::SharedPtr rc) {
236 |     std::vector<uint16_t> channels;
237 |     for(const uint16_t c : rc->channels) { channels.push_back(c); }
238 |     fcu->setRc(channels);
239 | }
240 | 
241 | CLASS_LOADER_REGISTER_CLASS(MultiWiiNode, rclcpp::Node)
242 | 


--------------------------------------------------------------------------------