Revert "UAVCAN performance audit (#6829)" (#6846)

This reverts commit 21e04c9f7a.

src/modules/uavcan/actuators/esc.cpp

@@ -136,25 +136,6 @@ 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,7 +107,6 @@ 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,6 +110,18 @@ 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()
@@ -152,6 +164,9 @@ 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);
 
@@ -682,6 +697,7 @@ 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) {
@@ -692,6 +708,8 @@ void UavcanNode::node_spin_once()
 	if (_tx_injector != nullptr) {
 		_tx_injector->injectTxFramesInto(_node);
 	}
+
+	perf_end(_perfcnt_node_spin_elapsed);
 }
 
 /*
@@ -850,8 +868,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
@@ -943,7 +965,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);
 		}
 
 

src/modules/uavcan/uavcan_main.hpp

@@ -209,6 +209,10 @@ 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;