diff --git a/msg/templates/uorb/msg.cpp.em b/msg/templates/uorb/msg.cpp.em index 691b6bea67..03776549f5 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/msg/templates/uorb/msg.cpp.em @@ -82,19 +82,11 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast(ORB_ID::@multi_topic)); @[end for] -void print_message(const @uorb_struct &message) +void print_message(const orb_metadata *meta, const @uorb_struct& message) { -@[if constrained_flash] - (void)message; - PX4_INFO_RAW("Not implemented on flash constrained hardware\n"); -@[else] - PX4_INFO_RAW(" @(uorb_struct)\n"); - - const hrt_abstime now = hrt_absolute_time(); - -@[for field in sorted_fields]@ - @( print_field(field) )@ -@[end for]@ -@[end if]@ - + if (sizeof(message) != meta->o_size) { + printf("unexpected message size for %s: %zu != %i\n", meta->o_name, sizeof(message), meta->o_size); + return; + } + orb_print_message_internal(meta, &message, true); } diff --git a/msg/templates/uorb/msg.h.em b/msg/templates/uorb/msg.h.em index adc4b585fa..9854e1eaef 100644 --- a/msg/templates/uorb/msg.h.em +++ b/msg/templates/uorb/msg.h.em @@ -133,5 +133,5 @@ ORB_DECLARE(@multi_topic); @[end for] #ifdef __cplusplus -void print_message(const @uorb_struct& message); +void print_message(const orb_metadata *meta, const @uorb_struct& message); #endif diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index d4d101f327..1f90a01ddf 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -241,128 +241,6 @@ def convert_type(spec_type, use_short_type=False): return c_type -def print_field(field): - """ - Echo printf line - """ - - # check if there are any upper case letters in the field name - assert not any(a.isupper() - for a in field.name), "%r field contains uppercase letters" % field.name - - # skip padding - if field.name.startswith('_padding'): - return - - bare_type = field.type - if '/' in field.type: - # removing prefix - bare_type = (bare_type.split('/'))[1] - - msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) - - field_name = "" - - if is_array: - c_type = "[" - - if msg_type in type_map: - p_type = type_printf_map[msg_type] - - else: - for i in range(array_length): - print(("PX4_INFO_RAW(\"\\t" + field.type + - " " + field.name + "[" + str(i) + "]\");")) - print((" print_message(message." + - field.name + "[" + str(i) + "]);")) - return - - for i in range(array_length): - - if i > 0: - c_type += ", " - field_name += ", " - - if "float32" in field.type: - field_name += "(double)message." + \ - field.name + "[" + str(i) + "]" - else: - field_name += "message." + field.name + "[" + str(i) + "]" - - c_type += str(p_type) - - c_type += "]" - - else: - c_type = msg_type - if msg_type in type_map: - c_type = type_printf_map[msg_type] - - field_name = "message." + field.name - - # cast double - if field.type == "float32": - field_name = "(double)" + field_name - elif field.type == "bool": - c_type = '%s' - field_name = "(" + field_name + " ? \"True\" : \"False\")" - - else: - print(("PX4_INFO_RAW(\"\\n\\t" + field.name + "\");")) - print(("\tprint_message(message." + field.name + ");")) - return - - if field.name == 'timestamp': - print(("if (message.timestamp != 0) {\n\t\tPX4_INFO_RAW(\"\\t" + field.name + - ": " + c_type + " (%.6f seconds ago)\\n\", " + field_name + - ", (now - message.timestamp) / 1e6);\n\t} else {\n\t\tPX4_INFO_RAW(\"\\n\");\n\t}")) - elif field.name == 'timestamp_sample': - print(("\n\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (" + c_type + - " us before timestamp)\\n\", " + field_name + ", message.timestamp - message.timestamp_sample);\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: %\" PRId32 \" (%s) \\n\", message.device_id, device_id_buffer);") - elif field.name == 'accel_device_id': - print("char accel_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(accel_device_id_buffer, sizeof(accel_device_id_buffer), message.accel_device_id);") - print("PX4_INFO_RAW(\"\\taccel_device_id: %\" PRId32 \" (%s) \\n\", message.accel_device_id, accel_device_id_buffer);") - elif field.name == 'gyro_device_id': - print("char gyro_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(gyro_device_id_buffer, sizeof(gyro_device_id_buffer), message.gyro_device_id);") - print("PX4_INFO_RAW(\"\\tgyro_device_id: %\" PRId32 \" (%s) \\n\", message.gyro_device_id, gyro_device_id_buffer);") - elif field.name == 'baro_device_id': - print("char baro_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(baro_device_id_buffer, sizeof(baro_device_id_buffer), message.baro_device_id);") - print("PX4_INFO_RAW(\"\\tbaro_device_id: %\" PRId32 \" (%s) \\n\", message.baro_device_id, baro_device_id_buffer);") - elif field.name == 'mag_device_id': - print("char mag_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(mag_device_id_buffer, sizeof(mag_device_id_buffer), message.mag_device_id);") - print("PX4_INFO_RAW(\"\\tmag_device_id: %\" PRId32 \" (%s) \\n\", message.mag_device_id, mag_device_id_buffer);") - elif (field.name == 'q' or 'q_' in field.name) and field.type == 'float32[4]': - # float32[4] q/q_d/q_reset/delta_q_reset - print("{") - print( - "\t\tmatrix::Eulerf euler{matrix::Quatf{message." + field.name + "}};") - print("\t\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg" ")\\n\", " + - field_name + ", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));\n\t") - print("\t}") - - elif ("flags" in field.name or "bits" in field.name) and "uint" in field.type: - # print bits of fixed width unsigned integers (uint8, uint16, uint32) if name contains flags or bits - print("PX4_INFO_RAW(\"\\t" + field.name + ": " + - c_type + " (0b\", " + field_name + ");") - print("\tfor (int i = (sizeof(" + field_name + ") * 8) - 1; i >= 0; i--) { PX4_INFO_RAW(\"%lu%s\", (unsigned long) " - + field_name + " >> i & 1, ((unsigned)i < (sizeof(" + field_name + ") * 8) - 1 && i % 4 == 0 && i > 0) ? \"'\" : \"\"); }") - print("\tPX4_INFO_RAW(\")\\n\");") - elif is_array and 'char' in field.type: - print(("PX4_INFO_RAW(\"\\t" + field.name + ": \\\"%." + - str(array_length) + "s\\\" \\n\", message." + field.name + ");")) - else: - print(("PX4_INFO_RAW(\"\\t" + field.name + ": " + - c_type + "\\n\", " + field_name + ");")) - - def print_field_def(field): """ Print the C type from a field diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 4f266ca489..27ce994eb0 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -41,6 +41,10 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" +#include +#include +#include + static uORB::DeviceMaster *g_dev = nullptr; int uorb_start(void) @@ -204,3 +208,270 @@ const char *orb_get_c_type(unsigned char short_type) return nullptr; } + + +void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name) +{ + if (print_topic_name) { + PX4_INFO_RAW(" %s\n", meta->o_name); + } + + const hrt_abstime now = hrt_absolute_time(); + hrt_abstime topic_timestamp = 0; + + const uint8_t *data_ptr = (const uint8_t *)data; + int data_offset = 0; + + for (int format_idx = 0; meta->o_fields[format_idx] != 0;) { + const char *end_field = strchr(meta->o_fields + format_idx, ';'); + + if (!end_field) { + PX4_ERR("Format error in %s", meta->o_fields); + return; + } + + const char *c_type = orb_get_c_type(meta->o_fields[format_idx]); + const int end_field_idx = end_field - meta->o_fields; + + int array_idx = -1; + int field_name_idx = -1; + + for (int field_idx = format_idx; field_idx != end_field_idx; ++field_idx) { + if (meta->o_fields[field_idx] == '[') { + array_idx = field_idx + 1; + + } else if (meta->o_fields[field_idx] == ' ') { + field_name_idx = field_idx + 1; + break; + } + } + + int array_size = 1; + + if (array_idx >= 0) { + array_size = strtol(meta->o_fields + array_idx, nullptr, 10); + } + + char field_name[80]; + size_t field_name_len = end_field_idx - field_name_idx; + + if (field_name_len >= sizeof(field_name)) { + PX4_ERR("field name too long %s (max: %u)", meta->o_fields, (unsigned)sizeof(field_name)); + return; + } + + memcpy(field_name, meta->o_fields + field_name_idx, field_name_len); + field_name[field_name_len] = '\0'; + + if (c_type) { // built-in type + bool dont_print = false; + + // handle special cases + if (strncmp(field_name, "_padding", 8) == 0) { + dont_print = true; + + } else if (strcmp(c_type, "char") == 0 && array_size > 1) { // string + PX4_INFO_RAW(" %s: \"%.*s\"\n", field_name, array_size, (char *)(data_ptr + data_offset)); + dont_print = true; + } + + if (!dont_print) { + PX4_INFO_RAW(" %s: ", field_name); + } + + if (!dont_print && array_size > 1) { + PX4_INFO_RAW("["); + } + + const int previous_data_offset = data_offset; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" // the caller ensures data is aligned + + for (int i = 0; i < array_size; ++i) { + if (strcmp(c_type, "int8_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi8, *(int8_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int8_t); + + } else if (strcmp(c_type, "int16_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi16, *(int16_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int16_t); + + } else if (strcmp(c_type, "int32_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi32, *(int32_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int32_t); + + } else if (strcmp(c_type, "int64_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi64, *(int64_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int64_t); + + } else if (strcmp(c_type, "uint8_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu8, *(uint8_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint8_t); + + } else if (strcmp(c_type, "uint16_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu16, *(uint16_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint16_t); + + } else if (strcmp(c_type, "uint32_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu32, *(uint32_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint32_t); + + } else if (strcmp(c_type, "uint64_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu64, *(uint64_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint64_t); + + } else if (strcmp(c_type, "float") == 0) { + if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); } + + data_offset += sizeof(float); + + } else if (strcmp(c_type, "double") == 0) { + if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); } + + data_offset += sizeof(double); + + } else if (strcmp(c_type, "bool") == 0) { + if (!dont_print) { PX4_INFO_RAW("%s", *(bool *)(data_ptr + data_offset) ? "True" : "False"); } + + data_offset += sizeof(bool); + + } else if (strcmp(c_type, "char") == 0) { + if (!dont_print) { PX4_INFO_RAW("%i", (int) * (char *)(data_ptr + data_offset)); } + + data_offset += sizeof(char); + + } else { + PX4_ERR("unknown type: %s", c_type); + return; + } + + if (!dont_print && i < array_size - 1) { + PX4_INFO_RAW(", "); + } + } + + if (!dont_print && array_size > 1) { + PX4_INFO_RAW("]"); + } + + // handle special cases + if (array_size == 1) { + if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp") == 0) { + topic_timestamp = *(uint64_t *)(data_ptr + previous_data_offset); + + if (topic_timestamp != 0) { + PX4_INFO_RAW(" (%.6f seconds ago)", (double)((now - topic_timestamp) / 1e6f)); + } + + } else if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp_sample") == 0) { + hrt_abstime timestamp = *(uint64_t *)(data_ptr + previous_data_offset); + + if (topic_timestamp != 0 && timestamp != 0) { + PX4_INFO_RAW(" (%i us before timestamp)", (int)(topic_timestamp - timestamp)); + } + + } else if (strstr(field_name, "flags") != nullptr) { + // bitfield + unsigned field_size = 0; + unsigned long value = 0; + + if (strcmp(c_type, "uint8_t") == 0) { + field_size = sizeof(uint8_t); + value = *(uint8_t *)(data_ptr + previous_data_offset); + + } else if (strcmp(c_type, "uint16_t") == 0) { + field_size = sizeof(uint16_t); + value = *(uint16_t *)(data_ptr + previous_data_offset); + + } else if (strcmp(c_type, "uint32_t") == 0) { + field_size = sizeof(uint32_t); + value = *(uint32_t *)(data_ptr + previous_data_offset); + } + + if (field_size > 0) { + PX4_INFO_RAW(" (0b"); + + for (int i = (field_size * 8) - 1; i >= 0; i--) { + PX4_INFO_RAW("%lu%s", (value >> i) & 1, ((unsigned)i < (field_size * 8) - 1 && i % 4 == 0 && i > 0) ? "'" : ""); + } + + PX4_INFO_RAW(")"); + } + + } else if (strcmp(c_type, "uint32_t") == 0 && strstr(field_name, "device_id") != nullptr) { + // Device ID + uint32_t device_id = *(uint32_t *)(data_ptr + previous_data_offset); + char device_id_buffer[80]; + device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), device_id); + PX4_INFO_RAW(" (%s)", device_id_buffer); + } + + } else if (array_size == 4 && strcmp(c_type, "float") == 0 && (strcmp(field_name, "q") == 0 + || strncmp(field_name, "q_", 2) == 0)) { + // attitude + float *attitude = (float *)(data_ptr + previous_data_offset); + matrix::Eulerf euler{matrix::Quatf{attitude}}; + PX4_INFO_RAW(" (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg)", + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + } + +#pragma GCC diagnostic pop + + PX4_INFO_RAW("\n"); + + } else { + + // extract the topic name + char topic_name[80]; + const size_t topic_name_len = array_size > 1 ? array_idx - format_idx - 1 : field_name_idx - format_idx - 1; + + if (topic_name_len >= sizeof(topic_name)) { + PX4_ERR("topic name too long in %s (max: %u)", meta->o_name, (unsigned)sizeof(topic_name)); + return; + } + + memcpy(topic_name, meta->o_fields + format_idx, topic_name_len); + field_name[topic_name_len] = '\0'; + + // find the metadata + const orb_metadata *const *topics = orb_get_topics(); + const orb_metadata *found_topic = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topics[i]->o_name, topic_name) == 0) { + found_topic = topics[i]; + break; + } + } + + if (!found_topic) { + PX4_ERR("Topic %s did not match any known topics", topic_name); + return; + } + + // print recursively + for (int i = 0; i < array_size; ++i) { + PX4_INFO_RAW(" %s", field_name); + + if (array_size > 1) { + PX4_INFO_RAW("[%i]", i); + } + + PX4_INFO_RAW(" (%s):\n", topic_name); + orb_print_message_internal(found_topic, data_ptr + data_offset, false); + data_offset += found_topic->o_size; + } + } + + format_idx = end_field_idx + 1; + } +} diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 27cc9a2863..c96f3c08d4 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -240,6 +240,14 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; */ const char *orb_get_c_type(unsigned char short_type); +/** + * Print a topic to console. Do not call this directly, use print_message() instead. + * @param meta orb topic metadata + * @param data expected to be aligned to the largest member + */ +void orb_print_message_internal(const struct orb_metadata *meta, const void *data, bool print_topic_name); + + __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index cea6077264..4be1a57176 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1045,7 +1045,7 @@ GPS::print_status() PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate); PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); - print_message(_report_gps_pos); + print_message(ORB_ID(sensor_gps), _report_gps_pos); } if (_instance == Instance::Main && _secondary_instance.load()) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8badca7f46..d474241579 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1508,7 +1508,7 @@ int PX4IO::print_status() uORB::SubscriptionData status_sub{ORB_ID(px4io_status)}; status_sub.update(); - print_message(status_sub.get()); + print_message(ORB_ID(px4io_status), status_sub.get()); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0x0000); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 9de2fc979c..f31d850002 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -861,7 +861,7 @@ int RCInput::print_status() perf_print_counter(_publish_interval_perf); if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) { - print_message(_rc_in); + print_message(ORB_ID(input_rc), _rc_in); } return 0; diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 6253f6e4d3..e48754b20b 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -72,7 +72,7 @@ int rpm_simulator_main(int argc, char *argv[]) // Publish data and let the user know what was published rpm_pub.publish(rpm); - print_message(rpm); + print_message(ORB_ID(rpm), rpm); return 0; } diff --git a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp index 7e1664c276..7a0a8365ec 100644 --- a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -120,7 +120,7 @@ public: _battery_status_pub.publish(bat_status); - print_message(bat_status); + print_message(ORB_ID(battery_status), bat_status); }; private: diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index db4f81338c..57ff120e13 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -170,7 +170,7 @@ int AirshipAttitudeControl::print_status() perf_print_counter(_loop_perf); - print_message(_actuators); + print_message(ORB_ID(actuator_controls), _actuators); return 0; } diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index 91f01658e5..e5f127fd1f 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -31,31 +31,11 @@ # ############################################################################ -add_custom_command(OUTPUT listener_generated.cpp - COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_listener.py ${PX4_SOURCE_DIR} ${msg_files} > listener_generated.cpp - DEPENDS generate_listener.py ${msg_files} -) - -add_custom_target(generate_topic_listener - DEPENDS - listener_generated.cpp - generate_listener.py -) -add_dependencies(prebuild_targets generate_topic_listener) - px4_add_module( MODULE systemcmds__topic_listener MAIN listener - COMPILE_FLAGS STACK_MAIN 4096 - INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR} - ${CMAKE_CURRENT_BINARY_DIR} SRCS listener_main.cpp - ${CMAKE_CURRENT_BINARY_DIR}/listener_generated.cpp - DEPENDS - generate_topic_listener - uorb_msgs ) diff --git a/src/systemcmds/topic_listener/generate_listener.py b/src/systemcmds/topic_listener/generate_listener.py deleted file mode 100755 index 7c07659784..0000000000 --- a/src/systemcmds/topic_listener/generate_listener.py +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/python - -import glob -import os -import sys -import re - -# This script is run from Build/_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener - -# argv[1] must be the full path of the top Firmware dir -# argv[2] - argv[n] is the full list of msg files - -raw_messages = sys.argv[2:] - -messages = [] -topics = [] -message_elements = [] - -for index,m in enumerate(raw_messages): - topic_list = [] - - msg_path = sys.argv[1]+ '/msg/' + m - - if os.path.isfile(msg_path): - # first try opening file in msg/ directory - f = open(msg_path,'r') - else: - # otherwise try opening directly (could be an external module msg) - f = open(m,'r') - - for line in f.readlines(): - items = re.split('\s+', line.strip()) - - if '# TOPICS' == ' '.join(items[:2]): - for topic in items[2:]: - topic_list.append(topic) - - f.close() - - (m_head, m_tail) = os.path.split(m) - message = m_tail.split('.')[0] - - if len(topic_list) == 0: - topic_list.append(message) - - for topic in topic_list: - messages.append(message) - topics.append(topic) - -num_messages = len(messages); - -print(""" - -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file topic_listener.cpp - * - * Autogenerated by Tools/generate_listener.py - * - * Tool for listening to topics when running flight stack on linux. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -""") -for m in set(messages): - print("#include " % m) - -print (""" -\nvoid listener_generated(char * topic_name, int topic_instance, int topic_rate, int num_msgs) { -""") - -print(""" - unsigned topic_interval = 0; - if (topic_rate != 0) { - topic_interval = 1000 / topic_rate; - } -""") - -for index, (m, t) in enumerate(zip(messages, topics)): - if index == 0: - print("\tif (strcmp(topic_name,\"%s\") == 0) {" % (t)) - else: - print("\t} else if (strcmp(topic_name,\"%s\") == 0) {" % (t)) - print("\t\tlistener(listener_print_topic<%s_s>, ORB_ID(%s), num_msgs, topic_instance, topic_interval);" % (m, t)) - -print("\t} else {") -print("\t\t PX4_INFO_RAW(\" Topic did not match any known topics\\n\");") -print("\t}") - -print("}\n") diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index c1a76c5ac4..b4db038c12 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -42,8 +42,8 @@ #include +#include #include "topic_listener.hpp" -#include "topic_listener_generated.hpp" // Amount of time to wait when listening for a message, before giving up. static constexpr float MESSAGE_TIMEOUT_S = 2.0f; @@ -52,7 +52,7 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]); static void usage(); -void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance, +void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, unsigned topic_interval) { @@ -69,7 +69,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, if (instances == 1) { PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name); int sub = orb_subscribe(id); - cb(id, sub); + listener_print_topic(id, sub); orb_unsubscribe(sub); } else if (instances > 1) { @@ -79,7 +79,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, if (orb_exists(id, i) == PX4_OK) { PX4_INFO_RAW("\nInstance %d:\n", i); int sub = orb_subscribe_multi(id, i); - cb(id, sub); + listener_print_topic(id, sub); orb_unsubscribe(sub); } } @@ -141,7 +141,7 @@ void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); - int ret = cb(id, sub); + int ret = listener_print_topic(id, sub); if (ret != PX4_OK) { PX4_ERR("listener callback failed (%i)", ret); @@ -208,7 +208,29 @@ int listener_main(int argc, char *argv[]) } } - listener_generated(topic_name, topic_instance, topic_rate, num_msgs); + + unsigned topic_interval = 0; + + if (topic_rate != 0) { + topic_interval = 1000 / topic_rate; + } + + const orb_metadata *const *topics = orb_get_topics(); + const orb_metadata *found_topic = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topics[i]->o_name, topic_name) == 0) { + found_topic = topics[i]; + } + } + + if (found_topic) { + listener(found_topic, num_msgs, topic_instance, topic_interval); + + } else { + PX4_INFO_RAW("Topic %s did not match any known topics\n", topic_name); + return -1; + } return 0; } diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp index aef71d3797..1131f4276f 100644 --- a/src/systemcmds/topic_listener/topic_listener.hpp +++ b/src/systemcmds/topic_listener/topic_listener.hpp @@ -50,21 +50,24 @@ #include #include -typedef int(*listener_print_topic_cb)(const orb_id_t &orb_id, int subscription); - -template -int listener_print_topic(const orb_id_t &orb_id, int subscription) +inline int listener_print_topic(const orb_id_t &orb_id, int subscription) { - T container; + static constexpr int max_size = 512; + alignas(8) char container[max_size]; + + if (orb_id->o_size > max_size) { + PX4_ERR("topic %s too large (%i > %i)", orb_id->o_name, orb_id->o_size, max_size); + return -1; + } int ret = orb_copy(orb_id, subscription, &container); if (ret == PX4_OK) { - print_message(container); + orb_print_message_internal(orb_id, &container, true); } return ret; } -void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance, +void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, unsigned topic_interval); diff --git a/src/systemcmds/topic_listener/topic_listener_generated.hpp b/src/systemcmds/topic_listener/topic_listener_generated.hpp deleted file mode 100644 index 195b0ac227..0000000000 --- a/src/systemcmds/topic_listener/topic_listener_generated.hpp +++ /dev/null @@ -1,36 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -void listener_generated(char *topic_name, int topic_instance, int topic_rate, int num_msgs);