Skip to content

Commit 7c45aef

Browse files
amilcarlucasamilcarlucas
committed
esc_telemetry.cpp: changes requested by vooon.
1 parent f9f7028 commit 7c45aef

File tree

1 file changed

+99
-48
lines changed

1 file changed

+99
-48
lines changed

‎mavros_extras/src/plugins/esc_telemetry.cpp‎

Lines changed: 99 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,90 @@ 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+
76+
// [[[end]]]
77+
/**/
78+
pnh.param("temp_min/nok", this->temp_min_nok, 0.0f);
79+
pnh.param("temp_min/ok", this->temp_min_ok, 1.0f);
80+
pnh.param("volt_min/nok", this->volt_min_nok, 14.0f);
81+
pnh.param("volt_min/ok", this->volt_min_ok, 15.0f);
82+
pnh.param("curr_min/nok", this->curr_min_nok, 4.0f);
83+
pnh.param("curr_min/ok", this->curr_min_ok, 5.0f);
84+
pnh.param("rpm_min/nok", this->rpm_min_nok, 2000);
85+
pnh.param("rpm_min/ok", this->rpm_min_ok, 3000);
86+
pnh.param("temp_max/nok", this->temp_max_nok, 90.0f);
87+
pnh.param("temp_max/ok", this->temp_max_ok, 85.0f);
88+
pnh.param("volt_max/nok", this->volt_max_nok, 17.0f);
89+
pnh.param("volt_max/ok", this->volt_max_ok, 16.0f);
90+
pnh.param("curr_max/nok", this->curr_max_nok, 10.0f);
91+
pnh.param("curr_max/ok", this->curr_max_ok, 8.0f);
92+
pnh.param("rpm_max/nok", this->rpm_max_nok, 12000);
93+
pnh.param("rpm_max/ok", this->rpm_max_ok, 9000);
94+
/**/
95+
96+
pnh.param("count/min_delta", this->count_min_delta, 10);
97+
}
98+
}
99+
};
100+
101+
class ESCDiag //: public diagnostic_updater::DiagnosticTask
102+
{
103+
public:
104+
ESCDiag(const std::string& name, const Limits& lim_):
105+
//diagnostic_updater::DiagnosticTask(name),
106+
lim(lim_)
107+
{}
108+
109+
const Limits& lim;
110+
};
111+
28112
/**
29113
* @brief ESC telemetry plugin
30114
*
@@ -47,27 +131,11 @@ class ESCTelemetryPlugin : public plugin::PluginBase
47131

48132
// Read the diagnostic variables
49133
auto pnh = ros::NodeHandle(ros::NodeHandle("~esc_telemetry"), "diagnostics");
50-
pnh.param("enabled", _diag_enabled, false);
51-
if (_diag_enabled)
134+
135+
lim = new Limits(pnh);
136+
if (lim->diag_enabled)
52137
{
53138
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);
71139
UAS_DIAG(m_uas).add("ESC temperature", this, &ESCTelemetryPlugin::run_temperature_diagnostics);
72140
UAS_DIAG(m_uas).add("ESC voltage", this, &ESCTelemetryPlugin::run_voltage_diagnostics);
73141
UAS_DIAG(m_uas).add("ESC current", this, &ESCTelemetryPlugin::run_current_diagnostics);
@@ -95,7 +163,8 @@ class ESCTelemetryPlugin : public plugin::PluginBase
95163
mavros_msgs::ESCTelemetry _esc_telemetry;
96164

97165
// vars used for diagnostics
98-
bool _diag_enabled;
166+
Limits* lim;
167+
std::vector<ESCDiag> v;
99168
static const std::size_t MAV_NR_ESCS = sizeof(mavros_msgs::ESCTelemetry)/sizeof(mavros_msgs::ESCTelemetryItem);
100169
bool _temp_min_errors[MAV_NR_ESCS];
101170
bool _volt_min_errors[MAV_NR_ESCS];
@@ -107,28 +176,6 @@ class ESCTelemetryPlugin : public plugin::PluginBase
107176
bool _rpm_max_errors[MAV_NR_ESCS];
108177
uint16_t _msg_count[MAV_NR_ESCS];
109178
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;
132179
int _consecutively_detected_delta_errors;
133180

134181
template <typename msgT>
@@ -203,6 +250,9 @@ class ESCTelemetryPlugin : public plugin::PluginBase
203250
_esc_count = MAV_NR_ESCS;
204251
}
205252
ROS_INFO("%d ESCs detected", _esc_count);
253+
for (uint i = 0; i < _esc_count; ++i) {
254+
v.emplace_back(utils::format("ESC%u", i), *lim);
255+
}
206256
}
207257

208258
void connection_cb(bool connected) override
@@ -216,6 +266,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
216266
else
217267
{
218268
_esc_count = 0;
269+
v.clear();
219270
}
220271
}
221272

@@ -328,27 +379,27 @@ class ESCTelemetryPlugin : public plugin::PluginBase
328379

329380
void run_temperature_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
330381
{
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);
382+
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);
332383
}
333384

334385
void run_voltage_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
335386
{
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);
387+
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);
337388
}
338389

339390
void run_current_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
340391
{
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);
392+
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);
342393
}
343394

344395
void run_rpm_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
345396
{
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);
397+
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);
347398
}
348399

349400
void run_count_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
350401
{
351-
stat.add("Min Count delta", _count_min_delta);
402+
stat.add("Min Count delta", lim->count_min_delta);
352403
std::string error_str;
353404
bool error = false;
354405
{
@@ -359,7 +410,7 @@ class ESCTelemetryPlugin : public plugin::PluginBase
359410
stat.add("Count reported by ESC " + std::to_string(i+1), t.count);
360411

361412
uint16_t count_delta = t.count - _msg_count[i];
362-
if (count_delta < uint16_t(_count_min_delta))
413+
if (count_delta < uint16_t(lim->count_min_delta))
363414
{
364415
error = true;
365416
error_str += (std::to_string(i+1) + " " + std::to_string(count_delta) + ", ");

0 commit comments

Comments
 (0)