improve end to end control latency measurement (#9388)

src/drivers/imu/adis16448/adis16448.cpp

@@ -271,8 +271,6 @@ private:
 
 	enum Rotation		_rotation;
 
-	perf_counter_t		_controller_latency_perf;
-
 #pragma pack(push, 1)
 	/**
 	 * Report conversation with in the ADIS16448, including command byte and interrupt status.
@@ -524,8 +522,7 @@ ADIS16448::ADIS16448(int bus, const char *path_accel, const char *path_gyro, con
 	_mag_filter_z(ADIS16448_MAG_DEFAULT_RATE, ADIS16448_MAG_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_int(1000000 / ADIS16448_ACCEL_MAX_OUTPUT_RATE, false),
 	_gyro_int(1000000 / ADIS16448_GYRO_MAX_OUTPUT_RATE, true),
-	_rotation(rotation),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
+	_rotation(rotation)
 {
 	// disable debug() calls
 	_debug_enabled = false;
@@ -1567,8 +1564,6 @@ ADIS16448::measure()
 	_mag->parent_poll_notify();
 
 	if (accel_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}

src/drivers/imu/bmi055/bmi055.hpp

@@ -269,7 +269,6 @@ protected:
 	perf_counter_t      _good_transfers;
 	perf_counter_t      _reset_retries;
 	perf_counter_t      _duplicates;
-	perf_counter_t      _controller_latency_perf;
 
 	uint8_t         _register_wait;
 	uint64_t        _reset_wait;

src/drivers/imu/bmi055/bmi055_accel.cpp

@@ -783,8 +783,6 @@ BMI055_accel::measure()
 	}
 
 	if (accel_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}

src/drivers/imu/bmi055/bmi055_gyro.cpp

@@ -753,8 +753,6 @@ BMI055_gyro::measure()
 	}
 
 	if (gyro_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
 	}

src/drivers/imu/bmi055/bmi055_main.cpp

@@ -442,7 +442,6 @@ BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device,
 	_good_transfers(perf_alloc(PC_COUNT, "bmi055_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "bmi055_reset_retries")),
 	_duplicates(perf_alloc(PC_COUNT, "bmi055_duplicates")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
 	_reset_wait(0),
 	_rotation(rotation),

src/drivers/imu/bmi160/bmi160.cpp

@@ -46,7 +46,6 @@ BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t
 	_good_transfers(perf_alloc(PC_COUNT, "bmi160_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")),
 	_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
 	_reset_wait(0),
 	_accel_filter_x(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -1246,8 +1245,6 @@ BMI160::measure()
 	}
 
 	if (accel_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}

src/drivers/imu/bmi160/bmi160.hpp

@@ -307,7 +307,6 @@ private:
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
 	perf_counter_t		_duplicates;
-	perf_counter_t		_controller_latency_perf;
 
 	uint8_t			_register_wait;
 	uint64_t		_reset_wait;

src/drivers/imu/mpu6000/mpu6000.cpp

@@ -208,7 +208,6 @@ private:
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
 	perf_counter_t		_duplicates;
-	perf_counter_t		_controller_latency_perf;
 
 	uint8_t			_register_wait;
 	uint64_t		_reset_wait;
@@ -510,7 +509,6 @@ MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char *
 	_good_transfers(perf_alloc(PC_COUNT, "mpu6k_good_trans")),
 	_reset_retries(perf_alloc(PC_COUNT, "mpu6k_reset")),
 	_duplicates(perf_alloc(PC_COUNT, "mpu6k_duplicates")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
 	_reset_wait(0),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -2076,8 +2074,6 @@ MPU6000::measure()
 	}
 
 	if (accel_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -148,7 +148,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
 	_good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")),
 	_reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset")),
 	_duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_register_wait(0),
 	_reset_wait(0),
 	_accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -1487,8 +1486,6 @@ MPU9250::measure()
 	}
 
 	if (accel_notify && !(_pub_blocked)) {
-		/* log the time of this report */
-		perf_begin(_controller_latency_perf);
 		/* publish it */
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}

src/drivers/imu/mpu9250/mpu9250.h

@@ -320,7 +320,6 @@ private:
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
 	perf_counter_t		_duplicates;
-	perf_counter_t		_controller_latency_perf;
 
 	uint8_t			_register_wait;
 	uint64_t		_reset_wait;

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -52,6 +52,7 @@
 #include <lib/mixer/mixer_load.h>
 #include <parameters/param.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
+#include <perf/perf_counter.h>
 
 #include "common.h"
 #include "navio_sysfs.h"
