From 7a344b933738d9caedc0fe177cc4615c294eaf51 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Wed, 3 Dec 2014 10:30:49 +0100
Subject: Display ESC data in the status output

---
 src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++++++++
 1 file changed, 22 insertions(+)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 8147a8b89..7213c72c6 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -46,6 +46,8 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
+#include <uORB/topics/esc_status.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
@@ -612,10 +614,30 @@ UavcanNode::print_info()
 
 	if (_outputs.noutputs != 0) {
 		printf("ESC output: ");
+
 		for (uint8_t i=0; i<_outputs.noutputs; i++) {
 			printf("%d ", (int)(_outputs.output[i]*1000));
 		}
 		printf("\n");
+
+		// ESC status
+		int esc_sub = orb_subscribe(ORB_ID(esc_status));
+		struct esc_status_s esc;
+		memset(&esc, 0, sizeof(esc));
+		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+		printf("ESC Status:\n");
+		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d\t",    esc.esc[i].esc_address);
+			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+			printf("%3.2f\t", (double)esc.esc[i].esc_current);
+			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+			printf("%d\t",    esc.esc[i].esc_rpm);
+			printf("%d",      esc.esc[i].esc_errorcount);
+			printf("\n");
+		}
 	}
 
 	// Sensor bridges
-- 
cgit v1.2.3


From f3fb32bc4790673e4a643f341b16d2434423dbea Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Wed, 3 Dec 2014 10:43:17 +0100
Subject: Unsubscribe from the topic.

---
 src/modules/uavcan/uavcan_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 7213c72c6..60901e0c7 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -638,6 +638,8 @@ UavcanNode::print_info()
 			printf("%d",      esc.esc[i].esc_errorcount);
 			printf("\n");
 		}
+
+		orb_unsubscribe(esc_sub);
 	}
 
 	// Sensor bridges
-- 
cgit v1.2.3