Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
22static int sp_fd;
23
25 GIOCondition condition,
26 gpointer data);
27static void parse_data(char *buf, int len);
28static void handle_frame(void);
29
30
31int 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 }
62 printf("setting term attributes (%s)\n", strerror(errno));
63 return -1;
64 }
68 return 0;
69}
70
71
72
73
75 GIOCondition condition __attribute__((unused)),
76 gpointer data __attribute__((unused)))
77{
78 char buf[255];
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) {
85 }
86 } else {
87 printf("error reading serial: %s\n", _err->message);
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
105static uint32_t idx = 0;
106
107static 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 {
124 }
125 break;
126 case STA_GOT_SYNC_2:
127 frame_buf[idx] = c;
128 idx++;
129 if (idx == FRAME_LEN) {
131 handle_frame();
132 }
133 break;
134 default:
136 break;
137 }
138 }
139}
140
141static void handle_frame(void)
142{
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
uint16_t foo
Definition main_demo5.c:58
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
#define TRUE
Definition std.h:4
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.