diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073d..be8968a4ea 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 7e3998fca1..510983d4bd 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 5147ac500d..a98b527b1b 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 1adefdea5b..08aef70692 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"); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 8b4c0cb308..1aef739c7b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,11 +53,7 @@ #include #include #include - -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#include struct gpio_led_s { struct work_s work; @@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; if (priv->use_io) { - gpio_dev = "/dev/px4io"; - + gpio_dev = PX4IO_DEVICE_PATH; } else { - gpio_dev = "/dev/px4fmu"; + gpio_dev = PX4FMU_DEVICE_PATH; } /* open GPIO device */ @@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ + /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ @@ -263,7 +259,6 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); - } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6ee5c28342..88d8cc87c1 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -157,6 +157,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ + +/* px4io relay bit definitions */ +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ enum { /* DSM bind states */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 805eb7eccd..a922362b69 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & (1 << 0) ? 1 : 0); - POWER_RELAY2(value & (1 << 1) ? 1 : 0); - POWER_ACC1(value & (1 << 2) ? 1 : 0); - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); + POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); + POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); + POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); break; case PX4IO_P_SETUP_SET_DEBUG: diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index ab536d9560..62a8732702 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[]) int fd; int ret = 0; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n");