diff options
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 59 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 2 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/dsm.c | 2 |
3 files changed, 47 insertions, 16 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 54b9d50e4..ad0112b9b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,8 +89,6 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -static int dsm_vcc_ctl; - class PX4IO : public device::I2C { public: @@ -131,6 +129,16 @@ public: */ void print_status(); + inline void set_dsm_vcc_ctl(bool enable) + { + _dsm_vcc_ctl = enable; + }; + + inline bool get_dsm_vcc_ctl() + { + return _dsm_vcc_ctl; + }; + private: // XXX unsigned _max_actuators; @@ -175,6 +183,12 @@ private: uint64_t _battery_last_timestamp; /** + * Relay1 is dedicated to controlling DSM receiver power + */ + + bool _dsm_vcc_ctl; + + /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); @@ -315,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -340,7 +354,8 @@ PX4IO::PX4IO() : _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _dsm_vcc_ctl(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -1487,18 +1502,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case GPIO_RESET: - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); + case GPIO_RESET: { + uint32_t bits = (1 << _max_relays) - 1; + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~1; + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; + } case GPIO_SET: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & 1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & 1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: @@ -1635,9 +1663,12 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + int dsm_vcc_ctl; + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { if (dsm_vcc_ctl) { - int fd = open("/dev/px4io", O_WRONLY); + g_dev->set_dsm_vcc_ctl(true); + int fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); ioctl(fd, DSM_BIND_POWER_UP, 0); @@ -1655,7 +1686,7 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); - if (dsm_vcc_ctl == 0) + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); if (argc < 3) @@ -1668,7 +1699,7 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); @@ -1694,8 +1725,8 @@ bind(int argc, char *argv[]) ioctl(fd, DSM_BIND_POWER_UP, 0); close(fd); close(console); - exit(0); - } + exit(0); + } } } } @@ -1709,7 +1740,7 @@ test(void) int direction = 1; int ret; - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 9561c9b1e..43d96fb06 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -145,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { + for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 79e892503..ab6e3fec4 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -339,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; + uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) |