4 files changed, 46 insertions, 11 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 9f682c7e1..51589e43e 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -49,6 +49,13 @@ 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");
+	}
 }
 
 UavcanEscController::~UavcanEscController()
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index e5d30f6c4..600cb47f3 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -38,7 +38,7 @@
 
 MODULE_COMMAND = uavcan
 
-MAXOPTIMIZATION = -Os
+MAXOPTIMIZATION = -O3
 
 # Main
 SRCS += uavcan_main.cpp              \
@@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 60901e0c7..4dc03b61b 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -81,6 +81,18 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	if (res < 0) {
 		std::abort();
 	}
+
+	if (_perfcnt_node_spin_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_output_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_total_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed");
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -118,6 +130,10 @@ UavcanNode::~UavcanNode()
 	}
 
 	_instance = nullptr;
+
+	perf_free(_perfcnt_node_spin_elapsed);
+	perf_free(_perfcnt_esc_mixer_output_elapsed);
+	perf_free(_perfcnt_esc_mixer_total_elapsed);
 }
 
 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -265,10 +281,12 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 void UavcanNode::node_spin_once()
 {
+	perf_begin(_perfcnt_node_spin_elapsed);
 	const int spin_res = _node.spin(uavcan::MonotonicTime());
 	if (spin_res < 0) {
 		warnx("node spin error %i", spin_res);
 	}
+	perf_end(_perfcnt_node_spin_elapsed);
 }
 
 /*
@@ -344,8 +362,12 @@ int UavcanNode::run()
 		// Mutex is unlocked while the thread is blocked on IO multiplexing
 		(void)pthread_mutex_unlock(&_node_mutex);
 
+		perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		perf_begin(_perfcnt_esc_mixer_total_elapsed);
+
 		(void)pthread_mutex_lock(&_node_mutex);
 
 		node_spin_once();  // Non-blocking
@@ -430,7 +452,9 @@ int UavcanNode::run()
 			}
 			// Output to the bus
 			_outputs.timestamp = hrt_absolute_time();
+			perf_begin(_perfcnt_esc_mixer_output_elapsed);
 			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+			perf_end(_perfcnt_esc_mixer_output_elapsed);
 		}
 
 
@@ -484,7 +508,7 @@ UavcanNode::teardown()
 			_control_subs[i] = -1;
 		}
 	}
-	return ::close(_armed_sub);
+	return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
 }
 
 int
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 98f2e5ad4..19b9b4b48 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -34,9 +34,9 @@
 #pragma once
 
 #include <nuttx/config.h>
-
 #include <uavcan_stm32/uavcan_stm32.hpp>
 #include <drivers/device/device.h>
+#include <systemlib/perf_counter.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -66,7 +66,7 @@
  */
 class UavcanNode : public device::CDev
 {
-	static constexpr unsigned MemPoolSize        = 10752;
+	static constexpr unsigned MemPoolSize        = 10752; ///< Refer to the libuavcan manual to learn why
 	static constexpr unsigned RxQueueLenPerIface = 64;
 	static constexpr unsigned StackSize          = 3000;
 
@@ -107,11 +107,11 @@ private:
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
 	int			_armed_sub = -1;		///< uORB subscription of the arming status
-	actuator_armed_s	_armed;				///< the arming request of the system
+	actuator_armed_s	_armed = {};			///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
 	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
-	test_motor_s		_test_motor;
+	test_motor_s		_test_motor = {};
 	bool			_test_in_progress = false;
 
 	unsigned		_output_count = 0;		///< number of actuators currently available
@@ -135,11 +135,15 @@ private:
 	unsigned		_poll_fds_num = 0;
 
 	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
-	uint8_t			_actuator_direct_poll_fd_num;
-	actuator_direct_s	_actuator_direct;
+	uint8_t			_actuator_direct_poll_fd_num = 0;
+	actuator_direct_s	_actuator_direct = {};
 
-	actuator_outputs_s	_outputs;
+	actuator_outputs_s	_outputs = {};
 
 	// index into _poll_fds for each _control_subs handle
 	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+
+	perf_counter_t _perfcnt_node_spin_elapsed        = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_total_elapsed  = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
 };