fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 02:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

improve end to end control latency measurement (#9388)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-02 03:03:32 -04:00
committed by GitHub
parent edea1b65cd
commit 977ab4e7b8
25 changed files with 132 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
-1
View File
@@ -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
-4
View File
@@ -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
-4
View File
@@ -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
+15 -2
View File
@@ -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
+2
View File
@@ -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
+4 -2
View File
@@ -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
+6 -6
View File
@@ -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
+6 -2
View File
@@ -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] =