UavcanEscController broadcasting percounter

2 files changed, 15 insertions, 0 deletions

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 9f682c7e1..995c8987c 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -49,12 +49,24 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_uavcan_sub_status(node),
 	_orb_timer(node)
 {
+	if (_perfcnt_invalid_input == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
+	}
+
+	if (_perfcnt_scaling_error == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
+	}
+
+	if (_perfcnt_broadcast_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed");
+	}
 }
 
 UavcanEscController::~UavcanEscController()
 {
 	perf_free(_perfcnt_invalid_input);
 	perf_free(_perfcnt_scaling_error);
+	perf_free(_perfcnt_broadcast_elapsed);
 }
 
 int UavcanEscController::init()
@@ -129,7 +141,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 * Publish the command message to the bus
 	 * Note that for a quadrotor it takes one CAN frame
 	 */
+	perf_begin(_perfcnt_broadcast_elapsed);
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+	perf_end(_perfcnt_broadcast_elapsed);
 }
 
 void UavcanEscController::arm_all_escs(bool arm)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index 12c035542..498fb9dd8 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -109,4 +109,5 @@ private:
 	 */
 	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
 	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
+	perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed");
 };