diff options
author | Lorenz Meier <lm@qgroundcontrol.org> | 2014-12-03 11:16:34 +0100 |
---|---|---|
committer | Lorenz Meier <lm@qgroundcontrol.org> | 2014-12-03 11:16:34 +0100 |
commit | 6bd586c8373607faeb0e6eeb1135c9623ce9954e (patch) | |
tree | f64654915137a6971f78255300e7c4a2ca4c6bb6 | |
parent | d5debc2f421290c23345a28f2f074f3630c75f26 (diff) | |
parent | f3fb32bc4790673e4a643f341b16d2434423dbea (diff) | |
download | px4-firmware-6bd586c8373607faeb0e6eeb1135c9623ce9954e.tar.gz px4-firmware-6bd586c8373607faeb0e6eeb1135c9623ce9954e.tar.bz2 px4-firmware-6bd586c8373607faeb0e6eeb1135c9623ce9954e.zip |
Merge pull request #1482 from sjwilks/uavcan_debug
Display ESC data in the status output
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 24 |
1 files changed, 24 insertions, 0 deletions
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8147a8b89..60901e0c7 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,32 @@ 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"); + } + + orb_unsubscribe(esc_sub); } // Sensor bridges |