From 76821607130eab7916b70d789a189b2a52a115da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 17:15:42 +0100 Subject: mc attitude controller: Log the controller latency --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 82d2ff23a..cfb237711 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -141,6 +141,7 @@ private: struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _controller_latency_perf; math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ @@ -289,7 +290,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -890,6 +892,7 @@ MulticopterAttitudeControl::task_main() if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { 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); -- cgit v1.2.3 From 7d56ae4ed65f67cfe159a436eb58da59e9e0b0f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 Jan 2015 12:59:20 +0100 Subject: mc attitude control: Log sensor time stamp in actuator output --- src/modules/mc_att_control/mc_att_control_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index cfb237711..a094ed2c6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -888,6 +888,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _v_att.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { -- cgit v1.2.3