Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
actuators_sbus.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #include "subsystems/actuators.h"
29 #include "generated/airframe.h"
30 #include "mcu_periph/uart.h"
31 
32 /* Currently we only support 7 channels */
33 #if SERVOS_SBUS_NB > ACTUATORS_SBUS_MAX_NB
34 #error SBUS actuators only support less then 7 servos
35 #endif
36 
37 /* Calculate the frequency divider to aim at 7 ms */
38 #if PERIODIC_FREQUENCY < 150
39 #error Sbus actuators need at leest a frequency of 150 Hz
40 #else
41 static uint8_t freq_trig = PERIODIC_FREQUENCY / 142.0 + 0.5; // Round it to nearest value
42 #endif
43 
44 /* Main actuator structure */
46 static inline void actuators_sbus_send(struct link_device *dev);
47 
48 /*
49  * Initialize the sbus devices (UART output devices)
50  */
52 {
53  actuators_sbus.device = &((ACTUATORS_SBUS_DEV).device);
54 
56  uart_periph_set_baudrate(&ACTUATORS_SBUS_DEV, B100000);
57 }
58 
59 /*
60  * Transmit the sbus output at 1 / 7ms
61  */
63 {
64  static uint8_t cnt = 0;
65 
66  // Only send every 7 ms
67  cnt++;
68  if (cnt == freq_trig) {
70  cnt = 0;
71  }
72 }
73 
74 /*
75  * Actually transmit the sbus output on a link device
76  *
77  * The protocol is 25 Byte long and is send every 14ms (analog mode) or 7ms (highspeed mode).
78  * One Byte = 1 startbit + 8 databit + 1 paritybit + 2 stopbit (8E2), baudrate = 100'000 bit/s
79  * The highest bit is send first. The logic is inverted (Level High = 1)
80  *
81  * [startbyte] [data1] [data2] .... [data22] [flags][endbyte]
82  *
83  * SBUS protocol
84  */
85 
86 #define SBUS_START_BYTE 0x0f
87 #define SBUS_END_BYTE 0x00
88 
89 #define SBUS_BIT_PER_CHANNEL 11
90 
91 static inline void actuators_sbus_send(struct link_device *dev)
92 {
93  uint8_t i = 0;
94  uint8_t bits_sent = 0;
95  uint8_t data[22];
96 
97  /* start */
98  dev->put_byte(dev->periph, 0, SBUS_START_BYTE);
99 
100  /* Fill all channels */
101  for (i = 0; i < 22; i++) {
102  data[i] = 0x00;
103  }
104  for (i = 0; i < ACTUATORS_SBUS_MAX_NB; i++) {
105  uint16_t chn = actuators_sbus.cmds[i] & 0x07ff; // 11 bit
106  uint8_t ind = bits_sent / SBUS_BIT_PER_CHANNEL;
107  uint8_t shift = bits_sent % SBUS_BIT_PER_CHANNEL;
108  data[ind] |= (chn >> (3 + shift)) & 0xff; // Sends (8 - shift) bits of the 11: 11-(8-shift) remain = 3 + shift
109  if (shift > 5) { // need 3 bytes to fit the 11 bits
110  data[ind + 1] |= (chn >> (shift - 5)) & 0xff; // Sends next 8
111  data[ind + 2] |= (chn << (3 - shift)) &
112  0xff; // Sends remaining 3 + shift - 8 bits = shift-5 bits: left aligned: 8 - (shift-5) = 3-shift
113  } else { // (shift <= 5) then it fits in 2 bytes
114  data[ind + 1] |= (chn << (5 - shift))) &
115  0xff; // Sends remaining 3 + shift bits left aligned: 8 - (3 + shift) = 5 - shift
116  }
117  bits_sent += SBUS_BIT_PER_CHANNEL;
118  }
119 
120  /* Transmit all channels */
121  for (i = 0; i < 22; i++) {
122  dev->put_byte(dev->periph, 0, data[i]);
123  }
124 
125  /* flags */
126  dev->put_byte(dev->periph, 0, 0x00); // No frame lost, switches off, no failsafe
127 
128  /* stop byte */
129  dev->put_byte(dev->periph, 0, SBUS_END_BYTE);
130 
131 }
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#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
#define SBUS_START_BYTE
Sbus actuator driver, which can output as 7 sbus channels at ~11ms.
void actuators_sbus_set(void)
static void actuators_sbus_send(struct link_device *dev)
struct link_device * device
void actuators_sbus_init(void)
#define UBITS_8
Definition: serial_port.c:50
#define USTOP_2
Definition: serial_port.c:53
Hardware independent API for actuators (servos, motor controllers).
#define SBUS_BIT_PER_CHANNEL
int32_t cmds[ACTUATORS_SBUS_MAX_NB]
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
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
#define ACTUATORS_SBUS_MAX_NB
#define SBUS_END_BYTE
struct ActuatorsSbus actuators_sbus