@@ -77,6 +78,8 @@ int     _armed_sub = -1;
 orb_advert_t    _outputs_pub = nullptr;
 orb_advert_t    _rc_pub = nullptr;
 
+perf_counter_t	_perf_control_latency = nullptr;
+
 // topic structures
 actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 orb_id_t 			_controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
@@ -212,6 +215,8 @@ void task_main(int argc, char *argv[])
 {
 	_is_running = true;
 
+	_perf_control_latency = perf_alloc(PC_ELAPSED, "linux_pwm_out control latency");
+
 	// Set up mixer
 	if (initialize_mixer(_mixer_filename) < 0) {
 		PX4_ERR("Mixer initialization failed.");
@@ -240,6 +245,7 @@ void task_main(int argc, char *argv[])
 	}
 
 	_mixer_group->groups_required(_groups_required);
+
 	// subscribe and set up polling
 	subscribe();
 
@@ -321,7 +327,6 @@ void task_main(int argc, char *argv[])
 		}
 
 		if (_mixer_group != nullptr) {
-			_outputs.timestamp = hrt_absolute_time();
 			/* do mixing */
 			_outputs.noutputs = _mixer_group->mix(_outputs.output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS);
 
@@ -379,6 +384,8 @@ void task_main(int argc, char *argv[])
 				pwm_out->send_output_pwm(pwm, _outputs.noutputs);
 			}
 
+			_outputs.timestamp = hrt_absolute_time();
+
 			if (_outputs_pub != nullptr) {
 				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
 
@@ -386,6 +393,17 @@ void task_main(int argc, char *argv[])
 				_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
 			}
 
+			// use first valid timestamp_sample for latency tracking
+			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				const bool required = _groups_required & (1 << i);
+				const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+				if (required && (timestamp_sample > 0)) {
+					perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
+					break;
+				}
+			}
+
 		} else {
 			PX4_ERR("Could not mix output! Exiting...");
 			_task_should_exit = true;
@@ -424,6 +442,8 @@ void task_main(int argc, char *argv[])
 		orb_unsubscribe(params_sub);
 	}
 
+	perf_free(_perf_control_latency);
+
 	_is_running = false;
 
 }

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -34,7 +34,8 @@
 #include "PWMSim.hpp"
 
 PWMSim::PWMSim() :
-	CDev("pwm_out_sim", PWM_OUTPUT0_DEVICE_PATH)
+	CDev("pwm_out_sim", PWM_OUTPUT0_DEVICE_PATH),
+	_perf_control_latency(perf_alloc(PC_ELAPSED, "pwm_out_sim control latency"))
 {
 	for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
 		_pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC;
@@ -56,6 +57,11 @@ PWMSim::PWMSim() :
 	set_mode(MODE_16PWM);
 }
 
+PWMSim::~PWMSim()
+{
+	perf_free(_perf_control_latency);
+}
+
 int
 PWMSim::set_mode(Mode mode)
 {
@@ -232,7 +238,6 @@ PWMSim::run()
 			/* do mixing */
 			unsigned num_outputs = _mixers->mix(&_actuator_outputs.output[0], _num_outputs);
 			_actuator_outputs.noutputs = num_outputs;
-			_actuator_outputs.timestamp = hrt_absolute_time();
 
 			/* disable unused ports by setting their output to NaN */
 			for (size_t i = 0; i < sizeof(_actuator_outputs.output) / sizeof(_actuator_outputs.output[0]); i++) {
@@ -284,7 +289,19 @@ PWMSim::run()
 			}
 
 			/* and publish for anyone that cares to see */
+			_actuator_outputs.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs);
+
+			// use first valid timestamp_sample for latency tracking
+			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				const bool required = _groups_required & (1 << i);
+				const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+				if (required && (timestamp_sample > 0)) {
+					perf_set_elapsed(_perf_control_latency, _actuator_outputs.timestamp - timestamp_sample);
+					break;
+				}
+			}
 		}
 
 		/* how about an arming update? */

src/drivers/pwm_out_sim/PWMSim.hpp

@@ -41,6 +41,7 @@
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_pwm_output.h>
 #include <lib/mixer/mixer.h>
+#include <perf/perf_counter.h>
 #include <px4_common.h>
 #include <px4_config.h>
 #include <px4_module.h>
@@ -67,7 +68,7 @@ public:
 	};
 
 	PWMSim();
-	virtual ~PWMSim() = default;
+	virtual ~PWMSim();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -132,6 +133,8 @@ private:
 
 	bool 	_airmode{false}; 	///< multicopter air-mode
 
