Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_radio_control_spektrum.c
Go to the documentation of this file.
1 #include "nps_radio_control.h"
2 
3 #include <glib.h>
4 #include <stdio.h>
5 #include <termios.h>
6 #include <sys/types.h>
7 #include <sys/stat.h>
8 #include <fcntl.h>
9 #include <string.h>
10 #include <errno.h>
11 #include <inttypes.h>
12 
13 // IUCLC flag translates upper case to lower case (actually needed here?)
14 // but is not in POSIX and OSX
15 #ifndef IUCLC
16 #define IUCLC 0
17 #endif
18 
19 static int sp_fd;
20 
21 static gboolean on_serial_data_received(GIOChannel *source,
22  GIOCondition condition,
23  gpointer data);
24 static void parse_data(char *buf, int len);
25 static void handle_frame(void);
26 
27 
28 int nps_radio_control_spektrum_init(const char *device)
29 {
30 
31  if ((sp_fd = open(device, O_RDWR | O_NONBLOCK)) < 0) {
32  printf("opening %s (%s)\n", device, strerror(errno));
33  return -1;
34  }
35  struct termios termios;
36  termios.c_iflag = 0; // properly initialize variable
37  /* input modes */
38  termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | INPCK | ISTRIP | INLCR | IGNCR
39  | ICRNL | IUCLC | IXON | IXANY | IXOFF | IMAXBEL);
40  termios.c_iflag |= IGNPAR;
41 
42  termios.c_cflag = 0; // properly initialize variable
43  /* control modes*/
44  termios.c_cflag &= ~(CSIZE | PARENB | CRTSCTS | PARODD | HUPCL);
45  termios.c_cflag |= CREAD | CS8 | CSTOPB | CLOCAL;
46 
47  termios.c_lflag = 0; // properly initialize variable
48  /* local modes */
49  termios.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | FLUSHO | PENDIN);
50  termios.c_lflag |= NOFLSH;
51  /* speed */
52  speed_t speed = B115200;
53 
54  if (cfsetispeed(&termios, speed)) {
55  printf("setting term speed (%s)\n", strerror(errno));
56  return -1;
57  }
58  if (tcsetattr(sp_fd, TCSADRAIN, &termios)) {
59  printf("setting term attributes (%s)\n", strerror(errno));
60  return -1;
61  }
62  GIOChannel *channel = g_io_channel_unix_new(sp_fd);
63  g_io_channel_set_encoding(channel, NULL, NULL);
64  g_io_add_watch(channel, G_IO_IN , on_serial_data_received, NULL);
65  return 0;
66 }
67 
68 
69 
70 
71 static gboolean on_serial_data_received(GIOChannel *source,
72  GIOCondition condition __attribute__((unused)),
73  gpointer data __attribute__((unused)))
74 {
75  char buf[255];
76  gsize bytes_read;
77  GError *_err = NULL;
78  GIOStatus st = g_io_channel_read_chars(source, buf, 255, &bytes_read, &_err);
79  if (!_err) {
80  if (st == G_IO_STATUS_NORMAL) {
81  parse_data(buf, bytes_read);
82  }
83  } else {
84  printf("error reading serial: %s\n", _err->message);
85  g_error_free(_err);
86  }
87  return TRUE;
88 }
89 
90 
91 #define SYNC_1 0x03
92 #define SYNC_2 0x12
93 
94 #define STA_UNINIT 0
95 #define STA_GOT_SYNC_1 1
96 #define STA_GOT_SYNC_2 2
97 
99 
100 #define FRAME_LEN 14
102 static uint32_t idx = 0;
103 
104 static void parse_data(char *buf, int len)
105 {
106  int i;
107  for (i = 0; i < len; i++) {
108  int8_t c = buf[i ];
109  switch (status) {
110  case STA_UNINIT:
111  if (c == SYNC_1) {
112  status = STA_GOT_SYNC_1;
113  }
114  break;
115  case STA_GOT_SYNC_1:
116  if (c == SYNC_2) {
117  status = STA_GOT_SYNC_2;
118  idx = 0;
119  } else {
120  status = STA_UNINIT;
121  }
122  break;
123  case STA_GOT_SYNC_2:
124  frame_buf[idx] = c;
125  idx++;
126  if (idx == FRAME_LEN) {
127  status = STA_UNINIT;
128  handle_frame();
129  }
130  break;
131  }
132  }
133 }
134 
135 
136 #define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512)
137 static void handle_frame(void)
138 {
139 
140 
141  nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0) / -340.;
142  nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1) + 340) / 680.;
143  nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2) / -340.;
144  nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3) / -340.;
145  nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5) / 340.;
146 
147 
148  // printf("%f %f %f %f %f \n", nps_radio_control.roll, nps_radio_control.throttle, nps_radio_control.pitch, nps_radio_control.yaw, nps_radio_control.mode);
149 
150 }
#define STA_GOT_SYNC_1
status
Definition: anemotaxis.c:10
static uint32_t idx
static gboolean on_serial_data_received(GIOChannel *source, GIOCondition condition, gpointer data)
#define CHANNEL_OF_FRAME(i)
#define B115200
Definition: uart_arch.h:48
#define STA_UNINIT
static int sp_fd
int nps_radio_control_spektrum_init(const char *device)
#define FRAME_LEN
#define STA_GOT_SYNC_2
struct NpsRadioControl nps_radio_control
static void handle_frame(void)
#define TRUE
Definition: std.h:4
static uint8_t frame_buf[FRAME_LEN]
unsigned long uint32_t
Definition: types.h:18
#define SYNC_2
unsigned char uint8_t
Definition: types.h:14
#define IUCLC
signed char int8_t
Definition: types.h:15
static void parse_data(char *buf, int len)
static uint8_t channel
Definition: ADS8344.c:80
#define SYNC_1