Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
sbus_common.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include BOARD_CONFIG
29 #include <string.h>
30 
31 /*
32  * SBUS protocol and state machine status
33  */
34 #define SBUS_START_BYTE 0x0f
35 #define SBUS_END_BYTE_0 0x00 // only possible end byte for SBUS v1
36 #define SBUS_END_BYTE_1 0x04 // with SBUS v2 and 14CH mode
37 #define SBUS_END_BYTE_2 0x14 // end byte is cycling
38 #define SBUS_END_BYTE_3 0x24 // through the following 4 types
39 #define SBUS_END_BYTE_4 0x34 // in order
40 #define SBUS_END_BYTE_5 0x08 // with SBUS v2 and 12CH mode
41 #define SBUS_BIT_PER_CHANNEL 11
42 #define SBUS_BIT_PER_BYTE 8
43 #define SBUS_FLAGS_BYTE 22
44 #define SBUS_FRAME_LOST_BIT 2
45 
46 #define SBUS_STATUS_UNINIT 0
47 #define SBUS_STATUS_GOT_START 1
48 
56 #ifndef RC_SET_POLARITY
57 #define RC_SET_POLARITY gpio_set
58 #endif
59 
60 
61 void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev,
62  gpio_port_t gpio_polarity_port, uint16_t gpio_polarity_pin)
63 {
64  sbus_p->frame_available = false;
65  sbus_p->status = SBUS_STATUS_UNINIT;
66 
67  // Set UART parameters (100K, 8 bits, 2 stops, even parity)
70  // Try to invert RX data logic when available in hardware periph
71  uart_periph_invert_data_logic(dev, true, false);
72 
73  // Set polarity (when not done in hardware, don't use both!)
74  if (gpio_polarity_port != 0) {
75  gpio_setup_output(gpio_polarity_port, gpio_polarity_pin);
76  RC_SET_POLARITY(gpio_polarity_port, gpio_polarity_pin);
77  }
78 
79 }
80 
81 
83 static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst, bool *available,
84  uint16_t *dstppm __attribute__((unused)))
85 {
86  // decode sbus data, unrolling the loop for efficiency
87  dst[0] = ((src[0]) | (src[1] << 8)) & 0x07FF;
88  dst[1] = ((src[1] >> 3) | (src[2] << 5)) & 0x07FF;
89  dst[2] = ((src[2] >> 6) | (src[3] << 2) | (src[4] << 10)) & 0x07FF;
90  dst[3] = ((src[4] >> 1) | (src[5] << 7)) & 0x07FF;
91  dst[4] = ((src[5] >> 4) | (src[6] << 4)) & 0x07FF;
92  dst[5] = ((src[6] >> 7) | (src[7] << 1) | (src[8] << 9)) & 0x07FF;
93  dst[6] = ((src[8] >> 2) | (src[9] << 6)) & 0x07FF;
94  dst[7] = ((src[9] >> 5) | (src[10] << 3)) & 0x07FF;
95  dst[8] = ((src[11]) | (src[12] << 8)) & 0x07FF;
96  dst[9] = ((src[12] >> 3) | (src[13] << 5)) & 0x07FF;
97  dst[10] = ((src[13] >> 6) | (src[14] << 2) | (src[15] << 10)) & 0x07FF;
98  dst[11] = ((src[15] >> 1) | (src[16] << 7)) & 0x07FF;
99  dst[12] = ((src[16] >> 4) | (src[17] << 4)) & 0x07FF;
100  dst[13] = ((src[17] >> 7) | (src[18] << 1) | (src[19] << 9)) & 0x07FF;
101  dst[14] = ((src[19] >> 2) | (src[20] << 6)) & 0x07FF;
102  dst[15] = ((src[20] >> 5) | (src[21] << 3)) & 0x07FF;
103 
104  // convert sbus to ppm
105 #if PERIODIC_TELEMETRY
106  for (int channel = 0; channel < SBUS_NB_CHANNEL; channel++) {
107  dstppm[channel] = USEC_OF_RC_PPM_TICKS(dst[channel]);
108  }
109 #endif
110 
111  // test frame lost flag
112  *available = !bit_is_set(src[SBUS_FLAGS_BYTE], SBUS_FRAME_LOST_BIT);
113 }
114 
115 // Decoding event function
116 // Reading from UART
117 void sbus_common_decode_event(struct Sbus *sbus_p, struct uart_periph *dev)
118 {
119  uint8_t rbyte;
120  if (uart_char_available(dev)) {
121  do {
122  rbyte = uart_getch(dev);
123  switch (sbus_p->status) {
124  case SBUS_STATUS_UNINIT:
125  // Wait for the start byte
126  if (rbyte == SBUS_START_BYTE) {
127  sbus_p->status++;
128  sbus_p->idx = 0;
129  }
130  break;
132  // Store buffer
133  sbus_p->buffer[sbus_p->idx] = rbyte;
134  sbus_p->idx++;
135  if (sbus_p->idx == SBUS_BUF_LENGTH) {
136  // Decode if last byte is (one of) the correct end byte
137  if (rbyte == SBUS_END_BYTE_0 ||
138  rbyte == SBUS_END_BYTE_1 ||
139  rbyte == SBUS_END_BYTE_2 ||
140  rbyte == SBUS_END_BYTE_3 ||
141  rbyte == SBUS_END_BYTE_4 ||
142  rbyte == SBUS_END_BYTE_5) {
143  decode_sbus_buffer(sbus_p->buffer, sbus_p->pulses, &sbus_p->frame_available, sbus_p->ppm);
144  }
145  sbus_p->status = SBUS_STATUS_UNINIT;
146  }
147  break;
148  default:
149  break;
150  }
151  } while (uart_char_available(dev));
152  }
153 }
unsigned short uint16_t
Definition: types.h:16
void sbus_common_decode_event(struct Sbus *sbus_p, struct uart_periph *dev)
Decoding event function.
Definition: sbus_common.c:117
#define UPARITY_EVEN
Definition: serial_port.c:57
#define B100000
Definition: uart_arch.h:47
void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud)
Set baudrate.
Definition: uart_arch.c:856
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:840
#define RC_SET_POLARITY
Set polarity using RC_POLARITY_GPIO.
Definition: sbus_common.c:57
#define SBUS_END_BYTE_4
Definition: sbus_common.c:39
uint16_t pulses[SBUS_NB_CHANNEL]
decoded values
Definition: sbus_common.h:80
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
#define SBUS_END_BYTE_1
Definition: sbus_common.c:36
ioportid_t gpio_port_t
Abstract gpio port type for hardware independent part.
Definition: gpio_arch.h:39
#define SBUS_STATUS_UNINIT
Definition: sbus_common.c:46
#define SBUS_END_BYTE_5
Definition: sbus_common.c:40
void WEAK uart_periph_invert_data_logic(struct uart_periph *p, bool invert_rx, bool invert_tx)
Definition: uart.c:299
#define SBUS_END_BYTE_2
Definition: sbus_common.c:37
#define UBITS_8
Definition: serial_port.c:50
void gpio_setup_output(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.c:33
uint8_t status
decoder state machine status
Definition: sbus_common.h:85
uint8_t idx
input index
Definition: sbus_common.h:84
#define USEC_OF_RC_PPM_TICKS(_v)
Definition: ppm_arch.h:46
#define USTOP_2
Definition: serial_port.c:53
void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev, gpio_port_t gpio_polarity_port, uint16_t gpio_polarity_pin)
Init function.
Definition: sbus_common.c:61
UART peripheral.
Definition: uart.h:70
#define SBUS_BUF_LENGTH
Generated code holding the description of a given transmitter.
Definition: sbus_common.h:62
Common sbus structs and defines.
uint16_t ppm[SBUS_NB_CHANNEL]
decoded and converted values
Definition: sbus_common.h:81
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define SBUS_STATUS_GOT_START
Definition: sbus_common.c:47
#define SBUS_FLAGS_BYTE
Definition: sbus_common.c:43
unsigned char uint8_t
Definition: types.h:14
void uart_periph_set_bits_stop_parity(struct uart_periph *p, uint8_t bits, uint8_t stop, uint8_t parity)
Set parity and stop bits.
Definition: uart_arch.c:885
uint8_t buffer[SBUS_BUF_LENGTH]
input buffer
Definition: sbus_common.h:83
#define SBUS_START_BYTE
Definition: sbus_common.c:34
#define SBUS_END_BYTE_3
Definition: sbus_common.c:38
#define SBUS_END_BYTE_0
Definition: sbus_common.c:35
static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst, bool *available, uint16_t *dstppm)
Decode the raw buffer.
Definition: sbus_common.c:83
bool frame_available
new frame available
Definition: sbus_common.h:82
static uint8_t channel
Definition: ADS8344.c:80
#define SBUS_NB_CHANNEL
Definition: sbus_common.h:63
#define SBUS_FRAME_LOST_BIT
Definition: sbus_common.c:44
SBUS structure.
Definition: sbus_common.h:79