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 : #pragma once
20 :
21 : #include <AP_HAL/AP_HAL.h>
22 :
23 : #ifndef AP_FETTEC_ONEWIRE_ENABLED
24 : #define AP_FETTEC_ONEWIRE_ENABLED HAL_PROGRAM_SIZE_LIMIT_KB > 1024
25 : #endif
26 :
27 : // Support both full-duplex at 500Kbit/s as well as half-duplex at 2Mbit/s (optional feature)
28 : #ifndef HAL_AP_FETTEC_HALF_DUPLEX
29 : #define HAL_AP_FETTEC_HALF_DUPLEX 0
30 : #endif
31 :
32 : // Get static info from the ESCs (optional feature)
33 : #ifndef HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
34 : #define HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO 0
35 : #endif
36 :
37 : // provide beep support (optional feature)
38 : #ifndef HAL_AP_FETTEC_ESC_BEEP
39 : #define HAL_AP_FETTEC_ESC_BEEP 0
40 : #endif
41 :
42 : // provide light support (optional feature)
43 : #ifndef HAL_AP_FETTEC_ESC_LIGHT
44 : #define HAL_AP_FETTEC_ESC_LIGHT 0
45 : #endif
46 :
47 : #if AP_FETTEC_ONEWIRE_ENABLED
48 :
49 : #define FTW_DEBUGGING 0
50 : #if FTW_DEBUGGING
51 : #include <stdio.h>
52 : #define fet_debug(fmt, args ...) do {::fprintf(stderr,"FETtec: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
53 : #else
54 : #define fet_debug(fmt, args ...)
55 : #endif
56 :
57 : #include <AP_ESC_Telem/AP_ESC_Telem.h>
58 : #include <AP_Param/AP_Param.h>
59 :
60 : #include <AP_Math/AP_Math.h>
61 : #include <AP_Math/crc.h>
62 :
63 : class AP_FETtecOneWire : public AP_ESC_Telem_Backend
64 : {
65 :
66 : public:
67 : AP_FETtecOneWire();
68 :
69 : /// Do not allow copies
70 : CLASS_NO_COPY(AP_FETtecOneWire);
71 :
72 : static const struct AP_Param::GroupInfo var_info[];
73 :
74 140495 : static AP_FETtecOneWire *get_singleton() {
75 140495 : return _singleton;
76 : }
77 :
78 : /// periodically called from SRV_Channels::push()
79 : void update();
80 :
81 : /// called from AP_Arming; should return false if arming should be
82 : /// disallowed
83 : bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const;
84 :
85 : #if HAL_AP_FETTEC_ESC_BEEP
86 : /**
87 : makes all connected ESCs beep
88 : @param beep_frequency a 8 bit value from 0-255. higher make a higher beep
89 : */
90 : void beep(const uint8_t beep_frequency);
91 : #endif
92 :
93 : #if HAL_AP_FETTEC_ESC_LIGHT
94 : /**
95 : sets the racewire color for all ESCs
96 : @param r red brightness
97 : @param g green brightness
98 : @param b blue brightness
99 : */
100 : void led_color(const uint8_t r, const uint8_t g, const uint8_t b);
101 : #endif
102 :
103 : private:
104 : static AP_FETtecOneWire *_singleton;
105 : AP_HAL::UARTDriver *_uart;
106 :
107 : AP_Int32 _motor_mask_parameter;
108 : AP_Int32 _reverse_mask_parameter;
109 : #if HAL_WITH_ESC_TELEM
110 : AP_Int8 _pole_count_parameter;
111 : #endif
112 :
113 : static constexpr uint8_t FRAME_OVERHEAD = 6; ///< OneWire message frame overhead (header+tail bytes)
114 : static constexpr uint8_t MAX_RECEIVE_LENGTH = 12; ///< OneWire max receive message payload length in bytes
115 : static constexpr uint8_t MAX_ESC_COUNT = 24; ///< Max ESCs supported (will be fewer with telemetry)
116 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
117 : static constexpr uint8_t SERIAL_NUMBER_LENGTH = 12; ///< ESC serial number length in bytes
118 : #endif
119 :
120 : /**
121 : initialize the device driver: configure serial port, wake-up and configure ESCs
122 : */
123 : void init();
124 :
125 : /**
126 : initialize the serial port
127 : */
128 : void init_uart();
129 :
130 : /**
131 : scan the OneWire bus, configure the ESCs requested in the _motor_mask_parameter
132 : */
133 : void configure_escs();
134 :
135 : // states configured ESCs can be in:
136 : enum class ESCState : uint8_t {
137 : UNINITIALISED = 5, // when we haven't tried to send anything to the ESC
138 :
139 : WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE = 10,
140 : WAITING_OK_FOR_RUNNING_SW_TYPE = 11,
141 :
142 : WANT_SEND_START_FW = 20,
143 : WAITING_OK_FOR_START_FW = 21,
144 :
145 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
146 : WANT_SEND_REQ_TYPE = 30,
147 : WAITING_ESC_TYPE = 31,
148 :
149 : WANT_SEND_REQ_SW_VER = 40,
150 : WAITING_SW_VER = 41,
151 :
152 : WANT_SEND_REQ_SN = 50,
153 : WAITING_SN = 51,
154 : #endif
155 :
156 : #if HAL_WITH_ESC_TELEM
157 : WANT_SEND_SET_TLM_TYPE = 60,
158 : WAITING_SET_TLM_TYPE_OK = 61,
159 : #endif
160 :
161 : WANT_SEND_SET_FAST_COM_LENGTH = 70,
162 : WAITING_SET_FAST_COM_LENGTH_OK = 71,
163 :
164 : RUNNING = 100,
165 : };
166 :
167 : class ESC {
168 : public:
169 :
170 : #if HAL_WITH_ESC_TELEM
171 : uint32_t last_telem_us; ///< last time we got telemetry from this ESC
172 : uint16_t unexpected_telem;
173 : uint16_t error_count_at_throttle_count_overflow; ///< overflow counter for error counter from the ESCs.
174 : bool telem_expected; ///< this ESC is fully configured and is now expected to send us telemetry
175 : bool telem_requested; ///< this ESC is fully configured and at some point was requested to send us telemetry
176 : #endif
177 :
178 : uint8_t id; ///< FETtec ESC ID
179 : uint8_t servo_ofs; ///< offset into ArduPilot servo array
180 : bool is_awake;
181 2017 : void set_state(ESCState _state) {
182 : fet_debug("Moving ESC.id=%u from state=%u to state=%u", (unsigned)id, (unsigned)state, (unsigned)_state);
183 2017 : state = _state;
184 2017 : };
185 : ESCState state = ESCState::UNINITIALISED;
186 :
187 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
188 : uint8_t serial_number[SERIAL_NUMBER_LENGTH];
189 : uint8_t firmware_version;
190 : uint8_t firmware_subversion;
191 : uint8_t type;
192 : #endif
193 : };
194 :
195 : uint32_t _min_fast_throttle_period_us; ///< minimum allowed fast-throttle command transmit period
196 : uint32_t _motor_mask; ///< an un-mutable copy of the _motor_mask_parameter taken before _init_done goes true
197 : uint32_t _reverse_mask; ///< a copy of the _reverse_mask_parameter taken while not armed
198 : uint32_t _running_mask; ///< a bitmask of the actively running ESCs
199 : uint32_t _last_transmit_us; ///< last time the transmit() function sent data
200 : ESC *_escs;
201 : uint8_t _esc_count; ///< number of allocated ESCs
202 : uint8_t _fast_throttle_byte_count; ///< pre-calculated number of bytes required to send an entire packed throttle message
203 :
204 : #if HAL_AP_FETTEC_HALF_DUPLEX
205 : uint8_t _ignore_own_bytes; ///< bytes to ignore while receiving, because we have transmitted them ourselves
206 : uint8_t _last_crc; ///< the CRC from the last sent fast-throttle command
207 : bool _use_hdplex; ///< use asynchronous half-duplex serial communication
208 : #endif
209 :
210 : bool _init_done; ///< device driver is initialized; ESCs may still need to be configured
211 : bool _invalid_mask; ///< true if the mask parameter is invalid
212 :
213 : enum class FrameSource : uint8_t {
214 : MASTER = 0x01, ///< master is always 0x01
215 : BOOTLOADER = 0x02,
216 : ESC = 0x03,
217 : };
218 :
219 : enum class MsgType : uint8_t
220 : {
221 : OK = 0,
222 : BL_PAGE_CORRECT = 1, ///< Bootloader only
223 : NOT_OK = 2,
224 : BL_START_FW = 3, ///< Bootloader only - exit the boot loader and start the standard firmware
225 : BL_PAGES_TO_FLASH = 4, ///< Bootloader only
226 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
227 : REQ_TYPE = 5, ///< ESC type
228 : REQ_SN = 6, ///< serial number
229 : REQ_SW_VER = 7, ///< software version
230 : #endif
231 : #if HAL_AP_FETTEC_ESC_BEEP
232 : BEEP = 13, ///< make noise
233 : #endif
234 : SET_FAST_COM_LENGTH = 26, ///< configure fast-throttle command
235 : SET_TLM_TYPE = 27, ///< telemetry operation mode
236 : #if HAL_AP_FETTEC_ESC_LIGHT
237 : SET_LED_TMP_COLOR = 51, ///< set ESC's LED color
238 : #endif
239 : };
240 :
241 : /**
242 : a frame looks like:
243 : byte 1 = frame header (master is always 0x01)
244 : byte 2 = target ID (5bit)
245 : byte 3 & 4 = frame type (always 0x00, 0x00 used for bootloader. here just for compatibility)
246 : byte 5 = frame length over all bytes
247 : byte 6 - X = request type, followed by the payload
248 : byte X+1 = 8bit CRC
249 : */
250 : template <typename T>
251 : class PACKED PackedMessage {
252 : public:
253 1268 : PackedMessage(uint8_t _esc_id, T _msg) :
254 1268 : esc_id(_esc_id),
255 1268 : msg(_msg)
256 : {
257 1268 : update_checksum();
258 1268 : }
259 : uint8_t frame_source { (uint8_t)FrameSource::MASTER };
260 : uint8_t esc_id;
261 : uint16_t frame_type { 0 }; // bootloader only, always zero
262 : uint8_t frame_length {sizeof(T) + FRAME_OVERHEAD}; // all bytes including frame_source and checksum
263 : T msg;
264 : uint8_t checksum;
265 :
266 1268 : void update_checksum() {
267 1268 : checksum = crc8_dvb_update(0, (const uint8_t*)this, frame_length-1);
268 1268 : }
269 : };
270 :
271 : class PACKED OK {
272 : public:
273 : uint8_t msgid { (uint8_t)MsgType::OK };
274 : };
275 :
276 : class PACKED START_FW {
277 : public:
278 : uint8_t msgid { (uint8_t)MsgType::BL_START_FW };
279 : };
280 :
281 : class PACKED SET_FAST_COM_LENGTH {
282 : public:
283 65 : SET_FAST_COM_LENGTH(uint8_t _byte_count, uint8_t _min_esc_id, uint8_t _esc_count) :
284 65 : byte_count{_byte_count},
285 65 : min_esc_id{_min_esc_id},
286 65 : esc_count{_esc_count}
287 65 : { }
288 : uint8_t msgid { (uint8_t)MsgType::SET_FAST_COM_LENGTH };
289 : uint8_t byte_count;
290 : uint8_t min_esc_id;
291 : uint8_t esc_count;
292 : };
293 :
294 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
295 : class PACKED REQ_TYPE {
296 : public:
297 : uint8_t msgid { (uint8_t)MsgType::REQ_TYPE };
298 : };
299 :
300 : class PACKED REQ_SW_VER {
301 : public:
302 : uint8_t msgid { (uint8_t)MsgType::REQ_SW_VER };
303 : };
304 :
305 : class PACKED REQ_SN {
306 : public:
307 : uint8_t msgid { (uint8_t)MsgType::REQ_SN };
308 : };
309 :
310 : class PACKED ESC_TYPE {
311 : public:
312 : ESC_TYPE(uint8_t _type) :
313 : type{_type} { }
314 : uint8_t type;
315 : };
316 :
317 : class PACKED SW_VER {
318 : public:
319 : SW_VER(uint8_t _version, uint8_t _subversion) :
320 : version{_version},
321 : subversion{_subversion}
322 : { }
323 : uint8_t version;
324 : uint8_t subversion;
325 : };
326 :
327 : class PACKED SN {
328 : public:
329 : SN(uint8_t *_sn, uint8_t snlen) {
330 : memcpy(sn, _sn, ARRAY_SIZE(sn));
331 : }
332 : uint8_t sn[SERIAL_NUMBER_LENGTH];
333 : };
334 :
335 : #endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
336 :
337 : void pack_fast_throttle_command(const uint16_t *motor_values, uint8_t *buffer, const uint8_t length, const uint8_t esc_id_to_request_telem_from);
338 :
339 : /*
340 : * Messages, methods and states for dealing with ESC telemetry
341 : */
342 : #if HAL_WITH_ESC_TELEM
343 : void handle_message_telem(ESC &esc);
344 :
345 : uint16_t _fast_throttle_cmd_count; ///< number of fast-throttle commands sent by the flight controller
346 :
347 : /// the ESC at this offset into _escs should be the next to send a
348 : /// telemetry request for:
349 : uint8_t _esc_ofs_to_request_telem_from;
350 :
351 : class PACKED SET_TLM_TYPE {
352 : public:
353 65 : SET_TLM_TYPE(uint8_t _tlm_type) :
354 65 : tlm_type{_tlm_type}
355 65 : { }
356 : uint8_t msgid { (uint8_t)MsgType::SET_TLM_TYPE };
357 : uint8_t tlm_type;
358 : };
359 :
360 : class PACKED TLM {
361 : public:
362 : TLM(int8_t _temp, uint16_t _voltage, uint16_t _current, int16_t _rpm, uint16_t _consumption_mah, uint16_t _tx_err_count) :
363 : temp{_temp},
364 : voltage{_voltage},
365 : current{_current},
366 : rpm{_rpm},
367 : consumption_mah{_consumption_mah},
368 : tx_err_count{_tx_err_count}
369 : { }
370 : int8_t temp; // centi-degrees
371 : uint16_t voltage; // centi-Volt
372 : uint16_t current; // centi-Ampere (signed?)
373 : int16_t rpm; // centi-rpm
374 : uint16_t consumption_mah; // milli-Ampere.hour
375 : uint16_t tx_err_count; // CRC error count, as perceived from the ESC receiving side
376 : };
377 :
378 : #endif // HAL_WITH_ESC_TELEM
379 :
380 : #if HAL_AP_FETTEC_ESC_BEEP
381 : class PACKED Beep {
382 : public:
383 : Beep(uint8_t _beep_frequency) :
384 : beep_frequency{_beep_frequency}
385 : { }
386 : uint8_t msgid { (uint8_t)MsgType::BEEP };
387 : uint8_t beep_frequency;
388 : // add two zeros to make sure all ESCs can catch their command as we don't wait for a response here (don't blame me --pb)
389 : uint16_t spacer = 0;
390 : };
391 : #endif // HAL_AP_FETTEC_ESC_BEEP
392 :
393 : #if HAL_AP_FETTEC_ESC_LIGHT
394 : class PACKED LEDColour {
395 : public:
396 : LEDColour(uint8_t _r, uint8_t _g, uint8_t _b) :
397 : r{_r},
398 : g{_g},
399 : b{_b}
400 : { }
401 : uint8_t msgid { (uint8_t)MsgType::SET_LED_TMP_COLOR };
402 : uint8_t r;
403 : uint8_t g;
404 : uint8_t b;
405 : // add two zeros to make sure all ESCs can catch their command as we don't wait for a response here (don't blame me --pb)
406 : uint16_t spacer = 0;
407 : };
408 : #endif // HAL_AP_FETTEC_ESC_LIGHT
409 :
410 : /*
411 : * Methods and data for transmitting data to the ESCSs:
412 : */
413 :
414 : /**
415 : transmits data to ESCs
416 : @param bytes bytes to transmit
417 : @param length number of bytes to transmit
418 : @return false there's no space in the UART for this message
419 : */
420 : bool transmit(const uint8_t* bytes, const uint8_t length);
421 :
422 : template <typename T>
423 : bool transmit(const PackedMessage<T> &msg) {
424 : return transmit((const uint8_t*)&msg, sizeof(msg));
425 : }
426 :
427 : /**
428 : transmits configuration request data to ESCs
429 : @param bytes bytes to transmit
430 : @param length number of bytes to transmit
431 : @return false if vehicle armed or there's no space in the UART for this message
432 : */
433 : bool transmit_config_request(const uint8_t* bytes, const uint8_t length);
434 :
435 : template <typename T>
436 1268 : bool transmit_config_request(const PackedMessage<T> &msg) {
437 1268 : return transmit_config_request((const uint8_t*)&msg, sizeof(msg));
438 : }
439 :
440 : /**
441 : sends a single fast-throttle frame containing the throttle for all configured OneWire ESCs.
442 : @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 999-0 reversed rotation
443 : */
444 : void escs_set_values(const uint16_t *motor_values);
445 :
446 : /*
447 : * Methods and data for receiving data from the ESCs:
448 : */
449 :
450 : // FIXME: this should be tighter - and probably calculated. Note
451 : // that we can't request telemetry faster than the loop interval,
452 : // which is 20ms on Plane, so that puts a constraint here. When
453 : // using fast-throttle with 12 ESCs on Plane you could expect
454 : // 240ms between telem updates. Why you have a Plane with 12 ESCs
455 : // is a bit of a puzzle.
456 : static const uint32_t max_telem_interval_us = 100000;
457 :
458 : void handle_message(ESC &esc, const uint8_t length);
459 :
460 : /**
461 : reads data from the UART, calling handle_message on any message found
462 : */
463 : void read_data_from_uart();
464 : union MessageUnion {
465 2094 : MessageUnion() { }
466 : PackedMessage<OK> packed_ok;
467 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
468 : PackedMessage<ESC_TYPE> packed_esc_type;
469 : PackedMessage<SW_VER> packed_sw_ver;
470 : PackedMessage<SN> packed_sn;
471 : #endif
472 : #if HAL_WITH_ESC_TELEM
473 : PackedMessage<TLM> packed_tlm;
474 : #endif
475 : uint8_t receive_buf[FRAME_OVERHEAD + MAX_RECEIVE_LENGTH];
476 : } u;
477 :
478 : static_assert(sizeof(u.packed_ok) <= sizeof(u.receive_buf),"packed_ok does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
479 : #if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
480 : static_assert(sizeof(u.packed_esc_type) <= sizeof(u.receive_buf),"packed_esc_type does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
481 : static_assert(sizeof(u.packed_sw_ver) <= sizeof(u.receive_buf),"packed_sw_ver does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
482 : static_assert(sizeof(u.packed_sn) <= sizeof(u.receive_buf),"packed_sn does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
483 : #endif
484 : #if HAL_WITH_ESC_TELEM
485 : static_assert(sizeof(u.packed_tlm) <= sizeof(u.receive_buf),"packed_tlm does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
486 : #endif
487 :
488 : uint16_t _unknown_esc_message;
489 : uint16_t _message_invalid_in_state_count;
490 : uint16_t _period_too_short;
491 : uint16_t crc_rec_err_cnt;
492 : uint8_t _receive_buf_used;
493 :
494 : /// shifts data to start of buffer based on magic header bytes
495 : void move_frame_source_in_receive_buffer(const uint8_t search_start_pos = 0);
496 :
497 : /// cut n bytes from start of buffer
498 : void consume_bytes(const uint8_t n);
499 :
500 : /// returns true if the first message in the buffer is OK
501 : bool buffer_contains_ok(const uint8_t length);
502 : };
503 : #endif // AP_FETTEC_ONEWIRE_ENABLED
|
