diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 927406108a..4decc324ef 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -100,4 +100,11 @@ struct rc_input_values { */ ORB_DECLARE(input_rc); +#define _RC_INPUT_BASE 0x2b00 + +/** Fetch R/C input values into (rc_input_values *)arg */ + +#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0) + + #endif /* _DRV_RC_INPUT_H */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index bdddd1b1af..51476ddd3b 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -313,6 +313,8 @@ PX4IO::init() if (ret != OK) return ret; + _retries = 2; + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); @@ -1032,6 +1034,36 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024)); break; + case RC_INPUT_GET: { + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; + + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); + if (ret != OK) + break; + + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } + + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); + break; + } + default: /* not a recognised value */ ret = -ENOTTY;