forked from Archive/PX4-Autopilot
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:
parent
328f4f8c87
commit
897b541b12
|
@ -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");
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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),
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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");
|
||||
|
|
Loading…
Reference in New Issue