Skip to content

Commit 014fca4

Browse files
amilcarlucasamilcarlucas
committed
esc_telemetry.cpp: changes requested by vooon.
1 parent 819760e commit 014fca4

File tree

1 file changed

+114
-48
lines changed

1 file changed

+114
-48
lines changed

‎mavros_extras/src/plugins/esc_telemetry.cpp‎

Lines changed: 114 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,105 @@ namespace mavros
2525
{
2626
namespace extra_plugins
2727
{
28+
// for all the dignostics parameters
29+
struct Limits {
30+
bool diag_enabled;
31+
// Temperature checks
32+
float temp_min_nok;
33+
float temp_min_ok;
34+
float temp_max_nok;
35+
float temp_max_ok;
36+
// Voltage checks
37+
float volt_min_nok;
38+
float volt_min_ok;
39+
float volt_max_nok;
40+
float volt_max_ok;
41+
// Current checks
42+
float curr_min_nok;
43+
float curr_min_ok;
44+
float curr_max_nok;
45+
float curr_max_ok;
46+
// RPM checks
47+
int rpm_min_nok;
48+
int rpm_min_ok;
49+
int rpm_max_nok;
50+
int rpm_max_ok;
51+
// Count checks, should be uint16_t but ROS only allows int parameters
52+
int count_min_delta;
53+
54+
explicit Limits(ros::NodeHandle &pnh) {
55+
if (!pnh.getParam("enabled", this->diag_enabled)) {
56+
ROS_FATAL_STREAM("Could not retrieve 'enabled' parameter from parameter server");
57+
this->diag_enabled = false;
58+
return;
59+
}
60+
if (this->diag_enabled)
61+
{
62+
// [[[cog:
63+
// fields = [
64+
// ("temp", (1.0, 0.0), (85.0, 90.0), ),
65+
// ("volt", (15.0, 14.0), (16.0, 17.0), ),
66+
// ("curr", (5.0, 4.0), (8.0, 10.0), ),
67+
// ("rpm", (3000, 2000), (9000, 12000), )
68+
// ]
69+
//
70+
// for field, min_, max_ in fields:
71+
// for fm, lim in zip(("min", "max"), (min_, max_)):
72+
// for fn, l in zip(("ok", "nok"), lim):
73+
// cog.outl(f"""pnh.param("{field}_{fm}/{fn}", this->{field}_{fm}_{fn}, {l});""")
74+
// ]]]
75+
pnh.param("temp_min/ok", this->temp_min_ok, 1.0);
76+
pnh.param("temp_min/nok", this->temp_min_nok, 0.0);
77+
pnh.param("temp_max/ok", this->temp_max_ok, 85.0);
78+
pnh.param("temp_max/nok", this->temp_max_nok, 90.0);
79+
pnh.param("volt_min/ok", this->volt_min_ok, 15.0);
80+
pnh.param("volt_min/nok", this->volt_min_nok, 14.0);
81+
pnh.param("volt_max/ok", this->volt_max_ok, 16.0);
82+
pnh.param("volt_max/nok", this->volt_max_nok, 17.0);
83+
pnh.param("curr_min/ok", this->curr_min_ok, 5.0);
84+
pnh.param("curr_min/nok", this->curr_min_nok, 4.0);
85+
pnh.param("curr_max/ok", this->curr_max_ok, 8.0);
86+
pnh.param("curr_max/nok", this->curr_max_nok, 10.0);
87+
pnh.param("rpm_min/ok", this->rpm_min_ok, 3000);
88+
pnh.param("rpm_min/nok", this->rpm_min_nok, 2000);
89+
pnh.param("rpm_max/ok", this->rpm_max_ok, 9000);
90+
pnh.param("rpm_max/nok", this->rpm_max_nok, 12000);
91+
// [[[end]]] (checksum: 0eefb906602662866799a8bbf948a1e6)
92+
/**
93+
pnh.param("temp_min/nok", this->temp_min_nok, 0.0f);
94+
pnh.param("temp_min/ok", this->temp_min_ok, 1.0f);
95+
pnh.param("volt_min/nok", this->volt_min_nok, 14.0f);
96+
pnh.param("volt_min/ok", this->volt_min_ok, 15.0f);
97+
pnh.param("curr_min/nok", this->curr_min_nok, 4.0f);
98+
pnh.param("curr_min/ok", this->curr_min_ok, 5.0f);
99+
pnh.param("rpm_min/nok", this->rpm_min_nok, 2000);
100+
pnh.param("rpm_min/ok", this->rpm_min_ok, 3000);
101+
pnh.param("temp_max/nok", this->temp_max_nok, 90.0f);
102+
pnh.param("temp_max/ok", this->temp_max_ok, 85.0f);
103+
pnh.param("volt_max/nok", this->volt_max_nok, 17.0f);
104+
pnh.param("volt_max/ok", this->volt_max_ok, 16.0f);
105+
pnh.param("curr_max/nok", this->curr_max_nok, 10.0f);
106+
pnh.param("curr_max/ok", this->curr_max_ok, 8.0f);
107+
pnh.param("rpm_max/nok", this->rpm_max_nok, 12000);
108+
pnh.param("rpm_max/ok", this->rpm_max_ok, 9000);
109+
/**/
110+
111+
pnh.param("count/min_delta", this->count_min_delta, 10);
112+
}
113+
}
114+
};
115+
116+
class ESCDiag //: public diagnostic_updater::DiagnosticTask
117+
{
118+
public:
119+
ESCDiag(const std::string& name, const Limits& lim_):
120+
//diagnostic_updater::DiagnosticTask(name),
121+
lim(lim_)
122+
{}
123+
124+
const Limits& lim;
125+
};
126+
28127
/**
29128
* @brief ESC telemetry plugin
30129
*
@@ -47,27 +146,11 @@ class ESCTelemetryPlugin : public plugin::PluginBase
47146

48147
// Read the diagnostic variables
49148
auto pnh = ros::NodeHandle(ros::NodeHandle("~esc_telemetry"), "diagnostics");
50-
pnh.param("enabled", _diag_enabled, false);
51-
if (_diag_enabled)
149+
150+
lim = new Limits(pnh);
151+
if (lim->diag_enabled)
52152
{
53153
ROS_INFO_STREAM("Diagnostics are enabled!!!");
54-
pnh.param("temp_min/nok", _temp_min_nok, 0.0f);
55-
pnh.param("temp_min/ok", _temp_min_ok, 1.0f);
56-
pnh.param("volt_min/nok", _volt_min_nok, 14.0f);
57-
pnh.param("volt_min/ok", _volt_min_ok, 15.0f);
58-
pnh.param("curr_min/nok", _curr_min_nok, 4.0f);
59-
pnh.param("curr_min/ok", _curr_min_ok, 5.0f);
60-
pnh.param("rpm_min/nok", _rpm_min_nok, 2000);
61-
pnh.param("rpm_min/ok", _rpm_min_ok, 3000);
62-
pnh.param("temp_max/nok", _temp_max_nok, 90.0f);
63-
pnh.param("temp_max/ok", _temp_max_ok, 85.0f);
64-
pnh.param("volt_max/nok", _volt_max_nok, 17.0f);
65-
pnh.param("volt_max/ok", _volt_max_ok, 16.0f);
66-
pnh.param("curr_max/nok", _curr_max_nok, 10.0f);
67-
pnh.param("curr_max/ok", _curr_max_ok, 8.0f);
68-
pnh.param("rpm_max/nok", _rpm_max_nok, 12000);
69-
pnh.param("rpm_max/ok", _rpm_max_ok, 9000);
70-
pnh.param("count/min_delta", _count_min_delta, 10);
71154
UAS_DIAG(m_uas).add("ESC temperature", this, &ESCTelemetryPlugin::run_temperature_diagnostics);
72155
UAS_DIAG(m_uas).add("ESC voltage", this, &ESCTelemetryPlugin::run_voltage_diagnostics);
73156
UAS_DIAG(m_uas).add("ESC current", this, &ESCTelemetryPlugin::run_current_diagnostics);
@@ -95,7 +178,8 @@ class ESCTelemetryPlugin : public plugin::PluginBase
95178
mavros_msgs::ESCTelemetry _esc_telemetry;
96179

97180
// vars used for diagnostics
98-
bool _diag_enabled;
181+
Limits* lim;
182+
std::vector<ESCDiag> v;
99183
static const std::size_t MAV_NR_ESCS = sizeof(mavros_msgs::ESCTelemetry)/sizeof(mavros_msgs::ESCTelemetryItem);
100184
bool _temp_min_errors[MAV_NR_ESCS];
101185
bool _volt_min_errors[MAV_NR_ESCS];
@@ -107,28 +191,6 @@ class ESCTelemetryPlugin : public plugin::PluginBase
107191
bool _rpm_max_errors[MAV_NR_ESCS];
108192
uint16_t _msg_count[MAV_NR_ESCS];
109193
uint8_t _esc_count = 0;
110-
// Temperature checks
111-
float _temp_min_nok;
112-
float _temp_min_ok;
113-
float _temp_max_nok;
114-
float _temp_max_ok;
115-
// Voltage checks
116-
float _volt_min_nok;
117-
float _volt_min_ok;
118-
float _volt_max_nok;
119-
float _volt_max_ok;
120-
// Current checks
121-
float _curr_min_nok;
122-
float _curr_min_ok;
123-
float _curr_max_nok;
124-
float _curr_max_ok;
125-
// RPM checks
126-
int _rpm_min_nok;
127-
int _rpm_min_ok;
128-
int _rpm_max_nok;
129-
int _rpm_max_ok;
130-
// Count checks, should be uint16_t but ROS only allows int parameters
131-
int _count_min_delta;
132194
int _consecutively_detected_delta_errors;
133195

134196
template <typename msgT>
@@ -203,6 +265,9 @@ class ESCTelemetryPlugin : public plugin::PluginBase
203265
_esc_count = MAV_NR_ESCS;
204266
}
205267
ROS_INFO("%d ESCs detected", _esc_count);
268+
for (uint i = 0; i < _esc_count; ++i) {
269+
v.emplace_back(utils::format("ESC%u", i), *lim);
270+
}
206271
}
207272

208273
void connection_cb(bool connected) override
@@ -216,6 +281,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
216281
else
217282
{
218283
_esc_count = 0;
284+
v.clear();
219285
}
220286
}
221287

@@ -328,27 +394,27 @@ class ESCTelemetryPlugin : public plugin::PluginBase
328394

329395
void run_temperature_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
330396
{
331-
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::temperature, _temp_min_errors, _temp_max_errors, "temperature", "(degC)", _temp_min_ok, _temp_min_nok, _temp_max_ok, _temp_max_nok);
397+
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::temperature, _temp_min_errors, _temp_max_errors, "temperature", "(degC)", lim->temp_min_ok, lim->temp_min_nok, lim->temp_max_ok, lim->temp_max_nok);
332398
}
333399

334400
void run_voltage_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
335401
{
336-
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::voltage, _volt_min_errors, _volt_max_errors, "voltage", "(V)", _volt_min_ok, _volt_min_nok, _volt_max_ok, _volt_max_nok);
402+
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::voltage, _volt_min_errors, _volt_max_errors, "voltage", "(V)", lim->volt_min_ok, lim->volt_min_nok, lim->volt_max_ok, lim->volt_max_nok);
337403
}
338404

339405
void run_current_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
340406
{
341-
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::current, _curr_min_errors, _curr_max_errors, "current", "(A)", _curr_min_ok, _curr_min_nok, _curr_max_ok, _curr_max_nok);
407+
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::current, _curr_min_errors, _curr_max_errors, "current", "(A)", lim->curr_min_ok, lim->curr_min_nok, lim->curr_max_ok, lim->curr_max_nok);
342408
}
343409

344410
void run_rpm_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
345411
{
346-
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::rpm, _rpm_min_errors, _rpm_max_errors, "RPM", "(1/min)", _rpm_min_ok, _rpm_min_nok, _rpm_max_ok, _rpm_max_nok);
412+
run_diagnostics(stat, &mavros_msgs::ESCTelemetryItem::rpm, _rpm_min_errors, _rpm_max_errors, "RPM", "(1/min)", lim->rpm_min_ok, lim->rpm_min_nok, lim->rpm_max_ok, lim->rpm_max_nok);
347413
}
348414

349415
void run_count_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
350416
{
351-
stat.add("Min Count delta", _count_min_delta);
417+
stat.add("Min Count delta", lim->count_min_delta);
352418
std::string error_str;
353419
bool error = false;
354420
{
@@ -359,7 +425,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
359425
stat.add("Count reported by ESC " + std::to_string(i+1), t.count);
360426

361427
uint16_t count_delta = t.count - _msg_count[i];
362-
if (count_delta < uint16_t(_count_min_delta))
428+
if (count_delta < uint16_t(lim->count_min_delta))
363429
{
364430
error = true;
365431
error_str += (std::to_string(i+1) + " " + std::to_string(count_delta) + ", ");

0 commit comments

Comments
 (0)