PX4 Firmware
PX4 Autopilot Software http://px4.io
linux_sbus.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2017 Mark Charl. All rights reserved.
4  * Copyright (C) 2017 Fan.zhang. All rights reserved.
5  * Copyright (C) 2017 PX4 Development Team. All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * 1. Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in
15  * the documentation and/or other materials provided with the
16  * distribution.
17  * 3. Neither the name PX4 nor the names of its contributors may be
18  * used to endorse or promote products derived from this software
19  * without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
28  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
29  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  ****************************************************************************/
35 #include "linux_sbus.h"
36 
37 #include <stdlib.h>
38 #include <string.h>
39 
40 using namespace linux_sbus;
41 
42 //---------------------------------------------------------------------------------------------------------//
44 {
45  int i;
46 
47  /**
48  * initialize the data of each channel
49  */
50  for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
51  _data.values[i] = UINT16_MAX;
52  }
53 
54  _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data);
55 
56  if (nullptr == _rcinput_pub) {
57  PX4_WARN("error: advertise failed");
58  return -1;
59  }
60 
61  /**
62  * open the serial port
63  */
64  _device_fd = open(_device, O_RDWR | O_NONBLOCK | O_CLOEXEC);
65 
66  if (-1 == _device_fd) {
67  PX4_ERR("Open SBUS input %s failed, status %d \n", _device,
68  (int) _device_fd);
69  fflush(stdout);
70  return -1;
71  }
72 
73  struct termios2 tio { };
74 
75  if (0 != ioctl(_device_fd, TCGETS2, &tio)) {
76  close(_device_fd);
77  _device_fd = -1;
78  return -1;
79  }
80 
81  /**
82  * Setting serial port,8E2, non-blocking.100Kbps
83  */
84  tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL
85  | IXON);
86  tio.c_iflag |= (INPCK | IGNPAR);
87  tio.c_oflag &= ~OPOST;
88  tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
89  tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
90  /**
91  * use BOTHER to specify speed directly in c_[io]speed member
92  */
93  tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
94  tio.c_ispeed = 100000;
95  tio.c_ospeed = 100000;
96  tio.c_cc[VMIN] = 25;
97  tio.c_cc[VTIME] = 0;
98 
99  if (0 != ioctl(_device_fd, TCSETS2, &tio)) {
100  close(_device_fd);
101  _device_fd = -1;
102  return -1;
103  }
104 
105  return 0;
106 }
107 //---------------------------------------------------------------------------------------------------------//
108 int RcInput::start(char *device, int channels)
109 {
110  int result = 0;
111  strcpy(_device, device);
112  PX4_INFO("Device %s , channels: %d \n", device, channels);
113  _channels = channels;
114  result = init();
115 
116  if (0 != result) {
117  PX4_WARN("error: RC initialization failed");
118  return -1;
119  }
120 
121  _isRunning = true;
122 
123  ScheduleNow();
124 
125  if (result == -1) {
126  _isRunning = false;
127  }
128 
129  return result;
130 }
131 //---------------------------------------------------------------------------------------------------------//
133 {
134  close(_device_fd);
135  _shouldExit = true;
136 }
137 
138 //---------------------------------------------------------------------------------------------------------//
140 {
141  _measure();
142 
143  if (!_shouldExit) {
144  ScheduleDelayed(RCINPUT_MEASURE_INTERVAL_US);
145  }
146 }
147 //---------------------------------------------------------------------------------------------------------//
149 {
150  uint64_t ts;
151  int nread;
152  fd_set fds;
153  FD_ZERO(&fds);
154  FD_SET(_device_fd, &fds);
155  /**
156  *error counter to count the lost frame
157  */
158  int count = 0; //
159 
160  while (1) {
161  nread = read(_device_fd, &_sbusData, sizeof(_sbusData));
162 
163  if (25 == nread) {
164  /**
165  * Notice: most sbus rx device support sbus1
166  */
167  if (0x0f == _sbusData[0] && 0x00 == _sbusData[24]) {
168  break;
169  }
170  }
171 
172  ++count;
174  }
175 
176  /**
177  * parse sbus data to pwm
178  */
179  _channels_data[0] =
180  (uint16_t)(((_sbusData[1] | _sbusData[2] << 8) & 0x07FF)
182  _channels_data[1] = (uint16_t)(((_sbusData[2] >> 3 | _sbusData[3] << 5)
183  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
184  _channels_data[2] = (uint16_t)(((_sbusData[3] >> 6 | _sbusData[4] << 2
185  | _sbusData[5] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
187  _channels_data[3] = (uint16_t)(((_sbusData[5] >> 1 | _sbusData[6] << 7)
188  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
189  _channels_data[4] = (uint16_t)(((_sbusData[6] >> 4 | _sbusData[7] << 4)
190  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
191  _channels_data[5] = (uint16_t)(((_sbusData[7] >> 7 | _sbusData[8] << 1
192  | _sbusData[9] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
194  _channels_data[6] = (uint16_t)(((_sbusData[9] >> 2 | _sbusData[10] << 6)
195  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
196  _channels_data[7] = (uint16_t)(((_sbusData[10] >> 5 | _sbusData[11] << 3)
197  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; // & the other 8 + 2 channels if you need them
198  _channels_data[8] = (uint16_t)(((_sbusData[12] | _sbusData[13] << 8)
199  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
200  _channels_data[9] = (uint16_t)(((_sbusData[13] >> 3 | _sbusData[14] << 5)
201  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
202  _channels_data[10] = (uint16_t)(((_sbusData[14] >> 6 | _sbusData[15] << 2
203  | _sbusData[16] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
205  _channels_data[11] = (uint16_t)(((_sbusData[16] >> 1 | _sbusData[17] << 7)
206  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
207  _channels_data[12] = (uint16_t)(((_sbusData[17] >> 4 | _sbusData[18] << 4)
208  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
209  _channels_data[13] = (uint16_t)(((_sbusData[18] >> 7 | _sbusData[19] << 1
210  | _sbusData[20] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f)
212  _channels_data[14] = (uint16_t)(((_sbusData[20] >> 2 | _sbusData[21] << 6)
213  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
214  _channels_data[15] = (uint16_t)(((_sbusData[21] >> 5 | _sbusData[22] << 3)
215  & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
216  int i = 0;
217 
218  for (i = 0; i < _channels; ++i) {
219  _data.values[i] = _channels_data[i];
220  }
221 
222  ts = hrt_absolute_time();
223  _data.timestamp = ts;
226  _data.rssi = 100;
227  _data.rc_lost_frame_count = count;
230  _data.rc_failsafe = (_sbusData[23] & (1 << 3)) ? true : false;
231  _data.rc_lost = (_sbusData[23] & (1 << 2)) ? true : false;
232  _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
233 
234  orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
235 }
236 //---------------------------------------------------------------------------------------------------------//
237 /**
238  * Print the correct usage.
239  */
240 static void linux_sbus::usage(const char *reason)
241 {
242  if (reason) {
243  PX4_ERR("%s", reason);
244  }
245 
246  PX4_INFO("Usage: linux_sbus {start|stop|status} -d <device> -c <channel>");
247 }
248 //---------------------------------------------------------------------------------------------------------//
249 int linux_sbus_main(int argc, char **argv)
250 {
251  int start;
252  int command = -1;
253  /**
254  * ttyS1 for default, it can be changed through -d parameter
255  */
256  char device[30] = "/dev/ttyS1";
257  /**
258  * 8 channel for default setting, it can be changed through -c parameter
259  */
260  int max_channel = 8;
261 
262  /**
263  * Parse command line and get device and channels count from consolex
264  */
265  for (start = 0; start < argc; ++start) {
266  if (0 == strcmp(argv[start], "start")) {
267  command = 0;
268  continue;
269  }
270 
271  if (0 == strcmp(argv[start], "stop")) {
272  command = 1;
273  continue;
274  }
275 
276  if (0 == strcmp(argv[start], "status")) {
277  command = 2;
278  continue;
279  }
280 
281  if (0 == strcmp(argv[start], "-d")) {
282  if (argc > (start + 1)) {
283  strcpy(device, argv[start + 1]);
284  }
285 
286  continue;
287  }
288 
289  if (0 == strcmp(argv[start], "-c")) {
290  if (argc > (start + 1)) {
291  max_channel = atoi(argv[start + 1]);
292  }
293 
294  continue;
295  }
296  }
297 
298  /**
299  * Channels count can't be higher than 16;
300  */
301  max_channel = (max_channel > 16) ? 16 : max_channel;
302 
303  if (0 == command) {
304  if (nullptr != rc_input && rc_input->isRunning()) {
305  PX4_WARN("running");
306  return 0;
307  }
308 
309  rc_input = new RcInput();
310 
311  /** Check if alloc worked. */
312  if (nullptr == rc_input) {
313  PX4_ERR("Sbus driver initialization failed");
314  return -1;
315  }
316 
317  int ret = rc_input->start(device, max_channel);
318 
319  if (ret != 0) {
320  PX4_ERR("Linux sbus module failure");
321  }
322 
323  return 0;
324  }
325 
326  if (1 == command) {
327  if (rc_input == nullptr || !rc_input->isRunning()) {
328  PX4_WARN("Not running \n");
329  /* this is not an error */
330  return 0;
331  }
332 
333  rc_input->stop();
334  /**
335  * Wait for task to die
336  */
337  int i = 0;
338 
339  do {
340  /* wait up to 100ms */
341  usleep(100000);
342  } while (rc_input->isRunning() && ++i < 30);
343 
344  delete rc_input;
345  rc_input = nullptr;
346  return 0;
347  }
348 
349  if (2 == command) {
350  if (rc_input != nullptr && rc_input->isRunning()) {
351  PX4_INFO("running");
352 
353  } else {
354  PX4_INFO("Not running \n");
355  }
356 
357  return 0;
358  }
359 
361  "Usage: linux_sbus start|stop|status -d <device> -c <channel>");
362  return 0;
363 }
364 //---------------------------------------------------------------------------------------------------------//
static void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
int32_t rssi
Definition: input_rc.h:72
#define SBUS_SCALE_FACTOR
Definition: linux_sbus.h:64
bool rc_failsafe
Definition: input_rc.h:77
#define RCINPUT_MEASURE_INTERVAL_US
Definition: linux_sbus.h:57
static RcInput * rc_input
Definition: linux_sbus.h:117
int linux_sbus_main(int argc, char **argv)
Definition: linux_sbus.cpp:249
struct input_rc_s _data
Definition: linux_sbus.h:104
uint16_t rc_ppm_frame_length
Definition: input_rc.h:75
int start(char *device, int channels)
Definition: linux_sbus.cpp:108
void Run() override
Definition: linux_sbus.cpp:139
uint8_t input_source
Definition: input_rc.h:79
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
uint64_t timestamp
Definition: input_rc.h:69
#define SBUS_SCALE_OFFSET
Definition: linux_sbus.h:65
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
uint16_t rc_lost_frame_count
Definition: input_rc.h:73
int _channels_data[16]
serial port device to read SBUS;
Definition: linux_sbus.h:108
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint16_t values[18]
Definition: input_rc.h:76
uint32_t channel_count
Definition: input_rc.h:71
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
orb_advert_t _rcinput_pub
Definition: linux_sbus.h:103
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
uint16_t rc_total_frame_count
Definition: input_rc.h:74
bool rc_lost
Definition: input_rc.h:78
uint8_t _sbusData[25]
Definition: linux_sbus.h:105
uint64_t timestamp_last_signal
Definition: input_rc.h:70
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).