diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template index 4baa534ead..47fade84d5 100644 --- a/msg/templates/uorb/msg.cpp.template +++ b/msg/templates/uorb/msg.cpp.template @@ -69,6 +69,7 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so #include #include #include +#include @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed" @# This is used for the logger diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index dca60043cf..cec4d08e4a 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -250,6 +250,10 @@ def print_field(field): print("if (message.timestamp != 0) {\n\t\tPX4_INFO_RAW(\"\\t" + field.name + \ ": " + c_type + " (%.6f seconds ago)\\n\", " + field_name + \ ", hrt_elapsed_time(&message.timestamp) / 1e6);\n\t} else {\n\t\tPX4_INFO_RAW(\"\\n\");\n\t}" ) + elif field.name == 'device_id': + print("char device_id_buffer[80];") + print("device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), message.device_id);") + print("PX4_INFO_RAW(\"\\tdevice_id: %d (%s) \\n\", message.device_id, device_id_buffer);" ) else: print("PX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + "\\n\", " + field_name + ");" ) diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 6fa0a1fe28..ff3c65e864 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -148,6 +148,24 @@ public: */ DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; } + static const char *get_device_bus_string(DeviceBusType bus) + { + switch (bus) { + case DeviceBusType_I2C: + return "I2C"; + + case DeviceBusType_SPI: + return "SPI"; + + case DeviceBusType_UAVCAN: + return "UAVCAN"; + + case DeviceBusType_UNKNOWN: + default: + return "UNKNOWN"; + } + } + /** * Return the bus address of the device. * @@ -184,6 +202,27 @@ public: uint32_t devid; }; + /** + * Print decoded device id string to a buffer. + * + * @param buffer buffer to write to + * @param length buffer length + * @param id The device id. + * @param return number of bytes written + */ + static int device_id_print_buffer(char *buffer, int length, uint32_t id) + { + DeviceId dev_id; + dev_id.devid = id; + + int num_written = snprintf(buffer, length, "Type: 0x%02X, %s:%d (0x%02X)", dev_id.devid_s.devtype, + get_device_bus_string(dev_id.devid_s.bus_type), dev_id.devid_s.bus, dev_id.devid_s.address); + + buffer[length - 1] = 0; // ensure 0-termination + + return num_written; + } + protected: union DeviceId _device_id; /**< device identifier information */