UAVCAN performance audit (#6829)

* UAVCAN ESC output: removing ESC output channels from published message that are always zero. This allows the UAVCAN stack to always transfer only the minimum number of output values, avoiding redundant zeroes and the associated increase in bus load and CPU time

* Added a separate mixer file for CAN quadrotor

* Sampling profiler improvements

* PMSP: Output more endpoints

* Matrix update

* libc usage workaround

* Removed UAVCAN perfcounters

src/modules/uavcan/actuators/esc.cpp

@@ -136,6 +136,25 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 		}
 	}
 
+	/*
+	 * Remove channels that are always zero.
+	 * The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame
+	 * transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
+	 * non-specified ESC setpoints should be considered zero.
+	 * The positive outcome is a (marginally) lower bus traffic and lower CPU load.
+	 *
+	 * From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
+	 * the mixer returns more outputs than are actually used.
+	 */
+	for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
+		if (msg.cmd[index] != 0) {
+			_max_number_of_nonzero_outputs = index + 1;
+			break;
+		}
+	}
+
+	msg.cmd.resize(_max_number_of_nonzero_outputs);
+
 	/*
 	 * Publish the command message to the bus
 	 * Note that for a quadrotor it takes one CAN frame

src/modules/uavcan/actuators/esc.hpp

@@ -107,6 +107,7 @@ private:
 	 * ESC states
 	 */
 	uint32_t 			_armed_mask = 0;
+	uint8_t				_max_number_of_nonzero_outputs = 0;
 
 	/*
 	 * Perf counters

src/modules/uavcan/uavcan_main.cpp

@@ -110,18 +110,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	}
 	/* _server_command_sem use case is a signal */
 	px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
-
-	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()
@@ -164,9 +152,6 @@ UavcanNode::~UavcanNode()
 
 	_instance = nullptr;
 
-	perf_free(_perfcnt_node_spin_elapsed);
-	perf_free(_perfcnt_esc_mixer_output_elapsed);
-	perf_free(_perfcnt_esc_mixer_total_elapsed);
 	pthread_mutex_destroy(&_node_mutex);
 	px4_sem_destroy(&_server_command_sem);
 
@@ -697,7 +682,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 void UavcanNode::node_spin_once()
 {
-	perf_begin(_perfcnt_node_spin_elapsed);
 	const int spin_res = _node.spinOnce();
 
 	if (spin_res < 0) {
@@ -708,8 +692,6 @@ void UavcanNode::node_spin_once()
 	if (_tx_injector != nullptr) {
 		_tx_injector->injectTxFramesInto(_node);
 	}
-
-	perf_end(_perfcnt_node_spin_elapsed);
 }
 
 /*
@@ -868,12 +850,8 @@ 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
@@ -965,9 +943,7 @@ 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);
 		}
 
 

src/modules/uavcan/uavcan_main.hpp

@@ -209,10 +209,6 @@ private:
 	// 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");
-
 	void handle_time_sync(const uavcan::TimerEvent &);
 
 	typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;