From 9bb529dc08c7c0a5d5d18a2d01a33f24fc117f48 Mon Sep 17 00:00:00 2001 From: Karthik Desai Date: Thu, 17 Sep 2020 13:23:09 +0200 Subject: [PATCH] publish BATTERY2 message as /mavros/battery2 topic --- mavros/src/plugins/sys_status.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index 25a59acc6..674332cd9 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -490,6 +490,7 @@ class SystemStatusPlugin : public plugin::PluginBase state_pub = nh.advertise("state", 10, true); extended_state_pub = nh.advertise("extended_state", 10); batt_pub = nh.advertise("battery", 10); + batt2_pub = nh.advertise("battery2", 10); estimator_status_pub = nh.advertise("estimator_status", 10); statustext_pub = nh.advertise("statustext/recv", 10); statustext_sub = nh.subscribe("statustext/send", 10, &SystemStatusPlugin::statustext_cb, this); @@ -514,6 +515,7 @@ class SystemStatusPlugin : public plugin::PluginBase make_handler(&SystemStatusPlugin::handle_extended_sys_state), make_handler(&SystemStatusPlugin::handle_battery_status), make_handler(&SystemStatusPlugin::handle_estimator_status), + make_handler(&SystemStatusPlugin::handle_battery2), }; } @@ -532,6 +534,7 @@ class SystemStatusPlugin : public plugin::PluginBase ros::Publisher state_pub; ros::Publisher extended_state_pub; ros::Publisher batt_pub; + ros::Publisher batt2_pub; ros::Publisher estimator_status_pub; ros::Publisher statustext_pub; ros::Subscriber statustext_sub; @@ -795,6 +798,19 @@ class SystemStatusPlugin : public plugin::PluginBase batt_pub.publish(batt_msg); } + void handle_battery2(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::BATTERY2 &batt) { + float volt = batt.voltage / 1000.0f; // mV + float curr = batt.current_battery / 100.0f; // 10 mA or -1 + + auto batt_msg = boost::make_shared(); + batt_msg->header.stamp = ros::Time::now(); + + batt_msg->voltage = volt; + batt_msg->current = curr; + + batt2_pub.publish(batt_msg); + } + void handle_statustext(const mavlink::mavlink_message_t *msg, mavlink::common::msg::STATUSTEXT &textm) { auto text = mavlink::to_string(textm.text);