PX4 Firmware
PX4 Autopilot Software http://px4.io
tap_esc_common.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /*
35  * tap_esc_common.cpp
36  *
37  */
38 
39 #include "tap_esc_common.h"
40 
41 #include <fcntl.h>
42 #include <termios.h>
43 
44 #include <systemlib/px4_macros.h> // arraySize
45 #include <px4_platform_common/px4_config.h>
46 #include <px4_platform_common/defines.h>
47 
48 #ifndef B250000
49 #define B250000 250000
50 #endif
51 
52 namespace tap_esc_common
53 {
54 static uint8_t crc8_esc(uint8_t *p, uint8_t len);
55 static uint8_t crc_packet(EscPacket &p);
56 
57 void select_responder(uint8_t sel)
58 {
59 #if defined(GPIO_S0)
60  px4_arch_gpiowrite(GPIO_S0, sel & 1);
61  px4_arch_gpiowrite(GPIO_S1, sel & 2);
62  px4_arch_gpiowrite(GPIO_S2, sel & 4);
63 #endif
64 }
65 
66 int initialise_uart(const char *const device, int &uart_fd)
67 {
68  // open uart
69  uart_fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
70  int termios_state = -1;
71 
72  if (uart_fd < 0) {
73  PX4_ERR("failed to open uart device!");
74  return -1;
75  }
76 
77  // set baud rate
78  int speed = B250000;
79  struct termios uart_config;
80  tcgetattr(uart_fd, &uart_config);
81 
82  // clear ONLCR flag (which appends a CR for every LF)
83  uart_config.c_oflag &= ~ONLCR;
84 
85  // set baud rate
86  if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
87  PX4_ERR("failed to set baudrate for %s: %d\n", device, termios_state);
88  close(uart_fd);
89  return -1;
90  }
91 
92  if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
93  PX4_ERR("tcsetattr failed for %s\n", device);
94  close(uart_fd);
95  return -1;
96  }
97 
98  // setup output flow control
99  if (enable_flow_control(uart_fd, false)) {
100  PX4_WARN("hardware flow disable failed");
101  }
102 
103  return 0;
104 }
105 
106 int deinitialise_uart(int &uart_fd)
107 {
108  int ret = close(uart_fd);
109 
110  if (ret == 0) {
111  uart_fd = -1;
112  }
113 
114  return ret;
115 }
116 
117 int enable_flow_control(int uart_fd, bool enabled)
118 {
119  struct termios uart_config;
120 
121  int ret = tcgetattr(uart_fd, &uart_config);
122 
123  if (ret != 0) {
124  PX4_ERR("error getting uart configuration");
125  return ret;
126  }
127 
128  if (enabled) {
129  uart_config.c_cflag |= CRTSCTS;
130 
131  } else {
132  uart_config.c_cflag &= ~CRTSCTS;
133  }
134 
135  return tcsetattr(uart_fd, TCSANOW, &uart_config);
136 }
137 
138 int send_packet(int uart_fd, EscPacket &packet, int responder)
139 {
140  if (responder >= 0) {
141  select_responder(responder);
142  }
143 
144  int packet_len = crc_packet(packet);
145  int ret = ::write(uart_fd, &packet.head, packet_len);
146 
147  if (ret != packet_len) {
148  PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno);
149  }
150 
151  return ret;
152 }
153 
154 int read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf)
155 {
156  uint8_t tmp_serial_buf[UART_BUFFER_SIZE];
157 
158  int len =::read(uart_fd, tmp_serial_buf, arraySize(tmp_serial_buf));
159 
160  if (len > 0 && (uart_buf->dat_cnt + len < UART_BUFFER_SIZE)) {
161  for (int i = 0; i < len; i++) {
162  uart_buf->esc_feedback_buf[uart_buf->tail++] = tmp_serial_buf[i];
163  uart_buf->dat_cnt++;
164 
165  if (uart_buf->tail >= UART_BUFFER_SIZE) {
166  uart_buf->tail = 0;
167  }
168  }
169 
170  } else if (len < 0) {
171  return len;
172  }
173 
174  return 0;
175 }
176 
177 int parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata)
178 {
179  static PARSR_ESC_STATE state = HEAD;
180  static uint8_t data_index = 0;
181  static uint8_t crc_data_cal;
182 
183  if (serial_buf->dat_cnt > 0) {
184  int count = serial_buf->dat_cnt;
185 
186  for (int i = 0; i < count; i++) {
187  switch (state) {
188  case HEAD:
189  if (serial_buf->esc_feedback_buf[serial_buf->head] == PACKET_HEAD) {
190  packetdata->head = PACKET_HEAD; //just_keep the format
191  state = LEN;
192  }
193 
194  break;
195 
196  case LEN:
197  if (serial_buf->esc_feedback_buf[serial_buf->head] < sizeof(packetdata->d)) {
198  packetdata->len = serial_buf->esc_feedback_buf[serial_buf->head];
199  state = ID;
200 
201  } else {
202  state = HEAD;
203  }
204 
205  break;
206 
207  case ID:
208  if (serial_buf->esc_feedback_buf[serial_buf->head] < ESCBUS_MSG_ID_MAX_NUM) {
209  packetdata->msg_id = serial_buf->esc_feedback_buf[serial_buf->head];
210  data_index = 0;
211  state = DATA;
212 
213  } else {
214  state = HEAD;
215  }
216 
217  break;
218 
219  case DATA:
220  packetdata->d.bytes[data_index++] = serial_buf->esc_feedback_buf[serial_buf->head];
221 
222  if (data_index >= packetdata->len) {
223 
224  crc_data_cal = crc8_esc((uint8_t *)(&packetdata->len), packetdata->len + 2);
225  state = CRC;
226  }
227 
228  break;
229 
230  case CRC:
231  if (crc_data_cal == serial_buf->esc_feedback_buf[serial_buf->head]) {
232  packetdata->crc_data = serial_buf->esc_feedback_buf[serial_buf->head];
233 
234  if (++serial_buf->head >= UART_BUFFER_SIZE) {
235  serial_buf->head = 0;
236  }
237 
238  serial_buf->dat_cnt--;
239  state = HEAD;
240  return 0;
241  }
242 
243  state = HEAD;
244  break;
245 
246  default:
247  state = HEAD;
248  break;
249 
250  }
251 
252  if (++serial_buf->head >= UART_BUFFER_SIZE) {
253  serial_buf->head = 0;
254  }
255 
256  serial_buf->dat_cnt--;
257  }
258  }
259 
260  return -1;
261 }
262 
263 static uint8_t crc8_esc(uint8_t *p, uint8_t len)
264 {
265  uint8_t crc = 0;
266 
267  for (uint8_t i = 0; i < len; i++) {
268  crc = crc_table[crc^*p++];
269  }
270 
271  return crc;
272 }
273 
274 static uint8_t crc_packet(EscPacket &p)
275 {
276  /* Calculate the crc over Len,ID,data */
277  p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2);
278  return p.len + offsetof(EscPacket, d) + 1;
279 }
280 
281 
282 const uint8_t crc_table[256] = {
283  0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
284  0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,
285  0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,
286  0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,
287  0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,
288  0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,
289  0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,
290  0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,
291  0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,
292  0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,
293  0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,
294  0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,
295  0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,
296  0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,
297  0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,
298  0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,
299  0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,
300  0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,
301  0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,
302  0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,
303  0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,
304  0xB8, 0x5F, 0x91, 0x76
305 };
306 
307 } /* tap_esc_common */
static enum @74 state
#define PACKET_HEAD
Definition: drv_tap_esc.h:48
union EscPacket::@22 d
int send_packet(int uart_fd, EscPacket &packet, int responder)
Sends a packet to all ESCs and requests a specific ESC to respond.
int read_data_from_uart(int uart_fd, ESC_UART_BUF *const uart_buf)
Read data from the UART into a buffer.
A set of useful macros for enhanced runtime and compile time error detection and warning suppression...
int deinitialise_uart(int &uart_fd)
Closes a device previously opened with initialise_uart().
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
int enable_flow_control(int uart_fd, bool enabled)
Enables/disables flow control for open UART device.
static void read(bootloader_app_shared_t *pshared)
static uint8_t crc_packet(EscPacket &p)
uint8_t tail
Definition: drv_tap_esc.h:166
#define UART_BUFFER_SIZE
Definition: drv_tap_esc.h:163
const uint8_t crc_table[256]
Lookup-table for faster CRC computation when sending ESC packets.
int parse_tap_esc_feedback(ESC_UART_BUF *const serial_buf, EscPacket *const packetdata)
Parse feedback from an ESC.
uint8_t msg_id
Definition: drv_tap_esc.h:150
uint8_t head
Definition: drv_tap_esc.h:148
uint8_t len
Definition: drv_tap_esc.h:149
static uint8_t crc8_esc(uint8_t *p, uint8_t len)
uint8_t crc_data
Definition: drv_tap_esc.h:159
uint8_t esc_feedback_buf[UART_BUFFER_SIZE]
Definition: drv_tap_esc.h:168
#define B250000
uint8_t bytes[100]
Definition: drv_tap_esc.h:157
uint8_t head
Definition: drv_tap_esc.h:165
static void write(bootloader_app_shared_t *pshared)
void select_responder(uint8_t sel)
Select tap esc responder data for serial interface by 74hct151.
PARSR_ESC_STATE
Definition: drv_tap_esc.h:243
int initialise_uart(const char *const device, int &uart_fd)
Opens a device for use as UART.
#define arraySize(a)
uint8_t dat_cnt
Definition: drv_tap_esc.h:167