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 : /* Initial protocol implementation was provided by FETtec */
17 : /* Strongly modified by Amilcar Lucas, IAV GmbH */
18 :
19 : #include <AP_Math/AP_Math.h>
20 : #include <AP_SerialManager/AP_SerialManager.h>
21 : #include <SRV_Channel/SRV_Channel.h>
22 : #include <GCS_MAVLink/GCS.h>
23 :
24 : #include "AP_FETtecOneWire.h"
25 : #if AP_FETTEC_ONEWIRE_ENABLED
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : // Set to 0 when no ESC hardware is available and you want to test the UART send function
30 : #ifndef HAL_AP_FETTEC_CONFIGURE_ESCS
31 : #define HAL_AP_FETTEC_CONFIGURE_ESCS 1
32 : #endif
33 :
34 : #if HAL_AP_FETTEC_HALF_DUPLEX
35 : static constexpr uint32_t HALF_DUPLEX_BAUDRATE = 2000000;
36 : #endif
37 : static constexpr uint32_t FULL_DUPLEX_BAUDRATE = 500000;
38 :
39 : const AP_Param::GroupInfo AP_FETtecOneWire::var_info[] {
40 :
41 : // @Param: MASK
42 : // @DisplayName: Servo channel output bitmask
43 : // @Description: Servo channel mask specifying FETtec ESC output.
44 : // @Bitmask: 0:SERVO1,1:SERVO2,2:SERVO3,3:SERVO4,4:SERVO5,5:SERVO6,6:SERVO7,7:SERVO8,8:SERVO9,9:SERVO10,10:SERVO11,11:SERVO12
45 : // @RebootRequired: True
46 : // @User: Standard
47 : AP_GROUPINFO_FLAGS("MASK", 1, AP_FETtecOneWire, _motor_mask_parameter, 0, AP_PARAM_FLAG_ENABLE),
48 :
49 : // @Param: RVMASK
50 : // @DisplayName: Servo channel reverse rotation bitmask
51 : // @Description: Servo channel mask to reverse rotation of FETtec ESC outputs.
52 : // @Bitmask: 0:SERVO1,1:SERVO2,2:SERVO3,3:SERVO4,4:SERVO5,5:SERVO6,6:SERVO7,7:SERVO8,8:SERVO9,9:SERVO10,10:SERVO11,11:SERVO12
53 : // @User: Standard
54 : AP_GROUPINFO("RVMASK", 2, AP_FETtecOneWire, _reverse_mask_parameter, 0),
55 :
56 : #if HAL_WITH_ESC_TELEM
57 : // @Param: POLES
58 : // @DisplayName: Nr. electrical poles
59 : // @Description: Number of motor electrical poles
60 : // @Range: 2 50
61 : // @User: Standard
62 : AP_GROUPINFO("POLES", 3, AP_FETtecOneWire, _pole_count_parameter, 14),
63 : #endif
64 :
65 : AP_GROUPEND
66 : };
67 :
68 : AP_FETtecOneWire *AP_FETtecOneWire::_singleton;
69 :
70 2094 : AP_FETtecOneWire::AP_FETtecOneWire()
71 : {
72 2094 : AP_Param::setup_object_defaults(this, var_info);
73 :
74 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
75 2094 : if (_singleton != nullptr) {
76 0 : AP_HAL::panic("AP_FETtecOneWire must be singleton");
77 : }
78 : #endif
79 2094 : _singleton = this;
80 2094 : }
81 :
82 : /**
83 : initialize the serial port
84 :
85 : */
86 41500776 : void AP_FETtecOneWire::init_uart()
87 : {
88 41500776 : if (_uart != nullptr) {
89 11385 : return;
90 : }
91 41489391 : const AP_SerialManager& serial_manager = AP::serialmanager();
92 41489391 : _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FETtecOneWire, 0);
93 41489391 : if (_uart == nullptr) {
94 41489374 : return; // no serial port available, so nothing to do here
95 : }
96 17 : _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
97 17 : _uart->set_unbuffered_writes(true);
98 :
99 17 : uint32_t uart_baud { FULL_DUPLEX_BAUDRATE };
100 : #if HAL_AP_FETTEC_HALF_DUPLEX
101 : if (_uart->get_options() & _uart->OPTION_HDPLEX) { //Half-Duplex is enabled
102 : _use_hdplex = true;
103 : uart_baud = HALF_DUPLEX_BAUDRATE;
104 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Half-Duplex");
105 : } else {
106 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Full-Duplex");
107 : }
108 : #endif
109 :
110 17 : _uart->begin(uart_baud);
111 : }
112 :
113 : /// initialize the device driver: configure serial port, wake-up and configure ESCs
114 41500776 : void AP_FETtecOneWire::init()
115 : {
116 41500776 : init_uart();
117 41500776 : if (_uart == nullptr) {
118 41489374 : return; // no serial port available, so nothing to do here
119 : }
120 :
121 11402 : _motor_mask = uint32_t(_motor_mask_parameter); // take a copy that will not change after we leave this function
122 11402 : _esc_count = __builtin_popcount(_motor_mask);
123 : #if HAL_WITH_ESC_TELEM
124 : // OneWire supports telemetry in at most 15 ESCs, because of the 4 bit limitation
125 : // on the fast-throttle command. But we are still limited to the
126 : // number of ESCs the telem library will collect data for.
127 11402 : const auto esc_count_limit = MIN(15, ESC_TELEM_MAX_ESCS);
128 : #else
129 : // OneWire supports at most 24 ESCs without telemetry
130 : const auto esc_count_limit = MIN(MAX_ESC_COUNT, NUM_SERVO_CHANNELS);
131 : #endif
132 11402 : if (_esc_count == 0 || _motor_mask >= (1U << esc_count_limit)) {
133 11385 : _invalid_mask = true;
134 11385 : return;
135 : }
136 :
137 : // we have a uart and the desired ESC combination id valid, allocate some memory:
138 78 : _escs = NEW_NOTHROW ESC[_esc_count];
139 17 : if (_escs == nullptr) {
140 0 : return;
141 : }
142 :
143 : // initialise ESC ids. This enforces that the FETtec ESC ids
144 : // inside FETtec ESCs need to be contiguous and start at ID 1
145 : // which is required by fast-throttle commands.
146 17 : uint8_t esc_offset = 0; // offset into our device-driver dynamically-allocated array of ESCs
147 17 : uint8_t esc_id = 1; // ESC ids inside FETtec protocol are one-indexed
148 17 : uint8_t servo_chan_offset = 0; // offset into _motor_mask_parameter array
149 145 : for (uint32_t mask = _motor_mask; mask != 0; mask >>= 1, servo_chan_offset++) {
150 128 : if (mask & 0x1) {
151 61 : _escs[esc_offset].servo_ofs = servo_chan_offset;
152 61 : _escs[esc_offset].id = esc_id++;
153 61 : esc_offset++;
154 : }
155 : }
156 17 : _invalid_mask = false; // mask is good
157 :
158 17 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FETtec: allocated %u motors", _esc_count);
159 :
160 : // We expect to be able to send a fast-throttle command in each loop.
161 : // 8 bits - OneWire Header
162 : // 4 bits - telemetry request
163 : // 11 bits - throttle value per ESC
164 : // 8 bits - frame CRC
165 17 : const uint16_t net_bit_count = 8 + 4 + (_esc_count * 11) + 8;
166 : // 7 dummy for rounding up the division by 8
167 17 : const uint16_t fast_throttle_byte_count = (net_bit_count + 7)/8;
168 17 : uint16_t telemetry_byte_count { 0U };
169 : #if HAL_WITH_ESC_TELEM
170 : // Telemetry is fetched from each loop in turn.
171 17 : telemetry_byte_count = sizeof(u.packed_tlm) + 1; // assume 9 pause bits between TX and RX
172 17 : _fast_throttle_byte_count = fast_throttle_byte_count;
173 : #endif
174 17 : uint32_t uart_baud { FULL_DUPLEX_BAUDRATE };
175 : #if HAL_AP_FETTEC_HALF_DUPLEX
176 : if (_use_hdplex == true) { //Half-Duplex is enabled
177 : uart_baud = HALF_DUPLEX_BAUDRATE;
178 : }
179 : #endif
180 17 : _min_fast_throttle_period_us = (fast_throttle_byte_count + telemetry_byte_count) * 9 * 1000000 / uart_baud + 300; // 300us extra reserve
181 :
182 : // tell SRV_Channels about ESC capabilities
183 : // this is a bit soonish because we have not seen the ESCs on the bus yet,
184 : // but saves us having to use a state variable to ensure doing this latter just once
185 17 : SRV_Channels::set_digital_outputs(_motor_mask, 0);
186 :
187 17 : _init_done = true;
188 : }
189 :
190 : /**
191 : transmits data to ESCs
192 : @param bytes bytes to transmit
193 : @param length number of bytes to transmit
194 : @return false there's no space in the UART for this message
195 : */
196 190625 : bool AP_FETtecOneWire::transmit(const uint8_t* bytes, const uint8_t length)
197 : {
198 190625 : const uint32_t now = AP_HAL::micros();
199 190625 : if (now - _last_transmit_us < _min_fast_throttle_period_us) {
200 : // in case the SRV_Channels::push() is running at very high rates, limit the period
201 : // this function gets executed because FETtec needs a time gap between frames
202 : // this also prevents one loop to do multiple actions, like reinitialize an ESC and sending a fast-throttle command without a gap.
203 0 : _period_too_short++;
204 0 : return false;
205 : }
206 190625 : _last_transmit_us = now;
207 190625 : if (length > _uart->txspace()) {
208 265 : return false;
209 : }
210 :
211 190360 : _uart->write(bytes, length);
212 : #if HAL_AP_FETTEC_HALF_DUPLEX
213 : if (_use_hdplex) {
214 : _ignore_own_bytes += length;
215 : }
216 : #endif
217 190360 : return true;
218 : }
219 :
220 : /**
221 : transmits a config request to ESCs
222 : @param bytes bytes to transmit
223 : @param length number of bytes to transmit
224 : @return false if vehicle is armed or if transmit(bytes, length) would return false
225 : */
226 1268 : bool AP_FETtecOneWire::transmit_config_request(const uint8_t* bytes, const uint8_t length)
227 : {
228 1268 : if (hal.util->get_soft_armed()) {
229 0 : return false;
230 : }
231 1268 : return transmit(bytes, length);
232 : }
233 :
234 : /// shifts data to start of buffer based on magic header bytes
235 188407 : void AP_FETtecOneWire::move_frame_source_in_receive_buffer(const uint8_t search_start_pos)
236 : {
237 : uint8_t i;
238 188407 : for (i=search_start_pos; i<_receive_buf_used; i++) {
239 : // FIXME: full-duplex should add MASTER here as we see our own data
240 188407 : if ((FrameSource)u.receive_buf[i] == FrameSource::BOOTLOADER ||
241 188342 : (FrameSource)u.receive_buf[i] == FrameSource::ESC) {
242 : break;
243 : }
244 : }
245 188407 : consume_bytes(i);
246 188407 : }
247 :
248 : /// cut n bytes from start of buffer
249 376814 : void AP_FETtecOneWire::consume_bytes(const uint8_t n)
250 : {
251 376814 : if (n == 0) {
252 188407 : return;
253 : }
254 : // assure the length of the memmove is positive
255 188407 : if (_receive_buf_used < n) {
256 0 : return;
257 : }
258 188407 : memmove(u.receive_buf, &u.receive_buf[n], _receive_buf_used-n);
259 188407 : _receive_buf_used = _receive_buf_used - n;
260 : }
261 :
262 : /// returns true if the first message in the buffer is OK
263 260 : bool AP_FETtecOneWire::buffer_contains_ok(const uint8_t length)
264 : {
265 260 : if (length != sizeof(u.packed_ok)) {
266 0 : _message_invalid_in_state_count++;
267 0 : return false;
268 : }
269 260 : if ((MsgType)u.packed_ok.msg.msgid != MsgType::OK) {
270 0 : return false;
271 : }
272 260 : return true;
273 : }
274 :
275 188407 : void AP_FETtecOneWire::handle_message(ESC &esc, const uint8_t length)
276 : {
277 : // only accept messages from the bootloader when we could
278 : // legitimately get a message from the bootloader. Swipes the OK
279 : // message for convenience
280 188407 : const FrameSource frame_source = (FrameSource)u.packed_ok.frame_source;
281 188407 : if (frame_source != FrameSource::ESC) {
282 65 : if (esc.state != ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE) {
283 0 : return;
284 : }
285 : }
286 :
287 188407 : switch (esc.state) {
288 0 : case ESCState::UNINITIALISED:
289 : case ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE:
290 0 : return;
291 65 : case ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE:
292 : // "OK" is the only valid response
293 65 : if (!buffer_contains_ok(length)) {
294 0 : return;
295 : }
296 : switch (frame_source) {
297 0 : case FrameSource::MASTER:
298 : // probably half-duplex; should be caught before we get here
299 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
300 0 : break;
301 65 : case FrameSource::BOOTLOADER:
302 65 : esc.set_state(ESCState::WANT_SEND_START_FW);
303 65 : esc.is_awake = true;
304 65 : break;
305 0 : case FrameSource::ESC:
306 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
307 : esc.set_state(ESCState::WANT_SEND_REQ_TYPE);
308 : #else
309 : #if HAL_WITH_ESC_TELEM
310 0 : esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
311 : #else
312 : esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
313 : #endif
314 : #endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
315 0 : esc.is_awake = true;
316 0 : break;
317 : }
318 65 : break;
319 :
320 0 : case ESCState::WANT_SEND_START_FW:
321 0 : return;
322 65 : case ESCState::WAITING_OK_FOR_START_FW:
323 65 : if (buffer_contains_ok(length)) {
324 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
325 : esc.set_state(ESCState::WANT_SEND_REQ_TYPE);
326 : #else
327 : #if HAL_WITH_ESC_TELEM
328 65 : esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
329 : #else
330 : esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
331 : #endif
332 : #endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
333 : }
334 65 : break;
335 :
336 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
337 : case ESCState::WANT_SEND_REQ_TYPE:
338 : return;
339 : case ESCState::WAITING_ESC_TYPE:
340 : if (length != sizeof(u.packed_esc_type)) {
341 : _message_invalid_in_state_count++;
342 : return;
343 : }
344 : esc.type = u.packed_esc_type.msg.type;
345 : esc.set_state(ESCState::WANT_SEND_REQ_SW_VER);
346 : break;
347 :
348 : case ESCState::WANT_SEND_REQ_SW_VER:
349 : return;
350 : case ESCState::WAITING_SW_VER:
351 : if (length != sizeof(u.packed_sw_ver)) {
352 : _message_invalid_in_state_count++;
353 : return;
354 : }
355 : esc.firmware_version = u.packed_sw_ver.msg.version;
356 : esc.firmware_subversion = u.packed_sw_ver.msg.subversion;
357 : esc.set_state(ESCState::WANT_SEND_REQ_SN);
358 : break;
359 :
360 : case ESCState::WANT_SEND_REQ_SN:
361 : return;
362 : case ESCState::WAITING_SN:
363 : if (length != sizeof(u.packed_sn)) {
364 : _message_invalid_in_state_count++;
365 : return;
366 : }
367 : static_assert(ARRAY_SIZE(u.packed_sn.msg.sn) == ARRAY_SIZE(esc.serial_number), "Serial number array length missmatch");
368 : memcpy(esc.serial_number, u.packed_sn.msg.sn, ARRAY_SIZE(u.packed_sn.msg.sn));
369 : #if HAL_WITH_ESC_TELEM
370 : esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
371 : #else
372 : esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
373 : #endif
374 : break;
375 : #endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
376 :
377 : #if HAL_WITH_ESC_TELEM
378 0 : case ESCState::WANT_SEND_SET_TLM_TYPE:
379 0 : return;
380 65 : case ESCState::WAITING_SET_TLM_TYPE_OK:
381 65 : if (buffer_contains_ok(length)) {
382 65 : esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
383 : }
384 65 : break;
385 : #endif
386 :
387 0 : case ESCState::WANT_SEND_SET_FAST_COM_LENGTH:
388 0 : return;
389 65 : case ESCState::WAITING_SET_FAST_COM_LENGTH_OK:
390 65 : if (buffer_contains_ok(length)) {
391 65 : esc.set_state(ESCState::RUNNING);
392 : }
393 65 : break;
394 188147 : case ESCState::RUNNING:
395 : // we only expect telemetry messages in this state
396 : #if HAL_WITH_ESC_TELEM
397 188147 : if (!esc.telem_expected) {
398 0 : esc.unexpected_telem++;
399 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
400 0 : AP_HAL::panic("unexpected telemetry");
401 : #endif
402 0 : return;
403 : }
404 188147 : esc.telem_expected = false;
405 188147 : return handle_message_telem(esc);
406 : #else
407 : return;
408 : #endif // HAL_WITH_ESC_TELEM
409 :
410 : }
411 : }
412 :
413 : #if HAL_WITH_ESC_TELEM
414 188147 : void AP_FETtecOneWire::handle_message_telem(ESC &esc)
415 : {
416 : // the following two methods are coming from AP_ESC_Telem:
417 188147 : const TLM &tlm = u.packed_tlm.msg;
418 :
419 : // update rpm and error rate
420 188147 : float error_rate_pct = 0;
421 188147 : if (_fast_throttle_cmd_count > _esc_count) {
422 188085 : error_rate_pct = (tlm.tx_err_count-esc.error_count_at_throttle_count_overflow)*(float)100/(float)_fast_throttle_cmd_count;
423 : } else {
424 : // the telemetry is requested in a round-robin, sequential fashion
425 : // so the in the first _esc_count times all ESCs get to initialize this
426 62 : esc.error_count_at_throttle_count_overflow = tlm.tx_err_count;
427 : }
428 376294 : update_rpm(esc.servo_ofs,
429 188147 : tlm.rpm*(100*2/_pole_count_parameter),
430 : error_rate_pct);
431 :
432 : // update power and temperature telem data
433 188147 : TelemetryData t {};
434 188147 : t.temperature_cdeg = tlm.temp * 100;
435 188147 : t.voltage = tlm.voltage * 0.01f;
436 188147 : t.current = tlm.current * 0.01f;
437 188147 : t.consumption_mah = tlm.consumption_mah;
438 188147 : update_telem_data(
439 188147 : esc.servo_ofs,
440 : t,
441 : TelemetryType::TEMPERATURE|
442 : TelemetryType::VOLTAGE|
443 : TelemetryType::CURRENT|
444 : TelemetryType::CONSUMPTION);
445 :
446 188147 : esc.last_telem_us = AP_HAL::micros();
447 188147 : }
448 : #endif // HAL_WITH_ESC_TELEM
449 :
450 : // reads data from the UART, calling handle_message on any message found
451 191388 : void AP_FETtecOneWire::read_data_from_uart()
452 : {
453 : /*
454 : a frame looks like:
455 : byte 1 = frame header (0x02 = bootloader, 0x03 = ESC firmware)
456 : byte 2 = sender ID (5bit)
457 : byte 3 & 4 = frame type (always 0x00, 0x00 used for bootloader. here just for compatibility)
458 : byte 5 = frame length over all bytes
459 : byte 6 - X = answer type, followed by the payload
460 : byte X+1 = 8bit CRC
461 : */
462 :
463 : #if HAL_AP_FETTEC_HALF_DUPLEX
464 : //ignore own bytes
465 : if (_use_hdplex) {
466 : while (_ignore_own_bytes > 0 && _uart->available()) {
467 : _ignore_own_bytes--;
468 : _uart->read();
469 : }
470 : }
471 : #endif
472 :
473 191388 : uint32_t bytes_to_read = MIN(_uart->available(), 128U);
474 191388 : uint32_t last_bytes_to_read = 0;
475 379795 : while (bytes_to_read &&
476 : bytes_to_read != last_bytes_to_read) {
477 188407 : last_bytes_to_read = bytes_to_read;
478 :
479 : // read as much from the uart as we can:
480 188407 : const uint8_t space = ARRAY_SIZE(u.receive_buf) - _receive_buf_used;
481 188407 : const uint32_t nbytes = _uart->read(&u.receive_buf[_receive_buf_used], space);
482 188407 : _receive_buf_used += nbytes;
483 188407 : bytes_to_read -= nbytes;
484 :
485 188407 : move_frame_source_in_receive_buffer();
486 :
487 : // borrow the "OK" message to retrieve the frame length from the buffer:
488 188407 : const uint8_t frame_length = u.packed_ok.frame_length;
489 188407 : if (_receive_buf_used < frame_length) {
490 0 : continue;
491 : }
492 :
493 188407 : if (crc8_dvb_update(0, u.receive_buf, frame_length-1) != u.receive_buf[frame_length-1]) {
494 : // bad message; shift away this frame_source byte to try to find
495 : // another message
496 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
497 0 : AP_HAL::panic("bad message");
498 : #endif
499 0 : crc_rec_err_cnt++;
500 0 : move_frame_source_in_receive_buffer(1);
501 0 : continue;
502 : }
503 :
504 : // borrow the "OK" message to retrieve the frame_source from the buffer:
505 188407 : const FrameSource frame_source = (FrameSource)u.packed_ok.frame_source;
506 188407 : if (frame_source == FrameSource::MASTER) {
507 : // this is our own message - we'd best be running in
508 : // half-duplex or we're in trouble!
509 0 : consume_bytes(frame_length);
510 0 : continue;
511 : }
512 :
513 : // borrow the "OK" message to retrieve the esc id from the buffer:
514 188407 : const uint8_t esc_id = u.packed_ok.esc_id;
515 188407 : bool handled = false;
516 : // FIXME: we could scribble down the last ESC we sent a
517 : // message to here and use it rather than doing this linear
518 : // search:
519 412356 : for (uint8_t i=0; i<_esc_count; i++) {
520 412356 : auto &esc = _escs[i];
521 412356 : if (esc.id != esc_id) {
522 223949 : continue;
523 : }
524 188407 : handle_message(esc, frame_length);
525 188407 : handled = true;
526 188407 : break;
527 : }
528 188407 : if (!handled) {
529 0 : _unknown_esc_message++;
530 : }
531 :
532 188407 : consume_bytes(frame_length);
533 : }
534 191388 : }
535 :
536 : /**
537 : packs a single fast-throttle command frame containing the throttle for all configured OneWire ESCs.
538 : @param motor_values a 16bit array containing the throttle values that should be sent to the motors. 0-2000 where 1001-2000 is positive rotation and 0-999 reversed rotation
539 : @param esc_id_to_request_telem_from the ESC to request telemetry from
540 : */
541 189357 : void AP_FETtecOneWire::pack_fast_throttle_command(const uint16_t *motor_values, uint8_t *fast_throttle_command, const uint8_t length, const uint8_t esc_id_to_request_telem_from)
542 : {
543 : // byte 1:
544 : // bit 0,1,2,3 = ESC ID, Bit 4 = MSB bit of first ESC (11bit) throttle value, bit 5,6,7 = frame header
545 : // so AAAABCCC
546 : // A = ID from the ESC telemetry is requested from. ESC ID == 0 means no request.
547 : // B = MSB from first throttle value
548 : // C = frame header
549 189357 : fast_throttle_command[0] = esc_id_to_request_telem_from << 4; // 0 here means no telemetry request
550 189357 : fast_throttle_command[0] |= ((motor_values[0] >> 10) & 0x01) << 3;
551 189357 : fast_throttle_command[0] |= (uint8_t)FrameSource::MASTER;
552 :
553 : // byte 2:
554 : // AAABBBBB
555 : // A = next 3 bits from (11bit) throttle value
556 : // B = 5bit target ID
557 189357 : fast_throttle_command[1] = (((motor_values[0] >> 7) & 0x07)) << 5;
558 189357 : fast_throttle_command[1] |= 0x1F; // All IDs
559 :
560 : // following bytes are the rest 7 bit of the first (11bit) throttle value,
561 : // and all bits from all other throttle values, followed by the CRC byte
562 189357 : uint8_t mot = 0;
563 189357 : uint8_t bits_remaining_in_this_pwm = 7;
564 1191499 : for (uint8_t out_byte_offset = 2; out_byte_offset<length; out_byte_offset++) {
565 1002142 : if (mot >= _esc_count) {
566 : // all motor bits consumed; remaining bytes are padding before CRC
567 50247 : fast_throttle_command[out_byte_offset] = 0;
568 50247 : continue;
569 : }
570 951895 : if (bits_remaining_in_this_pwm >= 8) {
571 : // const uint8_t mask = 0xFF << (11-bits_remaining_in_this_pwm);
572 311714 : fast_throttle_command[out_byte_offset] = (motor_values[mot] >> (bits_remaining_in_this_pwm-8)) & 0xFF;
573 311714 : bits_remaining_in_this_pwm -= 8;
574 : } else {
575 640181 : const uint8_t mask = (1U<<bits_remaining_in_this_pwm)-1;
576 640181 : const uint8_t bits_to_copy_from_second_pwm = 8-bits_remaining_in_this_pwm;
577 640181 : fast_throttle_command[out_byte_offset] = (motor_values[mot] & mask) << bits_to_copy_from_second_pwm;
578 : // move on to the next motor output
579 640181 : mot++;
580 640181 : if (mot < _esc_count) {
581 450824 : fast_throttle_command[out_byte_offset] |= motor_values[mot] >> (11-bits_to_copy_from_second_pwm);
582 : }
583 640181 : bits_remaining_in_this_pwm = 11 - bits_to_copy_from_second_pwm;
584 : }
585 : }
586 :
587 378714 : fast_throttle_command[length-1] =
588 189357 : crc8_dvb_update(0, fast_throttle_command, length-1);
589 189357 : }
590 :
591 189357 : void AP_FETtecOneWire::escs_set_values(const uint16_t* motor_values)
592 : {
593 189357 : uint8_t esc_id_to_request_telem_from = 0;
594 : #if HAL_WITH_ESC_TELEM
595 189357 : ESC &esc_to_req_telem_from = _escs[_esc_ofs_to_request_telem_from++];
596 189357 : if (_esc_ofs_to_request_telem_from >= _esc_count) {
597 68271 : _esc_ofs_to_request_telem_from = 0;
598 : }
599 189357 : esc_id_to_request_telem_from = esc_to_req_telem_from.id;
600 : #endif
601 :
602 189357 : uint8_t fast_throttle_command[_fast_throttle_byte_count];
603 189357 : pack_fast_throttle_command(motor_values, fast_throttle_command, sizeof(fast_throttle_command), esc_id_to_request_telem_from);
604 :
605 : #if HAL_AP_FETTEC_HALF_DUPLEX
606 : // last byte of signal can be used to make sure the first TLM byte is correct, in case of spike corruption
607 : // FIXME: use this somehow
608 : _last_crc = fast_throttle_command[_fast_throttle_byte_count - 1];
609 : #endif
610 :
611 : // No command was yet sent, so no reply is expected and all information
612 : // on the receive buffer is either garbage or noise. Discard it
613 189357 : _uart->discard_input();
614 189357 : _receive_buf_used = 0;
615 :
616 : // send throttle commands to all configured ESCs in a single packet transfer
617 189357 : if (transmit(fast_throttle_command, sizeof(fast_throttle_command))) {
618 : #if HAL_WITH_ESC_TELEM
619 189357 : esc_to_req_telem_from.telem_expected = true; // used to make sure that the returned telemetry comes from this ESC and not another
620 189357 : esc_to_req_telem_from.telem_requested = true; // used to check if this ESC is periodically sending telemetry
621 189357 : _fast_throttle_cmd_count++;
622 : #endif
623 : }
624 189357 : }
625 :
626 140495 : bool AP_FETtecOneWire::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
627 : {
628 140495 : if (_motor_mask_parameter == 0) {
629 140207 : return true; // No FETtec ESCs are expected, no need to run further pre-arm checks
630 : }
631 :
632 288 : if (_uart == nullptr) {
633 0 : hal.util->snprintf(failure_msg, failure_msg_len, "No uart");
634 0 : return false;
635 : }
636 288 : if (_invalid_mask) {
637 30 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid motor mask");
638 30 : return false;
639 : }
640 : #if HAL_WITH_ESC_TELEM
641 258 : if (_pole_count_parameter < 2) {
642 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Invalid pole count %u", uint8_t(_pole_count_parameter));
643 0 : return false;
644 : }
645 258 : uint8_t no_telem = 0;
646 258 : const uint32_t now = AP_HAL::micros();
647 : #endif
648 :
649 258 : uint8_t not_running = 0;
650 1003 : for (uint8_t i=0; i<_esc_count; i++) {
651 745 : auto &esc = _escs[i];
652 745 : if (esc.state != ESCState::RUNNING) {
653 18 : not_running++;
654 18 : continue;
655 : }
656 : #if HAL_WITH_ESC_TELEM
657 727 : if (now - esc.last_telem_us > max_telem_interval_us) {
658 26 : no_telem++;
659 : }
660 : #endif
661 : }
662 258 : if (not_running != 0) {
663 9 : hal.util->snprintf(failure_msg, failure_msg_len, "%u of %u ESCs are not running", not_running, _esc_count);
664 9 : return false;
665 : }
666 249 : if (!_init_done) {
667 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Not initialised");
668 0 : return false;
669 : }
670 : #if HAL_WITH_ESC_TELEM
671 249 : if (no_telem != 0) {
672 5 : hal.util->snprintf(failure_msg, failure_msg_len, "%u of %u ESCs are not sending telemetry", no_telem, _esc_count);
673 5 : return false;
674 : }
675 : #endif
676 :
677 244 : return true;
678 : }
679 :
680 2031 : void AP_FETtecOneWire::configure_escs()
681 : {
682 2031 : if (_uart->available()) {
683 : // don't attempt to configure if we've unread data
684 0 : return;
685 : }
686 :
687 : // note that we return as soon as we've transmitted anything in
688 : // case we're in one-wire mode
689 4278 : for (uint8_t i=0; i<_esc_count; i++) {
690 4257 : auto &esc = _escs[i];
691 4257 : switch (esc.state) {
692 58 : case ESCState::UNINITIALISED:
693 58 : esc.state = ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE;
694 : FALLTHROUGH;
695 1073 : case ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE:
696 : // probe for bootloader or running firmware
697 1073 : if (transmit_config_request(PackedMessage<OK>{esc.id, OK{}})) {
698 808 : esc.is_awake = false; // Assume the ESC is asleep or unavailable
699 808 : esc.set_state(ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE);
700 : }
701 1073 : return;
702 742 : case ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE:
703 742 : if (!esc.is_awake) {
704 742 : esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE); // go back to try to wake up the ESC
705 : }
706 742 : return;
707 65 : case ESCState::WANT_SEND_START_FW:
708 65 : if (transmit_config_request(PackedMessage<START_FW>{esc.id, START_FW{}})) {
709 65 : esc.set_state(ESCState::WAITING_OK_FOR_START_FW);
710 : }
711 65 : return;
712 0 : case ESCState::WAITING_OK_FOR_START_FW:
713 0 : return;
714 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
715 : case ESCState::WANT_SEND_REQ_TYPE:
716 : if (transmit_config_request(PackedMessage<REQ_TYPE>{esc.id, REQ_TYPE{}})) {
717 : esc.set_state(ESCState::WAITING_ESC_TYPE);
718 : }
719 : return;
720 : case ESCState::WAITING_ESC_TYPE:
721 : return;
722 : case ESCState::WANT_SEND_REQ_SW_VER:
723 : if (transmit_config_request(PackedMessage<REQ_SW_VER>{esc.id, REQ_SW_VER{}})) {
724 : esc.set_state(ESCState::WAITING_SW_VER);
725 : }
726 : return;
727 : case ESCState::WAITING_SW_VER:
728 : return;
729 : case ESCState::WANT_SEND_REQ_SN:
730 : if (transmit_config_request(PackedMessage<REQ_SN>{esc.id, REQ_SN{}})) {
731 : esc.set_state(ESCState::WAITING_SN);
732 : }
733 : return;
734 : case ESCState::WAITING_SN:
735 : return;
736 : #endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
737 : #if HAL_WITH_ESC_TELEM
738 65 : case ESCState::WANT_SEND_SET_TLM_TYPE:
739 65 : if (transmit_config_request(PackedMessage<SET_TLM_TYPE>{esc.id, SET_TLM_TYPE{1}})) {
740 65 : esc.set_state(ESCState::WAITING_SET_TLM_TYPE_OK);
741 : }
742 65 : return;
743 0 : case ESCState::WAITING_SET_TLM_TYPE_OK:
744 0 : return;
745 : #endif
746 65 : case ESCState::WANT_SEND_SET_FAST_COM_LENGTH:
747 65 : if (transmit_config_request(PackedMessage<SET_FAST_COM_LENGTH>{esc.id,
748 : SET_FAST_COM_LENGTH{
749 65 : _fast_throttle_byte_count,
750 65 : _escs[0].id,
751 65 : _esc_count
752 : }})) {
753 65 : esc.set_state(ESCState::WAITING_SET_FAST_COM_LENGTH_OK);
754 : }
755 65 : return;
756 0 : case ESCState::WAITING_SET_FAST_COM_LENGTH_OK:
757 0 : return;
758 2247 : case ESCState::RUNNING:
759 2247 : _running_mask |= (1U << esc.servo_ofs);
760 2247 : break;
761 : }
762 : }
763 : }
764 :
765 : /// periodically called from SRV_Channels::push()
766 41692164 : void AP_FETtecOneWire::update()
767 : {
768 41692164 : if (!_init_done) {
769 41500776 : init();
770 41502807 : return; // the rest of this function can only run after fully initted
771 : }
772 :
773 : // read all data from incoming serial:
774 191388 : read_data_from_uart();
775 :
776 191388 : const uint32_t now = AP_HAL::micros();
777 :
778 : #if HAL_WITH_ESC_TELEM
779 191388 : if (!hal.util->get_soft_armed()) {
780 :
781 106004 : _reverse_mask = _reverse_mask_parameter; // update this only when disarmed
782 :
783 : // if we haven't seen an ESC in a while, the user might
784 : // have power-cycled them. Try re-initialising.
785 412722 : for (uint8_t i=0; i<_esc_count; i++) {
786 306718 : auto &esc = _escs[i];
787 306718 : if (!esc.telem_requested || now - esc.last_telem_us < 1000000U ) {
788 : // telem OK
789 306706 : continue;
790 : }
791 12 : _running_mask &= ~(1U << esc.servo_ofs);
792 12 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "No telem from esc id=%u. Resetting it.", esc.id);
793 : //GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "unknown %u, invalid %u, too short %u, unexpected: %u, crc_err %u", _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, esc.unexpected_telem, crc_rec_err_cnt);
794 12 : esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE);
795 12 : esc.telem_requested = false;
796 : }
797 : }
798 : #endif // HAL_WITH_ESC_TELEM
799 :
800 191388 : if (now - _last_transmit_us < 700U) {
801 : // in case the SRV_Channels::push() is running at very high rates, limit the period
802 : // this function gets executed because FETtec needs a time gap between frames
803 0 : _period_too_short++;
804 0 : return;
805 : }
806 :
807 : #if defined(STM32F4)
808 : if (_uart->tx_pending()) {
809 : // there is unsent data in the send buffer,
810 : // do not send more data because FETtec needs a time gap between frames
811 : _period_too_short++;
812 : return;
813 : }
814 : #endif
815 :
816 : #if HAL_AP_FETTEC_CONFIGURE_ESCS
817 : // run ESC configuration state machines if needed
818 191388 : if (_running_mask != _motor_mask) {
819 2031 : configure_escs();
820 2031 : return; // do not send fast-throttle command if a configuration command just got sent
821 : }
822 : #endif
823 :
824 : uint16_t motor_pwm[MAX_ESC_COUNT];
825 829538 : for (uint8_t i = 0; i < _esc_count; i++) {
826 640181 : const ESC &esc = _escs[i];
827 640181 : const SRV_Channel* c = SRV_Channels::srv_channel(esc.servo_ofs);
828 : // check if safety switch has been pushed
829 640181 : if ( (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED)
830 640181 : || (c == nullptr)) { // this should never ever happen, but just in case ...
831 968 : motor_pwm[i] = 1000; // stop motor
832 968 : continue;
833 : }
834 639213 : motor_pwm[i] = constrain_int16(c->get_output_pwm(), 1000, 2000);
835 : fet_debug("esc=%u in: %u", esc.id, motor_pwm[i]);
836 639213 : if (_reverse_mask & (1U << i)) {
837 0 : motor_pwm[i] = 2000-motor_pwm[i];
838 : }
839 : }
840 :
841 : // send motor setpoints to ESCs, and request for telemetry data
842 189357 : escs_set_values(motor_pwm);
843 : }
844 :
845 : #if HAL_AP_FETTEC_ESC_BEEP
846 : /**
847 : makes all connected ESCs beep
848 : @param beep_frequency a 8 bit value from 0-255. higher make a higher beep
849 : */
850 : void AP_FETtecOneWire::beep(const uint8_t beep_frequency)
851 : {
852 : for (uint8_t i=0; i<_esc_count; i++) {
853 : auto &esc = _escs[i];
854 : if (esc.state != ESCState::RUNNING) {
855 : continue;
856 : }
857 : transmit_config_request(PackedMessage<Beep>{esc.id, Beep{beep_frequency}});
858 : }
859 : }
860 : #endif // HAL_AP_FETTEC_ESC_BEEP
861 :
862 : #if HAL_AP_FETTEC_ESC_LIGHT
863 : /**
864 : sets the racewire color for all ESCs
865 : r = red brightness
866 : g = green brightness
867 : b = blue brightness
868 : */
869 : void AP_FETtecOneWire::led_color(const uint8_t r, const uint8_t g, const uint8_t b)
870 : {
871 : for (uint8_t i=0; i<_esc_count; i++) {
872 : auto &esc = _escs[i];
873 : if (esc.state != ESCState::RUNNING) {
874 : continue;
875 : }
876 : transmit_config_request(PackedMessage<LEDColour>{esc.id, LEDColour{r, g, b}});
877 : }
878 : }
879 : #endif // HAL_AP_FETTEC_ESC_LIGHT
880 :
881 : #endif // AP_FETTEC_ONEWIRE_ENABLED
|
