Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_ESC_Telem.h"
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <GCS_MAVLink/GCS.h>
19 : #include <AP_Logger/AP_Logger.h>
20 :
21 : #if HAL_WITH_ESC_TELEM
22 :
23 : #include <AP_BoardConfig/AP_BoardConfig.h>
24 : #include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
25 :
26 : #include <AP_Math/AP_Math.h>
27 :
28 : //#define ESC_TELEM_DEBUG
29 :
30 : #define ESC_RPM_CHECK_TIMEOUT_US 210000UL // timeout for motor running validity
31 :
32 : extern const AP_HAL::HAL& hal;
33 :
34 : // table of user settable parameters
35 : const AP_Param::GroupInfo AP_ESC_Telem::var_info[] = {
36 :
37 : // @Param: _MAV_OFS
38 : // @DisplayName: ESC Telemetry mavlink offset
39 : // @Description: Offset to apply to ESC numbers when reporting as ESC_TELEMETRY packets over MAVLink. This allows high numbered motors to be displayed as low numbered ESCs for convenience on GCS displays. A value of 4 would send ESC on output 5 as ESC number 1 in ESC_TELEMETRY packets
40 : // @Increment: 1
41 : // @Range: 0 31
42 : // @User: Standard
43 : AP_GROUPINFO("_MAV_OFS", 1, AP_ESC_Telem, mavlink_offset, 0),
44 :
45 : AP_GROUPEND
46 : };
47 :
48 2098 : AP_ESC_Telem::AP_ESC_Telem()
49 : {
50 2098 : if (_singleton) {
51 0 : AP_HAL::panic("Too many AP_ESC_Telem instances");
52 : }
53 2098 : _singleton = this;
54 : #if !defined(IOMCU_FW)
55 2098 : AP_Param::setup_object_defaults(this, var_info);
56 : #endif
57 2098 : }
58 :
59 : // return the average motor RPM
60 77417 : float AP_ESC_Telem::get_average_motor_rpm(uint32_t servo_channel_mask) const
61 : {
62 77417 : float rpm_avg = 0.0f;
63 77417 : uint8_t valid_escs = 0;
64 :
65 : // average the rpm of each motor
66 2554761 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
67 2477344 : if (BIT_IS_SET(servo_channel_mask,i)) {
68 : float rpm;
69 2477344 : if (get_rpm(i, rpm)) {
70 360603 : rpm_avg += rpm;
71 360603 : valid_escs++;
72 : }
73 : }
74 : }
75 :
76 77417 : if (valid_escs > 0) {
77 75042 : rpm_avg /= valid_escs;
78 : }
79 :
80 77417 : return rpm_avg;
81 : }
82 :
83 : // return all the motor frequencies in Hz for dynamic filtering
84 151481 : uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const
85 : {
86 151481 : uint8_t valid_escs = 0;
87 :
88 : // average the rpm of each motor as reported by BLHeli and convert to Hz
89 4998873 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS && valid_escs < nfreqs; i++) {
90 : float rpm;
91 4847392 : if (get_rpm(i, rpm)) {
92 648536 : freqs[valid_escs++] = rpm * (1.0f / 60.0f);
93 4198856 : } else if (was_rpm_data_ever_reported(_rpm_data[i])) {
94 : // if we have ever received data on an ESC, mark it as valid but with no data
95 : // this prevents large frequency shifts when ESCs disappear
96 0 : freqs[valid_escs++] = 0.0f;
97 : }
98 : }
99 :
100 151481 : return MIN(valid_escs, nfreqs);
101 : }
102 :
103 : // get mask of ESCs that sent valid telemetry and/or rpm data in the last
104 : // ESC_TELEM_DATA_TIMEOUT_MS/ESC_RPM_DATA_TIMEOUT_US
105 0 : uint32_t AP_ESC_Telem::get_active_esc_mask() const {
106 0 : uint32_t ret = 0;
107 0 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
108 0 : if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
109 : // have never seen telem from this ESC
110 0 : continue;
111 : }
112 0 : if (_telem_data[i].stale() && !_rpm_data[i].data_valid) {
113 0 : continue;
114 : }
115 0 : ret |= (1U << i);
116 : }
117 0 : return ret;
118 : }
119 :
120 : // return an active ESC for the purposes of reporting (e.g. in the OSD)
121 0 : uint8_t AP_ESC_Telem::get_max_rpm_esc() const
122 : {
123 0 : uint32_t ret = 0;
124 0 : float max_rpm = 0;
125 0 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
126 0 : if (_telem_data[i].last_update_ms == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
127 : // have never seen telem from this ESC
128 0 : continue;
129 : }
130 0 : if (_telem_data[i].stale() && !_rpm_data[i].data_valid) {
131 0 : continue;
132 : }
133 0 : if (_rpm_data[i].rpm > max_rpm) {
134 0 : max_rpm = _rpm_data[i].rpm;
135 0 : ret = i;
136 : }
137 : }
138 0 : return ret;
139 : }
140 :
141 : // return number of active ESCs present
142 0 : uint8_t AP_ESC_Telem::get_num_active_escs() const {
143 0 : uint32_t active = get_active_esc_mask();
144 0 : return __builtin_popcount(active);
145 : }
146 :
147 : // return the whether all the motors in servo_channel_mask are running
148 3533 : bool AP_ESC_Telem::are_motors_running(uint32_t servo_channel_mask, float min_rpm, float max_rpm) const
149 : {
150 :
151 16037 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
152 15873 : if (BIT_IS_SET(servo_channel_mask, i)) {
153 4025 : const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[i];
154 : // we choose a relatively strict measure of health so that failsafe actions can rely on the results
155 4025 : if (!rpm_data_within_timeout(rpmdata, ESC_RPM_CHECK_TIMEOUT_US)) {
156 0 : return false;
157 : }
158 4025 : if (rpmdata.rpm < min_rpm) {
159 2156 : return false;
160 : }
161 1869 : if ((max_rpm > 0) && (rpmdata.rpm > max_rpm)) {
162 1213 : return false;
163 : }
164 : }
165 : }
166 164 : return true;
167 : }
168 :
169 : // is telemetry active for the provided channel mask
170 3533 : bool AP_ESC_Telem::is_telemetry_active(uint32_t servo_channel_mask) const
171 : {
172 116589 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
173 113056 : if (BIT_IS_SET(servo_channel_mask, i)) {
174 : // no data received
175 14132 : if (get_last_telem_data_ms(i) == 0 && !was_rpm_data_ever_reported(_rpm_data[i])) {
176 0 : return false;
177 : }
178 : }
179 : }
180 3533 : return true;
181 : }
182 :
183 : // get an individual ESC's slewed rpm if available, returns true on success
184 48520573 : bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const
185 : {
186 48520573 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
187 0 : return false;
188 : }
189 :
190 48520573 : const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
191 :
192 48520573 : if (is_zero(rpmdata.update_rate_hz)) {
193 7997273 : return false;
194 : }
195 :
196 40523300 : const uint32_t now = AP_HAL::micros();
197 40523301 : if (rpmdata.data_valid) {
198 40523271 : const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f));
199 40523271 : rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew);
200 :
201 : #if AP_SCRIPTING_ENABLED
202 40523271 : if ((1U<<esc_index) & rpm_scale_mask) {
203 0 : rpm *= rpm_scale_factor[esc_index];
204 : }
205 : #endif
206 :
207 40523271 : return true;
208 : }
209 30 : return false;
210 : }
211 :
212 : // get an individual ESC's raw rpm if available, returns true on success
213 36812358 : bool AP_ESC_Telem::get_raw_rpm_and_error_rate(uint8_t esc_index, float& rpm, float& error_rate) const
214 : {
215 36812358 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
216 0 : return false;
217 : }
218 :
219 36812358 : const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
220 :
221 36812358 : if (!rpmdata.data_valid) {
222 0 : return false;
223 : }
224 :
225 36812358 : rpm = rpmdata.rpm;
226 36812358 : error_rate = rpmdata.error_rate;
227 36812358 : return true;
228 : }
229 :
230 : // get an individual ESC's temperature in centi-degrees if available, returns true on success
231 1344 : bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const
232 : {
233 1344 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
234 0 : return false;
235 : }
236 :
237 1344 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
238 1344 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL)) {
239 1313 : return false;
240 : }
241 31 : temp = telemdata.temperature_cdeg;
242 31 : return true;
243 : }
244 :
245 : // get an individual motor's temperature in centi-degrees if available, returns true on success
246 0 : bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const
247 : {
248 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
249 0 : return false;
250 : }
251 :
252 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
253 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE | AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL)) {
254 0 : return false;
255 : }
256 0 : temp = telemdata.motor_temp_cdeg;
257 0 : return true;
258 : }
259 :
260 : // get the highest ESC temperature in centi-degrees if available, returns true if there is valid data for at least one ESC
261 42 : bool AP_ESC_Telem::get_highest_temperature(int16_t& temp) const
262 : {
263 42 : uint8_t valid_escs = 0;
264 :
265 1386 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
266 : int16_t temp_temp;
267 1344 : if (get_temperature(i, temp_temp)) {
268 31 : temp = MAX(temp, temp_temp);
269 31 : valid_escs++;
270 : }
271 : }
272 :
273 42 : return valid_escs > 0;
274 : }
275 :
276 : // get an individual ESC's current in Ampere if available, returns true on success
277 0 : bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const
278 : {
279 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
280 0 : return false;
281 : }
282 :
283 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
284 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CURRENT)) {
285 0 : return false;
286 : }
287 0 : amps = telemdata.current;
288 0 : return true;
289 : }
290 :
291 : // get an individual ESC's voltage in Volt if available, returns true on success
292 0 : bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const
293 : {
294 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
295 0 : return false;
296 : }
297 :
298 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
299 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) {
300 0 : return false;
301 : }
302 0 : volts = telemdata.voltage;
303 0 : return true;
304 : }
305 :
306 : // get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success
307 0 : bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const
308 : {
309 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
310 0 : return false;
311 : }
312 :
313 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
314 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) {
315 0 : return false;
316 : }
317 0 : consumption_mah = telemdata.consumption_mah;
318 0 : return true;
319 : }
320 :
321 : // get an individual ESC's usage time in seconds if available, returns true on success
322 0 : bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const
323 : {
324 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
325 0 : return false;
326 : }
327 :
328 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
329 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::USAGE)) {
330 0 : return false;
331 : }
332 0 : usage_s = telemdata.usage_s;
333 0 : return true;
334 : }
335 :
336 : #if AP_EXTENDED_ESC_TELEM_ENABLED
337 : // get an individual ESC's input duty cycle if available, returns true on success
338 0 : bool AP_ESC_Telem::get_input_duty(uint8_t esc_index, uint8_t& input_duty) const
339 : {
340 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
341 0 : return false;
342 : }
343 :
344 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
345 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY)) {
346 0 : return false;
347 : }
348 0 : input_duty = telemdata.input_duty;
349 0 : return true;
350 : }
351 :
352 : // get an individual ESC's output duty cycle if available, returns true on success
353 0 : bool AP_ESC_Telem::get_output_duty(uint8_t esc_index, uint8_t& output_duty) const
354 : {
355 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
356 0 : return false;
357 : }
358 :
359 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
360 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY)) {
361 0 : return false;
362 : }
363 0 : output_duty = telemdata.output_duty;
364 0 : return true;
365 : }
366 :
367 : // get an individual ESC's status flags if available, returns true on success
368 0 : bool AP_ESC_Telem::get_flags(uint8_t esc_index, uint32_t& flags) const
369 : {
370 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
371 0 : return false;
372 : }
373 :
374 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
375 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::FLAGS)) {
376 0 : return false;
377 : }
378 0 : flags = telemdata.flags;
379 0 : return true;
380 : }
381 :
382 : // get an individual ESC's percentage of output power if available, returns true on success
383 0 : bool AP_ESC_Telem::get_power_percentage(uint8_t esc_index, uint8_t& power_percentage) const
384 : {
385 0 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
386 0 : return false;
387 : }
388 :
389 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& telemdata = _telem_data[esc_index];
390 0 : if (!telemdata.valid(AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE)) {
391 0 : return false;
392 : }
393 0 : power_percentage = telemdata.power_percentage;
394 0 : return true;
395 : }
396 : #endif // AP_EXTENDED_ESC_TELEM_ENABLED
397 :
398 : // send ESC telemetry messages over MAVLink
399 1121055 : void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan)
400 : {
401 : #if HAL_GCS_ENABLED
402 1121055 : if (!_have_data) {
403 : // we've never had any data
404 256053 : return;
405 : }
406 :
407 : // loop through groups of 4 ESCs
408 865002 : const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1);
409 :
410 : // ensure we send out partially-full groups:
411 865002 : const uint8_t num_idx = (ESC_TELEM_MAX_ESCS + 3) / 4;
412 :
413 7785018 : for (uint8_t idx = 0; idx < num_idx; idx++) {
414 6920016 : const uint8_t i = (next_idx + idx) % num_idx;
415 :
416 : // return if no space in output buffer to send mavlink messages
417 6920016 : if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
418 : // not enough mavlink buffer space, start at this index next time
419 0 : next_idx = i;
420 0 : return;
421 : }
422 :
423 6920016 : bool all_stale = true;
424 31254604 : for (uint8_t j=0; j<4; j++) {
425 25430458 : const uint8_t esc_id = (i * 4 + j) + esc_offset;
426 50860916 : if (esc_id < ESC_TELEM_MAX_ESCS &&
427 25430458 : (!_telem_data[esc_id].stale() || _rpm_data[esc_id].data_valid)) {
428 1095870 : all_stale = false;
429 1095870 : break;
430 : }
431 : }
432 6920016 : if (all_stale) {
433 : // skip this group of ESCs if no data to send
434 5824146 : continue;
435 : }
436 :
437 :
438 : // arrays to hold output
439 1095870 : mavlink_esc_telemetry_1_to_4_t s {};
440 :
441 : // fill in output arrays
442 5479350 : for (uint8_t j = 0; j < 4; j++) {
443 4383480 : const uint8_t esc_id = (i * 4 + j) + esc_offset;
444 4383480 : if (esc_id >= ESC_TELEM_MAX_ESCS) {
445 0 : continue;
446 : }
447 4383480 : volatile AP_ESC_Telem_Backend::TelemetryData const &telemdata = _telem_data[esc_id];
448 :
449 4383480 : s.temperature[j] = telemdata.temperature_cdeg / 100;
450 4383480 : s.voltage[j] = constrain_float(telemdata.voltage * 100.0f, 0, UINT16_MAX);
451 4383480 : s.current[j] = constrain_float(telemdata.current * 100.0f, 0, UINT16_MAX);
452 4383480 : s.totalcurrent[j] = constrain_float(telemdata.consumption_mah, 0, UINT16_MAX);
453 : float rpmf;
454 4383480 : if (get_rpm(esc_id, rpmf)) {
455 : // rpm can be negative
456 2701774 : s.rpm[j] = constrain_float(fabsf(rpmf), 0, UINT16_MAX);
457 : }
458 4383480 : s.count[j] = telemdata.count;
459 : }
460 :
461 : // make sure a msg hasn't been extended
462 : static_assert(MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_5_TO_8_LEN &&
463 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_9_TO_12_LEN &&
464 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_13_TO_16_LEN &&
465 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_17_TO_20_LEN &&
466 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_21_TO_24_LEN &&
467 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_21_TO_24_LEN &&
468 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_25_TO_28_LEN &&
469 : MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4_LEN == MAVLINK_MSG_ID_ESC_TELEMETRY_29_TO_32_LEN,
470 : "telem messages not compatible");
471 :
472 1095870 : const mavlink_channel_t chan = (mavlink_channel_t)mav_chan;
473 : // send messages
474 1095870 : switch (i) {
475 864797 : case 0:
476 864797 : mavlink_msg_esc_telemetry_1_to_4_send_struct(chan, &s);
477 864797 : break;
478 226671 : case 1:
479 226671 : mavlink_msg_esc_telemetry_5_to_8_send_struct(chan, (const mavlink_esc_telemetry_5_to_8_t *)&s);
480 226671 : break;
481 3198 : case 2:
482 3198 : mavlink_msg_esc_telemetry_9_to_12_send_struct(chan, (const mavlink_esc_telemetry_9_to_12_t *)&s);
483 3198 : break;
484 1204 : case 3:
485 1204 : mavlink_msg_esc_telemetry_13_to_16_send_struct(chan, (const mavlink_esc_telemetry_13_to_16_t *)&s);
486 1204 : break;
487 : #if ESC_TELEM_MAX_ESCS > 16
488 0 : case 4:
489 0 : mavlink_msg_esc_telemetry_17_to_20_send_struct(chan, (const mavlink_esc_telemetry_17_to_20_t *)&s);
490 0 : break;
491 0 : case 5:
492 0 : mavlink_msg_esc_telemetry_21_to_24_send_struct(chan, (const mavlink_esc_telemetry_21_to_24_t *)&s);
493 0 : break;
494 0 : case 6:
495 0 : mavlink_msg_esc_telemetry_25_to_28_send_struct(chan, (const mavlink_esc_telemetry_25_to_28_t *)&s);
496 0 : break;
497 0 : case 7:
498 0 : mavlink_msg_esc_telemetry_29_to_32_send_struct(chan, (const mavlink_esc_telemetry_29_to_32_t *)&s);
499 0 : break;
500 : #endif
501 : }
502 : }
503 : // we checked for all sends without running out of buffer space,
504 : // start at zero next time
505 865002 : next_idx = 0;
506 :
507 : #endif // HAL_GCS_ENABLED
508 : }
509 :
510 : // record an update to the telemetry data together with timestamp
511 : // this should be called by backends when new telemetry values are available
512 140860545 : void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask)
513 : {
514 : // rpm and telemetry data are not protected by a semaphore even though updated from different threads
515 : // all data is per-ESC and only written from the update thread and read by the user thread
516 : // each element is a primitive type and the timestamp is only updated at the end, thus a caller
517 : // can only get slightly more up-to-date information that perhaps they were expecting or might
518 : // read data that has just gone stale - both of these are safe and avoid the overhead of locking
519 :
520 140860545 : if (esc_index >= ESC_TELEM_MAX_ESCS || data_mask == 0) {
521 0 : return;
522 : }
523 :
524 140860545 : _have_data = true;
525 140860545 : volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[esc_index];
526 :
527 : #if AP_TEMPERATURE_SENSOR_ENABLED
528 : // always allow external data. Block "internal" if external has ever its ever been set externally then ignore normal "internal" updates
529 281721090 : const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL) ||
530 140860545 : ((data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE_EXTERNAL));
531 :
532 281721090 : const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL) ||
533 140860545 : ((data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) && !(telemdata.types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE_EXTERNAL));
534 : #else
535 : const bool has_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
536 : const bool has_motor_temperature = (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE);
537 : #endif
538 :
539 140860545 : if (has_temperature) {
540 140860545 : telemdata.temperature_cdeg = new_data.temperature_cdeg;
541 : }
542 140860545 : if (has_motor_temperature) {
543 140672398 : telemdata.motor_temp_cdeg = new_data.motor_temp_cdeg;
544 : }
545 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) {
546 140860545 : telemdata.voltage = new_data.voltage;
547 : }
548 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) {
549 140860545 : telemdata.current = new_data.current;
550 : }
551 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) {
552 140860545 : telemdata.consumption_mah = new_data.consumption_mah;
553 : }
554 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) {
555 0 : telemdata.usage_s = new_data.usage_s;
556 : }
557 :
558 : #if AP_EXTENDED_ESC_TELEM_ENABLED
559 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY) {
560 140672398 : telemdata.input_duty = new_data.input_duty;
561 : }
562 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY) {
563 140672398 : telemdata.output_duty = new_data.output_duty;
564 : }
565 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::FLAGS) {
566 140672398 : telemdata.flags = new_data.flags;
567 : }
568 140860545 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE) {
569 140672398 : telemdata.power_percentage = new_data.power_percentage;
570 : }
571 : #endif //AP_EXTENDED_ESC_TELEM_ENABLED
572 :
573 : #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
574 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) {
575 : telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status);
576 : }
577 : if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STRESS) {
578 : telemdata.edt2_stress = merge_edt2_stress(telemdata.edt2_stress, new_data.edt2_stress);
579 : }
580 : #endif
581 :
582 140860545 : telemdata.count++;
583 140860545 : telemdata.types |= data_mask;
584 140860545 : telemdata.last_update_ms = AP_HAL::millis();
585 140860545 : telemdata.any_data_valid = true;
586 : }
587 :
588 : // record an update to the RPM together with timestamp, this allows the notch values to be slewed
589 : // this should be called by backends when new telemetry values are available
590 140860545 : void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, const float error_rate)
591 : {
592 140860545 : if (esc_index >= ESC_TELEM_MAX_ESCS) {
593 0 : return;
594 : }
595 :
596 140860545 : _have_data = true;
597 :
598 140860545 : const uint32_t now = MAX(1U ,AP_HAL::micros()); // don't allow a value of 0 in, as we use this as a flag in places
599 140860545 : volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index];
600 140860545 : const auto last_update_us = rpmdata.last_update_us;
601 :
602 140860545 : rpmdata.prev_rpm = rpmdata.rpm;
603 140860545 : rpmdata.rpm = new_rpm;
604 140860545 : rpmdata.update_rate_hz = 1.0e6f / constrain_uint32((now - last_update_us), 100, 1000000U*10U); // limit the update rate 0.1Hz to 10KHz
605 140860545 : rpmdata.last_update_us = now;
606 140860545 : rpmdata.error_rate = error_rate;
607 140860545 : rpmdata.data_valid = true;
608 :
609 : #ifdef ESC_TELEM_DEBUG
610 : hal.console->printf("RPM: rate=%.1fhz, rpm=%f)\n", rpmdata.update_rate_hz, new_rpm);
611 : #endif
612 : }
613 :
614 : #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
615 :
616 : // The following is based on https://github.com/bird-sanctuary/extended-dshot-telemetry.
617 : // For the following part we explain the bits of Extended DShot Telemetry v2 status telemetry:
618 : // - bits 0-3: the "stress level"
619 : // - bit 5: the "error" bit (e.g. the stall event in Bluejay)
620 : // - bit 6: the "warning" bit (e.g. the desync event in Bluejay)
621 : // - bit 7: the "alert" bit (e.g. the demag event in Bluejay)
622 :
623 : // Since logger can read out telemetry values less frequently than they are updated,
624 : // it makes sense to aggregate these status bits, and to collect the maximum observed stress level.
625 : // To reduce the logging rate of the EDT2 messages, we will try to log them only once a new frame comes.
626 : // To track this, we are going to (ab)use bit 15 of the field: 1 means there is something to write.
627 :
628 : // EDTv2 also features separate "stress" messages.
629 : // These come more frequently, and are scaled differently (the allowed range is from 0 to 255),
630 : // so we have to log them separately.
631 :
632 : constexpr uint16_t EDT2_TELEM_UPDATED = 0x8000U;
633 : constexpr uint16_t EDT2_STRESS_0F_MASK = 0xfU;
634 : constexpr uint16_t EDT2_STRESS_FF_MASK = 0xffU;
635 : constexpr uint16_t EDT2_ERROR_MASK = 0x20U;
636 : constexpr uint16_t EDT2_WARNING_MASK = 0x40U;
637 : constexpr uint16_t EDT2_ALERT_MASK = 0x80U;
638 : constexpr uint16_t EDT2_ALL_BITS = EDT2_ERROR_MASK | EDT2_WARNING_MASK | EDT2_ALERT_MASK;
639 :
640 : #define EDT2_HAS_NEW_DATA(status) bool((status) & EDT2_TELEM_UPDATED)
641 : #define EDT2_STRESS_FROM_STATUS(status) uint8_t((status) & EDT2_STRESS_0F_MASK)
642 : #define EDT2_ERROR_BIT_FROM_STATUS(status) bool((status) & EDT2_ERROR_MASK)
643 : #define EDT2_WARNING_BIT_FROM_STATUS(status) bool((status) & EDT2_WARNING_MASK)
644 : #define EDT2_ALERT_BIT_FROM_STATUS(status) bool((status) & EDT2_ALERT_MASK)
645 : #define EDT2_STRESS_FROM_STRESS(stress) uint8_t((stress) & EDT2_STRESS_FF_MASK)
646 :
647 : uint16_t AP_ESC_Telem::merge_edt2_status(uint16_t old_status, uint16_t new_status)
648 : {
649 : if (EDT2_HAS_NEW_DATA(old_status)) {
650 : new_status = uint16_t(
651 : (old_status & ~EDT2_STRESS_0F_MASK) | // old status except for the stress is preserved
652 : (new_status & EDT2_ALL_BITS) | // all new status bits are included
653 : MAX(old_status & EDT2_STRESS_0F_MASK, new_status & EDT2_STRESS_0F_MASK) // the stress is maxed out
654 : );
655 : }
656 : return EDT2_TELEM_UPDATED | new_status;
657 : }
658 :
659 : uint16_t AP_ESC_Telem::merge_edt2_stress(uint16_t old_stress, uint16_t new_stress)
660 : {
661 : if (EDT2_HAS_NEW_DATA(old_stress)) {
662 : new_stress = uint16_t(
663 : MAX(old_stress & EDT2_STRESS_FF_MASK, new_stress & EDT2_STRESS_FF_MASK) // the stress is maxed out
664 : );
665 : }
666 : return EDT2_TELEM_UPDATED | new_stress;
667 : }
668 :
669 : #endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
670 :
671 12756995 : void AP_ESC_Telem::update()
672 : {
673 : #if HAL_LOGGING_ENABLED
674 12756995 : AP_Logger *logger = AP_Logger::get_singleton();
675 12756995 : const uint64_t now_us64 = AP_HAL::micros64();
676 :
677 420980835 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
678 408223840 : const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i];
679 408223840 : volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i];
680 : // Push received telemetry data into the logging system
681 408223840 : if (logger && logger->logging_enabled()) {
682 780725696 : if (telemdata.last_update_ms != _last_telem_log_ms[i]
683 390362848 : || rpmdata.last_update_us != _last_rpm_log_us[i]) {
684 :
685 : // Update last log timestamps
686 36812358 : _last_telem_log_ms[i] = telemdata.last_update_ms;
687 36812358 : _last_rpm_log_us[i] = rpmdata.last_update_us;
688 :
689 36812358 : float rpm = AP_Logger::quiet_nanf();
690 36812358 : get_rpm(i, rpm);
691 36812358 : float raw_rpm = AP_Logger::quiet_nanf();
692 36812358 : float rpm_error_rate = AP_Logger::quiet_nanf();
693 36812358 : get_raw_rpm_and_error_rate(i, raw_rpm, rpm_error_rate);
694 :
695 : // Write ESC status messages
696 : // id starts from 0
697 : // rpm, raw_rpm is eRPM (in RPM units)
698 : // voltage is in Volt
699 : // current is in Ampere
700 : // esc_temp is in centi-degrees Celsius
701 : // current_tot is in milli-Ampere hours
702 : // motor_temp is in centi-degrees Celsius
703 : // error_rate is in percentage
704 36812358 : const struct log_Esc pkt{
705 : LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)),
706 : time_us : now_us64,
707 : instance : i,
708 : rpm : rpm,
709 : raw_rpm : raw_rpm,
710 36812358 : voltage : telemdata.voltage,
711 36812358 : current : telemdata.current,
712 36812358 : esc_temp : telemdata.temperature_cdeg,
713 36812358 : current_tot : telemdata.consumption_mah,
714 36812358 : motor_temp : telemdata.motor_temp_cdeg,
715 : error_rate : rpm_error_rate
716 36812358 : };
717 36812358 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
718 :
719 : #if AP_EXTENDED_ESC_TELEM_ENABLED
720 : // Write ESC extended status messages
721 : // id: starts from 0
722 : // input duty: duty cycle input to the ESC in percent
723 : // output duty: duty cycle output to the motor in percent
724 : // status flags: manufacurer-specific status flags
725 36812358 : const bool has_ext_data = telemdata.types &
726 : (AP_ESC_Telem_Backend::TelemetryType::INPUT_DUTY |
727 : AP_ESC_Telem_Backend::TelemetryType::OUTPUT_DUTY |
728 : AP_ESC_Telem_Backend::TelemetryType::FLAGS |
729 36812358 : AP_ESC_Telem_Backend::TelemetryType::POWER_PERCENTAGE);
730 36812358 : if (has_ext_data) {
731 : // @LoggerMessage: ESCX
732 : // @Description: ESC extended telemetry data
733 : // @Field: TimeUS: Time since system startup
734 : // @Field: Instance: starts from 0
735 : // @Field: inpct: input duty cycle in percent
736 : // @Field: outpct: output duty cycle in percent
737 : // @Field: flags: manufacturer-specific status flags
738 : // @Field: Pwr: Power percentage
739 73307074 : AP::logger().WriteStreaming("ESCX",
740 : "TimeUS,Instance,inpct,outpct,flags,Pwr",
741 : "s" "#" "%" "%" "-" "%",
742 : "F" "-" "-" "-" "-" "-",
743 : "Q" "B" "B" "B" "I" "B",
744 : AP_HAL::micros64(),
745 : i,
746 36653537 : telemdata.input_duty,
747 36653537 : telemdata.output_duty,
748 36653537 : telemdata.flags,
749 36653537 : telemdata.power_percentage);
750 : }
751 : #endif //AP_EXTENDED_ESC_TELEM_ENABLED
752 :
753 : #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
754 : // Write an EDTv2 message, if there is any update
755 : uint16_t edt2_status = telemdata.edt2_status;
756 : uint16_t edt2_stress = telemdata.edt2_stress;
757 : if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) {
758 : // Could probably be faster/smaller with bitmasking, but not sure
759 : uint8_t status = 0;
760 : if (EDT2_HAS_NEW_DATA(edt2_stress)) {
761 : status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA);
762 : }
763 : if (EDT2_HAS_NEW_DATA(edt2_status)) {
764 : status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA);
765 : }
766 : if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) {
767 : status |= uint8_t(log_Edt2_Status::ALERT_BIT);
768 : }
769 : if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) {
770 : status |= uint8_t(log_Edt2_Status::WARNING_BIT);
771 : }
772 : if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) {
773 : status |= uint8_t(log_Edt2_Status::ERROR_BIT);
774 : }
775 : // An EDT2 status message is:
776 : // id: starts from 0
777 : // stress: the current stress which comes from edt2_stress
778 : // max_stress: the maximum stress which comes from edt2_status
779 : // status: the status bits which come from both
780 : const struct log_Edt2 pkt_edt2{
781 : LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)),
782 : time_us : now_us64,
783 : instance : i,
784 : stress : EDT2_STRESS_FROM_STRESS(edt2_stress),
785 : max_stress : EDT2_STRESS_FROM_STATUS(edt2_status),
786 : status : status,
787 : };
788 : if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) {
789 : // Only clean the telem_updated bits if the write succeeded.
790 : // This is important because, if rate limiting is enabled,
791 : // the log-on-change behavior may lose a lot of entries
792 : telemdata.edt2_status &= ~EDT2_TELEM_UPDATED;
793 : telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED;
794 : }
795 : }
796 : #endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
797 : }
798 : }
799 : }
800 : #endif // HAL_LOGGING_ENABLED
801 :
802 420980835 : for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) {
803 : // copy the last_updated_us timestamp to avoid any race issues
804 408223840 : const uint32_t last_updated_us = _rpm_data[i].last_update_us;
805 408223840 : const uint32_t now_us = AP_HAL::micros();
806 : // Invalidate RPM data if not received for too long
807 408223840 : if (AP_HAL::timeout_expired(last_updated_us, now_us, ESC_RPM_DATA_TIMEOUT_US)) {
808 369339127 : _rpm_data[i].data_valid = false;
809 : }
810 408223840 : const uint32_t last_telem_data_ms = _telem_data[i].last_update_ms;
811 408223840 : const uint32_t now_ms = AP_HAL::millis();
812 : // Invalidate telemetry data if not received for too long
813 408223840 : if (AP_HAL::timeout_expired(last_telem_data_ms, now_ms, ESC_TELEM_DATA_TIMEOUT_MS)) {
814 355893590 : _telem_data[i].any_data_valid = false;
815 : }
816 : }
817 12756995 : }
818 :
819 : // NOTE: This function should only be used to check timeouts other than
820 : // ESC_RPM_DATA_TIMEOUT_US. Timeouts equal to ESC_RPM_DATA_TIMEOUT_US should
821 : // use RpmData::data_valid, which is cheaper and achieves the same result.
822 4025 : bool AP_ESC_Telem::rpm_data_within_timeout(const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t timeout_us)
823 : {
824 : // copy the last_update_us timestamp to avoid any race issues
825 4025 : const uint32_t last_update_us = instance.last_update_us;
826 4025 : const uint32_t now_us = AP_HAL::micros();
827 : // easy case, has the time window been crossed so it's invalid
828 4025 : if (AP_HAL::timeout_expired(last_update_us, now_us, timeout_us)) {
829 0 : return false;
830 : }
831 : // we never got a valid data, to it's invalid
832 4025 : if (last_update_us == 0) {
833 0 : return false;
834 : }
835 : // check if things generally expired on us, this is done to handle time wrapping
836 4025 : return instance.data_valid;
837 : }
838 :
839 4198856 : bool AP_ESC_Telem::was_rpm_data_ever_reported(const volatile AP_ESC_Telem_Backend::RpmData &instance)
840 : {
841 4198856 : return instance.last_update_us > 0;
842 : }
843 :
844 : #if AP_SCRIPTING_ENABLED
845 : /*
846 : set RPM scale factor from script
847 : */
848 0 : void AP_ESC_Telem::set_rpm_scale(const uint8_t esc_index, const float scale_factor)
849 : {
850 0 : if (esc_index < ESC_TELEM_MAX_ESCS) {
851 0 : rpm_scale_factor[esc_index] = scale_factor;
852 0 : rpm_scale_mask |= (1U<<esc_index);
853 : }
854 0 : }
855 : #endif
856 :
857 : AP_ESC_Telem *AP_ESC_Telem::_singleton = nullptr;
858 :
859 : /*
860 : * Get the AP_ESC_Telem singleton
861 : */
862 1357019 : AP_ESC_Telem *AP_ESC_Telem::get_singleton()
863 : {
864 1357019 : return AP_ESC_Telem::_singleton;
865 : }
866 :
867 : namespace AP {
868 :
869 1357019 : AP_ESC_Telem &esc_telem()
870 : {
871 1357019 : return *AP_ESC_Telem::get_singleton();
872 : }
873 :
874 : };
875 :
876 : #endif // HAL_WITH_ESC_TELEM
|
