Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
spektrum.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * 2010 Eric Parsonage <eric@eparsonage.com>
4  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
5  * 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, see
21  * <http://www.gnu.org/licenses/>.
22  */
23 
31 #include "std.h"
34 #include "modules/core/abi.h"
35 #include "mcu_periph/uart.h"
36 #include "mcu_periph/gpio.h"
37 #include "mcu_periph/sys_time.h"
38 
39 #if RADIO_CONTROL_NB_CHANNEL < SPEKTRUM_NB_CHANNEL
40 #warning "RADIO_CONTROL_NB_CHANNEL mustn't be lower than 14. X-Plus channel expansion is not (yet) usable"
41 #endif
42 
43 /* Check if primary receiver is defined */
44 #ifndef SPEKTRUM_PRIMARY_UART
45 #error "You must at least define the primary Spektrum satellite receiver."
46 #endif
47 
48 /* Number of low pulses sent during binding to the satellite receivers
49  * Spektrum documentation recommend that master and slave receivers
50  * should be configured in DSMX 11ms mode, other modes (DSM2, 22ms) will be
51  * automatically supported if transmitter is not compatible.
52  * But this this is only if receiver can handle DSMX. If it is not the case,
53  * DSM2 must be used, otherwise the system filed will be wrong.
54  * So DSM2 is used by default and DSMX can be enable with USE_DSMX flag.
55  */
56 #if USE_DSMX
57 #define SPEKTRUM_MASTER_RECEIVER_PULSES 9 // only one receiver should be in master mode
58 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 10
59 #else
60 #define SPEKTRUM_MASTER_RECEIVER_PULSES 5 // only one receiver should be in master mode
61 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 6
62 #endif
63 
64 /* Set polarity using RC_POLARITY_GPIO. */
65 #ifndef RC_SET_POLARITY
66 #define RC_SET_POLARITY gpio_clear
67 #endif
68 
69 /* Busy wait to let the receiver starts properly
70  * This should be reduced when the MCU takes longer to start
71  */
72 #ifndef SPEKTRUM_BIND_WAIT
73 #define SPEKTRUM_BIND_WAIT 60000
74 #endif
75 
76 /* Spektrum system type can be force (see list below)
77  * by default it is unknown type and will be determined from incoming frames
78  */
79 #ifndef SPEKTRUM_SYS_TYPE
80 #define SPEKTRUM_SYS_TYPE 0 // unknown type, determined from incoming frame
81 #endif
82 
83 // in case the number of channel is less than maximum
85 
86 /* Default spektrum values */
87 static struct spektrum_t spektrum = {
88  .valid = false,
89  .tx_type = SPEKTRUM_SYS_TYPE, // unknown type by default
90 };
91 
99 #define SPEKTRUM_SYS_22_1024_2 0x01 // 22ms 1024 DSM2
100 #define SPEKTRUM_SYS_11_2048_2 0x12 // 11ms 2048 DSM2
101 #define SPEKTRUM_SYS_22_2048_X 0xa2 // 22ms 2048 DSMX
102 #define SPEKTRUM_SYS_11_2048_X 0xb2 // 11ms 2048 DSMX
103 
104 static void spektrum_bind(void);
105 
107 static inline void spektrum_init_sat(struct spektrum_sat_t *sat)
108 {
109  sat->valid = false;
110  sat->timer = get_sys_time_msec();
111  sat->idx = 0;
112 
113  // Initialize values
114  for (uint8_t i = 0; i < SPEKTRUM_MAX_CHANNELS; i++) {
115  sat->values[i] = 0;
116  }
117 }
118 
119 /*****************************************************************************
120  *
121  * spektrum_try_bind(void) must called on powerup as spektrum
122  * satellites can only bind immediately after power up also it must be called
123  * before the call to SpektrumUartInit as we leave them with their Rx pins set
124  * as outputs.
125  *
126  *****************************************************************************/
128 {
129 #ifdef SPEKTRUM_BIND_PIN_PORT
130 #ifdef SPEKTRUM_BIND_PIN_HIGH
131  /* Init GPIO for the bind pin, we enable the pulldown resistor.
132  * (esden) As far as I can tell only navstick is using the PIN LOW version of
133  * the bind pin, but I assume this should not harm anything. If I am mistaken
134  * than I appologise for the inconvenience. :)
135  */
137 
138  sys_time_usleep(10); // wait for electrical level to stabilize
139 
140  /* Exit if the BIND_PIN is low, it needs to
141  be pulled high at startup to initiate bind */
143  spektrum_bind();
144  }
145 #else
146  /* Init GPIO for the bind pin, we enable the pullup resistor in case we have
147  * a floating pin that does not have a hardware pullup resistor as it is the
148  * case with Lisa/M and Lisa/MX prior to version 2.1.
149  */
151 
152  sys_time_usleep(10); // wait for electrical level to stabilize
153 
154  /* Exit if the BIND_PIN is high, it needs to
155  be pulled low at startup to initiate bind */
157  spektrum_bind();
158  }
159 #endif
160 #endif
161 }
162 
163 
165 void spektrum_init(void)
166 {
167 
168  for (uint8_t i = 0; i < SPEKTRUM_NB_CHANNEL; i++) {
169  spektrum.signs[i] = spektrum_signs[i];
170  }
172 
173  // Set polarity to normal on boards that can change this
174 #ifdef RC_POLARITY_GPIO_PORT
177 #endif
178 
179  // Initialize all the UART's in the satellites
181 #ifdef SPEKTRUM_SECONDARY_UART
183 #endif
184 }
185 
186 /* Parse a sattelite channel */
187 static inline void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
188 {
189  // This channel is not used
190  if (chan == 0xFFFF) {
191  return;
192  }
193 
195  // We got a 10bit precision packet
196  uint8_t chan_num = (chan & 0xFC00) >> 10;
197  sat->values[chan_num] = chan & 0x03FF;
198  if (chan_num == RADIO_THROTTLE) {
199  // scale full range to pprz_t
200  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
201  // remove an offset of 2400 = 171 * MAX_PPRZ / 684
202  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 684) - 2400;
203  } else {
204  sat->values[chan_num] -= (1 << 9); // substract 2^9 to get a value between [-512;512]
205  // scale full range to pprz_t
206  // but since 512 correspond to 150%, scale with 512/1.5 ~ 342
207  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 342;
208  }
209  }
210  else {
211  // We got a 11bit precision packet
212  uint8_t chan_num = (chan & 0x7800) >> 11;
213  sat->values[chan_num] = chan & 0x07FF;
214  if (chan_num == RADIO_THROTTLE) {
215  // scale full range to pprz_t
216  // but since 2048 correspond to 150%, scale with 2048/1.5 ~ 1368
217  // remove an offset of 2400 = 234 * MAX_PPRZ / 1368
218  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 1368) - 2400;
219  } else {
220  sat->values[chan_num] -= (1 << 10); // substract 2^10 to get a value between [-1024;1024]
221  // scale full range to pprz_t
222  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
223  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 684;
224  }
225  }
226 
227  // mark a valid frame
228  sat->valid = true;
229  spektrum.valid = true;
230 }
231 
232 /* Spektrum parser for a satellite */
233 static inline void spektrum_parser(struct spektrum_sat_t *sat)
234 {
235  // Parse packet
236  sat->lost_frame_cnt = sat->buf[0];
237  // For now ignore the second byte (which could be the TX type)
238  // if frame type is still unknown, try to find it in the 'system' byte
239  // only the primary receiver should have a valid type
240  if (spektrum.tx_type == 0) {
241  uint8_t type = sat->buf[1];
244  // we have a valid type, we assume it comes from primary receiver
246  } else {
247  // return and drop frame as we don't know what to do with it
248  return;
249  }
250  }
251  // parse servo channels
252  for (uint8_t i = 2; i < 2*SPEKTRUM_CHANNELS_PER_FRAME+2; i = i+2) {
253  uint16_t chan = (((uint16_t)sat->buf[i]) << 8) | ((uint16_t)sat->buf[i+1]);
254  spektrum_parse_channel(sat, chan);
255  }
256 }
257 
259 static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t *sat)
260 {
261  // detect sync space based on frame spacing
263  if (t - sat->timer > SPEKTRUM_MIN_FRAME_SPACE) {
264  sat->timer = t; // reset counter
265  uint16_t bytes_cnt = uart_char_available(dev); // The amount of bytes in the buffer
266  // sync space detected but buffer not empty, flush data
267  if (bytes_cnt > 0) {
268  for (uint8_t i = 0; i < bytes_cnt; i++) {
269  uart_getch(dev);
270  }
271  }
272  }
273  // check and parse bytes in uart buffer
274  while (uart_char_available(dev)) {
275  sat->buf[sat->idx++] = uart_getch(dev);
276  sat->timer = t; // reset counter
277  if (sat->idx == SPEKTRUM_FRAME_LEN) {
278  // buffer is full, parse frame
279  spektrum_parser(sat);
280  sat->idx = 0; // reset index
281  break; // stop here to handle RC frame
282  }
283  }
284 }
285 
287 void spektrum_event(void)
288 {
289  spektrum_uart_check(&SPEKTRUM_PRIMARY_UART, &spektrum.satellites[0]);
290 
291 #ifdef SPEKTRUM_SECONDARY_UART
292  spektrum_uart_check(&SPEKTRUM_SECONDARY_UART, &spektrum.satellites[1]);
293 #endif
294 
295  // Whenever we received a valid RC packet
296  if (spektrum.valid) {
298  spektrum.valid = false;
299 
300  // Find the first satellite that has a valid packet
301  for (uint8_t i = 0; i < SPEKTRUM_SATELLITES_NB; i++) {
302  if (i < sat_id) {
303  sat_id = i;
304  }
305  if (spektrum.satellites[i].valid) {
306  spektrum.satellites[i].valid = false;
307  }
308  }
309 
310  // Failsafe case if found satellite is out of bound (Should not happen)
311  if (sat_id >= SPEKTRUM_SATELLITES_NB) {
312  return;
313  }
314 
315  // Set the radio control status
319 
320  // Copy the radio control channels
321  for (uint8_t i = 0; i < radio_control.nb_channel; i++) {
323  Bound(radio_control.values[i], -MAX_PPRZ, MAX_PPRZ);
324  }
325 
326  // We got a valid frame so execute the frame handler
327  AbiSendMsgRADIO_CONTROL(RADIO_CONTROL_SPEKTRUM_ID, &radio_control);
328  }
329 }
330 
331 /* Defines needed for easy access of port information */
332 #define _UART_RX_PORT(i) i ## _PORT_RX
333 #define UART_RX_PORT(i) _UART_RX_PORT(i)
334 #define _UART_RX(i) i ## _RX
335 #define UART_RX(i) _UART_RX(i)
336 
341 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PORT
342 #define SPEKTRUM_PRIMARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_PRIMARY_UART_UPPER)
343 #endif
344 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PIN
345 #define SPEKTRUM_PRIMARY_BIND_CONF_PIN UART_RX(SPEKTRUM_PRIMARY_UART_UPPER)
346 #endif
347 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PORT
348 #define SPEKTRUM_SECONDARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_SECONDARY_UART_UPPER)
349 #endif
350 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PIN
351 #define SPEKTRUM_SECONDARY_BIND_CONF_PIN UART_RX(SPEKTRUM_SECONDARY_UART_UPPER)
352 #endif
353 
357 static void UNUSED spektrum_bind(void)
358 {
359 
360  /* Master receiver Rx push-pull */
362  /* Master receiver RX line, drive high */
364 
365 #ifdef SPEKTRUM_SECONDARY_UART
366  /* Slave receiver Rx push-pull */
368  /* Slave receiver RX line, drive high */
370 #endif
371 
372  /* bind pulses should be issued within 200ms after power up
373  * wait a bit to let the receiver start properly,
374  * set to 0 to disable */
375 #if SPEKTRUM_BIND_WAIT
377 #endif
378 
379  /* Transmit the bind pulses */
380  for (int i = 0; i < 2 * SPEKTRUM_MASTER_RECEIVER_PULSES ; i++) {
382  sys_time_usleep(120);
383  }
384 #ifdef SPEKTRUM_SECONDARY_UART
385  for (int i = 0; i < 2 * SPEKTRUM_SLAVE_RECEIVER_PULSES; i++) {
387  sys_time_usleep(120);
388  }
389 #endif
390 
391  /* Set conf pin as input in case it is different from RX pin */
393 #ifdef SPEKTRUM_SECONDARY_UART
395 #endif
396 }
397 
SPEKTRUM_SLAVE_RECEIVER_PULSES
#define SPEKTRUM_SLAVE_RECEIVER_PULSES
Definition: spektrum.c:61
radio_control.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
RC_POLARITY_GPIO_PIN
#define RC_POLARITY_GPIO_PIN
Definition: board.h:155
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
SPEKTRUM_BIND_WAIT
#define SPEKTRUM_BIND_WAIT
Definition: spektrum.c:73
RADIO_CONTROL_SPEKTRUM_SIGNS
#define RADIO_CONTROL_SPEKTRUM_SIGNS
Definition: spektrum_radio.h:92
spektrum
static struct spektrum_t spektrum
Definition: spektrum.c:87
SPEKTRUM_SATELLITES_NB
#define SPEKTRUM_SATELLITES_NB
Definition: spektrum.h:38
uart_getch
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:968
spektrum_t::valid
bool valid
True when we received a packet else false.
Definition: spektrum.h:59
abi.h
sys_time_usleep
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
Definition: sys_time_arch.c:104
spektrum_t::tx_type
uint8_t tx_type
Transmitter type encoded (see wiki)
Definition: spektrum.h:60
uart_char_available
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
gpio_setup_output
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
spektrum_sat_t
Definition: spektrum.h:48
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:101
SPEKTRUM_SYS_22_2048_X
#define SPEKTRUM_SYS_22_2048_X
Definition: spektrum.c:101
RadioControl::time_since_last_frame
uint8_t time_since_last_frame
Definition: radio_control.h:62
spektrum_parse_channel
static void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
Definition: spektrum.c:187
spektrum_sat_t::buf
uint8_t buf[SPEKTRUM_FRAME_LEN]
input buffer
Definition: spektrum.h:52
RadioControl::nb_channel
uint8_t nb_channel
Definition: radio_control.h:66
spektrum.h
SPEKTRUM_FRAME_LEN
#define SPEKTRUM_FRAME_LEN
16 bytes in a standard frame
Definition: spektrum.h:41
spektrum_t::signs
int8_t signs[SPEKTRUM_NB_CHANNEL]
Signs for the RC channels.
Definition: spektrum.h:61
spektrum_parser
static void spektrum_parser(struct spektrum_sat_t *sat)
Definition: spektrum.c:233
spektrum_try_bind
void spektrum_try_bind(void)
Definition: spektrum.c:127
SPEKTRUM_MAX_CHANNELS
#define SPEKTRUM_MAX_CHANNELS
Definition: spektrum.h:44
spektrum_t::satellites
struct spektrum_sat_t satellites[SPEKTRUM_SATELLITES_NB]
All the satellites connected.
Definition: spektrum.h:62
spektrum_init
void spektrum_init(void)
Main Radio initialization.
Definition: spektrum.c:165
std.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
gpio_setup_input_pulldown
void gpio_setup_input_pulldown(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as inputs with pull down resistors enabled.
Definition: gpio_arch.c:54
RC_SET_POLARITY
#define RC_SET_POLARITY
Definition: spektrum.c:66
spektrum_sat_t::idx
uint8_t idx
input buffer index
Definition: spektrum.h:53
SPEKTRUM_SYS_TYPE
#define SPEKTRUM_SYS_TYPE
Definition: spektrum.c:80
gpio_setup_input
void gpio_setup_input(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as inputs.
Definition: gpio_arch.c:40
SPEKTRUM_MIN_FRAME_SPACE
#define SPEKTRUM_MIN_FRAME_SPACE
Minum amount of time between frames (7ms), in fact either 11 or 22 ms.
Definition: spektrum.h:45
SPEKTRUM_MASTER_RECEIVER_PULSES
#define SPEKTRUM_MASTER_RECEIVER_PULSES
Definition: spektrum.c:60
SPEKTRUM_BIND_PIN
#define SPEKTRUM_BIND_PIN
Definition: board.h:389
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
SPEKTRUM_NB_CHANNEL
#define SPEKTRUM_NB_CHANNEL
Definition: spektrum_radio.h:32
SPEKTRUM_PRIMARY_BIND_CONF_PIN
#define SPEKTRUM_PRIMARY_BIND_CONF_PIN
Definition: spektrum.c:345
spektrum_sat_t::valid
bool valid
True when we received a packet else false.
Definition: spektrum.h:49
sys_time.h
Architecture independent timing functions.
get_sys_time_msec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:86
RADIO_CONTROL_SPEKTRUM_ID
#define RADIO_CONTROL_SPEKTRUM_ID
Definition: abi_sender_ids.h:557
spektrum_event
void spektrum_event(void)
Checks if there is one valid satellite and sets the radio_control structure.
Definition: spektrum.c:287
RadioControl::status
uint8_t status
Definition: radio_control.h:61
RadioControl::frame_cpt
uint8_t frame_cpt
Definition: radio_control.h:65
spektrum_uart_check
static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t *sat)
Check bytes on the UART.
Definition: spektrum.c:259
SPEKTRUM_SECONDARY_BIND_CONF_PIN
#define SPEKTRUM_SECONDARY_BIND_CONF_PIN
Definition: spektrum.c:351
spektrum_sat_t::timer
uint32_t timer
Timer to keep track of the UART synchronisation.
Definition: spektrum.h:50
gpio_toggle
static void gpio_toggle(ioportid_t port, uint16_t pin)
Toggle a gpio output to low level.
Definition: gpio_arch.h:118
SPEKTRUM_CHANNELS_PER_FRAME
#define SPEKTRUM_CHANNELS_PER_FRAME
Maximum amount of RC channels per frame.
Definition: spektrum.h:42
SPEKTRUM_SYS_11_2048_2
#define SPEKTRUM_SYS_11_2048_2
Definition: spektrum.c:100
spektrum_signs
const int8_t spektrum_signs[]
Definition: spektrum.c:84
SPEKTRUM_BIND_PIN_PORT
#define SPEKTRUM_BIND_PIN_PORT
Definition: board.h:390
gpio_get
static uint8_t gpio_get(ioportid_t port, uint16_t pin)
Get level of a gpio.
Definition: gpio_arch.h:88
SPEKTRUM_SYS_22_1024_2
#define SPEKTRUM_SYS_22_1024_2
Allowed system field valaues.
Definition: spektrum.c:99
SPEKTRUM_SECONDARY_BIND_CONF_PORT
#define SPEKTRUM_SECONDARY_BIND_CONF_PORT
Definition: spektrum.c:348
gpio.h
uart_periph
UART peripheral.
Definition: uart.h:72
RC_OK
#define RC_OK
Definition: radio_control.h:49
RC_POLARITY_GPIO_PORT
#define RC_POLARITY_GPIO_PORT
Definition: board.h:154
spektrum_bind
static void spektrum_bind(void)
This function puts the satellite in binding mode.
Definition: spektrum.c:357
type
timer subsystem type(config options) --------------------------------------------(advanced timers using RCC_APB1) TIM1 adc(if USE_AD_TIM1) radio_control/ppm(if USE_PPM_TIM1
spektrum_sat_t::lost_frame_cnt
uint8_t lost_frame_cnt
Amount of RC frames lost.
Definition: spektrum.h:51
spektrum_init_sat
static void spektrum_init_sat(struct spektrum_sat_t *sat)
Initialize a spektrum sattelite.
Definition: spektrum.c:107
SPEKTRUM_PRIMARY_BIND_CONF_PORT
#define SPEKTRUM_PRIMARY_BIND_CONF_PORT
By default, the same pin is used for pulse train and uart rx, but they can be different if needed.
Definition: spektrum.c:342
gpio_setup_input_pullup
void gpio_setup_input_pullup(ioportid_t port, uint16_t gpios)
Setup one or more pins of the given GPIO port as inputs with pull up resistor enabled.
Definition: gpio_arch.c:47
spektrum_sat_t::values
int16_t values[SPEKTRUM_MAX_CHANNELS]
RC channel values.
Definition: spektrum.h:54
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
gpio_set
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
SPEKTRUM_SYS_11_2048_X
#define SPEKTRUM_SYS_11_2048_X
Definition: spektrum.c:102
spektrum_t
Definition: spektrum.h:58
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67