Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-19 02:23:58 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

topic_listener: avoid code generation, use existing metadata at runtime

Browse Source 
This reduces flash size for v5 by ~110KB, the topic listener now only adds
about 1.2KB.
This commit is contained in:
Beat Küng
2021-10-19 15:19:42 +02:00
parent a0e72b35a4
commit 8a2b310b83
16 changed files with 330 additions and 347 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb/msg.cpp.em
+6 -14
View File
@@ -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<uint8_t>(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);
}
msg/templates/uorb/msg.h.em
+1 -1
View File
@@ -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
msg/tools/px_generate_uorb_topic_helper.py
-122
View File
@@ -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
platforms/common/uORB/uORB.cpp
+271
View File
@@ -41,6 +41,10 @@
#include "uORBManager.hpp"
#include "uORBCommon.hpp"
#include <lib/drivers/device/Device.hpp>
#include <matrix/Quaternion.hpp>
#include <mathlib/mathlib.h>
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;
}
}
platforms/common/uORB/uORB.h
+8
View File
@@ -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
src/drivers/gps/gps.cpp
+1 -1
View File
@@ -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()) {
src/drivers/px4io/px4io.cpp
+1 -1
View File
@@ -1508,7 +1508,7 @@ int PX4IO::print_status()
uORB::SubscriptionData<px4io_status_s> 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);
src/drivers/rc_input/RCInput.cpp
+1 -1
View File
@@ -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;
src/drivers/rpm/rpm_simulator/rpm_simulator.cpp
+1 -1
View File
@@ -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;
}
src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp
+1 -1
View File
@@ -120,7 +120,7 @@ public:
_battery_status_pub.publish(bat_status);
print_message(bat_status);
print_message(ORB_ID(battery_status), bat_status);
};
private:
src/modules/airship_att_control/airship_att_control_main.cpp
+1 -1
View File
@@ -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;
}
src/systemcmds/topic_listener/CMakeLists.txt
-20
View File
@@ -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
)
src/systemcmds/topic_listener/generate_listener.py
-135
View File
@@ -1,135 +0,0 @@
#!/usr/bin/python
import glob
import os
import sys
import re
# This script is run from Build/<target>_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 <drivers/drv_hrt.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <uORB/uORB.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <topic_listener.hpp>
#include <topic_listener_generated.hpp>
""")
for m in set(messages):
print("#include <uORB/topics/%s.h>" % 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")
src/systemcmds/topic_listener/listener_main.cpp
+28 -6
View File
@@ -42,8 +42,8 @@
#include <poll.h>
#include <uORB/topics/uORBTopics.hpp>
#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;
}
src/systemcmds/topic_listener/topic_listener.hpp
+10 -7
View File
@@ -50,21 +50,24 @@
#include <stdlib.h>
#include <inttypes.h>
typedef int(*listener_print_topic_cb)(const orb_id_t &orb_id, int subscription);
template <typename T>
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);
src/systemcmds/topic_listener/topic_listener_generated.hpp
-36
View File
@@ -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);