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 implementation
20 : */
21 :
22 : #pragma once
23 :
24 : #include <AP_Common/AP_Common.h>
25 : #include <AP_HAL/AP_HAL.h>
26 :
27 : #define HAVE_AP_BLHELI_SUPPORT HAL_SUPPORT_RCOUT_SERIAL
28 :
29 : #if HAL_SUPPORT_RCOUT_SERIAL
30 :
31 : #include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
32 :
33 : #include <AP_Param/AP_Param.h>
34 : #include <Filter/LowPassFilter.h>
35 : #include <AP_MSP/msp_protocol.h>
36 : #include "blheli_4way_protocol.h"
37 :
38 : #define AP_BLHELI_MAX_ESCS 8
39 :
40 : class AP_BLHeli : public AP_ESC_Telem_Backend {
41 :
42 : public:
43 : AP_BLHeli();
44 :
45 : void update(void);
46 : void init(uint32_t motor_mask, AP_HAL::RCOutput::output_mode mode);
47 : void update_telemetry(void);
48 : bool process_input(uint8_t b);
49 :
50 : static const struct AP_Param::GroupInfo var_info[];
51 :
52 0 : bool has_bidir_dshot(uint8_t esc_index) const {
53 0 : return channel_bidir_dshot_mask.get() & (1U << motor_map[esc_index]);
54 : }
55 :
56 : uint32_t get_bidir_dshot_mask() const { return channel_bidir_dshot_mask.get(); }
57 : uint8_t get_motor_poles() const { return motor_poles.get(); }
58 : uint16_t get_telemetry_rate() const { return telem_rate.get(); }
59 :
60 : static AP_BLHeli *get_singleton(void) {
61 : return _singleton;
62 : }
63 :
64 : private:
65 : static AP_BLHeli *_singleton;
66 :
67 : // mask of channels to use for BLHeli protocol
68 : AP_Int32 channel_mask;
69 : AP_Int32 channel_reversible_mask;
70 : AP_Int32 channel_reversed_mask;
71 : AP_Int8 channel_auto;
72 : AP_Int8 run_test;
73 : AP_Int16 timeout_sec;
74 : AP_Int16 telem_rate;
75 : AP_Int8 debug_level;
76 : AP_Int8 output_type;
77 : AP_Int8 control_port;
78 : AP_Int8 motor_poles;
79 : // mask of channels with bi-directional dshot enabled
80 : AP_Int32 channel_bidir_dshot_mask;
81 :
82 : enum mspState {
83 : MSP_IDLE=0,
84 : MSP_HEADER_START,
85 : MSP_HEADER_M,
86 : MSP_HEADER_ARROW,
87 : MSP_HEADER_SIZE,
88 : MSP_HEADER_CMD,
89 : MSP_COMMAND_RECEIVED
90 : };
91 :
92 : enum mspPacketType {
93 : MSP_PACKET_COMMAND,
94 : MSP_PACKET_REPLY
95 : };
96 :
97 : enum escProtocol {
98 : PROTOCOL_SIMONK = 0,
99 : PROTOCOL_BLHELI = 1,
100 : PROTOCOL_KISS = 2,
101 : PROTOCOL_KISSALL = 3,
102 : PROTOCOL_CASTLE = 4,
103 : PROTOCOL_MAX = 5,
104 : PROTOCOL_NONE = 0xfe,
105 : PROTOCOL_4WAY = 0xff
106 : };
107 :
108 : enum motorPwmProtocol {
109 : PWM_TYPE_STANDARD = 0,
110 : PWM_TYPE_ONESHOT125,
111 : PWM_TYPE_ONESHOT42,
112 : PWM_TYPE_MULTISHOT,
113 : PWM_TYPE_BRUSHED,
114 : PWM_TYPE_DSHOT150,
115 : PWM_TYPE_DSHOT300,
116 : PWM_TYPE_DSHOT600,
117 : PWM_TYPE_DSHOT1200,
118 : PWM_TYPE_PROSHOT1000,
119 : };
120 :
121 : enum MSPFeatures {
122 : FEATURE_RX_PPM = 1 << 0,
123 : FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
124 : FEATURE_RX_SERIAL = 1 << 3,
125 : FEATURE_MOTOR_STOP = 1 << 4,
126 : FEATURE_SERVO_TILT = 1 << 5,
127 : FEATURE_SOFTSERIAL = 1 << 6,
128 : FEATURE_GPS = 1 << 7,
129 : FEATURE_RANGEFINDER = 1 << 9,
130 : FEATURE_TELEMETRY = 1 << 10,
131 : FEATURE_3D = 1 << 12,
132 : FEATURE_RX_PARALLEL_PWM = 1 << 13,
133 : FEATURE_RX_MSP = 1 << 14,
134 : FEATURE_RSSI_ADC = 1 << 15,
135 : FEATURE_LED_STRIP = 1 << 16,
136 : FEATURE_DASHBOARD = 1 << 17,
137 : FEATURE_OSD = 1 << 18,
138 : FEATURE_CHANNEL_FORWARDING = 1 << 20,
139 : FEATURE_TRANSPONDER = 1 << 21,
140 : FEATURE_AIRMODE = 1 << 22,
141 : FEATURE_RX_SPI = 1 << 25,
142 : FEATURE_SOFTSPI = 1 << 26,
143 : FEATURE_ESC_SENSOR = 1 << 27,
144 : FEATURE_ANTI_GRAVITY = 1 << 28,
145 : FEATURE_DYNAMIC_FILTER = 1 << 29,
146 : };
147 :
148 :
149 : /*
150 : state of MSP command processing
151 : */
152 : struct {
153 : enum mspState state;
154 : enum mspPacketType packetType;
155 : uint8_t offset;
156 : uint8_t dataSize;
157 : uint8_t checksum;
158 : uint8_t buf[192];
159 : uint8_t cmdMSP;
160 : enum escProtocol escMode;
161 : uint8_t portIndex;
162 : } msp;
163 :
164 : enum blheliState {
165 : BLHELI_IDLE=0,
166 : BLHELI_HEADER_START,
167 : BLHELI_HEADER_CMD,
168 : BLHELI_HEADER_ADDR_LOW,
169 : BLHELI_HEADER_ADDR_HIGH,
170 : BLHELI_HEADER_LEN,
171 : BLHELI_CRC1,
172 : BLHELI_CRC2,
173 : BLHELI_COMMAND_RECEIVED
174 : };
175 :
176 : /*
177 : state of blheli 4way protocol handling
178 : */
179 : struct {
180 : enum blheliState state;
181 : uint8_t command;
182 : uint16_t address;
183 : uint16_t param_len;
184 : uint16_t offset;
185 : uint8_t buf[256+3+8];
186 : uint8_t crc1;
187 : uint16_t crc;
188 : bool connected[AP_BLHELI_MAX_ESCS];
189 : uint8_t interface_mode[AP_BLHELI_MAX_ESCS];
190 : uint8_t deviceInfo[AP_BLHELI_MAX_ESCS][4];
191 : uint8_t chan;
192 : uint8_t ack;
193 : } blheli;
194 :
195 : static constexpr uint16_t esc_status_addr = 0xEB00;
196 :
197 : // protocol reported by ESC in esc_status
198 : enum esc_protocol {
199 : ESC_PROTOCOL_NONE=0,
200 : ESC_PROTOCOL_NORMAL=1,
201 : ESC_PROTOCOL_ONESHOT125=2,
202 : ESC_PROTOCOL_DSHOT=5,
203 : };
204 :
205 : // ESC status structure at address 0xEB00
206 : struct PACKED esc_status {
207 : uint8_t unknown[3];
208 : enum esc_protocol protocol;
209 : uint32_t good_frames;
210 : uint32_t bad_frames;
211 : uint32_t unknown2;
212 : };
213 :
214 :
215 : AP_HAL::UARTDriver *uart;
216 : AP_HAL::UARTDriver *debug_uart;
217 : AP_HAL::UARTDriver *telem_uart;
218 :
219 : static const uint8_t max_motors = AP_BLHELI_MAX_ESCS;
220 : uint8_t num_motors;
221 :
222 : // last log output to avoid beat frequencies
223 : uint32_t last_log_ms[max_motors];
224 :
225 : // have we initialised the interface?
226 : bool initialised;
227 :
228 : // last valid packet timestamp
229 : uint32_t last_valid_ms;
230 :
231 : // when did we start the serial ESC output?
232 : uint32_t serial_start_ms;
233 :
234 : // have we disabled motor outputs?
235 : bool motors_disabled;
236 : // mask of channels that should normally be disabled
237 : uint32_t motors_disabled_mask;
238 :
239 : // have we locked the UART?
240 : bool uart_locked;
241 :
242 : // true if we have a mix of reversible and normal ESC
243 : bool mixed_type;
244 :
245 : // mapping from BLHeli motor numbers to RC output channels
246 : uint8_t motor_map[max_motors];
247 : uint32_t motor_mask;
248 :
249 : // convert between servo number and FMU channel number for ESC telemetry
250 : uint8_t chan_offset;
251 :
252 : // when did we last request telemetry?
253 : uint32_t last_telem_request_us;
254 : uint8_t last_telem_esc;
255 : static const uint8_t telem_packet_size = 10;
256 : bool telem_uart_started;
257 : uint32_t last_telem_byte_read_us;
258 : int8_t last_control_port;
259 :
260 : void serial_end();
261 : bool msp_process_byte(uint8_t c);
262 : void blheli_crc_update(uint8_t c);
263 : bool blheli_4way_process_byte(uint8_t c);
264 : uint8_t blheli_chan_to_output_chan(uint8_t motor);
265 : void msp_send_ack(uint8_t cmd);
266 : void msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len);
267 : void putU16(uint8_t *b, uint16_t v);
268 : uint16_t getU16(const uint8_t *b);
269 : void putU32(uint8_t *b, uint32_t v);
270 : void putU16_BE(uint8_t *b, uint16_t v);
271 : void msp_process_command(void);
272 : void blheli_send_reply(const uint8_t *buf, uint16_t len);
273 : uint16_t BL_CRC(const uint8_t *buf, uint16_t len);
274 : bool isMcuConnected(void);
275 : void setDisconnected(void);
276 : bool BL_SendBuf(const uint8_t *buf, uint16_t len);
277 : bool BL_ReadBuf(uint8_t *buf, uint16_t len);
278 : uint8_t BL_GetACK(uint16_t timeout_ms=2);
279 : bool BL_SendCMDSetAddress();
280 : bool BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n);
281 : bool BL_ConnectEx(void);
282 : bool BL_SendCMDKeepAlive(void);
283 : bool BL_PageErase(void);
284 : void BL_SendCMDRunRestartBootloader(void);
285 : uint8_t BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes);
286 : bool BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout);
287 : bool BL_WriteFlash(const uint8_t *buf, uint16_t n);
288 : bool BL_VerifyFlash(const uint8_t *buf, uint16_t n);
289 : void blheli_process_command(void);
290 : void run_connection_test(uint8_t chan);
291 : void read_telemetry_packet(void);
292 : void log_bidir_telemetry(void);
293 :
294 : // protocol handler hook
295 : bool protocol_handler(uint8_t , AP_HAL::UARTDriver *);
296 : };
297 :
298 : #endif // HAL_SUPPORT_RCOUT_SERIAL
|