+	perf_counter_t	_perf_control_latency;
+
 	static int	control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
 
 	void 	subscribe();

src/drivers/px4fmu/fmu.cpp

@@ -277,7 +277,7 @@ private:
 	float _thr_mdl_fac;	// thrust to pwm modelling factor
 	bool _airmode; 		// multicopter air-mode
 
-	perf_counter_t	_ctl_latency;
+	perf_counter_t	_perf_control_latency;
 
 	static bool	arm_nothrottle()
 	{
@@ -387,7 +387,7 @@ PX4FMU::PX4FMU(bool run_as_task) :
 	_mot_t_max(0.0f),
 	_thr_mdl_fac(0.0f),
 	_airmode(false),
-	_ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat"))
+	_perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency"))
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -465,7 +465,7 @@ PX4FMU::~PX4FMU()
 	/* clean up the alternate device node */
 	unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
 
-	perf_free(_ctl_latency);
+	perf_free(_perf_control_latency);
 }
 
 int
@@ -1238,8 +1238,6 @@ PX4FMU::cycle()
 			//			PX4_WARN("no PWM: failsafe");
 
 		} else {
-			perf_begin(_ctl_latency);
-
 			if (_mixers != nullptr) {
 				/* get controls for required topics */
 				unsigned poll_id = 0;
@@ -1358,13 +1356,21 @@ PX4FMU::cycle()
 					motor_limits.timestamp = hrt_absolute_time();
 					motor_limits.saturation_status = saturation_status.value;
 
-					orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance,
-							 ORB_PRIO_DEFAULT);
+					orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance, ORB_PRIO_DEFAULT);
 				}
 
 				_mixers->set_airmode(_airmode);
 
-				perf_end(_ctl_latency);
+				// use first valid timestamp_sample for latency tracking
+				for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+					const bool required = _groups_required & (1 << i);
+					const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+					if (required && (timestamp_sample > 0)) {
+						perf_set_elapsed(_perf_control_latency, actuator_outputs.timestamp - timestamp_sample);
+						break;
+					}
+				}
 			}
 		}
 

src/drivers/px4io/px4io.cpp

@@ -468,7 +468,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_mavlink_log_pub(nullptr),
 	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
-	_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
+	_perf_sample_latency(perf_alloc(PC_ELAPSED, "io control latency")),
 	_status(0),
 	_alarms(0),
 	_last_written_arming_s(0),

src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp

@@ -52,6 +52,7 @@
 #include <lib/mixer/mixer_load.h>
 #include <lib/mixer/mixer_multirotor_normalized.generated.h>
 #include <parameters/param.h>
+#include <perf/perf_counter.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
 #include <dev_fs_lib_pwm.h>
 
@@ -113,6 +114,7 @@ int32_t _pwm_max;
 
 MultirotorMixer *_mixer = nullptr;
 
+perf_counter_t	_perf_control_latency = nullptr;
 
 /*
  * forward declaration
@@ -338,8 +340,6 @@ void send_outputs_pwm(const uint16_t *pwm)
 	}
 }
 
-
-
 void task_main(int argc, char *argv[])
 {
 	if (pwm_initialize(_device) < 0) {
@@ -368,6 +368,8 @@ void task_main(int argc, char *argv[])
 	// set max min pwm
 	pwm_limit_init(&_pwm_limit);
 
+	_perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency");
+
 	_is_running = true;
 
 	// Main loop
@@ -421,8 +423,6 @@ void task_main(int argc, char *argv[])
 			continue;
 		}
 
-		_outputs.timestamp = hrt_absolute_time();
-
 		/* do  mixing for virtual control group */
 		_outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS);
 
@@ -453,6 +453,8 @@ void task_main(int argc, char *argv[])
 			send_outputs_pwm(pwm);
 		}
 
+		_outputs.timestamp = hrt_absolute_time();
+
 		if (_outputs_pub != nullptr) {
 			orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
 
@@ -460,6 +462,17 @@ void task_main(int argc, char *argv[])
 			_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
 		}
 
+		// use first valid timestamp_sample for latency tracking
+		for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			const bool required = _groups_required & (1 << i);
+			const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+			if (required && (timestamp_sample > 0)) {
+				perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
+				break;
+			}
+		}
+
 		/* check for parameter updates */
 		bool param_updated = false;
 		orb_check(params_sub, &param_updated);
