uORB messages generated pretty print

msg/CMakeLists.txt

@@ -165,7 +165,10 @@ add_custom_command(OUTPUT ${uorb_headers}
 		-e templates/uorb
 		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/headers
 		-q
-	DEPENDS ${msg_files}
+	DEPENDS
+		${msg_files}
+		templates/uorb/msg.h.template
+		tools/px_generate_uorb_topic_files.py
 	COMMENT "Generating uORB topic headers"
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	VERBATIM
@@ -182,7 +185,10 @@ add_custom_command(OUTPUT ${uorb_sources}
 		-e templates/uorb
 		-t ${CMAKE_CURRENT_BINARY_DIR}/tmp/sources
 		-q
-	DEPENDS ${msg_files}
+	DEPENDS
+		${msg_files}
+		templates/uorb/msg.cpp.template
+		tools/px_generate_uorb_topic_files.py
 	COMMENT "Generating uORB topic sources"
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	VERBATIM

msg/templates/uorb/msg.cpp.template

@@ -64,7 +64,11 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
 }@
 
+#include <cinttypes>
+#include <cstdio>
+#include <px4_defines.h>
 #include <uORB/topics/@(topic_name).h>
+#include <drivers/drv_hrt.h>
 
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
 @# This is used for the logger
@@ -73,3 +77,17 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
 @[for multi_topic in topics]@
 ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields);
 @[end for]
+
+void print_message(const @uorb_struct& message)
+{
+	printf(" @(uorb_struct)\n");
+	printf("\ttimestamp: %" PRIu64, message.timestamp);
+	if (message.timestamp != 0) {
+		printf(" (%.6f seconds ago)\n", hrt_elapsed_time(&message.timestamp) / 1e6);
+	} else {
+		printf("\n");
+	}
+@[for field in sorted_fields]@
+	@( print_field(field) )@
+@[end for]@
+}

msg/templates/uorb/msg.h.template

@@ -133,3 +133,7 @@ for constant in spec.constants:
 @[for multi_topic in topics]@
 ORB_DECLARE(@multi_topic);
 @[end for]
+
+#ifdef __cplusplus
+void print_message(const @uorb_struct& message);
+#endif

msg/tools/px_generate_uorb_topic_helper.py

@@ -68,6 +68,20 @@
     'char': 1,
 }
 
+type_printf_map = {
+    'int8': '%d',
+    'int16': '%d',
+    'int32': '%" PRId32 "',
+    'int64': '%" PRId64 "',
+    'uint8': '%u',
+    'uint16': '%u',
+    'uint32': '%" PRIu32 "',
+    'uint64': '%" PRIu64 "',
+    'float32': '%.3f',
+    'float64': '%.3f',
+    'bool': '%u',
+    'char': '%c',
+}
 
 def bare_name(msg_type):
     """
@@ -165,6 +179,70 @@ def convert_type(spec_type):
     return c_type
 
 
+def print_field(field):
+    """
+    Echo printf line
+    """
+
+    # 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("printf(\"\\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
+
+        else:
+            print("printf(\"\\n\\t" + field.name + "\");")
+            print("\tprint_message(message."+ field.name + ");")
+            return
+
+    print("printf(\"\\t" + field.name + ": " + c_type + "\\n\", " + field_name + ");" )
+
+
 def print_field_def(field):
     """
     Print the C type from a field

src/drivers/barometer/bmp280/bmp280.cpp

@@ -595,11 +595,10 @@ BMP280::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	printf("poll interval:  %u us \n", _report_ticks * USEC_PER_TICK);
-	_reports->print_info("report queue");
-	printf("P Pa:              %.3f\n", (double)_P);
-	printf("T:              %.3f\n", (double)_T);
-	printf("MSL pressure Pa:   %u\n", _msl_pressure);
 
