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.
This commit is contained in:
Jean Cyr 2013-07-09 20:37:00 -04:00
parent 328f4f8c87
commit 897b541b12
8 changed files with 26 additions and 55 deletions

View File

@ -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");

View File

@ -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

View File

@ -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),

View File

@ -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");

View File

@ -53,11 +53,7 @@
#include <uORB/topics/vehicle_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#define PX4IO_RELAY1 (1<<0)
#define PX4IO_RELAY2 (1<<1)
#define PX4IO_ACC1 (1<<2)
#define PX4IO_ACC2 (1<<3)
#include <modules/px4iofirmware/protocol.h>
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);
}

View File

@ -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 */

View File

@ -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:

View File

@ -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");