Paparazzi UAS  v5.15_devel-109-gee85905
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 "mcu_periph/sys_time.h"
29 #include BOARD_CONFIG
30 #include <string.h>
31 #include <stdbool.h>
32 
33 /*
34  * SBUS protocol and state machine status
35  */
36 #define SBUS_START_BYTE 0x0f
37 #define SBUS_END_BYTE_0 0x00 // only possible end byte for SBUS v1
38 #define SBUS_END_BYTE_1 0x04 // with SBUS v2 and 14CH mode
39 #define SBUS_END_BYTE_2 0x14 // end byte is cycling
40 #define SBUS_END_BYTE_3 0x24 // through the following 4 types
41 #define SBUS_END_BYTE_4 0x34 // in order
42 #define SBUS_END_BYTE_5 0x08 // with SBUS v2 and 12CH mode
43 #define SBUS_BIT_PER_CHANNEL 11
44 #define SBUS_BIT_PER_BYTE 8
45 #define SBUS_FLAGS_BYTE 22
46 #define SBUS_FRAME_LOST_BIT 2
47 #define SBUS_RC_FAILSAFE_BIT 4
48 #define SBUS_RC_LOST_BIT 5
49 
50 #define SBUS_STATUS_UNINIT 0
51 #define SBUS_STATUS_GOT_START 1
52 
57 #define SBUS_TIMEOUT_MS 4
58 
66 #ifndef RC_SET_POLARITY
67 #define RC_SET_POLARITY gpio_set
68 #endif
69 
70 void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev,
71  gpio_port_t gpio_polarity_port, uint16_t gpio_polarity_pin)
72 {
73  //Assume the worst ;)
74  sbus_p->frame_available = false;
75  sbus_p->rc_failsafe = true;
76  sbus_p->rc_lost = true;
77  sbus_p->status = SBUS_STATUS_UNINIT;
78  sbus_p->start_time = 0;
79 
80  // Set UART parameter, SBUS used a baud rate of 100000, 8 data bits, even parity bit, and 2 stop bits
83  // Try to invert RX data logic when available in hardware periph
84  uart_periph_invert_data_logic(dev, true, false);
85 
86  // Set polarity (when not done in hardware, don't use both!)
87  if (gpio_polarity_port != 0) {
88  gpio_setup_output(gpio_polarity_port, gpio_polarity_pin);
89  RC_SET_POLARITY(gpio_polarity_port, gpio_polarity_pin);
90  }
91 
92 }
93 
95 static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst,
96  bool *is_frame_available, bool *is_rc_failsafe, bool *is_rc_lost,
97  uint16_t *dstppm __attribute__((unused)))
98 {
99  // decode sbus data, unrolling the loop for efficiency
100  dst[0] = ((src[0]) | (src[1] << 8)) & 0x07FF;
101  dst[1] = ((src[1] >> 3) | (src[2] << 5)) & 0x07FF;
102  dst[2] = ((src[2] >> 6) | (src[3] << 2) | (src[4] << 10)) & 0x07FF;
103  dst[3] = ((src[4] >> 1) | (src[5] << 7)) & 0x07FF;
104  dst[4] = ((src[5] >> 4) | (src[6] << 4)) & 0x07FF;
105  dst[5] = ((src[6] >> 7) | (src[7] << 1) | (src[8] << 9)) & 0x07FF;
106  dst[6] = ((src[8] >> 2) | (src[9] << 6)) & 0x07FF;
107  dst[7] = ((src[9] >> 5) | (src[10] << 3)) & 0x07FF;
108  dst[8] = ((src[11]) | (src[12] << 8)) & 0x07FF;
109  dst[9] = ((src[12] >> 3) | (src[13] << 5)) & 0x07FF;
110  dst[10] = ((src[13] >> 6) | (src[14] << 2) | (src[15] << 10)) & 0x07FF;
111  dst[11] = ((src[15] >> 1) | (src[16] << 7)) & 0x07FF;
112  dst[12] = ((src[16] >> 4) | (src[17] << 4)) & 0x07FF;
113  dst[13] = ((src[17] >> 7) | (src[18] << 1) | (src[19] << 9)) & 0x07FF;
114  dst[14] = ((src[19] >> 2) | (src[20] << 6)) & 0x07FF;
115  dst[15] = ((src[20] >> 5) | (src[21] << 3)) & 0x07FF;
116 
117  // Convert sbus to ppm
118 #if PERIODIC_TELEMETRY
119  for (int channel = 0; channel < SBUS_NB_CHANNEL; channel++) {
120  dstppm[channel] = USEC_OF_RC_PPM_TICKS(dst[channel]);
121  }
122 #endif
123 
124  // Test frame loss flag
125  *is_frame_available = !bit_is_set(src[SBUS_FLAGS_BYTE], SBUS_FRAME_LOST_BIT);
126  // Check if receiver is in Failsafe mode
127  *is_rc_failsafe = bit_is_set(src[SBUS_FLAGS_BYTE], SBUS_RC_FAILSAFE_BIT);
128  // Also check if the RC link is lost
129  *is_rc_lost = bit_is_set(src[SBUS_FLAGS_BYTE], SBUS_RC_LOST_BIT);
130 
131 }
132 
133 // Decoding event function
134 // Reading from UART
135 void sbus_common_decode_event(struct Sbus *sbus_p, struct uart_periph *dev)
136 {
137  uint8_t rbyte;
138  if (uart_char_available(dev)) {
139  // Took too long to receive a full SBUS frame (usually during boot to synchronize)
140  if(get_sys_time_msec() > (sbus_p->start_time+SBUS_TIMEOUT_MS)) {
141  sbus_p->status = SBUS_STATUS_UNINIT;
142  }
143 
144  do {
145  rbyte = uart_getch(dev);
146  switch (sbus_p->status) {
147  case SBUS_STATUS_UNINIT:
148  // Wait for the start byte
149  if (rbyte == SBUS_START_BYTE) {
150  sbus_p->status++;
151  sbus_p->idx = 0;
152  sbus_p->start_time = get_sys_time_msec();
153  }
154  break;
156  // Store buffer
157  sbus_p->buffer[sbus_p->idx] = rbyte;
158  sbus_p->idx++;
159  if (sbus_p->idx == SBUS_BUF_LENGTH) {
160  // Decode if last byte is (one of) the correct end byte
161  if (rbyte == SBUS_END_BYTE_0 ||
162  rbyte == SBUS_END_BYTE_1 ||
163  rbyte == SBUS_END_BYTE_2 ||
164  rbyte == SBUS_END_BYTE_3 ||
165  rbyte == SBUS_END_BYTE_4 ||
166  rbyte == SBUS_END_BYTE_5) {
167  decode_sbus_buffer(sbus_p->buffer, sbus_p->pulses, &sbus_p->frame_available, &sbus_p->rc_failsafe, &sbus_p->rc_lost, sbus_p->ppm);
168  }
169  sbus_p->status = SBUS_STATUS_UNINIT;
170  }
171  break;
172  default:
173  break;
174  }
175  } while (uart_char_available(dev));
176  }
177 }
#define SBUS_RC_LOST_BIT
Definition: sbus_common.c:48
unsigned short uint16_t
Definition: types.h:16
#define SBUS_RC_FAILSAFE_BIT
Definition: sbus_common.c:47
void sbus_common_decode_event(struct Sbus *sbus_p, struct uart_periph *dev)
Decoding event function.
Definition: sbus_common.c:135
#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:861
static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst, bool *is_frame_available, bool *is_rc_failsafe, bool *is_rc_lost, uint16_t *dstppm)
Decode the raw buffer.
Definition: sbus_common.c:95
uint32_t start_time
Decoder start time.
Definition: sbus_common.h:88
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:845
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
#define RC_SET_POLARITY
Set polarity using RC_POLARITY_GPIO.
Definition: sbus_common.c:67
#define SBUS_END_BYTE_4
Definition: sbus_common.c:41
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:38
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:50
#define SBUS_END_BYTE_5
Definition: sbus_common.c:42
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:39
bool rc_lost
RC reception is lost.
Definition: sbus_common.h:84
#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:87
uint8_t idx
Input index.
Definition: sbus_common.h:86
#define USEC_OF_RC_PPM_TICKS(_v)
Definition: ppm_arch.h:46
#define USTOP_2
Definition: serial_port.c:53
Architecture independent timing functions.
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:70
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 to ppm 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:51
#define SBUS_FLAGS_BYTE
Definition: sbus_common.c:45
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:890
uint8_t buffer[SBUS_BUF_LENGTH]
Input buffer.
Definition: sbus_common.h:85
#define SBUS_START_BYTE
Definition: sbus_common.c:36
#define SBUS_END_BYTE_3
Definition: sbus_common.c:40
bool rc_failsafe
Receiver set to in failsafe mode.
Definition: sbus_common.h:83
#define SBUS_END_BYTE_0
Definition: sbus_common.c:37
bool frame_available
A data frame is 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:46
SBUS structure.
Definition: sbus_common.h:79
#define SBUS_TIMEOUT_MS
Time before timeout when receiving a full SBUS frame.
Definition: sbus_common.c:57