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 : implementation of MSP and BLHeli-4way protocols for pass-through ESC
17 : calibration and firmware update
18 :
19 : With thanks to betaflight for a great reference
20 : implementation. Several of the functions below are based on
21 : betaflight equivalent functions
22 : */
23 :
24 : #include "AP_BLHeli.h"
25 :
26 : #if HAVE_AP_BLHELI_SUPPORT
27 :
28 : #include <AP_Math/crc.h>
29 : #include <AP_Vehicle/AP_Vehicle_Type.h>
30 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
31 : #include <AR_Motors/AP_MotorsUGV.h>
32 : #else
33 : #include <AP_Motors/AP_Motors_Class.h>
34 : #endif
35 : #include <GCS_MAVLink/GCS_MAVLink.h>
36 : #include <GCS_MAVLink/GCS.h>
37 : #include <AP_SerialManager/AP_SerialManager.h>
38 : #include <AP_BoardConfig/AP_BoardConfig.h>
39 : #include <AP_ESC_Telem/AP_ESC_Telem.h>
40 : #include <SRV_Channel/SRV_Channel.h>
41 : #include <AP_BattMonitor/AP_BattMonitor.h>
42 :
43 : extern const AP_HAL::HAL& hal;
44 :
45 : #define debug(fmt, args ...) do { if (debug_level) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: " fmt, ## args); } } while (0)
46 : #ifdef BLHELI_DEBUG
47 : #define debug_console(fmt, args ...) do { hal.console->printf(fmt "\n", ## args); } while (0)
48 : #else
49 : #define debug_console(fmt, args ...) do {} while(0)
50 : #endif
51 :
52 : // key for locking UART for exclusive use. This prevents any other writes from corrupting
53 : // the MSP protocol on hal.console
54 : #define BLHELI_UART_LOCK_KEY 0x20180402
55 :
56 : // if no packets are received for this time and motor control is active BLH will disconnect (stoping motors)
57 : #define MOTOR_ACTIVE_TIMEOUT 1000
58 :
59 : const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
60 : // @Param: MASK
61 : // @DisplayName: BLHeli Channel Bitmask
62 : // @Description: Enable of BLHeli pass-thru servo protocol support to specific channels. This mask is in addition to motors enabled using SERVO_BLH_AUTO (if any)
63 : // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
64 : // @User: Advanced
65 : // @RebootRequired: True
66 : AP_GROUPINFO("MASK", 1, AP_BLHeli, channel_mask, 0),
67 :
68 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
69 : // @Param: AUTO
70 : // @DisplayName: BLHeli pass-thru auto-enable for multicopter motors
71 : // @Description: If set to 1 this auto-enables BLHeli pass-thru support for all multicopter motors
72 : // @Values: 0:Disabled,1:Enabled
73 : // @User: Standard
74 : // @RebootRequired: True
75 : AP_GROUPINFO("AUTO", 2, AP_BLHeli, channel_auto, 0),
76 : #endif
77 :
78 : // @Param: TEST
79 : // @DisplayName: BLHeli internal interface test
80 : // @Description: Setting SERVO_BLH_TEST to a motor number enables an internal test of the BLHeli ESC protocol to the corresponding ESC. The debug output is displayed on the USB console.
81 : // @Values: 0:Disabled,1:TestMotor1,2:TestMotor2,3:TestMotor3,4:TestMotor4,5:TestMotor5,6:TestMotor6,7:TestMotor7,8:TestMotor8
82 : // @User: Advanced
83 : AP_GROUPINFO("TEST", 3, AP_BLHeli, run_test, 0),
84 :
85 : // @Param: TMOUT
86 : // @DisplayName: BLHeli protocol timeout
87 : // @Description: This sets the inactivity timeout for the BLHeli protocol in seconds. If no packets are received in this time normal MAVLink operations are resumed. A value of 0 means no timeout
88 : // @Units: s
89 : // @Range: 0 300
90 : // @User: Standard
91 : AP_GROUPINFO("TMOUT", 4, AP_BLHeli, timeout_sec, 0),
92 :
93 : // @Param: TRATE
94 : // @DisplayName: BLHeli telemetry rate
95 : // @Description: This sets the rate in Hz for requesting telemetry from ESCs. It is the rate per ESC. Setting to zero disables telemetry requests
96 : // @Units: Hz
97 : // @Range: 0 500
98 : // @User: Standard
99 : AP_GROUPINFO("TRATE", 5, AP_BLHeli, telem_rate, 10),
100 :
101 : // @Param: DEBUG
102 : // @DisplayName: BLHeli debug level
103 : // @Description: When set to 1 this enabled verbose debugging output over MAVLink when the blheli protocol is active. This can be used to diagnose failures.
104 : // @Values: 0:Disabled,1:Enabled
105 : // @User: Standard
106 : AP_GROUPINFO("DEBUG", 6, AP_BLHeli, debug_level, 0),
107 :
108 : // @Param: OTYPE
109 : // @DisplayName: BLHeli output type override
110 : // @Description: When set to a non-zero value this overrides the output type for the output channels given by SERVO_BLH_MASK. This can be used to enable DShot on outputs that are not part of the multicopter motors group.
111 : // @Values: 0:None,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200
112 : // @User: Advanced
113 : // @RebootRequired: True
114 : AP_GROUPINFO("OTYPE", 7, AP_BLHeli, output_type, 0),
115 :
116 : // @Param: PORT
117 : // @DisplayName: Control port
118 : // @Description: This sets the mavlink channel to use for blheli pass-thru. The channel number is determined by the number of serial ports configured to use mavlink. So 0 is always the console, 1 is the next serial port using mavlink, 2 the next after that and so on.
119 : // @Values: 0:Console,1:Mavlink Serial Channel1,2:Mavlink Serial Channel2,3:Mavlink Serial Channel3,4:Mavlink Serial Channel4,5:Mavlink Serial Channel5
120 : // @User: Advanced
121 : AP_GROUPINFO("PORT", 8, AP_BLHeli, control_port, 0),
122 :
123 : // @Param: POLES
124 : // @DisplayName: BLHeli Motor Poles
125 : // @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14.
126 : // @Range: 1 127
127 : // @User: Advanced
128 : // @RebootRequired: True
129 : AP_GROUPINFO("POLES", 9, AP_BLHeli, motor_poles, 14),
130 :
131 : // @Param: 3DMASK
132 : // @DisplayName: BLHeli bitmask of 3D channels
133 : // @Description: Mask of channels which are dynamically reversible. This is used to configure ESCs in '3D' mode, allowing for the motor to spin in either direction. Note that setting an ESC as reversible with this option on AM32 will result in the forward direction of the ESC changing. You can combine with parameter with the SERVO_BLH_RVMASK parameter to maintain the same direction when the ESC is in 3D mode as it has in unidirectional (non-3D) mode.
134 : // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
135 : // @User: Advanced
136 : // @RebootRequired: True
137 : AP_GROUPINFO("3DMASK", 10, AP_BLHeli, channel_reversible_mask, 0),
138 :
139 : #if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT
140 : // @Param: BDMASK
141 : // @DisplayName: BLHeli bitmask of bi-directional dshot channels
142 : // @Description: Mask of channels which support bi-directional dshot telemetry. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch.
143 : // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
144 : // @User: Advanced
145 : // @RebootRequired: True
146 : AP_GROUPINFO("BDMASK", 11, AP_BLHeli, channel_bidir_dshot_mask, 0),
147 : #endif
148 :
149 : // @Param: RVMASK
150 : // @DisplayName: BLHeli bitmask of reversed channels
151 : // @Description: Mask of channels which are reversed. This is used to configure ESCs to reverse motor direction. Note that when combined with SERVO_BLH_3DMASK this will change what direction is considered to be forward.
152 : // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
153 : // @User: Advanced
154 : // @RebootRequired: True
155 : AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0),
156 :
157 : AP_GROUPEND
158 : };
159 :
160 : #define RPM_SLEW_RATE 50
161 :
162 : AP_BLHeli *AP_BLHeli::_singleton;
163 :
164 : // constructor
165 2094 : AP_BLHeli::AP_BLHeli(void)
166 : {
167 : // set defaults from the parameter table
168 2094 : AP_Param::setup_object_defaults(this, var_info);
169 2094 : _singleton = this;
170 2094 : last_control_port = -1;
171 2094 : }
172 :
173 : // map an incoming BLHeli motor request to the appropriate
174 : // output channel for use in serial output so that motor numbers
175 : // are observed
176 0 : uint8_t AP_BLHeli::blheli_chan_to_output_chan(uint8_t motor)
177 : {
178 : // user has overidden the default output mask, we must use the motor map
179 0 : if (channel_mask.get() != 0) {
180 0 : return motor_map[motor];
181 : }
182 : // user is using motor mask and so we can use the servo channel options
183 0 : uint8_t chan = 0; // 0 means motor 1 and is ok as a fallback
184 0 : SRV_Channels::find_channel(SRV_Channels::get_motor_function(motor), chan);
185 0 : return chan;
186 : }
187 :
188 : /*
189 : process one byte of serial input for MSP protocol
190 : */
191 0 : bool AP_BLHeli::msp_process_byte(uint8_t c)
192 : {
193 0 : if (msp.state == MSP_IDLE) {
194 0 : msp.escMode = PROTOCOL_NONE;
195 0 : if (c == '$') {
196 0 : msp.state = MSP_HEADER_START;
197 : } else {
198 0 : return false;
199 : }
200 0 : } else if (msp.state == MSP_HEADER_START) {
201 0 : msp.state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
202 0 : } else if (msp.state == MSP_HEADER_M) {
203 0 : msp.state = MSP_IDLE;
204 0 : switch (c) {
205 0 : case '<': // COMMAND
206 0 : msp.packetType = MSP_PACKET_COMMAND;
207 0 : msp.state = MSP_HEADER_ARROW;
208 0 : break;
209 0 : case '>': // REPLY
210 0 : msp.packetType = MSP_PACKET_REPLY;
211 0 : msp.state = MSP_HEADER_ARROW;
212 0 : break;
213 0 : default:
214 0 : break;
215 : }
216 0 : } else if (msp.state == MSP_HEADER_ARROW) {
217 0 : if (c > sizeof(msp.buf)) {
218 0 : msp.state = MSP_IDLE;
219 : } else {
220 0 : msp.dataSize = c;
221 0 : msp.offset = 0;
222 0 : msp.checksum = 0;
223 0 : msp.checksum ^= c;
224 0 : msp.state = MSP_HEADER_SIZE;
225 : }
226 0 : } else if (msp.state == MSP_HEADER_SIZE) {
227 0 : msp.cmdMSP = c;
228 0 : msp.checksum ^= c;
229 0 : msp.state = MSP_HEADER_CMD;
230 0 : } else if (msp.state == MSP_HEADER_CMD && msp.offset < msp.dataSize) {
231 0 : msp.checksum ^= c;
232 0 : msp.buf[msp.offset++] = c;
233 0 : } else if (msp.state == MSP_HEADER_CMD && msp.offset >= msp.dataSize) {
234 0 : if (msp.checksum == c) {
235 0 : msp.state = MSP_COMMAND_RECEIVED;
236 : } else {
237 0 : msp.state = MSP_IDLE;
238 : }
239 : }
240 0 : return true;
241 : }
242 :
243 : /*
244 : update CRC state for blheli protocol
245 : */
246 0 : void AP_BLHeli::blheli_crc_update(uint8_t c)
247 : {
248 0 : blheli.crc = crc_xmodem_update(blheli.crc, c);
249 0 : }
250 :
251 : /*
252 : process one byte of serial input for blheli 4way protocol
253 : */
254 0 : bool AP_BLHeli::blheli_4way_process_byte(uint8_t c)
255 : {
256 0 : if (blheli.state == BLHELI_IDLE) {
257 0 : if (c == cmd_Local_Escape) {
258 0 : blheli.state = BLHELI_HEADER_START;
259 0 : blheli.crc = 0;
260 0 : blheli_crc_update(c);
261 : } else {
262 0 : return false;
263 : }
264 0 : } else if (blheli.state == BLHELI_HEADER_START) {
265 0 : blheli.command = c;
266 0 : blheli_crc_update(c);
267 0 : blheli.state = BLHELI_HEADER_CMD;
268 0 : } else if (blheli.state == BLHELI_HEADER_CMD) {
269 0 : blheli.address = c<<8;
270 0 : blheli.state = BLHELI_HEADER_ADDR_HIGH;
271 0 : blheli_crc_update(c);
272 0 : } else if (blheli.state == BLHELI_HEADER_ADDR_HIGH) {
273 0 : blheli.address |= c;
274 0 : blheli.state = BLHELI_HEADER_ADDR_LOW;
275 0 : blheli_crc_update(c);
276 0 : } else if (blheli.state == BLHELI_HEADER_ADDR_LOW) {
277 0 : blheli.state = BLHELI_HEADER_LEN;
278 0 : blheli.param_len = c?c:256;
279 0 : blheli.offset = 0;
280 0 : blheli_crc_update(c);
281 0 : } else if (blheli.state == BLHELI_HEADER_LEN) {
282 0 : blheli.buf[blheli.offset++] = c;
283 0 : blheli_crc_update(c);
284 0 : if (blheli.offset == blheli.param_len) {
285 0 : blheli.state = BLHELI_CRC1;
286 : }
287 0 : } else if (blheli.state == BLHELI_CRC1) {
288 0 : blheli.crc1 = c;
289 0 : blheli.state = BLHELI_CRC2;
290 0 : } else if (blheli.state == BLHELI_CRC2) {
291 0 : uint16_t crc = blheli.crc1<<8 | c;
292 0 : if (crc == blheli.crc) {
293 0 : blheli.state = BLHELI_COMMAND_RECEIVED;
294 : } else {
295 0 : blheli.state = BLHELI_IDLE;
296 : }
297 : }
298 0 : return true;
299 : }
300 :
301 :
302 : /*
303 : send a MSP protocol ack
304 : */
305 0 : void AP_BLHeli::msp_send_ack(uint8_t cmd)
306 : {
307 0 : msp_send_reply(cmd, 0, 0);
308 0 : }
309 :
310 : /*
311 : send a MSP protocol reply
312 : */
313 0 : void AP_BLHeli::msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len)
314 : {
315 0 : uint8_t *b = &msp.buf[0];
316 0 : *b++ = '$';
317 0 : *b++ = 'M';
318 0 : *b++ = '>';
319 0 : *b++ = len;
320 0 : *b++ = cmd;
321 : // acks do not have a payload
322 0 : if (len > 0) {
323 0 : memcpy(b, buf, len);
324 : }
325 0 : b += len;
326 0 : uint8_t c = 0;
327 0 : for (uint8_t i=0; i<len+2; i++) {
328 0 : c ^= msp.buf[i+3];
329 : }
330 0 : *b++ = c;
331 0 : uart->write_locked(&msp.buf[0], len+6, BLHELI_UART_LOCK_KEY);
332 0 : }
333 :
334 0 : void AP_BLHeli::putU16(uint8_t *b, uint16_t v)
335 : {
336 0 : b[0] = v;
337 0 : b[1] = v >> 8;
338 0 : }
339 :
340 0 : uint16_t AP_BLHeli::getU16(const uint8_t *b)
341 : {
342 0 : return b[0] | (b[1]<<8);
343 : }
344 :
345 0 : void AP_BLHeli::putU32(uint8_t *b, uint32_t v)
346 : {
347 0 : b[0] = v;
348 0 : b[1] = v >> 8;
349 0 : b[2] = v >> 16;
350 0 : b[3] = v >> 24;
351 0 : }
352 :
353 0 : void AP_BLHeli::putU16_BE(uint8_t *b, uint16_t v)
354 : {
355 0 : b[0] = v >> 8;
356 0 : b[1] = v;
357 0 : }
358 :
359 : /*
360 : process a MSP command from GCS
361 : */
362 0 : void AP_BLHeli::msp_process_command(void)
363 : {
364 0 : debug("MSP cmd %u len=%u", msp.cmdMSP, msp.dataSize);
365 0 : switch (msp.cmdMSP) {
366 0 : case MSP_API_VERSION: {
367 0 : debug("MSP_API_VERSION");
368 0 : uint8_t buf[3] = { MSP_PROTOCOL_VERSION, API_VERSION_MAJOR, API_VERSION_MINOR };
369 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
370 0 : break;
371 : }
372 :
373 0 : case MSP_FC_VARIANT:
374 0 : debug("MSP_FC_VARIANT");
375 0 : msp_send_reply(msp.cmdMSP, (const uint8_t *)ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
376 0 : break;
377 :
378 : /*
379 : Notes:
380 : version 3.3.1 adds a reply to MSP_SET_MOTOR which was missing
381 : version 3.3.0 requires a workaround in blheli suite to handle MSP_SET_MOTOR without an ack
382 : */
383 0 : case MSP_FC_VERSION: {
384 0 : debug("MSP_FC_VERSION");
385 0 : uint8_t version[3] = { 3, 3, 1 };
386 0 : msp_send_reply(msp.cmdMSP, version, sizeof(version));
387 0 : break;
388 : }
389 0 : case MSP_BOARD_INFO: {
390 0 : debug("MSP_BOARD_INFO");
391 : // send a generic 'ArduPilot ChibiOS' board type
392 0 : uint8_t buf[7] = { 'A', 'R', 'C', 'H', 0, 0, 0 };
393 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
394 0 : break;
395 : }
396 :
397 0 : case MSP_BUILD_INFO: {
398 0 : debug("MSP_BUILD_INFO");
399 : // build date, build time, git version
400 0 : uint8_t buf[26] {
401 : 0x4d, 0x61, 0x72, 0x20, 0x31, 0x36, 0x20, 0x32, 0x30,
402 : 0x31, 0x38, 0x30, 0x38, 0x3A, 0x34, 0x32, 0x3a, 0x32, 0x39,
403 : 0x62, 0x30, 0x66, 0x66, 0x39, 0x32, 0x38};
404 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
405 0 : break;
406 : }
407 :
408 0 : case MSP_REBOOT:
409 0 : debug("MSP: ignoring reboot command, end serial comms");
410 0 : serial_end();
411 0 : blheli.connected[blheli.chan] = false;
412 0 : break;
413 :
414 0 : case MSP_UID: {
415 : // MCU identifier
416 0 : debug("MSP_UID");
417 0 : uint8_t uid[12] {};
418 0 : uint8_t len = sizeof(uid);
419 0 : hal.util->get_system_id_unformatted(uid, len);
420 0 : msp_send_reply(msp.cmdMSP, uid, sizeof(uid));
421 0 : break;
422 : }
423 :
424 : // a literal "4" is used for the PWMType here to allow Rover
425 : // to use the same number for the same protocol. At time of
426 : // writing the AP_MotorsUGV::PWMType has not been unified with
427 : // AP_Motors::PWMType.
428 0 : case MSP_ADVANCED_CONFIG: {
429 0 : debug("MSP_ADVANCED_CONFIG");
430 : uint8_t buf[10];
431 0 : buf[0] = 1; // gyro sync denom
432 0 : buf[1] = 4; // pid process denom
433 0 : buf[2] = 0; // use unsynced pwm
434 0 : buf[3] = 4; // (uint8_t)AP_Motors::PWMType::DSHOT150;
435 0 : putU16(&buf[4], 480); // motor PWM Rate
436 0 : putU16(&buf[6], 450); // idle offset value
437 0 : buf[8] = 0; // use 32kHz
438 0 : buf[9] = 0; // motor PWM inversion
439 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
440 0 : break;
441 : }
442 :
443 0 : case MSP_FEATURE_CONFIG: {
444 0 : debug("MSP_FEATURE_CONFIG");
445 : uint8_t buf[4];
446 0 : putU32(buf, (channel_reversible_mask.get() != 0) ? FEATURE_3D : 0); // from MSPFeatures enum
447 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
448 0 : break;
449 : }
450 :
451 0 : case MSP_STATUS: {
452 0 : debug("MSP_STATUS");
453 : uint8_t buf[21];
454 0 : putU16(&buf[0], 1000); // loop time usec
455 0 : putU16(&buf[2], 0); // i2c error count
456 0 : putU16(&buf[4], 0x27); // available sensors
457 0 : putU32(&buf[6], 0); // flight modes
458 0 : buf[10] = 0; // pid profile index
459 0 : putU16(&buf[11], 5); // system load percent
460 0 : putU16(&buf[13], 0); // gyro cycle time
461 0 : buf[15] = 0; // flight mode flags length
462 0 : buf[16] = 18; // arming disable flags count
463 0 : putU32(&buf[17], 0); // arming disable flags
464 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
465 0 : break;
466 : }
467 :
468 0 : case MSP_MOTOR_3D_CONFIG: {
469 0 : debug("MSP_MOTOR_3D_CONFIG");
470 : uint8_t buf[6];
471 0 : putU16(&buf[0], 1406); // 3D deadband low
472 0 : putU16(&buf[2], 1514); // 3D deadband high
473 0 : putU16(&buf[4], 1460); // 3D neutral
474 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
475 0 : break;
476 : }
477 :
478 0 : case MSP_BATTERY_STATE: {
479 0 : debug("MSP_BATTERY_STATE");
480 : // ESC configurator seems to care a lot about the battery state,
481 : // try and at least provide something believable
482 0 : uint8_t buf[11] {};
483 : #if AP_BATTERY_ENABLED
484 0 : AP_BattMonitor &battery = AP::battery();
485 : float v;
486 : #if HAL_WITH_ESC_TELEM
487 0 : if (!AP::esc_telem().get_voltage(blheli_chan_to_output_chan(blheli.chan), v)) {
488 0 : v = battery.voltage();
489 : }
490 : #else
491 : v = battery.voltage();
492 : #endif
493 0 : buf[0] = battery.healthy() ? uint8_t(roundf(v / 3.85)) : 0; // cell count, 0 means no battery
494 0 : putU16(&buf[1], uint16_t(battery.pack_capacity_mah())); // capacity in mAh
495 0 : buf[3] = uint8_t(roundf(v * 10.0)); // legacy V in 0.1V steps
496 :
497 0 : float cons = 0;
498 0 : UNUSED_RESULT(battery.consumed_mah(cons));
499 0 : putU16(&buf[4], uint16_t(roundf(cons))); // mAh used
500 :
501 0 : float amps = 0.0;
502 0 : UNUSED_RESULT(battery.current_amps(amps));
503 0 : putU16(&buf[6], uint16_t(roundf(amps * 100.0))); // A in 0.01A steps
504 0 : buf[8] = battery.healthy(); // alerts/state
505 : // We are advertising MSP v1.42 which means supporting the new voltage field
506 0 : putU16(&buf[9], uint16_t(roundf(v * 100.0))); // Voltage in 0.01V steps
507 : #endif
508 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
509 0 : break;
510 : }
511 :
512 0 : case MSP_MOTOR_CONFIG: {
513 0 : debug("MSP_MOTOR_CONFIG(n=%u, p=%u)", num_motors, motor_poles.get());
514 : uint8_t buf[10];
515 0 : SRV_Channel* channel = SRV_Channels::srv_channel(blheli_chan_to_output_chan(0));
516 0 : putU16(&buf[0], channel->get_output_min()); // min throttle
517 0 : putU16(&buf[2], channel->get_output_max()); // max throttle
518 0 : putU16(&buf[4], channel->get_output_min()); // min command
519 : // API 1.42
520 0 : buf[6] = num_motors; // motorCount
521 0 : buf[7] = motor_poles; // motorPoleCount
522 0 : buf[8] = 0; // useDshotTelemetry
523 0 : buf[9] = 0; // FEATURE_ESC_SENSOR
524 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
525 0 : break;
526 : }
527 :
528 0 : case MSP_MOTOR: {
529 0 : debug("MSP_MOTOR");
530 : // get the output going to each motor
531 0 : uint8_t buf[16] {};
532 0 : for (uint8_t i = 0; i < num_motors; i++) {
533 : // if we have a mix of reversible and normal report a PWM of zero, this allows BLHeliSuite to conect
534 0 : uint8_t chan = blheli_chan_to_output_chan(i);
535 0 : uint16_t v = mixed_type ? 0 : hal.rcout->read(blheli_chan_to_output_chan(i));
536 0 : putU16(&buf[2*i], v);
537 0 : debug("MOTOR %u chan: %u val: %u",i,chan,v);
538 : }
539 0 : msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
540 0 : break;
541 : }
542 :
543 0 : case MSP_SET_MOTOR: {
544 0 : debug("MSP_SET_MOTOR");
545 0 : if (!mixed_type) {
546 : // set the output to each motor
547 0 : uint8_t nmotors = msp.dataSize / 2;
548 0 : debug("MSP_SET_MOTOR %u", nmotors);
549 0 : motors_disabled_mask = SRV_Channels::get_disabled_channel_mask();
550 0 : SRV_Channels::set_disabled_channel_mask(0xFFFF);
551 0 : motors_disabled = true;
552 0 : EXPECT_DELAY_MS(1000);
553 0 : hal.rcout->cork();
554 0 : for (uint8_t i = 0; i < nmotors; i++) {
555 0 : if (i >= num_motors) {
556 0 : break;
557 : }
558 0 : uint16_t v = getU16(&msp.buf[i*2]);
559 0 : debug("MSP_SET_MOTOR %u %u", i, v);
560 : // map from a MSP value to a value in the range 1000 to 2000
561 0 : uint16_t pwm = (v < 1000)?0:v;
562 0 : hal.rcout->write(blheli_chan_to_output_chan(i), pwm);
563 : }
564 0 : hal.rcout->push();
565 0 : } else {
566 0 : debug("mixed type, Motors Disabled");
567 : }
568 0 : msp_send_ack(msp.cmdMSP);
569 0 : break;
570 : }
571 :
572 0 : case MSP_SET_PASSTHROUGH: {
573 0 : debug("MSP_SET_PASSTHROUGH");
574 0 : if (msp.dataSize == 0) {
575 0 : msp.escMode = PROTOCOL_4WAY;
576 0 : } else if (msp.dataSize == 2) {
577 0 : msp.escMode = (enum escProtocol)msp.buf[0];
578 0 : msp.portIndex = msp.buf[1];
579 : }
580 0 : debug("escMode=%u portIndex=%u num_motors=%u", msp.escMode, msp.portIndex, num_motors);
581 0 : uint8_t n = num_motors;
582 0 : switch (msp.escMode) {
583 0 : case PROTOCOL_4WAY:
584 0 : break;
585 0 : default:
586 0 : n = 0;
587 0 : serial_end();
588 0 : break;
589 : }
590 : // doing the serial setup here avoids delays when doing it on demand and makes
591 : // BLHeliSuite considerably more reliable
592 0 : EXPECT_DELAY_MS(1000);
593 0 : if (!hal.rcout->serial_setup_output(blheli_chan_to_output_chan(0), 19200, motor_mask)) {
594 0 : msp_send_ack(ACK_D_GENERAL_ERROR);
595 0 : break;
596 : } else {
597 0 : msp_send_reply(msp.cmdMSP, &n, 1);
598 : }
599 0 : break;
600 : }
601 0 : default:
602 0 : debug("Unknown MSP command %u", msp.cmdMSP);
603 0 : break;
604 : }
605 0 : }
606 :
607 : /*
608 : send a blheli 4way protocol reply
609 : */
610 0 : void AP_BLHeli::blheli_send_reply(const uint8_t *buf, uint16_t len)
611 : {
612 0 : uint8_t *b = &blheli.buf[0];
613 0 : *b++ = cmd_Remote_Escape;
614 0 : *b++ = blheli.command;
615 0 : putU16_BE(b, blheli.address); b += 2;
616 0 : *b++ = len==256?0:len;
617 0 : memcpy(b, buf, len);
618 0 : b += len;
619 0 : *b++ = blheli.ack;
620 0 : putU16_BE(b, crc_xmodem(&blheli.buf[0], len+6));
621 0 : uart->write_locked(&blheli.buf[0], len+8, BLHELI_UART_LOCK_KEY);
622 0 : debug("OutB(%u) 0x%02x ack=0x%02x", len+8, (unsigned)blheli.command, blheli.ack);
623 0 : }
624 :
625 : /*
626 : CRC used when talking to ESCs
627 : */
628 0 : uint16_t AP_BLHeli::BL_CRC(const uint8_t *buf, uint16_t len)
629 : {
630 0 : uint16_t crc = 0;
631 0 : while (len--) {
632 0 : uint8_t xb = *buf++;
633 0 : for (uint8_t i = 0; i < 8; i++) {
634 0 : if (((xb & 0x01) ^ (crc & 0x0001)) !=0 ) {
635 0 : crc = crc >> 1;
636 0 : crc = crc ^ 0xA001;
637 : } else {
638 0 : crc = crc >> 1;
639 : }
640 0 : xb = xb >> 1;
641 : }
642 : }
643 0 : return crc;
644 : }
645 :
646 0 : bool AP_BLHeli::isMcuConnected(void)
647 : {
648 0 : return blheli.connected[blheli.chan];
649 : }
650 :
651 0 : void AP_BLHeli::setDisconnected(void)
652 : {
653 0 : blheli.connected[blheli.chan] = false;
654 0 : blheli.deviceInfo[blheli.chan][0] = 0;
655 0 : blheli.deviceInfo[blheli.chan][1] = 0;
656 0 : }
657 :
658 : /*
659 : send a set of bytes to an RC output channel
660 : */
661 0 : bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len)
662 : {
663 0 : bool send_crc = isMcuConnected();
664 0 : if (blheli.chan >= num_motors) {
665 0 : return false;
666 : }
667 0 : EXPECT_DELAY_MS(1000);
668 0 : hal.scheduler->delay_microseconds(100);
669 0 : if (!hal.rcout->serial_setup_output(blheli_chan_to_output_chan(blheli.chan), 19200, motor_mask)) {
670 : debug_console("serial_setup_output() failed\n");
671 0 : blheli.ack = ACK_D_GENERAL_ERROR;
672 0 : return false;
673 : }
674 : // ensure that the next write does not go out immediately in case the receiving side is not yet
675 : // ready
676 0 : hal.scheduler->delay_microseconds(100);
677 :
678 0 : if (serial_start_ms == 0) {
679 0 : serial_start_ms = AP_HAL::millis();
680 : }
681 0 : memcpy(blheli.buf, buf, len);
682 0 : uint16_t crc = BL_CRC(buf, len);
683 0 : blheli.buf[len] = uint8_t(crc & 0xFF);
684 0 : blheli.buf[len+1] = uint8_t(crc>>8);
685 0 : if (!hal.rcout->serial_write_bytes(blheli.buf, len+(send_crc?2:0))) {
686 : debug_console("serial_write_bytes() failed\n");
687 0 : blheli.ack = ACK_D_GENERAL_ERROR;
688 0 : return false;
689 : }
690 : // 19200 baud is 52us per bit - wait for half a bit between sending and receiving to avoid reading
691 : // the end of the last sent bit by accident
692 0 : hal.scheduler->delay_microseconds(26);
693 0 : return true;
694 0 : }
695 :
696 : /*
697 : read bytes from the ESC connection
698 : */
699 0 : bool AP_BLHeli::BL_ReadBuf(uint8_t *buf, uint16_t len)
700 : {
701 0 : bool check_crc = isMcuConnected();
702 0 : uint16_t req_bytes = len+(check_crc?3:1);
703 0 : EXPECT_DELAY_MS(1000);
704 : // byte time is 520us so 1000 per byte should be more than enough
705 0 : uint16_t n = hal.rcout->serial_read_bytes(blheli.buf, req_bytes, req_bytes * 1000);
706 0 : debug("BL_ReadBuf %u -> %u", len, n);
707 0 : if (req_bytes != n) {
708 0 : debug("short read");
709 0 : blheli.ack = ACK_D_GENERAL_ERROR;
710 0 : return false;
711 : }
712 0 : if (check_crc) {
713 0 : uint16_t crc = BL_CRC(blheli.buf, len);
714 0 : if ((crc & 0xff) != blheli.buf[len] ||
715 0 : (crc >> 8) != blheli.buf[len+1]) {
716 0 : debug("bad CRC");
717 0 : blheli.ack = ACK_D_GENERAL_ERROR;
718 0 : return false;
719 : }
720 0 : if (blheli.buf[len+2] != brSUCCESS) {
721 0 : debug("bad ACK 0x%02x", blheli.buf[len+2]);
722 0 : blheli.ack = ACK_D_GENERAL_ERROR;
723 0 : return false;
724 : }
725 : } else {
726 0 : if (blheli.buf[len] != brSUCCESS) {
727 0 : debug("bad ACK1 0x%02x", blheli.buf[len]);
728 0 : blheli.ack = ACK_D_GENERAL_ERROR;
729 0 : return false;
730 : }
731 : }
732 0 : if (len > 0) {
733 0 : memcpy(buf, blheli.buf, len);
734 : }
735 0 : return true;
736 0 : }
737 :
738 0 : uint8_t AP_BLHeli::BL_GetACK(uint16_t timeout_ms)
739 : {
740 : uint8_t ack;
741 0 : EXPECT_DELAY_MS(timeout_ms);
742 0 : if (hal.rcout->serial_read_bytes(&ack, 1, timeout_ms * 1000) == 1) {
743 0 : return ack;
744 : }
745 : // return brNONE, meaning no ACK received in the timeout
746 0 : return brNONE;
747 0 : }
748 :
749 0 : bool AP_BLHeli::BL_SendCMDSetAddress()
750 : {
751 : // skip if adr == 0xFFFF
752 0 : if (blheli.address == 0xFFFF) {
753 0 : return true;
754 : }
755 0 : debug("BL_SendCMDSetAddress 0x%04x", blheli.address);
756 0 : uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, uint8_t(blheli.address>>8), uint8_t(blheli.address)};
757 0 : if (!BL_SendBuf(sCMD, 4)) {
758 0 : return false;
759 : }
760 0 : return BL_GetACK() == brSUCCESS;
761 : }
762 :
763 0 : bool AP_BLHeli::BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n)
764 : {
765 0 : if (BL_SendCMDSetAddress()) {
766 0 : uint8_t sCMD[] = {cmd, uint8_t(n==256?0:n)};
767 0 : if (!BL_SendBuf(sCMD, 2)) {
768 0 : return false;
769 : }
770 0 : bool ret = BL_ReadBuf(buf, n);
771 0 : if (ret && n == sizeof(esc_status) && blheli.address == esc_status_addr) {
772 : // display esc_status structure if we see it
773 : struct esc_status status;
774 0 : memcpy(&status, buf, n);
775 0 : debug("Prot %u Good %u Bad %u %x %x %x x%x\n",
776 : (unsigned)status.protocol,
777 : (unsigned)status.good_frames,
778 : (unsigned)status.bad_frames,
779 : (unsigned)status.unknown[0],
780 : (unsigned)status.unknown[1],
781 : (unsigned)status.unknown[2],
782 : (unsigned)status.unknown2);
783 : }
784 0 : return ret;
785 : }
786 0 : return false;
787 : }
788 :
789 : /*
790 : connect to a blheli ESC
791 : */
792 0 : bool AP_BLHeli::BL_ConnectEx(void)
793 : {
794 0 : debug("BL_ConnectEx %u/%u at %u", blheli.chan, num_motors, blheli_chan_to_output_chan(blheli.chan));
795 0 : setDisconnected();
796 0 : const uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
797 0 : if (!BL_SendBuf(BootInit, 21)) {
798 0 : return false;
799 : }
800 :
801 : uint8_t BootInfo[9];
802 0 : if (!BL_ReadBuf(BootInfo, 8)) {
803 0 : return false;
804 : }
805 :
806 : // reply must start with 471
807 0 : if (strncmp((const char *)BootInfo, "471", 3) != 0) {
808 0 : blheli.ack = ACK_D_GENERAL_ERROR;
809 0 : return false;
810 : }
811 :
812 : // extract device information
813 0 : blheli.deviceInfo[blheli.chan][2] = BootInfo[3];
814 0 : blheli.deviceInfo[blheli.chan][1] = BootInfo[4];
815 0 : blheli.deviceInfo[blheli.chan][0] = BootInfo[5];
816 :
817 0 : blheli.interface_mode[blheli.chan] = 0;
818 :
819 : uint16_t devword;
820 0 : memcpy(&devword, blheli.deviceInfo[blheli.chan], sizeof(devword));
821 0 : switch (devword) {
822 0 : case 0x9307:
823 : case 0x930A:
824 : case 0x930F:
825 : case 0x940B:
826 0 : blheli.interface_mode[blheli.chan] = imATM_BLB;
827 0 : debug("Interface type imATM_BLB");
828 0 : break;
829 0 : case 0xF310:
830 : case 0xF330:
831 : case 0xF410:
832 : case 0xF390:
833 : case 0xF850:
834 : case 0xE8B1:
835 : case 0xE8B2:
836 0 : blheli.interface_mode[blheli.chan] = imSIL_BLB;
837 0 : debug("Interface type imSIL_BLB");
838 0 : break;
839 0 : default:
840 : // BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
841 0 : if ((blheli.deviceInfo[blheli.chan][1] > 0x00) && (blheli.deviceInfo[blheli.chan][1] < 0x90) && (blheli.deviceInfo[blheli.chan][0] == 0x06)) {
842 0 : blheli.interface_mode[blheli.chan] = imARM_BLB;
843 0 : debug("Interface type imARM_BLB");
844 : } else {
845 0 : blheli.ack = ACK_D_GENERAL_ERROR;
846 0 : debug("Unknown interface type 0x%04x", devword);
847 0 : break;
848 : }
849 : }
850 0 : blheli.deviceInfo[blheli.chan][3] = blheli.interface_mode[blheli.chan];
851 0 : if (blheli.interface_mode[blheli.chan] != 0) {
852 0 : blheli.connected[blheli.chan] = true;
853 : }
854 0 : return true;
855 : }
856 :
857 0 : bool AP_BLHeli::BL_SendCMDKeepAlive(void)
858 : {
859 0 : uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
860 0 : if (!BL_SendBuf(sCMD, 2)) {
861 0 : return false;
862 : }
863 0 : if (BL_GetACK() != brERRORCOMMAND) {
864 0 : return false;
865 : }
866 0 : return true;
867 : }
868 :
869 0 : bool AP_BLHeli::BL_PageErase(void)
870 : {
871 0 : if (BL_SendCMDSetAddress()) {
872 0 : uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
873 0 : if (!BL_SendBuf(sCMD, 2)) {
874 0 : return false;
875 : }
876 0 : return BL_GetACK(3000) == brSUCCESS;
877 : }
878 0 : return false;
879 : }
880 :
881 0 : void AP_BLHeli::BL_SendCMDRunRestartBootloader(void)
882 : {
883 0 : uint8_t sCMD[] = {RestartBootloader, 0};
884 0 : blheli.deviceInfo[blheli.chan][0] = 1;
885 0 : BL_SendBuf(sCMD, 2);
886 0 : }
887 :
888 0 : uint8_t AP_BLHeli::BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes)
889 : {
890 0 : uint8_t sCMD[] = {CMD_SET_BUFFER, 0, uint8_t(nbytes>>8), uint8_t(nbytes&0xff)};
891 0 : if (nbytes == 0) {
892 : // set high byte since 0 bytes == 256 bytes in this protocol
893 0 : sCMD[2] = 1;
894 : }
895 0 : if (!BL_SendBuf(sCMD, 4)) {
896 : debug_console("BL_SendCMDSetBuffer send cmd failed");
897 0 : return false;
898 : }
899 0 : uint8_t ack = BL_GetACK(5); // match betaflight timing
900 : // generally no ack returned for CMD_SET_BUFFER when flashing firmware
901 0 : if (ack != brNONE && ack != brSUCCESS) {
902 0 : debug("BL_SendCMDSetBuffer ack failed 0x%02x", ack);
903 0 : blheli.ack = ACK_D_GENERAL_ERROR;
904 0 : return false;
905 : }
906 :
907 0 : if (!BL_SendBuf(buf, nbytes)) {
908 0 : debug("BL_SendCMDSetBuffer send failed");
909 0 : blheli.ack = ACK_D_GENERAL_ERROR;
910 0 : return false;
911 : }
912 0 : return (BL_GetACK(40) == brSUCCESS);
913 : }
914 :
915 0 : bool AP_BLHeli::BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout_ms)
916 : {
917 0 : if (BL_SendCMDSetAddress()) {
918 0 : if (!BL_SendCMDSetBuffer(buf, nbytes)) {
919 : debug_console("BL_SendCMDSetBuffer failed\n");
920 0 : blheli.ack = ACK_D_GENERAL_ERROR;
921 0 : return false;
922 : }
923 0 : uint8_t sCMD[] = {cmd, 0x01};
924 0 : if (!BL_SendBuf(sCMD, 2)) {
925 : debug_console("BL_SendBuf failed\n");
926 0 : return false;
927 : }
928 0 : uint8_t ack = BL_GetACK(timeout_ms);
929 : if (ack != brSUCCESS) {
930 : debug_console("BL_GetACK failed 0x%x\n", ack);
931 : }
932 0 : return (ack == brSUCCESS);
933 : }
934 : debug_console("BL_SendCMDSetAddress failed\n");
935 0 : blheli.ack = ACK_D_GENERAL_ERROR;
936 0 : return false;
937 : }
938 :
939 0 : bool AP_BLHeli::BL_WriteFlash(const uint8_t *buf, uint16_t n)
940 : {
941 0 : return BL_WriteA(CMD_PROG_FLASH, buf, n, 500);
942 : }
943 :
944 0 : bool AP_BLHeli::BL_VerifyFlash(const uint8_t *buf, uint16_t n)
945 : {
946 0 : if (BL_SendCMDSetAddress()) {
947 0 : if (!BL_SendCMDSetBuffer(buf, n)) {
948 0 : return false;
949 : }
950 0 : uint8_t sCMD[] = {CMD_VERIFY_FLASH_ARM, 0x01};
951 0 : if (!BL_SendBuf(sCMD, 2)) {
952 0 : return false;
953 : }
954 0 : uint8_t ack = BL_GetACK(40);
955 0 : switch (ack) {
956 0 : case brSUCCESS:
957 0 : blheli.ack = ACK_OK;
958 0 : break;
959 0 : case brERRORVERIFY:
960 0 : blheli.ack = ACK_I_VERIFY_ERROR;
961 0 : break;
962 0 : default:
963 0 : blheli.ack = ACK_D_GENERAL_ERROR;
964 0 : break;
965 : }
966 0 : return true;
967 : }
968 0 : return false;
969 : }
970 :
971 : /*
972 : process a blheli 4way command from GCS
973 : */
974 0 : void AP_BLHeli::blheli_process_command(void)
975 : {
976 0 : debug("BLHeli cmd 0x%02x len=%u", blheli.command, blheli.param_len);
977 0 : blheli.ack = ACK_OK;
978 0 : switch (blheli.command) {
979 0 : case cmd_InterfaceTestAlive: {
980 0 : debug("cmd_InterfaceTestAlive");
981 0 : if (!isMcuConnected()) {
982 0 : blheli.ack = ACK_D_GENERAL_ERROR;
983 : } else {
984 0 : BL_SendCMDKeepAlive();
985 0 : if (blheli.ack != ACK_OK) {
986 0 : setDisconnected();
987 : }
988 : }
989 0 : uint8_t b = 0;
990 0 : blheli_send_reply(&b, 1);
991 0 : break;
992 : }
993 0 : case cmd_ProtocolGetVersion: {
994 0 : debug("cmd_ProtocolGetVersion");
995 : uint8_t buf[1];
996 0 : buf[0] = SERIAL_4WAY_PROTOCOL_VER;
997 0 : blheli_send_reply(buf, sizeof(buf));
998 0 : break;
999 : }
1000 0 : case cmd_InterfaceGetName: {
1001 0 : debug("cmd_InterfaceGetName");
1002 0 : uint8_t buf[5] = { 4, 'A', 'R', 'D', 'U' };
1003 0 : blheli_send_reply(buf, sizeof(buf));
1004 0 : break;
1005 : }
1006 0 : case cmd_InterfaceGetVersion: {
1007 0 : debug("cmd_InterfaceGetVersion");
1008 0 : uint8_t buf[2] = { SERIAL_4WAY_VERSION_HI, SERIAL_4WAY_VERSION_LO };
1009 0 : blheli_send_reply(buf, sizeof(buf));
1010 0 : break;
1011 : }
1012 0 : case cmd_InterfaceExit: {
1013 0 : debug("cmd_InterfaceExit");
1014 0 : msp.escMode = PROTOCOL_NONE;
1015 0 : uint8_t b = 0;
1016 0 : blheli_send_reply(&b, 1);
1017 0 : serial_end();
1018 0 : if (motors_disabled) {
1019 0 : motors_disabled = false;
1020 0 : SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
1021 : }
1022 0 : if (uart_locked) {
1023 0 : debug("Unlocked UART");
1024 0 : uart->lock_port(0, 0);
1025 0 : uart_locked = false;
1026 : }
1027 0 : memset(blheli.connected, 0, sizeof(blheli.connected));
1028 0 : break;
1029 : }
1030 0 : case cmd_DeviceReset: {
1031 0 : debug("cmd_DeviceReset(%u)", unsigned(blheli.buf[0]));
1032 0 : if (blheli.buf[0] >= num_motors) {
1033 0 : debug("bad reset channel %u", blheli.buf[0]);
1034 0 : blheli.ack = ACK_I_INVALID_CHANNEL;
1035 0 : blheli_send_reply(&blheli.buf[0], 1);
1036 0 : break;
1037 : }
1038 0 : blheli.chan = blheli.buf[0];
1039 0 : switch (blheli.interface_mode[blheli.chan]) {
1040 0 : case imSIL_BLB:
1041 : case imATM_BLB:
1042 : case imARM_BLB:
1043 0 : BL_SendCMDRunRestartBootloader();
1044 0 : break;
1045 0 : case imSK:
1046 0 : break;
1047 : }
1048 0 : blheli_send_reply(&blheli.chan, 1);
1049 0 : setDisconnected();
1050 0 : break;
1051 : }
1052 :
1053 0 : case cmd_DeviceInitFlash: {
1054 0 : uint8_t chan = blheli.buf[0];
1055 :
1056 0 : debug("cmd_DeviceInitFlash(%u)", unsigned(blheli.buf[0]));
1057 0 : if (blheli.buf[0] >= num_motors) {
1058 0 : debug("bad channel %u", blheli.buf[0]);
1059 0 : blheli.ack = ACK_I_INVALID_CHANNEL;
1060 0 : blheli_send_reply(&chan, 1);
1061 0 : break;
1062 : }
1063 : // betaflight tries three times to connect, this avoids the need to wait some arbitrary
1064 : // period for the interface to be up.
1065 0 : bool failed = true;
1066 0 : for (uint8_t i = 0; i<3; i++) {
1067 0 : blheli.chan = chan;
1068 0 : blheli.ack = ACK_OK;
1069 0 : if (BL_ConnectEx()) {
1070 0 : uint8_t buf[4] = {blheli.deviceInfo[blheli.chan][0],
1071 0 : blheli.deviceInfo[blheli.chan][1],
1072 0 : blheli.deviceInfo[blheli.chan][2],
1073 0 : blheli.deviceInfo[blheli.chan][3]}; // device ID
1074 0 : blheli_send_reply(buf, sizeof(buf));
1075 0 : failed = false;
1076 0 : break;
1077 : }
1078 : }
1079 :
1080 0 : if (failed) {
1081 0 : blheli.ack = ACK_D_GENERAL_ERROR;
1082 0 : blheli_send_reply(&chan, 1);
1083 0 : setDisconnected();
1084 : }
1085 0 : break;
1086 : }
1087 :
1088 0 : case cmd_InterfaceSetMode: {
1089 0 : debug("cmd_InterfaceSetMode(%u)", unsigned(blheli.buf[0]));
1090 0 : blheli.interface_mode[blheli.chan] = blheli.buf[0];
1091 0 : blheli_send_reply(&blheli.interface_mode[blheli.chan], 1);
1092 0 : break;
1093 : }
1094 :
1095 0 : case cmd_DeviceRead: {
1096 0 : uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256;
1097 0 : debug("cmd_DeviceRead(%u) n=%u", blheli.chan, nbytes);
1098 0 : uint8_t buf[nbytes];
1099 0 : uint8_t cmd = blheli.interface_mode[blheli.chan]==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL;
1100 0 : if (!BL_ReadA(cmd, buf, nbytes)) {
1101 0 : nbytes = 1;
1102 : }
1103 0 : blheli_send_reply(buf, nbytes);
1104 0 : break;
1105 0 : }
1106 :
1107 0 : case cmd_DevicePageErase: {
1108 0 : uint8_t page = blheli.buf[0];
1109 0 : debug("cmd_DevicePageErase(%u) im=%u", page, blheli.interface_mode[blheli.chan]);
1110 0 : switch (blheli.interface_mode[blheli.chan]) {
1111 0 : case imSIL_BLB:
1112 : case imARM_BLB: {
1113 0 : if (blheli.interface_mode[blheli.chan] == imARM_BLB) {
1114 : // Address =Page * 1024
1115 0 : blheli.address = page << 10;
1116 : } else {
1117 : // Address =Page * 512
1118 0 : blheli.address = page << 9;
1119 : }
1120 0 : debug("ARM PageErase 0x%04x", blheli.address);
1121 0 : BL_PageErase();
1122 0 : blheli.address = 0;
1123 0 : blheli_send_reply(&page, 1);
1124 0 : break;
1125 : }
1126 0 : default:
1127 0 : blheli.ack = ACK_I_INVALID_CMD;
1128 0 : blheli_send_reply(&page, 1);
1129 0 : break;
1130 : }
1131 0 : break;
1132 : }
1133 :
1134 0 : case cmd_DeviceWrite: {
1135 0 : uint16_t nbytes = blheli.param_len;
1136 0 : debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]);
1137 0 : uint8_t buf[nbytes];
1138 0 : memcpy(buf, blheli.buf, nbytes);
1139 0 : switch (blheli.interface_mode[blheli.chan]) {
1140 0 : case imSIL_BLB:
1141 : case imATM_BLB:
1142 : case imARM_BLB: {
1143 0 : if (!BL_WriteFlash(buf, nbytes)) {
1144 : // For reasons unknown BL_WriteFlash can bork the DMA engine, make some attempt to get
1145 : // back to a sane state if this happens. We can't call serial_end() as that will
1146 : // restart dshot output
1147 : debug_console("cmd_DeviceWrite failed 0x%x: %u bytes", blheli.address, nbytes);
1148 0 : hal.rcout->serial_reset(motor_mask);
1149 : }
1150 0 : break;
1151 : }
1152 0 : case imSK: {
1153 0 : debug("Unsupported flash mode imSK");
1154 0 : break;
1155 : }
1156 : }
1157 0 : uint8_t b=0;
1158 0 : blheli_send_reply(&b, 1);
1159 0 : break;
1160 0 : }
1161 :
1162 0 : case cmd_DeviceVerify: {
1163 0 : uint16_t nbytes = blheli.param_len;
1164 0 : debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]);
1165 0 : switch (blheli.interface_mode[blheli.chan]) {
1166 0 : case imARM_BLB: {
1167 0 : uint8_t buf[nbytes];
1168 0 : memcpy(buf, blheli.buf, nbytes);
1169 0 : BL_VerifyFlash(buf, nbytes);
1170 0 : break;
1171 0 : }
1172 0 : default:
1173 0 : blheli.ack = ACK_I_INVALID_CMD;
1174 0 : break;
1175 : }
1176 0 : uint8_t b=0;
1177 0 : blheli_send_reply(&b, 1);
1178 0 : break;
1179 : }
1180 :
1181 0 : case cmd_DeviceReadEEprom: {
1182 0 : uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256;
1183 0 : uint8_t buf[nbytes];
1184 0 : debug("cmd_DeviceReadEEprom n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]);
1185 0 : switch (blheli.interface_mode[blheli.chan]) {
1186 0 : case imATM_BLB: {
1187 0 : if (!BL_ReadA(CMD_READ_EEPROM, buf, nbytes)) {
1188 0 : blheli.ack = ACK_D_GENERAL_ERROR;
1189 : }
1190 0 : break;
1191 : }
1192 0 : default:
1193 0 : blheli.ack = ACK_I_INVALID_CMD;
1194 0 : break;
1195 : }
1196 0 : if (blheli.ack != ACK_OK) {
1197 0 : nbytes = 1;
1198 0 : buf[0] = 0;
1199 : }
1200 0 : blheli_send_reply(buf, nbytes);
1201 0 : break;
1202 0 : }
1203 :
1204 0 : case cmd_DeviceWriteEEprom: {
1205 0 : uint16_t nbytes = blheli.param_len;
1206 0 : uint8_t buf[nbytes];
1207 0 : memcpy(buf, blheli.buf, nbytes);
1208 0 : debug("cmd_DeviceWriteEEprom n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]);
1209 0 : switch (blheli.interface_mode[blheli.chan]) {
1210 0 : case imATM_BLB:
1211 0 : BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 3000);
1212 0 : break;
1213 0 : default:
1214 0 : blheli.ack = ACK_D_GENERAL_ERROR;
1215 0 : break;
1216 : }
1217 0 : uint8_t b = 0;
1218 0 : blheli_send_reply(&b, 1);
1219 0 : break;
1220 0 : }
1221 :
1222 0 : case cmd_DeviceEraseAll:
1223 : case cmd_DeviceC2CK_LOW:
1224 : default:
1225 : // ack=unknown command
1226 0 : blheli.ack = ACK_I_INVALID_CMD;
1227 0 : debug("Unknown BLHeli protocol 0x%02x", blheli.command);
1228 0 : uint8_t b = 0;
1229 0 : blheli_send_reply(&b, 1);
1230 0 : break;
1231 : }
1232 0 : }
1233 :
1234 : /*
1235 : process an input byte, return true if we have received a whole
1236 : packet with correct CRC
1237 : */
1238 0 : bool AP_BLHeli::process_input(uint8_t b)
1239 : {
1240 0 : bool valid_packet = false;
1241 :
1242 0 : if (msp.escMode == PROTOCOL_4WAY && blheli.state == BLHELI_IDLE && b == '$') {
1243 0 : debug("Change to MSP mode");
1244 0 : msp.escMode = PROTOCOL_NONE;
1245 0 : serial_end();
1246 : }
1247 0 : if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') {
1248 0 : debug("Change to BLHeli mode");
1249 0 : memset(blheli.connected, 0, sizeof(blheli.connected));
1250 0 : msp.escMode = PROTOCOL_4WAY;
1251 : }
1252 0 : if (msp.escMode == PROTOCOL_4WAY) {
1253 0 : blheli_4way_process_byte(b);
1254 : } else {
1255 0 : msp_process_byte(b);
1256 : }
1257 0 : if (msp.escMode == PROTOCOL_4WAY) {
1258 0 : if (blheli.state == BLHELI_COMMAND_RECEIVED) {
1259 0 : valid_packet = true;
1260 0 : last_valid_ms = AP_HAL::millis();
1261 0 : if (uart->lock_port(BLHELI_UART_LOCK_KEY, 0)) {
1262 0 : uart_locked = true;
1263 : }
1264 0 : blheli_process_command();
1265 0 : blheli.state = BLHELI_IDLE;
1266 0 : msp.state = MSP_IDLE;
1267 : }
1268 0 : } else if (msp.state == MSP_COMMAND_RECEIVED) {
1269 0 : if (msp.packetType == MSP_PACKET_COMMAND) {
1270 0 : valid_packet = true;
1271 0 : if (uart->lock_port(BLHELI_UART_LOCK_KEY, 0)) {
1272 0 : uart_locked = true;
1273 : }
1274 0 : last_valid_ms = AP_HAL::millis();
1275 0 : msp_process_command();
1276 : }
1277 0 : msp.state = MSP_IDLE;
1278 0 : blheli.state = BLHELI_IDLE;
1279 : }
1280 :
1281 0 : return valid_packet;
1282 : }
1283 :
1284 : /*
1285 : protocol handler for detecting BLHeli input
1286 : */
1287 0 : bool AP_BLHeli::protocol_handler(uint8_t b, AP_HAL::UARTDriver *_uart)
1288 : {
1289 0 : uart = _uart;
1290 0 : if (hal.util->get_soft_armed()) {
1291 : // don't allow MSP control when armed
1292 0 : return false;
1293 : }
1294 0 : return process_input(b);
1295 : }
1296 :
1297 : /*
1298 : run a connection test to the ESCs. This is used to test the
1299 : operation of the BLHeli ESC protocol
1300 : */
1301 0 : void AP_BLHeli::run_connection_test(uint8_t chan)
1302 : {
1303 0 : run_test.set_and_notify(0);
1304 0 : debug_uart = hal.console;
1305 0 : uint8_t saved_chan = blheli.chan;
1306 0 : if (chan >= num_motors) {
1307 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: bad channel %u", chan);
1308 0 : return;
1309 : }
1310 0 : blheli.chan = chan;
1311 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Running test on channel %u", blheli.chan);
1312 0 : bool passed = false;
1313 0 : for (uint8_t tries=0; tries<5; tries++) {
1314 0 : EXPECT_DELAY_MS(3000);
1315 0 : blheli.ack = ACK_OK;
1316 0 : setDisconnected();
1317 0 : if (BL_ConnectEx()) {
1318 : uint8_t buf[256];
1319 0 : uint8_t cmd = blheli.interface_mode[blheli.chan]==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL;
1320 0 : passed = true;
1321 0 : blheli.address = blheli.interface_mode[blheli.chan]==imATM_BLB?0:0x7c00;
1322 0 : passed &= BL_ReadA(cmd, buf, sizeof(buf));
1323 0 : if (blheli.interface_mode[blheli.chan]==imARM_BLB) {
1324 0 : if (passed) {
1325 : // read status structure
1326 0 : blheli.address = esc_status_addr;
1327 0 : passed &= BL_SendCMDSetAddress();
1328 : }
1329 0 : if (passed) {
1330 : struct esc_status status;
1331 0 : passed &= BL_ReadA(CMD_READ_FLASH_SIL, (uint8_t *)&status, sizeof(status));
1332 : }
1333 : }
1334 0 : BL_SendCMDRunRestartBootloader();
1335 0 : break;
1336 : }
1337 0 : }
1338 0 : serial_end();
1339 0 : SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
1340 0 : motors_disabled = false;
1341 0 : blheli.chan = saved_chan;
1342 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Test %s", passed?"PASSED":"FAILED");
1343 0 : debug_uart = nullptr;
1344 : }
1345 :
1346 : /*
1347 : update BLHeli
1348 : */
1349 151226 : void AP_BLHeli::update(void)
1350 : {
1351 151226 : bool motor_control_active = false;
1352 151226 : for (uint8_t i = 0; i < num_motors; i++) {
1353 0 : bool reversed = ((1U<< motor_map[i]) & channel_reversible_mask.get()) != 0;
1354 0 : if (hal.rcout->read( motor_map[i]) != (reversed ? 1500 : 1000)) {
1355 0 : motor_control_active = true;
1356 : }
1357 : }
1358 :
1359 151226 : uint32_t now = AP_HAL::millis();
1360 151226 : if (initialised && uart_locked &&
1361 0 : ((timeout_sec && now - last_valid_ms > uint32_t(timeout_sec.get())*1000U) ||
1362 0 : (motor_control_active && now - last_valid_ms > MOTOR_ACTIVE_TIMEOUT))) {
1363 : // we're not processing requests any more, shutdown serial
1364 : // output
1365 0 : if (serial_start_ms) {
1366 0 : serial_end();
1367 : }
1368 0 : if (motors_disabled) {
1369 0 : motors_disabled = false;
1370 0 : SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
1371 : }
1372 0 : if (uart != nullptr) {
1373 0 : debug("Unlocked UART");
1374 0 : uart->lock_port(0, 0);
1375 0 : uart_locked = false;
1376 : }
1377 0 : if (motor_control_active) {
1378 0 : for (uint8_t i = 0; i < num_motors; i++) {
1379 0 : bool reversed = ((1U<<motor_map[i]) & channel_reversible_mask.get()) != 0;
1380 0 : hal.rcout->write(motor_map[i], reversed ? 1500 : 1000);
1381 : }
1382 : }
1383 : }
1384 :
1385 151226 : if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) {
1386 151226 : if (initialised && run_test.get() > 0) {
1387 0 : run_connection_test(run_test.get() - 1);
1388 : }
1389 : }
1390 151226 : }
1391 :
1392 0 : void AP_BLHeli::serial_end()
1393 : {
1394 0 : hal.rcout->serial_end(motor_mask);
1395 0 : serial_start_ms = 0;
1396 0 : }
1397 :
1398 : /*
1399 : Initialize BLHeli, called by SRV_Channels::init()
1400 : Used to install protocol handler
1401 : The motor mask of enabled motors can be passed in
1402 : */
1403 2086 : void AP_BLHeli::init(uint32_t mask, AP_HAL::RCOutput::output_mode otype)
1404 : {
1405 2086 : initialised = true;
1406 :
1407 2086 : run_test.set_and_notify(0);
1408 :
1409 : #if HAL_GCS_ENABLED
1410 : // only install pass-thru protocol handler if either auto or the motor mask are set
1411 2086 : if (channel_mask.get() != 0 || channel_auto.get() != 0) {
1412 0 : if (last_control_port > 0 && last_control_port != control_port) {
1413 0 : gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+last_control_port), nullptr);
1414 0 : last_control_port = -1;
1415 : }
1416 0 : if (gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+control_port),
1417 : FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler,
1418 : bool, uint8_t, AP_HAL::UARTDriver *))) {
1419 0 : debug("BLHeli installed on port %u", (unsigned)control_port);
1420 0 : last_control_port = control_port;
1421 : }
1422 : }
1423 : #endif // HAL_GCS_ENABLED
1424 :
1425 : #if HAL_WITH_IO_MCU
1426 : if (AP_BoardConfig::io_enabled()) {
1427 : // with IOMCU the local (FMU) channels start at 8
1428 : chan_offset = 8;
1429 : }
1430 : #endif
1431 :
1432 2086 : mask |= uint32_t(channel_mask.get());
1433 :
1434 : /*
1435 : allow mode override - this makes it possible to use DShot for
1436 : rovers and subs, plus for quadplane fwd motors
1437 : */
1438 : // +1 converts from AP_Motors::pwm_type to AP_HAL::RCOutput::output_mode and saves doing a param conversion
1439 : // this is the only use of the param, but this is still a bit of a hack
1440 2086 : const int16_t type = output_type.get() + 1;
1441 2086 : if (otype == AP_HAL::RCOutput::MODE_PWM_NONE) {
1442 2086 : otype = ((type > AP_HAL::RCOutput::MODE_PWM_NONE) && (type < AP_HAL::RCOutput::MODE_NEOPIXEL)) ? AP_HAL::RCOutput::output_mode(type) : AP_HAL::RCOutput::MODE_PWM_NONE;
1443 : }
1444 2086 : switch (otype) {
1445 0 : case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
1446 : case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:
1447 : case AP_HAL::RCOutput::MODE_PWM_BRUSHED:
1448 : case AP_HAL::RCOutput::MODE_PWM_DSHOT150:
1449 : case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
1450 : case AP_HAL::RCOutput::MODE_PWM_DSHOT600:
1451 : case AP_HAL::RCOutput::MODE_PWM_DSHOT1200:
1452 0 : if (mask) {
1453 0 : hal.rcout->set_output_mode(mask, otype);
1454 : }
1455 0 : break;
1456 2086 : default:
1457 2086 : break;
1458 : }
1459 :
1460 2086 : uint32_t digital_mask = 0;
1461 : // setting the digital mask changes the min/max PWM values
1462 : // it's important that this is NOT done for non-digital channels as otherwise
1463 : // PWM min can result in motors turning. set for individual overrides first
1464 2086 : if (mask && hal.rcout->is_dshot_protocol(otype)) {
1465 0 : digital_mask = mask;
1466 : }
1467 :
1468 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
1469 : /*
1470 : plane and copter can use AP_Motors to get an automatic mask
1471 : */
1472 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
1473 283 : AP_MotorsUGV *motors = AP::motors_ugv();
1474 : #else
1475 1689 : AP_Motors *motors = AP::motors();
1476 : #endif
1477 1972 : if (motors) {
1478 1482 : uint32_t motormask = motors->get_motor_mask();
1479 : // set the rest of the digital channels
1480 1482 : if (motors->is_digital_pwm_type()) {
1481 0 : digital_mask |= motormask;
1482 : }
1483 1482 : mask |= motormask;
1484 : }
1485 : #endif
1486 : // tell SRV_Channels about ESC capabilities
1487 2086 : SRV_Channels::set_digital_outputs(digital_mask, uint32_t(channel_reversible_mask.get()) & digital_mask);
1488 : // the dshot ESC type is required in order to send the reversed/reversible dshot command correctly
1489 2086 : hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
1490 2086 : hal.rcout->set_reversible_mask(uint32_t(channel_reversible_mask.get()) & digital_mask);
1491 2086 : hal.rcout->set_reversed_mask(uint32_t(channel_reversed_mask.get()) & digital_mask);
1492 : #ifdef HAL_WITH_BIDIR_DSHOT
1493 : // possibly enable bi-directional dshot
1494 : hal.rcout->set_motor_poles(motor_poles);
1495 : #endif
1496 : #if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT
1497 : hal.rcout->set_bidir_dshot_mask(uint32_t(channel_bidir_dshot_mask.get()) & digital_mask);
1498 : #endif
1499 : // add motors from channel mask
1500 35462 : for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
1501 33376 : if (digital_mask & (1U<<i)) {
1502 0 : motor_map[num_motors] = i;
1503 0 : num_motors++;
1504 : }
1505 : }
1506 2086 : motor_mask = mask;
1507 2086 : debug("%u motors mask=0x%08lx", num_motors, (unsigned long)digital_mask);
1508 :
1509 : // check if we have a combination of reversible and normal
1510 2086 : mixed_type = (mask != (mask & channel_reversible_mask.get())) && (channel_reversible_mask.get() != 0);
1511 :
1512 2086 : if (num_motors != 0 && telem_rate > 0) {
1513 0 : AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
1514 0 : if (serial_manager) {
1515 0 : telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0);
1516 : }
1517 : }
1518 2086 : }
1519 :
1520 : /*
1521 : read an ESC telemetry packet
1522 : */
1523 0 : void AP_BLHeli::read_telemetry_packet(void)
1524 : {
1525 : #if HAL_WITH_ESC_TELEM
1526 : uint8_t buf[telem_packet_size];
1527 0 : if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) {
1528 : // short read, we should have 10 bytes ready when this function is called
1529 0 : return;
1530 : }
1531 :
1532 : // calculate crc
1533 0 : uint8_t crc = 0;
1534 0 : for (uint8_t i=0; i<telem_packet_size-1; i++) {
1535 0 : crc = crc8_dvb(buf[i], crc, 0x07);
1536 : }
1537 :
1538 0 : if (buf[telem_packet_size-1] != crc) {
1539 : // bad crc
1540 0 : debug("Bad CRC on %u", last_telem_esc);
1541 0 : return;
1542 : }
1543 : // record the previous rpm so that we can slew to the new one
1544 0 : uint16_t new_rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
1545 0 : const uint8_t motor_idx = motor_map[last_telem_esc];
1546 : // we have received valid data, mark the ESC as now active
1547 0 : hal.rcout->set_active_escs_mask(1<<motor_idx);
1548 0 : update_rpm(motor_idx, new_rpm);
1549 :
1550 0 : TelemetryData t {
1551 0 : .temperature_cdeg = int16_t(buf[0] * 100),
1552 0 : .voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
1553 0 : .current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
1554 0 : .consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
1555 0 : };
1556 :
1557 0 : update_telem_data(motor_idx, t,
1558 : AP_ESC_Telem_Backend::TelemetryType::CURRENT
1559 : | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
1560 : | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
1561 : | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
1562 :
1563 0 : if (debug_level >= 2) {
1564 0 : uint16_t trpm = new_rpm;
1565 0 : if (has_bidir_dshot(last_telem_esc)) {
1566 0 : trpm = hal.rcout->get_erpm(motor_idx);
1567 0 : if (trpm != 0xFFFF) {
1568 0 : trpm = trpm * 200 / motor_poles;
1569 : }
1570 : }
1571 0 : DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
1572 : last_telem_esc,
1573 : t.temperature_cdeg,
1574 : t.voltage,
1575 : t.current,
1576 : t.consumption_mah,
1577 : trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
1578 : }
1579 : #endif // HAL_WITH_ESC_TELEM
1580 : }
1581 :
1582 : /*
1583 : log bidir telemetry - only called if BLH telemetry is not active
1584 : */
1585 0 : void AP_BLHeli::log_bidir_telemetry(void)
1586 : {
1587 0 : uint32_t now = AP_HAL::millis();
1588 :
1589 0 : if (debug_level >= 2 && now - last_log_ms[last_telem_esc] > 100) {
1590 0 : if (has_bidir_dshot(last_telem_esc)) {
1591 0 : const uint8_t motor_idx = motor_map[last_telem_esc];
1592 0 : uint16_t trpm = hal.rcout->get_erpm(motor_idx);
1593 0 : if (trpm != 0xFFFF) { // don't log invalid values as they are never used
1594 0 : trpm = trpm * 200 / motor_poles;
1595 : }
1596 :
1597 0 : if (trpm > 0) {
1598 0 : last_log_ms[last_telem_esc] = now;
1599 0 : DEV_PRINTF("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
1600 : }
1601 : }
1602 : }
1603 :
1604 0 : if (!SRV_Channels::have_digital_outputs()) {
1605 0 : return;
1606 : }
1607 :
1608 : // ask the next ESC for telemetry
1609 0 : uint8_t idx_pos = last_telem_esc;
1610 0 : uint8_t idx = (idx_pos + 1) % num_motors;
1611 0 : for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
1612 0 : if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
1613 0 : break;
1614 : }
1615 : }
1616 0 : if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
1617 0 : last_telem_esc = idx;
1618 : }
1619 : }
1620 :
1621 : /*
1622 : update BLHeli telemetry handling
1623 : This is called on push() in SRV_Channels
1624 : */
1625 41692164 : void AP_BLHeli::update_telemetry(void)
1626 : {
1627 : #ifdef HAL_WITH_BIDIR_DSHOT
1628 : // we might only have bi-dir dshot
1629 : if (channel_bidir_dshot_mask.get() != 0 && !telem_uart) {
1630 : log_bidir_telemetry();
1631 : }
1632 : #endif
1633 41692164 : if (!telem_uart || !SRV_Channels::have_digital_outputs()) {
1634 41692164 : return;
1635 : }
1636 0 : uint32_t now = AP_HAL::micros();
1637 0 : uint32_t telem_rate_us = 1000000U / uint32_t(telem_rate.get() * num_motors);
1638 0 : if (telem_rate_us < 2000) {
1639 : // make sure we have a gap between frames
1640 0 : telem_rate_us = 2000;
1641 : }
1642 0 : if (!telem_uart_started || !telem_uart->is_owned_by_current_thread()) {
1643 : // we need to use begin() here to ensure the correct thread owns the uart
1644 0 : telem_uart->begin(115200);
1645 0 : telem_uart_started = true;
1646 : }
1647 :
1648 0 : uint32_t nbytes = telem_uart->available();
1649 :
1650 0 : if (nbytes > telem_packet_size) {
1651 : // if we have more than 10 bytes then we don't know which ESC
1652 : // they are from. Throw them all away
1653 0 : telem_uart->discard_input();
1654 0 : return;
1655 : }
1656 0 : if (nbytes > 0 &&
1657 0 : nbytes < telem_packet_size &&
1658 0 : (last_telem_byte_read_us == 0 ||
1659 0 : now - last_telem_byte_read_us < 1000)) {
1660 : // wait a bit longer, we don't have enough bytes yet
1661 0 : if (last_telem_byte_read_us == 0) {
1662 0 : last_telem_byte_read_us = now;
1663 : }
1664 0 : return;
1665 : }
1666 0 : if (nbytes > 0 && nbytes < telem_packet_size) {
1667 : // we've waited long enough, discard bytes if we don't have 10 yet
1668 0 : telem_uart->discard_input();
1669 0 : return;
1670 : }
1671 0 : if (nbytes == telem_packet_size) {
1672 : // we have a full packet ready to parse
1673 0 : read_telemetry_packet();
1674 0 : last_telem_byte_read_us = 0;
1675 : }
1676 : // we need to keep requesting telemetry even if we don't receive anything
1677 : // as the request mask will be reset next cycle.
1678 0 : if (now - last_telem_request_us >= telem_rate_us) {
1679 : // ask the next ESC for telemetry
1680 0 : uint8_t idx_pos = last_telem_esc;
1681 0 : uint8_t idx = (idx_pos + 1) % num_motors;
1682 0 : for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
1683 0 : if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
1684 0 : break;
1685 : }
1686 : }
1687 0 : uint32_t mask = 1U << motor_map[idx];
1688 0 : if (SRV_Channels::have_digital_outputs(mask)) {
1689 0 : hal.rcout->set_telem_request_mask(mask);
1690 0 : last_telem_esc = idx;
1691 0 : last_telem_request_us = now;
1692 : }
1693 : }
1694 : }
1695 :
1696 : #endif // HAVE_AP_BLHELI_SUPPORT
|
