From df30f159be6b9f2be74ab5eeee594bea43f9d246 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Thu, 18 Jan 2024 13:21:52 +0100 Subject: [PATCH 1/5] draft (compiling, not tested) --- crazyflie/src/crazyflie_server.cpp | 100 ++++++++++++++++++++++++++-- crazyflie_interfaces/CMakeLists.txt | 1 + crazyflie_interfaces/msg/Status.msg | 31 +++++++++ 3 files changed, 125 insertions(+), 7 deletions(-) create mode 100644 crazyflie_interfaces/msg/Status.msg diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index c49ac63c4..bfcefafac 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -20,6 +20,7 @@ #include "motion_capture_tracking_interfaces/msg/named_pose_array.hpp" #include "crazyflie_interfaces/msg/full_state.hpp" #include "crazyflie_interfaces/msg/position.hpp" +#include "crazyflie_interfaces/msg/status.hpp" #include "crazyflie_interfaces/msg/log_data_generic.hpp" #include "crazyflie_interfaces/msg/connection_statistics_array.hpp" @@ -102,6 +103,19 @@ class CrazyflieROS uint16_t right; } __attribute__((packed)); + struct logStatus { + // general status + uint16_t supervisorInfo; // supervisor.info + // battery related + // TODO: would it be better to use pm.batteryLevel? + uint16_t vbatMV; // pm.vbatMV + uint8_t pmState; // pm.state + // radio related + uint8_t rssi; // radio.rssi + uint16_t numRxBc; // radio.numRxBc + uint16_t numRxUc; // radio.numRxUc + } __attribute__((packed)); + public: CrazyflieROS( const std::string& link_uri, @@ -110,6 +124,7 @@ class CrazyflieROS rclcpp::Node* node, rclcpp::CallbackGroup::SharedPtr callback_group_cf_cmd, rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv, + const CrazyflieBroadcaster* cfbc, bool enable_parameters = true) : logger_(rclcpp::get_logger(name)) , cf_logger_(logger_) @@ -121,6 +136,7 @@ class CrazyflieROS , node_(node) , tf_broadcaster_(node) , last_on_latency_(std::chrono::steady_clock::now()) + , cfbc_(cfbc) { auto sub_opt_cf_cmd = rclcpp::SubscriptionOptions(); sub_opt_cf_cmd.callback_group = callback_group_cf_cmd; @@ -355,6 +371,28 @@ class CrazyflieROS }, cb)); log_block_scan_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds } + else if (i.first.find("default_topics.status") == 0) { + int freq = log_config_map["default_topics.status.frequency"].get(); + RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq); + + publisher_status_ = node->create_publisher(name + "/status", 10); + + std::function cb = std::bind(&CrazyflieROS::on_logging_status, this, std::placeholders::_1, std::placeholders::_2); + + log_block_status_.reset(new LogBlock( + &cf_,{ + // general status + {"supervisor", "info"}, + // battery related + {"pm", "vbatMV"}, + {"pm", "state"}, + // radio related + {"radio", "rssi"}, + {"radio", "numRxBc"}, + {"radio", "numRxUc"}, + }, cb)); + log_block_status_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds + } else if (i.first.find("custom_topics") == 0 && i.first.rfind(".vars") != std::string::npos) { std::string topic_name = i.first.substr(14, i.first.size() - 14 - 5); @@ -690,6 +728,46 @@ class CrazyflieROS } } + void on_logging_status(uint32_t time_in_ms, const logStatus* data) { + if (publisher_status_) { + + crazyflie_interfaces::msg::Status msg; + msg.header.stamp = node_->get_clock()->now(); + msg.header.frame_id = name_; + msg.supervisor_info = data->supervisorInfo; + msg.battery_voltage = data->vbatMV / 1000.0f; + msg.pm_state = data->pmState; + msg.rssi = data->rssi; + int32_t deltaRxBc = data->numRxBc - previous_numRxBc; + int32_t deltaRxUc = data->numRxUc - previous_numRxUc; + // handle overflow + if (deltaRxBc < 0) { + deltaRxBc += std::numeric_limits::max(); + } + if (deltaRxUc) { + deltaRxUc += std::numeric_limits::max(); + } + msg.num_rx_broadcast = deltaRxBc; + msg.num_rx_unicast = deltaRxUc; + previous_numRxBc = data->numRxBc; + previous_numRxUc = data->numRxUc; + + // compare with connection stats (unicast) + const auto statsUc = cf_.connectionStats(); + size_t deltaTxUc = statsUc.sent_count - previous_stats_unicast_.sent_count; + msg.num_tx_unicast = deltaTxUc; + previous_stats_unicast_ = statsUc; + + // compare with connection stats (broadcast) + const auto statsBc = cfbc_->connectionStats(); + size_t deltaTxBc = statsBc.sent_count - previous_stats_broadcast_.sent_count; + msg.num_tx_broadcast = deltaTxBc; + previous_stats_broadcast_ = statsBc; + + publisher_status_->publish(msg); + } + } + void on_logging_custom(uint32_t time_in_ms, const std::vector* values, void* userData) { auto pub = reinterpret_cast::SharedPtr*>(userData); @@ -774,6 +852,14 @@ class CrazyflieROS std::unique_ptr> log_block_scan_; rclcpp::Publisher::SharedPtr publisher_scan_; + std::unique_ptr> log_block_status_; + rclcpp::Publisher::SharedPtr publisher_status_; + uint16_t previous_numRxBc; + uint16_t previous_numRxUc; + bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_unicast_; + bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_broadcast_; + const CrazyflieBroadcaster* cfbc_; + std::list> log_blocks_generic_; std::list::SharedPtr> publishers_generic_; @@ -888,19 +974,19 @@ class CrazyflieServer : public rclcpp::Node // if it is a Crazyflie, try to connect if (constr == "crazyflie") { std::string uri = parameter_overrides.at("robots." + name + ".uri").get(); + auto broadcastUri = Crazyflie::broadcastUriFromUnicastUri(uri); + if (broadcaster_.count(broadcastUri) == 0) { + broadcaster_.emplace(broadcastUri, std::make_unique(broadcastUri)); + } + crazyflies_.emplace(name, std::make_unique( uri, cf_type, name, this, callback_group_cf_cmd_, - callback_group_cf_srv_)); - - auto broadcastUri = crazyflies_[name]->broadcastUri(); - RCLCPP_INFO(logger_, "%s", broadcastUri.c_str()); - if (broadcaster_.count(broadcastUri) == 0) { - broadcaster_.emplace(broadcastUri, std::make_unique(broadcastUri)); - } + callback_group_cf_srv_, + broadcaster_.at(broadcastUri).get())); update_name_to_id_map(name, crazyflies_[name]->id()); } diff --git a/crazyflie_interfaces/CMakeLists.txt b/crazyflie_interfaces/CMakeLists.txt index 583e95fa1..0e92f4d09 100644 --- a/crazyflie_interfaces/CMakeLists.txt +++ b/crazyflie_interfaces/CMakeLists.txt @@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Hover.msg" "msg/LogBlock.msg" "msg/Position.msg" + "msg/Status.msg" "msg/TrajectoryPolynomialPiece.msg" "msg/VelocityWorld.msg" "srv/GoTo.srv" diff --git a/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg new file mode 100644 index 000000000..bebd42a71 --- /dev/null +++ b/crazyflie_interfaces/msg/Status.msg @@ -0,0 +1,31 @@ + +# Constants +uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 0 +uint16 SUPERVISOR_INFO_IS_ARMED = 2 +uint16 SUPERVISOR_INFO_AUTO_ARM = 4 +uint16 SUPERVISOR_INFO_CAN_FLY = 8 +uint16 SUPERVISOR_INFO_IS_FLYING = 16 +uint16 SUPERVISOR_INFO_IS_TUMBLED = 32 +uint16 SUPERVISOR_INFO_IS_LOCKED = 64 + +uint8 PM_STATE_BATTERY = 0 +uint8 PM_STATE_CHARGING = 1 +uint8 PM_STATE_CHARGED = 2 +uint8 PM_STATE_LOW_POWER = 3 +uint8 PM_STATE_SHUTDOWN = 4 + +std_msgs/Header header + +# general status +uint16 supervisor_info # Bitfield, see SUPERVISOR_INFO for individual bits + +# battery related +float32 battery_voltage # internal battery voltage [V] +uint8 pm_state # See PM_STATE_* for potential values + +# radio related +uint8 rssi # latest radio signal strength indicator [dBm] +uint32 num_rx_broadcast # number of received broadcast packets during the last period +uint32 num_tx_broadcast # number of broadcast packets sent during the last period +uint32 num_rx_unicast # number of received unicast packets during the last period +uint32 num_tx_unicast # number of unicast packets sent during the last period From a6af35f1653704820c720668605119b17eca5bd4 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Jan 2024 12:02:07 +0100 Subject: [PATCH 2/5] overflow bugfix --- crazyflie/src/crazyflie_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index bfcefafac..374aad419 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -744,7 +744,7 @@ class CrazyflieROS if (deltaRxBc < 0) { deltaRxBc += std::numeric_limits::max(); } - if (deltaRxUc) { + if (deltaRxUc < 0) { deltaRxUc += std::numeric_limits::max(); } msg.num_rx_broadcast = deltaRxBc; From 7fe9bdbc133f83f0c77284f33cf4a2119b7334d9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Jan 2024 15:48:22 +0100 Subject: [PATCH 3/5] add safeguard for older firmware versions --- crazyflie/src/crazyflie_server.cpp | 75 ++++++++++++++++++++---------- 1 file changed, 50 insertions(+), 25 deletions(-) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 374aad419..065bce59d 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -379,18 +379,37 @@ class CrazyflieROS std::function cb = std::bind(&CrazyflieROS::on_logging_status, this, std::placeholders::_1, std::placeholders::_2); + std::list > logvars({ + // general status + {"supervisor", "info"}, + // battery related + {"pm", "vbatMV"}, + {"pm", "state"}, + // radio related + {"radio", "rssi"} + }); + + // check if this firmware version has radio.numRx{Bc,Uc} + status_has_radio_stats_ = false; + for (auto iter = cf_.logVariablesBegin(); iter != cf_.logVariablesEnd(); ++iter) { + auto entry = *iter; + if (entry.group == "radio" && entry.name == "numRxBc") { + logvars.push_back({"radio", "numRxBc"}); + logvars.push_back({"radio", "numRxUc"}); + status_has_radio_stats_ = true; + break; + } + } + + // older firmware -> use other 16-bit variables + if (!status_has_radio_stats_) { + RCLCPP_WARN(logger_, "Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero."); + logvars.push_back({"pm", "vbatMV"}); + logvars.push_back({"pm", "vbatMV"}); + } + log_block_status_.reset(new LogBlock( - &cf_,{ - // general status - {"supervisor", "info"}, - // battery related - {"pm", "vbatMV"}, - {"pm", "state"}, - // radio related - {"radio", "rssi"}, - {"radio", "numRxBc"}, - {"radio", "numRxUc"}, - }, cb)); + &cf_,logvars, cb)); log_block_status_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds } else if (i.first.find("custom_topics") == 0 @@ -738,27 +757,32 @@ class CrazyflieROS msg.battery_voltage = data->vbatMV / 1000.0f; msg.pm_state = data->pmState; msg.rssi = data->rssi; - int32_t deltaRxBc = data->numRxBc - previous_numRxBc; - int32_t deltaRxUc = data->numRxUc - previous_numRxUc; - // handle overflow - if (deltaRxBc < 0) { - deltaRxBc += std::numeric_limits::max(); - } - if (deltaRxUc < 0) { - deltaRxUc += std::numeric_limits::max(); + if (status_has_radio_stats_) { + int32_t deltaRxBc = data->numRxBc - previous_numRxBc; + int32_t deltaRxUc = data->numRxUc - previous_numRxUc; + // handle overflow + if (deltaRxBc < 0) { + deltaRxBc += std::numeric_limits::max(); + } + if (deltaRxUc < 0) { + deltaRxUc += std::numeric_limits::max(); + } + msg.num_rx_broadcast = deltaRxBc; + msg.num_rx_unicast = deltaRxUc; + previous_numRxBc = data->numRxBc; + previous_numRxUc = data->numRxUc; + } else { + msg.num_rx_broadcast = 0; + msg.num_rx_unicast = 0; } - msg.num_rx_broadcast = deltaRxBc; - msg.num_rx_unicast = deltaRxUc; - previous_numRxBc = data->numRxBc; - previous_numRxUc = data->numRxUc; - // compare with connection stats (unicast) + // connection sent stats (unicast) const auto statsUc = cf_.connectionStats(); size_t deltaTxUc = statsUc.sent_count - previous_stats_unicast_.sent_count; msg.num_tx_unicast = deltaTxUc; previous_stats_unicast_ = statsUc; - // compare with connection stats (broadcast) + // connection sent stats (broadcast) const auto statsBc = cfbc_->connectionStats(); size_t deltaTxBc = statsBc.sent_count - previous_stats_broadcast_.sent_count; msg.num_tx_broadcast = deltaTxBc; @@ -853,6 +877,7 @@ class CrazyflieROS rclcpp::Publisher::SharedPtr publisher_scan_; std::unique_ptr> log_block_status_; + bool status_has_radio_stats_; rclcpp::Publisher::SharedPtr publisher_status_; uint16_t previous_numRxBc; uint16_t previous_numRxUc; From 384193395ee8f3e2501c6feb086712018ef0072f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 19 Jan 2024 15:55:37 +0100 Subject: [PATCH 4/5] update submodule --- crazyflie/deps/crazyflie_tools | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index 8cf7ee6b0..36ea3d401 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit 8cf7ee6b0e91b78e428e167914c7a9a267e11ac2 +Subproject commit 36ea3d40161db8f580dbb8846789bc036a2ccedd From 1a6c1e5a8dd19e6f555ddb00f8406ecff0ae8e56 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 27 Jan 2024 14:31:48 +0100 Subject: [PATCH 5/5] update TODO based on Tobias/Kimberlys comments --- crazyflie/src/crazyflie_server.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index 065bce59d..d79e307f7 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/crazyflie/src/crazyflie_server.cpp @@ -107,7 +107,9 @@ class CrazyflieROS // general status uint16_t supervisorInfo; // supervisor.info // battery related - // TODO: would it be better to use pm.batteryLevel? + // Note that using BQ-deck/Bolt one can actually have two batteries at the same time. + // vbat refers to the battery directly connected to the CF board and might not reflect + // the "external" battery on BQ/Bolt builds uint16_t vbatMV; // pm.vbatMV uint8_t pmState; // pm.state // radio related