PX4 Firmware
PX4 Autopilot Software http://px4.io
spektrum_rc.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 
35 /**
36  * @file spektrum_rc.cpp
37  *
38  * This is a driver for a Spektrum satellite receiver connected to a Snapdragon
39  * on the serial port. By default port J12 (next to J13, power module side) is used.
40  */
41 
42 #include <string.h>
43 
44 #include <px4_platform_common/tasks.h>
45 #include <px4_platform_common/posix.h>
46 #include <px4_platform_common/getopt.h>
47 
48 #include <lib/rc/dsm.h>
49 #include <drivers/drv_rc_input.h>
50 #include <drivers/drv_hrt.h>
51 
52 #include <uORB/uORB.h>
53 #include <uORB/topics/input_rc.h>
54 
55 // Snapdraogon: use J12 (next to J13, power module side)
56 #define SPEKTRUM_UART_DEVICE_PATH "/dev/tty-3"
57 
58 #define UNUSED(x) (void)(x)
59 
60 extern "C" { __EXPORT int spektrum_rc_main(int argc, char *argv[]); }
61 
62 
63 namespace spektrum_rc
64 {
65 
66 volatile bool _task_should_exit = false;
67 static bool _is_running = false;
68 static px4_task_t _task_handle = -1;
69 
70 int start();
71 int stop();
72 int info();
73 void usage();
74 void task_main(int argc, char *argv[]);
75 
76 void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
77  hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi,
78  input_rc_s &input_rc);
79 
80 void task_main(int argc, char *argv[])
81 {
82  const char *device_path = SPEKTRUM_UART_DEVICE_PATH;
83  int ch;
84  int myoptind = 1;
85  const char *myoptarg = NULL;
86 
87  while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
88  switch (ch) {
89  case 'd':
90  device_path = myoptarg;
91  break;
92 
93  default:
94  break;
95  }
96  }
97 
98  int uart_fd = dsm_init(device_path);
99 
100  if (uart_fd < 1) {
101  PX4_ERR("dsm init failed");
102  return;
103  }
104 
105  orb_advert_t rc_pub = nullptr;
106 
107  // Use a buffer size of the double of the minimum, just to be safe.
108  uint8_t rx_buf[2 * DSM_BUFFER_SIZE];
109 
110  _is_running = true;
111  uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
112  uint16_t raw_rc_count = 0;
113 
114  // Main loop
115  while (!_task_should_exit) {
116 
117  int newbytes = ::read(uart_fd, &rx_buf[0], sizeof(rx_buf));
118 
119  if (newbytes < 0) {
120  PX4_WARN("read failed");
121  continue;
122  }
123 
124  if (newbytes == 0) {
125  continue;
126  }
127 
128  const hrt_abstime now = hrt_absolute_time();
129 
130  bool dsm_11_bit;
131  unsigned frame_drops;
132  int8_t dsm_rssi;
133 
134  // parse new data
135  bool rc_updated = dsm_parse(now, rx_buf, newbytes, &raw_rc_values[0], &raw_rc_count,
136  &dsm_11_bit, &frame_drops, &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS);
137  UNUSED(dsm_11_bit);
138 
139  if (rc_updated) {
140 
141  input_rc_s input_rc = {};
142 
143  fill_input_rc(raw_rc_count, raw_rc_values, now, false, false, frame_drops, dsm_rssi,
144  input_rc);
145 
146  if (rc_pub == nullptr) {
147  rc_pub = orb_advertise(ORB_ID(input_rc), &input_rc);
148 
149  } else {
150  orb_publish(ORB_ID(input_rc), rc_pub, &input_rc);
151  }
152  }
153 
154  // sleep since no poll for qurt
155  usleep(10000);
156 
157  }
158 
159  orb_unadvertise(rc_pub);
160  dsm_deinit();
161 
162  _is_running = false;
163 }
164 
165 void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
166  hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi,
167  input_rc_s &input_rc)
168 {
169  input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
170 
171  input_rc.channel_count = raw_rc_count;
172 
173  if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
174  input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
175  }
176 
177  unsigned valid_chans = 0;
178 
179  for (unsigned i = 0; i < input_rc.channel_count; ++i) {
180  input_rc.values[i] = raw_rc_values[i];
181 
182  if (raw_rc_values[i] != UINT16_MAX) {
183  valid_chans++;
184  }
185  }
186 
187  input_rc.timestamp = now;
188  input_rc.timestamp_last_signal = input_rc.timestamp;
189  input_rc.rc_ppm_frame_length = 0;
190 
191  /* fake rssi if no value was provided */
192  if (rssi == -1) {
193 
194  input_rc.rssi = 255;
195 
196  } else {
197  input_rc.rssi = rssi;
198  }
199 
200  if (valid_chans == 0) {
201  input_rc.rssi = 0;
202  }
203 
204  input_rc.rc_failsafe = failsafe;
205  input_rc.rc_lost = (valid_chans == 0);
206  input_rc.rc_lost_frame_count = frame_drops;
207  input_rc.rc_total_frame_count = 0;
208 }
209 
210 int start(int argc, char *argv[])
211 {
212  if (_is_running) {
213  PX4_WARN("already running");
214  return -1;
215  }
216 
217  _task_should_exit = false;
218 
219  _task_handle = px4_task_spawn_cmd("spektrum_rc_main",
220  SCHED_DEFAULT,
221  SCHED_PRIORITY_DEFAULT,
222  2000,
223  (px4_main_t)&task_main,
224  (char *const *)argv);
225 
226  if (_task_handle < 0) {
227  PX4_ERR("task start failed");
228  return -1;
229  }
230 
231  return 0;
232 }
233 
234 int stop()
235 {
236  if (!_is_running) {
237  PX4_WARN("not running");
238  return -1;
239  }
240 
241  _task_should_exit = true;
242 
243  while (_is_running) {
244  usleep(200000);
245  PX4_INFO(".");
246  }
247 
248  _task_handle = -1;
249  return 0;
250 }
251 
252 int info()
253 {
254  PX4_INFO("running: %s", _is_running ? "yes" : "no");
255 
256  return 0;
257 }
258 
259 void
261 {
262  PX4_INFO("Usage: spektrum_rc {start|info|stop}");
263 }
264 
265 } // namespace spektrum_rc
266 
267 
268 int spektrum_rc_main(int argc, char *argv[])
269 {
270  int myoptind = 1;
271 
272  if (argc <= 1) {
274  return 1;
275  }
276 
277  const char *verb = argv[myoptind];
278 
279 
280  if (!strcmp(verb, "start")) {
281  return spektrum_rc::start(argc - 1, argv + 1);
282  }
283 
284  else if (!strcmp(verb, "stop")) {
285  return spektrum_rc::stop();
286  }
287 
288  else if (!strcmp(verb, "info")) {
289  return spektrum_rc::info();
290  }
291 
292  else {
294  return 1;
295  }
296 }
R/C input interface.
RC protocol definition for Spektrum RC.
int32_t rssi
Definition: input_rc.h:72
bool rc_failsafe
Definition: input_rc.h:77
API for the uORB lightweight object broker.
volatile bool _task_should_exit
Definition: spektrum_rc.cpp:66
uint16_t rc_ppm_frame_length
Definition: input_rc.h:75
Definition: I2C.hpp:51
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
High-resolution timer with callouts and timekeeping.
uint16_t rc_lost_frame_count
Definition: input_rc.h:73
void dsm_deinit()
Definition: dsm.cpp:313
int stop()
Stop the driver.
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
void task_main(int argc, char *argv[])
Definition: spektrum_rc.cpp:80
uint16_t values[18]
Definition: input_rc.h:76
uint32_t channel_count
Definition: input_rc.h:71
static px4_task_t _task_handle
Definition: spektrum_rc.cpp:68
bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, int8_t *rssi_percent, uint16_t max_channels)
Definition: dsm.cpp:640
int info()
Print a little info about the driver.
#define SPEKTRUM_UART_DEVICE_PATH
Definition: spektrum_rc.cpp:56
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
int dsm_init(const char *device)
Initialize the DSM receive functionality.
Definition: dsm.cpp:293
uint16_t rc_total_frame_count
Definition: input_rc.h:74
bool rc_lost
Definition: input_rc.h:78
#define UNUSED(x)
Definition: spektrum_rc.cpp:58
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
static bool _is_running
Definition: spektrum_rc.cpp:67
__EXPORT int spektrum_rc_main(int argc, char *argv[])
#define DSM_BUFFER_SIZE
Definition: dsm.h:54
void usage()
Prints info about the driver argument usage.
int start()
Attempt to start driver on all available I2C busses.
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).
void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi, input_rc_s &input_rc)