From c5cca2277bc7df4700c2924cb8ec3cc2b50ba47b Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Wed, 16 Feb 2022 19:13:40 +0100 Subject: [PATCH] esc_telemetry.cpp: use private node handle to shorten paths --- mavros_extras/src/plugins/esc_telemetry.cpp | 37 +++++++++++---------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/mavros_extras/src/plugins/esc_telemetry.cpp b/mavros_extras/src/plugins/esc_telemetry.cpp index 8d2dca6e1..b297a9d5e 100644 --- a/mavros_extras/src/plugins/esc_telemetry.cpp +++ b/mavros_extras/src/plugins/esc_telemetry.cpp @@ -46,27 +46,28 @@ class ESCTelemetryPlugin : public plugin::PluginBase enable_connection_cb(); // Read the diagnostic variables - nh.param("esc_telemetry/diagnostics/enabled", _diag_enabled, false); + auto pnh = ros::NodeHandle(ros::NodeHandle("~esc_telemetry"), "diagnostics"); + pnh.param("enabled", _diag_enabled, false); if (_diag_enabled) { ROS_INFO_STREAM("Diagnostics are enabled!!!"); - nh.param("esc_telemetry/diagnostics/temp_min/nok", _temp_min_nok, 0.0f); - nh.param("esc_telemetry/diagnostics/temp_min/ok", _temp_min_ok, 1.0f); - nh.param("esc_telemetry/diagnostics/volt_min/nok", _volt_min_nok, 14.0f); - nh.param("esc_telemetry/diagnostics/volt_min/ok", _volt_min_ok, 15.0f); - nh.param("esc_telemetry/diagnostics/curr_min/nok", _curr_min_nok, 4.0f); - nh.param("esc_telemetry/diagnostics/curr_min/ok", _curr_min_ok, 5.0f); - nh.param("esc_telemetry/diagnostics/rpm_min/nok", _rpm_min_nok, 2000); - nh.param("esc_telemetry/diagnostics/rpm_min/ok", _rpm_min_ok, 3000); - nh.param("esc_telemetry/diagnostics/temp_max/nok", _temp_max_nok, 90.0f); - nh.param("esc_telemetry/diagnostics/temp_max/ok", _temp_max_ok, 85.0f); - nh.param("esc_telemetry/diagnostics/volt_max/nok", _volt_max_nok, 17.0f); - nh.param("esc_telemetry/diagnostics/volt_max/ok", _volt_max_ok, 16.0f); - nh.param("esc_telemetry/diagnostics/curr_max/nok", _curr_max_nok, 10.0f); - nh.param("esc_telemetry/diagnostics/curr_max/ok", _curr_max_ok, 8.0f); - nh.param("esc_telemetry/diagnostics/rpm_max/nok", _rpm_max_nok, 12000); - nh.param("esc_telemetry/diagnostics/rpm_max/ok", _rpm_max_ok, 9000); - nh.param("esc_telemetry/diagnostics/count/min_delta", _count_min_delta, 10); + pnh.param("temp_min/nok", _temp_min_nok, 0.0f); + pnh.param("temp_min/ok", _temp_min_ok, 1.0f); + pnh.param("volt_min/nok", _volt_min_nok, 14.0f); + pnh.param("volt_min/ok", _volt_min_ok, 15.0f); + pnh.param("curr_min/nok", _curr_min_nok, 4.0f); + pnh.param("curr_min/ok", _curr_min_ok, 5.0f); + pnh.param("rpm_min/nok", _rpm_min_nok, 2000); + pnh.param("rpm_min/ok", _rpm_min_ok, 3000); + pnh.param("temp_max/nok", _temp_max_nok, 90.0f); + pnh.param("temp_max/ok", _temp_max_ok, 85.0f); + pnh.param("volt_max/nok", _volt_max_nok, 17.0f); + pnh.param("volt_max/ok", _volt_max_ok, 16.0f); + pnh.param("curr_max/nok", _curr_max_nok, 10.0f); + pnh.param("curr_max/ok", _curr_max_ok, 8.0f); + pnh.param("rpm_max/nok", _rpm_max_nok, 12000); + pnh.param("rpm_max/ok", _rpm_max_ok, 9000); + pnh.param("count/min_delta", _count_min_delta, 10); UAS_DIAG(m_uas).add("ESC temperature", this, &ESCTelemetryPlugin::run_temperature_diagnostics); UAS_DIAG(m_uas).add("ESC voltage", this, &ESCTelemetryPlugin::run_voltage_diagnostics); UAS_DIAG(m_uas).add("ESC current", this, &ESCTelemetryPlugin::run_current_diagnostics);