From e1b06a1b99d85074dd0ff3333dbd310a443af41b Mon Sep 17 00:00:00 2001 From: pedro-fuoco Date: Thu, 30 Mar 2023 13:05:17 -0300 Subject: [PATCH] AP_DDS: Integrate AP_BattMonitor to work with AP_DDS * Edit BatteryState.idl * Add BatteryState to AP_DDS_Topic_table.h * Add BatteryState to the DDS Client * Add voltage * Add temperature * Add current * Add charge * Add capacity with NAN value * Add design_capacity * Add percentage * Add power_supply_status * Add power_supply_health * Add power_supply_technology with 0 value * Add present * Cell_voltage and Serial_number need to be implemented in the future * Did not add cell_temperature as AP_BattMonitor doesn't support it * Did not add location as this is a generic implementation * Parameterize battery instance number --- libraries/AP_DDS/AP_DDS_Client.cpp | 98 ++++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 7 ++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 12 ++- .../Idl/sensor_msgs/msg/BatteryState.idl | 5 +- libraries/AP_DDS/dds_xrce_profile.xml | 25 +++++ 5 files changed, 142 insertions(+), 5 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index f82c102d75..94c8213f65 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -8,12 +8,13 @@ #include #include #include +#include #include "AP_DDS_Client.h" - static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000; +static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; static char WGS_84_FRAME_ID[] = "WGS-84"; // https://www.ros.org/reps/rep-0105.html#base-link static char BASE_LINK_FRAME_ID[] = "base_link"; @@ -160,6 +161,78 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) } +void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) +{ + if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { + return; + } + + update_topic(msg.header.stamp); + auto &battery = AP::battery(); + + if (!battery.healthy(instance)) + { + msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD + msg.present = false; + return; + } + msg.present = true; + + msg.voltage = battery.voltage(instance); + + float temperature; + msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; + + float current; + msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; + + const float design_capacity = (float)battery.pack_capacity_mah(instance)/1000.0; + msg.design_capacity = design_capacity; + + uint8_t percentage; + if (battery.capacity_remaining_pct(percentage, instance)) + { + msg.percentage = percentage/100.0; + msg.charge = (percentage * design_capacity)/100.0; + } + else + { + msg.percentage = NAN; + msg.charge = NAN; + } + + msg.capacity = NAN; + + if (battery.current_amps(current, instance)) + { + if (percentage == 100) { + msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL + } + else if (current < 0.0) { + msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING + } + else if (current > 0.0) { + msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING + } + else { + msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING + } + } + else + { + msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN + } + + msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD + + msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN + + if (battery.has_cell_voltages(instance)) + { + const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells; + std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage); + } +} /* class constructor @@ -339,10 +412,24 @@ void AP_DDS_Client::write_static_transforms() } } +void AP_DDS_Client::write_battery_state_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = sensor_msgs_msg_BatteryState_size_of_topic(&battery_state_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[3].dw_id,&ub,topic_size); + const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} + void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); - const auto cur_time_ms = AP_HAL::millis64(); if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) { @@ -358,6 +445,13 @@ void AP_DDS_Client::update() write_nav_sat_fix_topic(); } + if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { + constexpr uint8_t instance = 0; + update_topic(battery_state_topic, instance); + last_battery_state_time_ms = cur_time_ms; + write_battery_state_topic(); + } + connected = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 630481b37a..948bce659f 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -9,6 +9,7 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" +#include "sensor_msgs/msg/BatteryState.h" #include #include @@ -44,6 +45,7 @@ private: builtin_interfaces_msg_Time time_topic; sensor_msgs_msg_NavSatFix nav_sat_fix_topic; tf2_msgs_msg_TFMessage static_transforms_topic; + sensor_msgs_msg_BatteryState battery_state_topic; HAL_Semaphore csem; @@ -53,11 +55,14 @@ private: static void update_topic(builtin_interfaces_msg_Time& msg); static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance); static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); + static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; // The last ms timestamp AP_DDS wrote a NavSatFix message uint64_t last_nav_sat_fix_time_ms; + // The last ms timestamp AP_DDS wrote a BatteryState message + uint64_t last_battery_state_time_ms; public: @@ -81,6 +86,8 @@ public: void write_nav_sat_fix_topic(); //! @brief Serialize the static transforms and publish to the IO stream(s) void write_static_transforms(); + //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s) + void write_battery_state_topic(); //! @brief Update the internally stored DDS messages with latest data void update(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 107721b5d8..ec65c110d6 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -1,7 +1,7 @@ #include "builtin_interfaces/msg/Time.h" #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" - +#include "sensor_msgs/msg/BatteryState.h" #include "AP_DDS_Generic_Fn_T.h" #include "uxr/client/client.h" @@ -42,4 +42,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic), .size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic), }, + { + .topic_id = 0x04, + .pub_id = 0x04, + .dw_id = uxrObjectId{.id=0x04, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "batterystate0__t", + .dw_profile_label = "batterystate0__dw", + .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_BatteryState_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_BatteryState_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_BatteryState_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/Idl/sensor_msgs/msg/BatteryState.idl b/libraries/AP_DDS/Idl/sensor_msgs/msg/BatteryState.idl index 691e0d1dba..0ca3292d96 100644 --- a/libraries/AP_DDS/Idl/sensor_msgs/msg/BatteryState.idl +++ b/libraries/AP_DDS/Idl/sensor_msgs/msg/BatteryState.idl @@ -6,6 +6,7 @@ module sensor_msgs { module msg { + typedef float float__14[14]; module BatteryState_Constants { @verbatim (language="comment", text= "Constants are chosen to match the enums in the linux kernel" "\n" "defined in include/linux/power_supply.h as of version 3.7" "\n" "The one difference is for style reasons the constants are" "\n" "all uppercase not mixed case." "\n" "Power supply status constants") @@ -85,12 +86,12 @@ module sensor_msgs { @verbatim (language="comment", text= "An array of individual cell voltages for each cell in the pack" "\n" "If individual voltages unknown but number of cells known set each to NaN") - sequence cell_voltage; + float__14 cell_voltage; @verbatim (language="comment", text= "An array of individual cell temperatures for each cell in the pack" "\n" "If individual temperatures unknown but number of cells known set each to NaN") - sequence cell_temperature; + float__14 cell_temperature; @verbatim (language="comment", text= "The location into which the battery is inserted. (slot number or plug)") diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index aeed08bc83..1b19f7e867 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -86,4 +86,29 @@ + + rt/ROS2_BatteryState0 + sensor_msgs::msg::dds_::BatteryState_ + + KEEP_LAST + 20 + + + + PREALLOCATED_WITH_REALLOC + + + RELIABLE + + + + NO_KEY + rt/ROS2_BatteryState0 + sensor_msgs::msg::dds_::BatteryState_ + + KEEP_LAST + 20 + + +