@@ -481,8 +494,10 @@ void task_main(int argc, char *argv[])
 
 	orb_unsubscribe(_armed_sub);
 	orb_unsubscribe(params_sub);
-	_is_running = false;
 
+	perf_free(_perf_control_latency);
+
+	_is_running = false;
 }
 
 void task_main_trampoline(int argc, char *argv[])

src/drivers/tap_esc/tap_esc.cpp

@@ -44,6 +44,7 @@
 
 #include <lib/mathlib/mathlib.h>
 #include <drivers/device/device.h>
+#include <perf/perf_counter.h>
 #include <px4_module_params.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
@@ -118,6 +119,8 @@ private:
 	actuator_outputs_s      _outputs = {};
 	actuator_armed_s	_armed = {};
 
+	perf_counter_t	_perf_control_latency;
+
 	int			_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	actuator_controls_s 	_controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 	orb_id_t		_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
@@ -153,6 +156,7 @@ const uint8_t TAP_ESC::_device_dir_map[TAP_ESC_MAX_MOTOR_NUM] = ESC_DIR;
 TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count):
 	CDev("tap_esc", TAP_ESC_DEVICE_PATH),
 	ModuleParams(nullptr),
+	_perf_control_latency(perf_alloc(PC_ELAPSED, "tap_esc control latency")),
 	_channels_count(channels_count)
 {
 	strncpy(_device, device, sizeof(_device));
@@ -196,6 +200,8 @@ TAP_ESC::~TAP_ESC()
 	tap_esc_common::deinitialise_uart(_uart_fd);
 
 	DEVICE_LOG("stopping");
+
+	perf_free(_perf_control_latency);
 }
 
 /** @see ModuleBase */
@@ -447,7 +453,6 @@ void TAP_ESC::cycle()
 			/* do mixing */
 			num_outputs = _mixers->mix(&_outputs.output[0], num_outputs);
 			_outputs.noutputs = num_outputs;
-			_outputs.timestamp = hrt_absolute_time();
 
 			/* publish mixer status */
 			multirotor_motor_limits_s multirotor_motor_limits = {};
@@ -546,6 +551,8 @@ void TAP_ESC::cycle()
 			motor_out[i] = RPMSTOPPED;
 		}
 
+		_outputs.timestamp = hrt_absolute_time();
+
 		send_esc_outputs(motor_out, num_outputs);
 		tap_esc_common::read_data_from_uart(_uart_fd, &_uartbuf);
 
@@ -571,6 +578,17 @@ void TAP_ESC::cycle()
 		/* and publish for anyone that cares to see */
 		orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
 
+		// use first valid timestamp_sample for latency tracking
+		for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			const bool required = _groups_required & (1 << i);
+			const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+			if (required && (timestamp_sample > 0)) {
+				perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
+				break;
+			}
+		}
+
 	}
 
 	bool updated;

src/modules/mc_att_control/mc_att_control.hpp

@@ -164,7 +164,6 @@ private:
 	MultirotorMixer::saturation_status _saturation_status{};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
-	perf_counter_t	_controller_latency_perf;
 
 	math::LowPassFilter2p _lp_filters_d[3];                      /**< low-pass filters for D-term (roll, pitch & yaw) */
 	static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -100,8 +100,6 @@ To reduce control latency, the module directly polls on the gyro topic published
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	ModuleParams(nullptr),
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
-
 	_lp_filters_d{
 	{initial_update_rate_hz, 50.f},
 	{initial_update_rate_hz, 50.f},
@@ -757,7 +755,6 @@ MulticopterAttitudeControl::run()
 					if (_actuators_0_pub != nullptr) {
 
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-						perf_end(_controller_latency_perf);
 
 					} else if (_actuators_id) {
 						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
@@ -799,7 +796,6 @@ MulticopterAttitudeControl::run()
 						if (_actuators_0_pub != nullptr) {
 
 							orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-							perf_end(_controller_latency_perf);
 
 						} else if (_actuators_id) {
 							_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);

src/modules/simulator/gyrosim/gyrosim.cpp

@@ -168,7 +168,6 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_good_transfers;
 	perf_counter_t		_reset_retries;
-	perf_counter_t		_controller_latency_perf;
 
 	Integrator _accel_int;
 	Integrator _gyro_int;
@@ -323,7 +322,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
 	_sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")),
 	_good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")),
 	_reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")),
-	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
 	_accel_int(1000000 / GYROSIM_ACCEL_DEFAULT_RATE, true),
 	_gyro_int(1000000 / GYROSIM_GYRO_DEFAULT_RATE, true),
 	_rotation(rotation),
@@ -1093,8 +1091,6 @@ GYROSIM::_measure()
 
 	if (accel_notify) {
 		if (!(_pub_blocked)) {
-			/* log the time of this report */
-			perf_begin(_controller_latency_perf);
 			/* publish it */
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 		}

src/modules/uavcan/uavcan_main.cpp

@@ -86,9 +86,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_time_sync_master(_node),
 	_time_sync_slave(_node),
 	_node_status_monitor(_node),
+	_perf_control_latency(perf_alloc(PC_ELAPSED, "uavcan control latency")),
 	_master_timer(_node),
 	_setget_response(0)
-
 {
 	_task_should_exit = false;
 	_fw_server_action = None;
@@ -162,6 +162,8 @@ UavcanNode::~UavcanNode()
 	if (_mixers != nullptr) {
 		delete _mixers;
 	}
+
+	perf_free(_perf_control_latency);
 }
 
 int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
@@ -964,8 +966,19 @@ int UavcanNode::run()
 			}
 
 			// Output to the bus
-			_outputs.timestamp = hrt_absolute_time();
 			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+			_outputs.timestamp = hrt_absolute_time();
+
+			// use first valid timestamp_sample for latency tracking
+			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				const bool required = _groups_required & (1 << i);
+				const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
+
+				if (required && (timestamp_sample > 0)) {
+					perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
+					break;
+				}
+			}
 		}
 
 

