Paparazzi UAS  v6.0_unstable-20-gfdbd467-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
25 
33 #include "std.h"
35 #include "mcu_periph/uart.h"
36 #include "mcu_periph/gpio.h"
37 #include "mcu_periph/sys_time.h"
38 
39 /* Check if primary receiver is defined */
40 #ifndef SPEKTRUM_PRIMARY_UART
41 #error "You must at least define the primary Spektrum satellite receiver."
42 #endif
43 
44 /* Number of low pulses sent during binding to the satellite receivers
45  * Spektrum documentation recommend that master and slave receivers
46  * should be configured in DSMX 11ms mode, other modes (DSM2, 22ms) will be
47  * automatically supported if transmitter is not compatible.
48  * But this this is only if receiver can handle DSMX. If it is not the case,
49  * DSM2 must be used, otherwise the system filed will be wrong.
50  * So DSM2 is used by default and DSMX can be enable with USE_DSMX flag.
51  */
52 #if USE_DSMX
53 #define SPEKTRUM_MASTER_RECEIVER_PULSES 9 // only one receiver should be in master mode
54 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 10
55 #else
56 #define SPEKTRUM_MASTER_RECEIVER_PULSES 5 // only one receiver should be in master mode
57 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 6
58 #endif
59 
60 /* Set polarity using RC_POLARITY_GPIO. */
61 #ifndef RC_SET_POLARITY
62 #define RC_SET_POLARITY gpio_clear
63 #endif
64 
65 /* Busy wait to let the receiver starts properly
66  * This should be reduced when the MCU takes longer to start
67  */
68 #ifndef SPEKTRUM_BIND_WAIT
69 #define SPEKTRUM_BIND_WAIT 60000
70 #endif
71 
72 /* Spektrum system type can be force (see list below)
73  * by default it is unknown type and will be determined from incoming frames
74  */
75 #ifndef SPEKTRUM_SYS_TYPE
76 #define SPEKTRUM_SYS_TYPE 0 // unknown type, determined from incoming frame
77 #endif
78 
79 // in case the number of channel is less than maximum
81 
82 /* Default spektrum values */
83 static struct spektrum_t spektrum = {
84  .valid = false,
85  .tx_type = SPEKTRUM_SYS_TYPE, // unknown type by default
86 };
87 
95 #define SPEKTRUM_SYS_22_1024_2 0x01 // 22ms 1024 DSM2
96 #define SPEKTRUM_SYS_11_2048_2 0x12 // 11ms 2048 DSM2
97 #define SPEKTRUM_SYS_22_2048_X 0xa2 // 22ms 2048 DSMX
98 #define SPEKTRUM_SYS_11_2048_X 0xb2 // 11ms 2048 DSMX
99 
100 static void spektrum_bind(void);
101 
103 static inline void spektrum_init_sat(struct spektrum_sat_t *sat)
104 {
105  sat->valid = false;
106  sat->timer = get_sys_time_msec();
107  sat->idx = 0;
108 
109  // Initialize values
110  for (uint8_t i = 0; i < SPEKTRUM_MAX_CHANNELS; i++) {
111  sat->values[i] = 0;
112  }
113 }
114 
115 /*****************************************************************************
116  *
117  * spektrum_try_bind(void) must called on powerup as spektrum
118  * satellites can only bind immediately after power up also it must be called
119  * before the call to SpektrumUartInit as we leave them with their Rx pins set
120  * as outputs.
121  *
122  *****************************************************************************/
124 {
125 #ifdef SPEKTRUM_BIND_PIN_PORT
126 #ifdef SPEKTRUM_BIND_PIN_HIGH
127  /* Init GPIO for the bind pin, we enable the pulldown resistor.
128  * (esden) As far as I can tell only navstick is using the PIN LOW version of
129  * the bind pin, but I assume this should not harm anything. If I am mistaken
130  * than I appologise for the inconvenience. :)
131  */
133 
134  sys_time_usleep(10); // wait for electrical level to stabilize
135 
136  /* Exit if the BIND_PIN is low, it needs to
137  be pulled high at startup to initiate bind */
139  spektrum_bind();
140  }
141 #else
142  /* Init GPIO for the bind pin, we enable the pullup resistor in case we have
143  * a floating pin that does not have a hardware pullup resistor as it is the
144  * case with Lisa/M and Lisa/MX prior to version 2.1.
145  */
147 
148  sys_time_usleep(10); // wait for electrical level to stabilize
149 
150  /* Exit if the BIND_PIN is high, it needs to
151  be pulled low at startup to initiate bind */
153  spektrum_bind();
154  }
155 #endif
156 #endif
157 }
158 
159 
162 {
163 
164  for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
165  spektrum.signs[i] = spektrum_signs[i];
166  }
167 
168  // Set polarity to normal on boards that can change this
169 #ifdef RC_POLARITY_GPIO_PORT
172 #endif
173 
174  // Initialize all the UART's in the satellites
175  spektrum_init_sat(&spektrum.satellites[0]);
176 #ifdef SPEKTRUM_SECONDARY_UART
177  spektrum_init_sat(&spektrum.satellites[1]);
178 #endif
179 }
180 
181 /* Parse a sattelite channel */
182 static inline void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
183 {
184  // This channel is not used
185  if (chan == 0xFFFF) {
186  return;
187  }
188 
189  if (spektrum.tx_type == SPEKTRUM_SYS_22_1024_2) {
190  // We got a 10bit precision packet
191  uint8_t chan_num = (chan & 0xFC00) >> 10;
192  sat->values[chan_num] = chan & 0x03FF;
193  if (chan_num == RADIO_THROTTLE) {
194  // scale full range to pprz_t
195  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
196  // remove an offset of 2400 = 171 * MAX_PPRZ / 684
197  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 684) - 2400;
198  } else {
199  sat->values[chan_num] -= (1 << 9); // substract 2^9 to get a value between [-512;512]
200  // scale full range to pprz_t
201  // but since 512 correspond to 150%, scale with 512/1.5 ~ 342
202  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 342;
203  }
204  }
205  else {
206  // We got a 11bit precision packet
207  uint8_t chan_num = (chan & 0x7800) >> 11;
208  sat->values[chan_num] = chan & 0x07FF;
209  if (chan_num == RADIO_THROTTLE) {
210  // scale full range to pprz_t
211  // but since 2048 correspond to 150%, scale with 2048/1.5 ~ 1368
212  // remove an offset of 2400 = 234 * MAX_PPRZ / 1368
213  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 1368) - 2400;
214  } else {
215  sat->values[chan_num] -= (1 << 10); // substract 2^10 to get a value between [-1024;1024]
216  // scale full range to pprz_t
217  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
218  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 684;
219  }
220  }
221 
222  // mark a valid frame
223  sat->valid = true;
224  spektrum.valid = true;
225 }
226 
227 /* Spektrum parser for a satellite */
228 static inline void spektrum_parser(struct spektrum_sat_t *sat)
229 {
230  // Parse packet
231  sat->lost_frame_cnt = sat->buf[0];
232  // For now ignore the second byte (which could be the TX type)
233  // if frame type is still unknown, try to find it in the 'system' byte
234  // only the primary receiver should have a valid type
235  if (spektrum.tx_type == 0) {
236  uint8_t type = sat->buf[1];
237  if (type == SPEKTRUM_SYS_22_1024_2 || type == SPEKTRUM_SYS_11_2048_2 ||
238  type == SPEKTRUM_SYS_22_2048_X || type == SPEKTRUM_SYS_11_2048_X) {
239  // we have a valid type, we assume it comes from primary receiver
240  spektrum.tx_type = type;
241  } else {
242  // return and drop frame as we don't know what to do with it
243  return;
244  }
245  }
246  // parse servo channels
247  for (uint8_t i = 2; i < 2*SPEKTRUM_CHANNELS_PER_FRAME+2; i = i+2) {
248  uint16_t chan = (((uint16_t)sat->buf[i]) << 8) | ((uint16_t)sat->buf[i+1]);
249  spektrum_parse_channel(sat, chan);
250  }
251 }
252 
254 static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t *sat)
255 {
256  // detect sync space based on frame spacing
258  if (t - sat->timer > SPEKTRUM_MIN_FRAME_SPACE) {
259  sat->timer = t; // reset counter
260  uint16_t bytes_cnt = uart_char_available(dev); // The amount of bytes in the buffer
261  // sync space detected but buffer not empty, flush data
262  if (bytes_cnt > 0) {
263  for (uint8_t i = 0; i < bytes_cnt; i++) {
264  uart_getch(dev);
265  }
266  }
267  }
268  // check and parse bytes in uart buffer
269  while (uart_char_available(dev)) {
270  sat->buf[sat->idx++] = uart_getch(dev);
271  sat->timer = t; // reset counter
272  if (sat->idx == SPEKTRUM_FRAME_LEN) {
273  // buffer is full, parse frame
274  spektrum_parser(sat);
275  sat->idx = 0; // reset index
276  break; // stop here to handle RC frame
277  }
278  }
279 }
280 
282 void spektrum_event(void (*frame_handler)(void))
283 {
284  spektrum_uart_check(&SPEKTRUM_PRIMARY_UART, &spektrum.satellites[0]);
285 
286 #ifdef SPEKTRUM_SECONDARY_UART
287  spektrum_uart_check(&SPEKTRUM_SECONDARY_UART, &spektrum.satellites[1]);
288 #endif
289 
290  // Whenever we received a valid RC packet
291  if (spektrum.valid) {
293  spektrum.valid = false;
294 
295  // Find the first satellite that has a valid packet
296  for (uint8_t i = 0; i < SPEKTRUM_SATELLITES_NB; i++) {
297  if (i < sat_id) {
298  sat_id = i;
299  }
300  if (spektrum.satellites[i].valid) {
301  spektrum.satellites[i].valid = false;
302  }
303  }
304 
305  // Failsafe case if found satellite is out of bound (Should not happen)
306  if (sat_id >= SPEKTRUM_SATELLITES_NB) {
307  return;
308  }
309 
310  // Set the radio control status
314 
315  // Copy the radio control channels
316  for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
317  radio_control.values[i] = spektrum.satellites[sat_id].values[i] * spektrum.signs[i];
318  Bound(radio_control.values[i], -MAX_PPRZ, MAX_PPRZ);
319  }
320 
321  // We got a valid frame so execute the frame handler
322  (*frame_handler)();
323  }
324 }
325 
326 /* Defines needed for easy access of port information */
327 #define _UART_RX_PORT(i) i ## _PORT_RX
328 #define UART_RX_PORT(i) _UART_RX_PORT(i)
329 #define _UART_RX(i) i ## _RX
330 #define UART_RX(i) _UART_RX(i)
331 
336 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PORT
337 #define SPEKTRUM_PRIMARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_PRIMARY_UART_UPPER)
338 #endif
339 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PIN
340 #define SPEKTRUM_PRIMARY_BIND_CONF_PIN UART_RX(SPEKTRUM_PRIMARY_UART_UPPER)
341 #endif
342 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PORT
343 #define SPEKTRUM_SECONDARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_SECONDARY_UART_UPPER)
344 #endif
345 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PIN
346 #define SPEKTRUM_SECONDARY_BIND_CONF_PIN UART_RX(SPEKTRUM_SECONDARY_UART_UPPER)
347 #endif
348 
352 static void UNUSED spektrum_bind(void)
353 {
354 
355  /* Master receiver Rx push-pull */
357  /* Master receiver RX line, drive high */
359 
360 #ifdef SPEKTRUM_SECONDARY_UART
361  /* Slave receiver Rx push-pull */
363  /* Slave receiver RX line, drive high */
365 #endif
366 
367  /* bind pulses should be issued within 200ms after power up
368  * wait a bit to let the receiver start properly,
369  * set to 0 to disable */
370 #if SPEKTRUM_BIND_WAIT
372 #endif
373 
374  /* Transmit the bind pulses */
375  for (int i = 0; i < 2 * SPEKTRUM_MASTER_RECEIVER_PULSES ; i++) {
377  sys_time_usleep(120);
378  }
379 #ifdef SPEKTRUM_SECONDARY_UART
380  for (int i = 0; i < 2 * SPEKTRUM_SLAVE_RECEIVER_PULSES; i++) {
382  sys_time_usleep(120);
383  }
384 #endif
385 
386  /* Set conf pin as input in case it is different from RX pin */
388 #ifdef SPEKTRUM_SECONDARY_UART
390 #endif
391 }
392 
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
void radio_control_impl_init(void)
Main Radio initialization.
Definition: spektrum.c:161
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t *sat)
Check bytes on the UART.
Definition: spektrum.c:254
#define SPEKTRUM_MAX_CHANNELS
Definition: spektrum.h:44
#define RC_POLARITY_GPIO_PORT
Definition: board.h:157
#define SPEKTRUM_SECONDARY_BIND_CONF_PORT
Definition: spektrum.c:343
#define SPEKTRUM_SYS_22_1024_2
Allowed system field valaues.
Definition: spektrum.c:95
#define RC_SET_POLARITY
Definition: spektrum.c:62
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:968
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t status
Definition: radio_control.h:64
struct spektrum_sat_t satellites[SPEKTRUM_SATELLITES_NB]
All the satellites connected.
Definition: spektrum.h:65
Some architecture independent helper functions for GPIOs.
uint8_t last_wp UNUSED
Definition: navigation.c:96
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
#define SPEKTRUM_PRIMARY_BIND_CONF_PIN
Definition: spektrum.c:340
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
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
static void gpio_toggle(ioportid_t port, uint16_t pin)
Toggle a gpio output to low level.
Definition: gpio_arch.h:118
const int8_t spektrum_signs[]
Definition: spektrum.c:80
#define RC_POLARITY_GPIO_PIN
Definition: board.h:158
#define SPEKTRUM_SYS_11_2048_X
Definition: spektrum.c:98
void spektrum_try_bind(void)
Definition: spektrum.c:123
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
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
void sys_time_usleep(uint32_t us)
sys_time_usleep(uint32_t us)
Definition: sys_time_arch.c:95
void spektrum_event(void(*frame_handler)(void))
Checks if there is one valid satellite and sets the radio_control structure.
Definition: spektrum.c:282
static void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
Definition: spektrum.c:182
#define SPEKTRUM_MASTER_RECEIVER_PULSES
Definition: spektrum.c:56
Architecture independent timing functions.
#define SPEKTRUM_SYS_11_2048_2
Definition: spektrum.c:96
uint8_t buf[SPEKTRUM_FRAME_LEN]
input buffer
Definition: spektrum.h:55
UART peripheral.
Definition: uart.h:70
bool valid
True when we received a packet else false.
Definition: spektrum.h:62
uint32_t timer
Timer to keep track of the UART synchronisation.
Definition: spektrum.h:53
struct RadioControl radio_control
Definition: radio_control.c:30
#define SPEKTRUM_FRAME_LEN
16 bytes in a standard frame
Definition: spektrum.h:41
uint8_t lost_frame_cnt
Amount of RC frames lost.
Definition: spektrum.h:54
uint8_t tx_type
Transmitter type encoded (see wiki)
Definition: spektrum.h:63
#define SPEKTRUM_SYS_TYPE
Definition: spektrum.c:76
#define SPEKTRUM_SECONDARY_BIND_CONF_PIN
Definition: spektrum.c:346
#define SPEKTRUM_BIND_PIN
Definition: board.h:460
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
#define SPEKTRUM_SATELLITES_NB
Definition: spektrum.h:38
#define RADIO_CONTROL_SPEKTRUM_SIGNS
#define RC_OK
Definition: radio_control.h:56
int8_t signs[RADIO_CONTROL_NB_CHANNEL]
Signs for the RC channels.
Definition: spektrum.h:64
static void spektrum_bind(void)
This function puts the satellite in binding mode.
Definition: spektrum.c:352
static uint8_t gpio_get(ioportid_t port, uint16_t pin)
Get level of a gpio.
Definition: gpio_arch.h:88
int16_t values[SPEKTRUM_MAX_CHANNELS]
RC channel values.
Definition: spektrum.h:57
#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:337
#define SPEKTRUM_BIND_PIN_PORT
Definition: board.h:461
bool valid
True when we received a packet else false.
Definition: spektrum.h:52
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
uint8_t time_since_last_frame
Definition: radio_control.h:65
uint8_t idx
input buffer index
Definition: spektrum.h:56
Radio control spektrum interface.
#define MAX_PPRZ
Definition: paparazzi.h:8
#define SPEKTRUM_MIN_FRAME_SPACE
Minum amount of time between frames (7ms), in fact either 11 or 22 ms.
Definition: spektrum.h:45
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
static void spektrum_parser(struct spektrum_sat_t *sat)
Definition: spektrum.c:228
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
#define SPEKTRUM_BIND_WAIT
Definition: spektrum.c:69
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
#define SPEKTRUM_SLAVE_RECEIVER_PULSES
Definition: spektrum.c:57
#define SPEKTRUM_CHANNELS_PER_FRAME
Maximum amount of RC channels per frame.
Definition: spektrum.h:42
static struct spektrum_t spektrum
Definition: spektrum.c:83
uint8_t frame_cpt
Definition: radio_control.h:68
#define SPEKTRUM_SYS_22_2048_X
Definition: spektrum.c:97
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
static void spektrum_init_sat(struct spektrum_sat_t *sat)
Initialize a spektrum sattelite.
Definition: spektrum.c:103