Skip to content

Commit c39664b

Browse files
amilcarlucasamilcarlucas
committed
esc_telemetry.cpp: changes requested by vooon.
1 parent c5cca22 commit c39664b

File tree

2 files changed

+98
-49
lines changed

2 files changed

+98
-49
lines changed

‎mavros_extras/src/plugins/esc_telemetry.cpp‎

Lines changed: 97 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,88 @@ 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.0f, 0.0f), (85.0f, 90.0f), ),
65+
// ("volt", (15.0f, 14.0f), (16.0f, 17.0f), ),
66+
// ("curr", (5.0f, 4.0f), (8.0f, 10.0f), ),
67+
// ("rpm", (3000, 2000), (9000, 12000), ),
68+
// ...
69+
// ]
70+
//
71+
// for field, min_, max_ = range fields:
72+
// for fm, lim = zip(("min", "max"), (min_, max_)):
73+
// for fn, l = zip(("ok", "nok"), lim):
74+
// cog.outl(f"""pnh.param("{field}_{fm}/{fn}", this->{field}_{fm}_{fn}, {l});""")
75+
// ]]]
76+
/**/
77+
pnh.param("temp_min/nok", this->temp_min_nok, 0.0f);
78+
pnh.param("temp_min/ok", this->temp_min_ok, 1.0f);
79+
pnh.param("volt_min/nok", this->volt_min_nok, 14.0f);
80+
pnh.param("volt_min/ok", this->volt_min_ok, 15.0f);
81+
pnh.param("curr_min/nok", this->curr_min_nok, 4.0f);
82+
pnh.param("curr_min/ok", this->curr_min_ok, 5.0f);
83+
pnh.param("rpm_min/nok", this->rpm_min_nok, 2000);
84+
pnh.param("rpm_min/ok", this->rpm_min_ok, 3000);
85+
pnh.param("temp_max/nok", this->temp_max_nok, 90.0f);
86+
pnh.param("temp_max/ok", this->temp_max_ok, 85.0f);
87+
pnh.param("volt_max/nok", this->volt_max_nok, 17.0f);
88+
pnh.param("volt_max/ok", this->volt_max_ok, 16.0f);
89+
pnh.param("curr_max/nok", this->curr_max_nok, 10.0f);
90+
pnh.param("curr_max/ok", this->curr_max_ok, 8.0f);
91+
pnh.param("rpm_max/nok", this->rpm_max_nok, 12000);
92+
pnh.param("rpm_max/ok", this->rpm_max_ok, 9000);
93+
/**/
94+
pnh.param("count/min_delta", this->count_min_delta, 10);
95+
}
96+
}
97+
};
98+
99+
class ESCDiag : public diagnostic_updater::DiagnosticTask
100+
{
101+
public:
102+
ESCDiag(const std::string& name, const Limits& lim_):
103+
diagnostic_updater::DiagnosticTask(name),
104+
lim(lim_)
105+
{}
106+
107+
const Limits& lim;
108+
};
109+
28110
/**
29111
* @brief ESC telemetry plugin
30112
*
@@ -47,27 +129,11 @@ class ESCTelemetryPlugin : public plugin::PluginBase
47129

48130
// Read the diagnostic variables
49131
auto pnh = ros::NodeHandle(ros::NodeHandle("~esc_telemetry"), "diagnostics");
50-
pnh.param("enabled", _diag_enabled, false);
51-
if (_diag_enabled)
132+
133+
lim = new Limits(pnh);
134+
if (lim->diag_enabled)
52135
{
53136
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);
71137
UAS_DIAG(m_uas).add("ESC temperature", this, &ESCTelemetryPlugin::run_temperature_diagnostics);
72138
UAS_DIAG(m_uas).add("ESC voltage", this, &ESCTelemetryPlugin::run_voltage_diagnostics);
73139
UAS_DIAG(m_uas).add("ESC current", this, &ESCTelemetryPlugin::run_current_diagnostics);
@@ -95,7 +161,8 @@ class ESCTelemetryPlugin : public plugin::PluginBase
95161
mavros_msgs::ESCTelemetry _esc_telemetry;
96162

97163
// vars used for diagnostics
98-
bool _diag_enabled;
164+
Limits* lim;
165+
std::vector<ESCDiag> v;
99166
static const std::size_t MAV_NR_ESCS = sizeof(mavros_msgs::ESCTelemetry)/sizeof(mavros_msgs::ESCTelemetryItem);
100167
bool _temp_min_errors[MAV_NR_ESCS];
101168
bool _volt_min_errors[MAV_NR_ESCS];
@@ -107,28 +174,6 @@ class ESCTelemetryPlugin : public plugin::PluginBase
107174
bool _rpm_max_errors[MAV_NR_ESCS];
108175
uint16_t _msg_count[MAV_NR_ESCS];
109176
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;
132177
int _consecutively_detected_delta_errors;
133178

134179
template <typename msgT>
@@ -203,6 +248,9 @@ class ESCTelemetryPlugin : public plugin::PluginBase
203248
_esc_count = MAV_NR_ESCS;
204249
}
205250
ROS_INFO("%d ESCs detected", _esc_count);
251+
for (uint i = 0; i < _esc_count; ++i) {
252+
v.emplace_back("ESC1"/*utils::format("ESC%u", i)*/, *lim);
253+
}
206254
}
207255

208256
void connection_cb(bool connected) override
@@ -216,6 +264,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
216264
else
217265
{
218266
_esc_count = 0;
267+
v.clear();
219268
}
220269
}
221270

@@ -328,27 +377,27 @@ class ESCTelemetryPlugin : public plugin::PluginBase
328377

329378
void run_temperature_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
330379
{
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);
380+
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);
332381
}
333382

334383
void run_voltage_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
335384
{
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);
385+
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);
337386
}
338387

339388
void run_current_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
340389
{
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);
390+
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);
342391
}
343392

344393
void run_rpm_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
345394
{
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);
395+
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);
347396
}
348397

349398
void run_count_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
350399
{
351-
stat.add("Min Count delta", _count_min_delta);
400+
stat.add("Min Count delta", lim->count_min_delta);
352401
std::string error_str;
353402
bool error = false;
354403
{
@@ -359,7 +408,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
359408
stat.add("Count reported by ESC " + std::to_string(i+1), t.count);
360409

361410
uint16_t count_delta = t.count - _msg_count[i];
362-
if (count_delta < uint16_t(_count_min_delta))
411+
if (count_delta < uint16_t(lim->count_min_delta))
363412
{
364413
error = true;
365414
error_str += (std::to_string(i+1) + " " + std::to_string(count_delta) + ", ");

‎mavros_msgs/msg/ESCTelemetry.msg‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,4 +7,4 @@
77

88
std_msgs/Header header
99

10-
mavros_msgs/ESCTelemetryItem[12] esc_telemetry
10+
mavros_msgs/ESCTelemetryItem[] esc_telemetry

0 commit comments

Comments
 (0)