diff options
author | Jean Cyr <jcyr@dillobits.com> | 2013-09-18 20:04:22 -0400 |
---|---|---|
committer | Jean Cyr <jcyr@dillobits.com> | 2013-09-18 20:04:22 -0400 |
commit | 89d3e1db281414571fb55b87fb87385a97263cf1 (patch) | |
tree | 2632dde353627381bb48af2e308de1c705c130e8 /src/drivers/px4io | |
parent | 626f433630697a630e5063f4f53cfa570bb4a9df (diff) | |
download | px4-firmware-89d3e1db281414571fb55b87fb87385a97263cf1.tar.gz px4-firmware-89d3e1db281414571fb55b87fb87385a97263cf1.tar.bz2 px4-firmware-89d3e1db281414571fb55b87fb87385a97263cf1.zip |
Implement Spektrum DSM pairing in V2
- Bind control for V2
- Relays and accessory power not supported on V2 hardware
Diffstat (limited to 'src/drivers/px4io')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 67 |
1 files changed, 56 insertions, 11 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5fff1feac..f9fb9eea9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -204,6 +204,7 @@ public: */ int disable_rc_handling(); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Set the DSM VCC is controlled by relay one flag * @@ -223,6 +224,9 @@ public: { return _dsm_vcc_ctl; }; +#endif + + inline uint16_t system_status() const {return _status;} private: device::Device *_interface; @@ -274,8 +278,9 @@ private: float _battery_mamphour_total;///<amp hours consumed so far uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power - +#endif /** * Trampoline to the worker task @@ -461,8 +466,11 @@ PX4IO::PX4IO(device::Device *interface) : _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), - _dsm_vcc_ctl(false) + _battery_last_timestamp(0) +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + ,_dsm_vcc_ctl(false) +#endif + { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -855,7 +863,7 @@ PX4IO::task_main() // See if bind parameter has been set, and reset it to -1 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val >= 0) { + if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); @@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ - if ((dsmMode >= 0) && (dsmMode <= 1)) { + if ((dsmMode == 0) || (dsmMode == 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); } else { @@ -1638,11 +1646,19 @@ PX4IO::print_status() ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + printf("rates 0x%04x default %u alt %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); +#endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) @@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) } case GPIO_RESET: { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; } case GPIO_SET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + } + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_CLEAR: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else + break; + } ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_GET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case MIXERIOCGETOUTPUTCOUNT: @@ -1990,6 +2028,7 @@ start(int argc, char *argv[]) } } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { @@ -1998,6 +2037,7 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } +#endif exit(0); } @@ -2037,8 +2077,10 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); +#endif if (argc < 3) errx(0, "needs argument, use dsm2 or dsmx"); @@ -2049,9 +2091,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + errx(1, "system must not be armed"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - +#endif g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); |