diff options
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 7 | ||||
| -rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 2 | ||||
| -rw-r--r-- | src/modules/uORB/topics/actuator_controls.h | 1 |
3 files changed, 8 insertions, 2 deletions
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]; }; |