Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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 /* Changed radio control order Notice */
45 INFO("Radio-Control now follows PPRZ sign convention: this means you might need to reverese some channels in your transmitter: RollRight / PitchUp / YawRight / FullThrottle / Auto2 are positive deflections")
46 
47 /* Number of low pulses sent during binding to the satellite receivers
48  * Spektrum documentation recommend that master and slave receivers
49  * should be configured in DSMX 11ms mode, other modes (DSM2, 22ms) will be
50  * automatically supported if transmitter is not compatible.
51  * But this this is only if receiver can handle DSMX. If it is not the case,
52  * DSM2 must be used, otherwise the system filed will be wrong.
53  * So DSM2 is used by default and DSMX can be enable with USE_DSMX flag.
54  */
55 #if USE_DSMX
56 #define SPEKTRUM_MASTER_RECEIVER_PULSES 9 // only one receiver should be in master mode
57 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 10
58 #else
59 #define SPEKTRUM_MASTER_RECEIVER_PULSES 5 // only one receiver should be in master mode
60 #define SPEKTRUM_SLAVE_RECEIVER_PULSES 6
61 #endif
62 
63 /* Set polarity using RC_POLARITY_GPIO. */
64 #ifndef RC_SET_POLARITY
65 #define RC_SET_POLARITY gpio_clear
66 #endif
67 
68 /* Busy wait to let the receiver starts properly
69  * This should be reduced when the MCU takes longer to start
70  */
71 #ifndef SPEKTRUM_BIND_WAIT
72 #define SPEKTRUM_BIND_WAIT 60000
73 #endif
74 
75 /* Spektrum system type can be force (see list below)
76  * by default it is unknown type and will be determined from incoming frames
77  */
78 #ifndef SPEKTRUM_SYS_TYPE
79 #define SPEKTRUM_SYS_TYPE 0 // unknown type, determined from incoming frame
80 #endif
81 
82 // in case the number of channel is less than maximum
84 
85 /* Default spektrum values */
86 static struct spektrum_t spektrum = {
87  .valid = false,
88  .tx_type = SPEKTRUM_SYS_TYPE, // unknown type by default
89 };
90 
98 #define SPEKTRUM_SYS_22_1024_2 0x01 // 22ms 1024 DSM2
99 #define SPEKTRUM_SYS_11_2048_2 0x12 // 11ms 2048 DSM2
100 #define SPEKTRUM_SYS_22_2048_X 0xa2 // 22ms 2048 DSMX
101 #define SPEKTRUM_SYS_11_2048_X 0xb2 // 11ms 2048 DSMX
102 
103 static void spektrum_bind(void);
104 
106 static inline void spektrum_init_sat(struct spektrum_sat_t *sat)
107 {
108  sat->valid = false;
109  sat->timer = get_sys_time_msec();
110  sat->idx = 0;
111 
112  // Initialize values
113  for (uint8_t i = 0; i < SPEKTRUM_MAX_CHANNELS; i++) {
114  sat->values[i] = 0;
115  }
116 }
117 
118 /*****************************************************************************
119  *
120  * spektrum_try_bind(void) must called on powerup as spektrum
121  * satellites can only bind immediately after power up also it must be called
122  * before the call to SpektrumUartInit as we leave them with their Rx pins set
123  * as outputs.
124  *
125  *****************************************************************************/
127 {
128 #ifdef SPEKTRUM_BIND_PIN_PORT
129 #ifdef SPEKTRUM_BIND_PIN_HIGH
130  /* Init GPIO for the bind pin, we enable the pulldown resistor.
131  * (esden) As far as I can tell only navstick is using the PIN LOW version of
132  * the bind pin, but I assume this should not harm anything. If I am mistaken
133  * than I appologise for the inconvenience. :)
134  */
136 
137  sys_time_usleep(10); // wait for electrical level to stabilize
138 
139  /* Exit if the BIND_PIN is low, it needs to
140  be pulled high at startup to initiate bind */
142  spektrum_bind();
143  }
144 #else
145  /* Init GPIO for the bind pin, we enable the pullup resistor in case we have
146  * a floating pin that does not have a hardware pullup resistor as it is the
147  * case with Lisa/M and Lisa/MX prior to version 2.1.
148  */
150 
151  sys_time_usleep(10); // wait for electrical level to stabilize
152 
153  /* Exit if the BIND_PIN is high, it needs to
154  be pulled low at startup to initiate bind */
156  spektrum_bind();
157  }
158 #endif
159 #endif
160 }
161 
162 
165 {
166 
167  for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
168  spektrum.signs[i] = spektrum_signs[i];
169  }
170 
171  // Set polarity to normal on boards that can change this
172 #ifdef RC_POLARITY_GPIO_PORT
175 #endif
176 
177  // Initialize all the UART's in the satellites
178  spektrum_init_sat(&spektrum.satellites[0]);
179 #ifdef SPEKTRUM_SECONDARY_UART
180  spektrum_init_sat(&spektrum.satellites[1]);
181 #endif
182 }
183 
184 /* Parse a sattelite channel */
185 static inline void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
186 {
187  // This channel is not used
188  if (chan == 0xFFFF) {
189  return;
190  }
191 
192  if (spektrum.tx_type == SPEKTRUM_SYS_22_1024_2) {
193  // We got a 10bit precision packet
194  uint8_t chan_num = (chan & 0xFC00) >> 10;
195  sat->values[chan_num] = chan & 0x03FF;
196  if (chan_num == RADIO_THROTTLE) {
197  // scale full range to pprz_t
198  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
199  // remove an offset of 2400 = 171 * MAX_PPRZ / 684
200  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 684) - 2400;
201  } else {
202  sat->values[chan_num] -= (1 << 9); // substract 2^9 to get a value between [-512;512]
203  // scale full range to pprz_t
204  // but since 512 correspond to 150%, scale with 512/1.5 ~ 342
205  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 342;
206  }
207  }
208  else {
209  // We got a 11bit precision packet
210  uint8_t chan_num = (chan & 0x7800) >> 11;
211  sat->values[chan_num] = chan & 0x07FF;
212  if (chan_num == RADIO_THROTTLE) {
213  // scale full range to pprz_t
214  // but since 2048 correspond to 150%, scale with 2048/1.5 ~ 1368
215  // remove an offset of 2400 = 234 * MAX_PPRZ / 1368
216  sat->values[chan_num] = ((MAX_PPRZ * sat->values[chan_num]) / 1368) - 2400;
217  } else {
218  sat->values[chan_num] -= (1 << 10); // substract 2^10 to get a value between [-1024;1024]
219  // scale full range to pprz_t
220  // but since 1024 correspond to 150%, scale with 1024/1.5 ~ 684
221  sat->values[chan_num] = (MAX_PPRZ * sat->values[chan_num]) / 684;
222  }
223  }
224 
225  // mark a valid frame
226  sat->valid = true;
227  spektrum.valid = true;
228 }
229 
230 /* Spektrum parser for a satellite */
231 static inline void spektrum_parser(struct spektrum_sat_t *sat)
232 {
233  // Parse packet
234  sat->lost_frame_cnt = sat->buf[0];
235  // For now ignore the second byte (which could be the TX type)
236  // if frame type is still unknown, try to find it in the 'system' byte
237  // only the primary receiver should have a valid type
238  if (spektrum.tx_type == 0) {
239  uint8_t type = sat->buf[1];
240  if (type == SPEKTRUM_SYS_22_1024_2 || type == SPEKTRUM_SYS_11_2048_2 ||
241  type == SPEKTRUM_SYS_22_2048_X || type == SPEKTRUM_SYS_11_2048_X) {
242  // we have a valid type, we assume it comes from primary receiver
243  spektrum.tx_type = type;
244  } else {
245  // return and drop frame as we don't know what to do with it
246  return;
247  }
248  }
249  // parse servo channels
250  for (uint8_t i = 2; i < 2*SPEKTRUM_CHANNELS_PER_FRAME+2; i = i+2) {
251  uint16_t chan = (((uint16_t)sat->buf[i]) << 8) | ((uint16_t)sat->buf[i+1]);
252  spektrum_parse_channel(sat, chan);
253  }
254 }
255 
257 static void spektrum_uart_check(struct uart_periph *dev, struct spektrum_sat_t *sat)
258 {
259  // detect sync space based on frame spacing
261  if (t - sat->timer > SPEKTRUM_MIN_FRAME_SPACE) {
262  sat->timer = t; // reset counter
263  uint16_t bytes_cnt = uart_char_available(dev); // The amount of bytes in the buffer
264  // sync space detected but buffer not empty, flush data
265  if (bytes_cnt > 0) {
266  for (uint8_t i = 0; i < bytes_cnt; i++) {
267  uart_getch(dev);
268  }
269  }
270  }
271  // check and parse bytes in uart buffer
272  while (uart_char_available(dev)) {
273  sat->buf[sat->idx++] = uart_getch(dev);
274  sat->timer = t; // reset counter
275  if (sat->idx == SPEKTRUM_FRAME_LEN) {
276  // buffer is full, parse frame
277  spektrum_parser(sat);
278  sat->idx = 0; // reset index
279  break; // stop here to handle RC frame
280  }
281  }
282 }
283 
285 void spektrum_event(void (*frame_handler)(void))
286 {
287  spektrum_uart_check(&SPEKTRUM_PRIMARY_UART, &spektrum.satellites[0]);
288 
289 #ifdef SPEKTRUM_SECONDARY_UART
290  spektrum_uart_check(&SPEKTRUM_SECONDARY_UART, &spektrum.satellites[1]);
291 #endif
292 
293  // Whenever we received a valid RC packet
294  if (spektrum.valid) {
296  spektrum.valid = false;
297 
298  // Find the first satellite that has a valid packet
299  for (uint8_t i = 0; i < SPEKTRUM_SATELLITES_NB; i++) {
300  if (i < sat_id) {
301  sat_id = i;
302  }
303  if (spektrum.satellites[i].valid) {
304  spektrum.satellites[i].valid = false;
305  }
306  }
307 
308  // Failsafe case if found satellite is out of bound (Should not happen)
309  if (sat_id >= SPEKTRUM_SATELLITES_NB) {
310  return;
311  }
312 
313  // Set the radio control status
317 
318  // Copy the radio control channels
319  for (uint8_t i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) {
320  radio_control.values[i] = spektrum.satellites[sat_id].values[i] * spektrum.signs[i];
321  Bound(radio_control.values[i], -MAX_PPRZ, MAX_PPRZ);
322  }
323 
324  // We got a valid frame so execute the frame handler
325  (*frame_handler)();
326  }
327 }
328 
329 /* Defines needed for easy access of port information */
330 #define _UART_RX_PORT(i) i ## _PORT_RX
331 #define UART_RX_PORT(i) _UART_RX_PORT(i)
332 #define _UART_RX(i) i ## _RX
333 #define UART_RX(i) _UART_RX(i)
334 
339 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PORT
340 #define SPEKTRUM_PRIMARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_PRIMARY_UART_UPPER)
341 #endif
342 #ifndef SPEKTRUM_PRIMARY_BIND_CONF_PIN
343 #define SPEKTRUM_PRIMARY_BIND_CONF_PIN UART_RX(SPEKTRUM_PRIMARY_UART_UPPER)
344 #endif
345 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PORT
346 #define SPEKTRUM_SECONDARY_BIND_CONF_PORT UART_RX_PORT(SPEKTRUM_SECONDARY_UART_UPPER)
347 #endif
348 #ifndef SPEKTRUM_SECONDARY_BIND_CONF_PIN
349 #define SPEKTRUM_SECONDARY_BIND_CONF_PIN UART_RX(SPEKTRUM_SECONDARY_UART_UPPER)
350 #endif
351 
355 static void UNUSED spektrum_bind(void)
356 {
357 
358  /* Master receiver Rx push-pull */
360  /* Master receiver RX line, drive high */
362 
363 #ifdef SPEKTRUM_SECONDARY_UART
364  /* Slave receiver Rx push-pull */
366  /* Slave receiver RX line, drive high */
368 #endif
369 
370  /* bind pulses should be issued within 200ms after power up
371  * wait a bit to let the receiver start properly,
372  * set to 0 to disable */
373 #if SPEKTRUM_BIND_WAIT
375 #endif
376 
377  /* Transmit the bind pulses */
378  for (int i = 0; i < 2 * SPEKTRUM_MASTER_RECEIVER_PULSES ; i++) {
380  sys_time_usleep(120);
381  }
382 #ifdef SPEKTRUM_SECONDARY_UART
383  for (int i = 0; i < 2 * SPEKTRUM_SLAVE_RECEIVER_PULSES; i++) {
385  sys_time_usleep(120);
386  }
387 #endif
388 
389  /* Set conf pin as input in case it is different from RX pin */
391 #ifdef SPEKTRUM_SECONDARY_UART
393 #endif
394 }
395 
void radio_control_impl_init(void)
Main Radio initialization.
Definition: spektrum.c:164
unsigned short uint16_t
Definition: types.h:16
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:257
#define SPEKTRUM_MAX_CHANNELS
Definition: spektrum.h:44
#define RC_POLARITY_GPIO_PORT
Definition: board.h:154
#define SPEKTRUM_SECONDARY_BIND_CONF_PORT
Definition: spektrum.c:346
#define SPEKTRUM_SYS_22_1024_2
Allowed system field valaues.
Definition: spektrum.c:98
#define RC_SET_POLARITY
Definition: spektrum.c:65
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:840
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.
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:324
uint8_t last_wp UNUSED
Definition: navigation.c:92
#define SPEKTRUM_PRIMARY_BIND_CONF_PIN
Definition: spektrum.c:343
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:83
#define RC_POLARITY_GPIO_PIN
Definition: board.h:155
#define SPEKTRUM_SYS_11_2048_X
Definition: spektrum.c:101
void spektrum_try_bind(void)
Definition: spektrum.c:126
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:48
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:285
static void spektrum_parse_channel(struct spektrum_sat_t *sat, uint16_t chan)
Definition: spektrum.c:185
#define SPEKTRUM_MASTER_RECEIVER_PULSES
Definition: spektrum.c:59
Architecture independent timing functions.
#define SPEKTRUM_SYS_11_2048_2
Definition: spektrum.c:99
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
unsigned long uint32_t
Definition: types.h:18
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:79
#define SPEKTRUM_SECONDARY_BIND_CONF_PIN
Definition: spektrum.c:349
#define SPEKTRUM_BIND_PIN
Definition: board.h:364
#define RADIO_THROTTLE
Definition: intermcu_ap.h:39
#define SPEKTRUM_SATELLITES_NB
Definition: spektrum.h:38
#define RADIO_CONTROL_SPEKTRUM_SIGNS
#define RC_OK
Definition: radio_control.h:56
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
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:355
static uint8_t gpio_get(ioportid_t port, uint16_t pin)
Get level of a gpio.
Definition: gpio_arch.h:88
unsigned char uint8_t
Definition: types.h:14
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:340
#define SPEKTRUM_BIND_PIN_PORT
Definition: board.h:365
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
signed char int8_t
Definition: types.h:15
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:231
static void gpio_set(ioportid_t port, uint16_t pin)
Set a gpio output to high level.
Definition: gpio_arch.h:98
#define SPEKTRUM_BIND_WAIT
Definition: spektrum.c:72
#define SPEKTRUM_SLAVE_RECEIVER_PULSES
Definition: spektrum.c:60
#define SPEKTRUM_CHANNELS_PER_FRAME
Maximum amount of RC channels per frame.
Definition: spektrum.h:42
static struct spektrum_t spektrum
Definition: spektrum.c:86
uint8_t frame_cpt
Definition: radio_control.h:68
#define SPEKTRUM_SYS_22_2048_X
Definition: spektrum.c:100
static void spektrum_init_sat(struct spektrum_sat_t *sat)
Initialize a spektrum sattelite.
Definition: spektrum.c:106