+	sensor_baro_s brp = {};
+	_reports->get(&brp);
+	print_message(brp);
 }
 
 /**
@@ -778,11 +777,7 @@ test(enum BMP280_BUS busid)
 		exit(1);
 	}
 
-	PX4_WARN("single read");
-	PX4_WARN("pressure:    %10.4f", (double)report.pressure);
-	PX4_WARN("altitude:    %11.4f", (double)report.altitude);
-	PX4_WARN("temperature: %8.4f", (double)report.temperature);
-	PX4_WARN("time:        %lld", report.timestamp);
+	print_message(report);
 
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
@@ -818,11 +813,7 @@ test(enum BMP280_BUS busid)
 			exit(1);
 		}
 
-		PX4_WARN("periodic read %u", i);
-		PX4_WARN("pressure:    %10.4f", (double)report.pressure);
-		PX4_WARN("altitude:    %11.4f", (double)report.altitude);
-		PX4_WARN("temperature K: %8.4f", (double)report.temperature);
-		PX4_WARN("time:        %lld", report.timestamp);
+		print_message(report);
 	}
 
 	close(fd);

src/drivers/barometer/lps25h/lps25h.cpp

@@ -799,9 +799,7 @@ LPS25H::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
-	printf("pressure    %.2f\n", (double)_last_report.pressure);
-	printf("altitude:    %.2f\n", (double)_last_report.altitude);
-	printf("temperature %.2f\n", (double)_last_report.temperature);
+	print_message(_last_report);
 
 	_reports->print_info("report queue");
 }
@@ -959,11 +957,7 @@ test(enum LPS25H_BUS busid)
 		err(1, "immediate read failed");
 	}
 
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", report.timestamp);
+	print_message(report);
 
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
@@ -995,11 +989,7 @@ test(enum LPS25H_BUS busid)
 			err(1, "periodic read failed");
 		}
 
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature K: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", report.timestamp);
+		print_message(report);
 	}
 
 	close(fd);

src/drivers/barometer/mpl3115a2/mpl3115a2.cpp

@@ -732,10 +732,10 @@ MPL3115A2::print_info()
 	perf_print_counter(_comms_errors);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	_reports->print_info("report queue");
-	printf("device:         mpl3115a2\n");
-	printf("P:              %.3f\n", (double)_P);
-	printf("T:              %.3f\n", (double)_T);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
+
+	sensor_baro_s brp = {};
+	_reports->get(&brp);
+	print_message(brp);
 }
 
 /**
@@ -899,11 +899,7 @@ test(enum MPL3115A2_BUS busid)
 		err(1, "immediate read failed");
 	}
 
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", report.timestamp);
+	print_message(report);
 
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
@@ -935,11 +931,7 @@ test(enum MPL3115A2_BUS busid)
 			err(1, "periodic read failed");
 		}
 
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", report.timestamp);
+		print_message(report);
 	}
 
 	close(fd);

src/drivers/barometer/ms5611/ms5611.cpp

@@ -895,9 +895,6 @@ MS5611::print_info()
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
-	printf("P:              %.3f\n", (double)_P);
-	printf("T:              %.3f\n", (double)_T);
-	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
 
 	printf("factory_setup             %u\n", _prom.factory_setup);
 	printf("c1_pressure_sens          %u\n", _prom.c1_pressure_sens);
@@ -907,6 +904,10 @@ MS5611::print_info()
 	printf("c5_reference_temp         %u\n", _prom.c5_reference_temp);
 	printf("c6_temp_coeff_temp        %u\n", _prom.c6_temp_coeff_temp);
 	printf("serial_and_crc            %u\n", _prom.serial_and_crc);
+
+	sensor_baro_s brp = {};
+	_reports->get(&brp);
+	print_message(brp);
 }
 
 /**
@@ -1118,11 +1119,7 @@ test(enum MS5611_BUS busid)
 		err(1, "immediate read failed");
 	}
 
-	warnx("single read");
-	warnx("pressure:    %10.4f", (double)report.pressure);
-	warnx("altitude:    %11.4f", (double)report.altitude);
-	warnx("temperature: %8.4f", (double)report.temperature);
-	warnx("time:        %lld", report.timestamp);
+	print_message(report);
 
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
@@ -1154,11 +1151,7 @@ test(enum MS5611_BUS busid)
 			err(1, "periodic read failed");
 		}
 
-		warnx("periodic read %u", i);
-		warnx("pressure:    %10.4f", (double)report.pressure);
-		warnx("altitude:    %11.4f", (double)report.altitude);
-		warnx("temperature: %8.4f", (double)report.temperature);
-		warnx("time:        %lld", report.timestamp);
+		print_message(report);
 	}
 
 	close(fd);

src/drivers/batt_smbus/batt_smbus.cpp

@@ -317,10 +317,7 @@ BATT_SMBUS::test()
 
 		if (updated) {
 			if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
-				PX4_INFO("V=%4.2f C=%4.2f AveC=%4.2f DismAh=%f Cap:%hu TempC:%4.2f Remaining:%3.2f\n RunTimeToEmpty:%hu AveTimeToEmpty:%hu CycleCount:%hu SerialNum:%04x",
-					 (double)status.voltage_v, (double)status.current_a, (double)status.average_current_a, (double)status.discharged_mah,
-					 (uint16_t)status.capacity, (double)status.temperature, (double)status.remaining, (uint16_t)status.run_time_to_empty,
-					 (uint16_t)status.average_time_to_empty, (uint16_t)status.cycle_count, (uint16_t)status.serial_number);
+				print_message(status);
 			}
 		}
 

src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp

@@ -801,10 +801,7 @@ test()
 		err(1, "immediate read failed");
 	}
 
-	warnx("single read");
-	warnx("measurement: %0.2f of sonar %d,id=%d", (double)report.distance_vector[report.just_updated], report.just_updated,
-	      report.id[report.just_updated]);
-	warnx("time:        %lld", report.timestamp);
+	print_message(report);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -831,14 +828,7 @@ test()
 			err(1, "periodic read failed");
 		}
 
-		warnx("periodic read %u", i);
-
-		/* Print the sonar rangefinder report sonar distance vector */
-		for (uint8_t count = 0; count < SRF02_MAX_RANGEFINDERS; count++) {
-			warnx("measurement: %0.3f of sonar %u, id=%d", (double)report.distance_vector[count], count + 1, report.id[count]);
-		}
-
-		warnx("time:        %lld", report.timestamp);
+		print_message(report);
 	}
 
 	/* reset the sensor polling to default rate */

src/drivers/distance_sensor/leddar_one/leddar_one.cpp

@@ -231,16 +231,7 @@ int leddar_one_main(int argc, char *argv[])
 			return PX4_ERROR;
 		}
 
-		bool valid = false;
-
-		if (report.current_distance > report.min_distance
-		    && report.current_distance < report.max_distance) {
-			valid = true;
-		}
-
-		warnx("valid: %u\n", valid);
-		warnx("distance: %0.3fm\n", (double)report.current_distance);
-		warnx("time: %llu\n", report.timestamp);
+		print_message(report);
 
 	} else {
 		help();

src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp

@@ -123,7 +123,8 @@ void LidarLitePWM::print_info()
 	perf_print_counter(_read_errors);
 	perf_print_counter(_sensor_zero_resets);
 	warnx("poll interval:  %u ticks", getMeasureTicks());
-	warnx("distance: %.3fm", (double)_range.current_distance);
+
+	print_message(_range);
 }
 
 void LidarLitePWM::print_registers()