improve end to end control latency measurement

src/drivers/imu/adis16448/adis16448.cpp

@@ -271,8 +271,6 @@ class ADIS16448 : public device::SPI
 
 	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 @@ class BMI055 : public device::SPI
 	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 @@ class BMI160 : public device::SPI
 	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 @@ class MPU6000 : public device::CDev
 	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 @@ class MPU9250 : public device::CDev
 	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,13 +384,26 @@ 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);
 
 			} else {
 				_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 @@ class PWMSim : public device::CDev, public ModuleBase<PWMSim>
 	};
 
 	PWMSim();
-	virtual ~PWMSim() = default;
+	virtual ~PWMSim();
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -132,6 +133,8 @@ class PWMSim : public device::CDev, public ModuleBase<PWMSim>
 
 	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 @@ class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
 	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),