From 1bee73af2237d72a64adbf0a4bedd8b20581b4bd Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 7 Jan 2015 18:19:47 +0100
Subject: Latency measurements: Estimate latency based on sensor timestamp
 through full system

---
 src/drivers/px4io/px4io.cpp                        | 7 +++++--
 src/modules/fw_att_control/fw_att_control_main.cpp | 2 ++
 src/modules/uORB/topics/actuator_controls.h        | 1 +
 3 files changed, 8 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ed9487cf9..72b2dc772 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -263,7 +263,8 @@ private:
 	perf_counter_t		_perf_update;		///<local performance counter for status updates
 	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
 	perf_counter_t		_perf_chan_count;	///<local performance counter for channel number changes
-	perf_counter_t		_perf_system_latency;	///< total system latency
+	perf_counter_t		_perf_system_latency;	///< total system latency (based on perf)
+	perf_counter_t		_perf_sample_latency;	///< total system latency (based on passed-through timestamp)
 
 	/* cached IO state */
 	uint16_t		_status;		///< Various IO status flags
@@ -496,6 +497,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
 	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
 	_perf_system_latency(perf_alloc_once(PC_ELAPSED, "sys_latency")),
+	_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
 	_status(0),
 	_alarms(0),
 	_t_actuator_controls_0(-1),
@@ -1088,7 +1090,6 @@ int
 PX4IO::io_set_control_groups()
 {
 	int ret = io_set_control_state(0);
-	perf_end(_perf_system_latency);
 
 	/* send auxiliary control groups */
 	(void)io_set_control_state(1);
@@ -1114,6 +1115,8 @@ PX4IO::io_set_control_state(unsigned group)
 
 			if (changed) {
 				orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
+				perf_end(_perf_system_latency);
+				perf_set(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample));
 			}
 		}
 		break;
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index d2e993ba9..6f225bb48 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -1060,7 +1060,9 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators.timestamp_sample = _att.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp_sample = _att.timestamp;
 
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 4d1f9a638..668f8f164 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -62,6 +62,7 @@
 
 struct actuator_controls_s {
 	uint64_t timestamp;
+	uint64_t timestamp_sample;			/**< the timestamp the data this control response is based on was sampled */
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
-- 
cgit v1.2.3