src/modules/uavcan/uavcan_main.hpp

@@ -208,6 +208,8 @@ private:
 
 	actuator_outputs_s		_outputs = {};
 
+	perf_counter_t			_perf_control_latency;
+
 	bool 				_airmode = false;
 
 	// index into _poll_fds for each _control_subs handle

src/modules/vtol_att_control/standard.cpp

@@ -398,7 +398,8 @@ void Standard::update_fw_state()
 void Standard::fill_actuator_outputs()
 {
 	// multirotor controls
-	_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
+	_actuators_out_0->timestamp = hrt_absolute_time();
+	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
 
 	// roll
 	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
@@ -415,7 +416,8 @@ void Standard::fill_actuator_outputs()
 
 
 	// fixed wing controls
-	_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
+	_actuators_out_1->timestamp = hrt_absolute_time();
+	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
 	if (_vtol_schedule.flight_mode != MC_MODE) {
 		// roll

src/modules/vtol_att_control/tailsitter.cpp

@@ -271,9 +271,14 @@ void Tailsitter::update_fw_state()
 */
 void Tailsitter::fill_actuator_outputs()
 {
+	_actuators_out_0->timestamp = hrt_absolute_time();
+	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+
+	_actuators_out_1->timestamp = hrt_absolute_time();
+	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+
 	switch (_vtol_mode) {
 	case ROTARY_WING:
-		_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
 		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
 		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
 			_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
@@ -281,8 +286,6 @@ void Tailsitter::fill_actuator_outputs()
 		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
 			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 
-		_actuators_out_1->timestamp = _actuators_mc_in->timestamp;
-
 		if (_params->elevons_mc_lock) {
 			_actuators_out_1->control[0] = 0;
 			_actuators_out_1->control[1] = 0;
@@ -299,7 +302,6 @@ void Tailsitter::fill_actuator_outputs()
 
 	case FIXED_WING:
 		// in fixed wing mode we use engines only for providing thrust, no moments are generated
-		_actuators_out_0->timestamp = _actuators_fw_in->timestamp;
 		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
 		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
 		_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
@@ -319,8 +321,6 @@ void Tailsitter::fill_actuator_outputs()
 	case TRANSITION_TO_FW:
 	case TRANSITION_TO_MC:
 		// in transition engines are mixed by weight (BACK TRANSITION ONLY)
-		_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
-		_actuators_out_1->timestamp = _actuators_mc_in->timestamp;
 		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
 				* _mc_roll_weight;
 		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =

src/modules/vtol_att_control/tiltrotor.cpp

@@ -336,7 +336,9 @@ void Tiltrotor::waiting_on_tecs()
 */
 void Tiltrotor::fill_actuator_outputs()
 {
-	_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
+	_actuators_out_0->timestamp = hrt_absolute_time();
+	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
+
 	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
 			* _mc_roll_weight;
 	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
@@ -359,7 +361,9 @@ void Tiltrotor::fill_actuator_outputs()
 			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
 	}
 
-	_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
+	_actuators_out_1->timestamp = hrt_absolute_time();
+	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
+
 	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
 		-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
 	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =