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