From 897b541b12d5782af51ce0a78658cc153bd13544 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Tue, 9 Jul 2013 20:37:00 -0400 Subject: General cleanup of /dev/px4io and /dev/px4fmu - Use distinct common symbols for px4io and px4fmu device files, and use instead of hardcoded filenames - Use common symbols defining px4io bits consistently between px4fmu and px4io builds. --- .../ardrone_interface/ardrone_motor_control.c | 2 +- src/drivers/drv_gpio.h | 37 ++-------------------- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 10 +++--- 4 files changed, 10 insertions(+), 41 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073..be8968a4e 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -109,7 +109,7 @@ int ar_multiplexing_init() { int fd; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { warn("GPIO: open fail"); diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 7e3998fca..510983d4b 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -63,43 +63,12 @@ * they also export GPIO-like things. This is always the GPIOs on the * main board. */ -# define GPIO_DEVICE_PATH "/dev/px4fmu" +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" - -#endif - -#ifndef GPIO_DEVICE_PATH +#ifndef PX4IO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5147ac500..a98b527b1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -166,7 +166,7 @@ PX4FMU *g_fmu; } // namespace PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), + CDev("fmuservo", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), _pwm_alt_rate(50), diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1adefdea5..08aef7069 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -329,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -1507,7 +1507,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~1; + bits &= ~PX4IO_RELAY1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1515,7 +1515,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1524,7 +1524,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); @@ -1731,7 +1731,7 @@ test(void) int direction = 1; int ret; - fd = open(GPIO_DEVICE_PATH, O_WRONLY); + fd = open(PX4IO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); -- cgit v1.2.3