44 #include <px4_platform_common/tasks.h> 45 #include <px4_platform_common/posix.h> 46 #include <px4_platform_common/getopt.h> 56 #define SPEKTRUM_UART_DEVICE_PATH "/dev/tty-3" 58 #define UNUSED(x) (void)(x) 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,
85 const char *myoptarg = NULL;
87 while ((ch = px4_getopt(argc, argv,
"d:", &myoptind, &myoptarg)) != EOF) {
90 device_path = myoptarg;
101 PX4_ERR(
"dsm init failed");
111 uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
112 uint16_t raw_rc_count = 0;
115 while (!_task_should_exit) {
117 int newbytes =
::read(uart_fd, &rx_buf[0],
sizeof(rx_buf));
120 PX4_WARN(
"read failed");
131 unsigned frame_drops;
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);
143 fill_input_rc(raw_rc_count, raw_rc_values, now,
false,
false, frame_drops, dsm_rssi,
146 if (rc_pub ==
nullptr) {
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,
169 input_rc.
input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
173 if (input_rc.
channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
177 unsigned valid_chans = 0;
180 input_rc.
values[i] = raw_rc_values[i];
182 if (raw_rc_values[i] != UINT16_MAX) {
197 input_rc.
rssi = rssi;
200 if (valid_chans == 0) {
205 input_rc.
rc_lost = (valid_chans == 0);
213 PX4_WARN(
"already running");
217 _task_should_exit =
false;
219 _task_handle = px4_task_spawn_cmd(
"spektrum_rc_main",
221 SCHED_PRIORITY_DEFAULT,
224 (
char *
const *)argv);
226 if (_task_handle < 0) {
227 PX4_ERR(
"task start failed");
237 PX4_WARN(
"not running");
241 _task_should_exit =
true;
243 while (_is_running) {
254 PX4_INFO(
"running: %s", _is_running ?
"yes" :
"no");
262 PX4_INFO(
"Usage: spektrum_rc {start|info|stop}");
277 const char *verb = argv[myoptind];
280 if (!strcmp(verb,
"start")) {
284 else if (!strcmp(verb,
"stop")) {
288 else if (!strcmp(verb,
"info")) {
RC protocol definition for Spektrum RC.
API for the uORB lightweight object broker.
volatile bool _task_should_exit
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
High-resolution timer with callouts and timekeeping.
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.
void task_main(int argc, char *argv[])
static px4_task_t _task_handle
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)
int info()
Print a little info about the driver.
#define SPEKTRUM_UART_DEVICE_PATH
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
int dsm_init(const char *device)
Initialize the DSM receive functionality.
int orb_unadvertise(orb_advert_t handle)
__EXPORT int spektrum_rc_main(int argc, char *argv[])
void usage()
Prints info about the driver argument usage.
int start()
Attempt to start driver on all available I2C busses.
__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)