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