From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 20 Dec 2012 21:31:02 -0800 Subject: [PATCH 1/5] Initial implementation of application access to the PX4IO relays. --- apps/drivers/drv_pwm_output.h | 2 +- apps/drivers/px4io/px4io.cpp | 32 +++++++++++++++++++++++++++++++- apps/px4io/comms.c | 25 +++++++++++++++++++++---- 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 97033f2cd1e3..b2fee65aca0b 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2700 +#define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba73ee..d662e634f2f3 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -114,6 +115,8 @@ class PX4IO : public device::CDev bool _primary_pwm_device; ///< true if we are the default PWM output + uint32_t _relays; ///< state of the PX4IO relays, one bit per relay + volatile bool _switch_armed; ///< PX4IO switch armed state // XXX how should this work? @@ -185,6 +188,7 @@ PX4IO::PX4IO() : _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), + _relays(0), _switch_armed(false), _send_needed(false), _config_needed(false) @@ -497,7 +501,9 @@ PX4IO::io_send() /* publish as we send */ orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); - // XXX relays + /* update relays */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; /* armed and not locked down */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); @@ -559,6 +565,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; break; + case GPIO_RESET: + _relays = 0; + _send_needed = true; + break; + + case GPIO_SET: + case GPIO_CLEAR: + /* make sure only valid bits are being set */ + if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { + ret = EINVAL; + break; + } + if (cmd == GPIO_SET) { + _relays |= arg; + } else { + _relays &= ~arg; + } + _send_needed = true; + break; + + case GPIO_GET: + *(uint32_t *)arg = _relays; + break; + case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _max_actuators; break; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf743c..ffb7fd6799e3 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -178,7 +178,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.fmu_channel_data[i] = cmd->servo_command[i]; /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if(system_state.arm_ok && !cmd->arm_ok) { + if (system_state.arm_ok && !cmd->arm_ok) { system_state.armed = false; } @@ -191,9 +191,26 @@ comms_handle_command(const void *buffer, size_t length) // if (!system_state.arm_ok && system_state.armed) // system_state.armed = false; - /* XXX do relay changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - system_state.relays[i] = cmd->relay_state[i]; + /* handle relay state changes here */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { + if (system_state.relays[i] != cmd->relay_state[i]) { + switch (i) { + case 0: + POWER_ACC1(cmd->relay_state[i]); + break; + case 1: + POWER_ACC2(cmd->relay_state[i]); + break; + case 2: + POWER_RELAY1(cmd->relay_state[i]); + break; + case 3: + POWER_RELAY2(cmd->relay_state[i]); + break; + } + } + system_state.relays[i] != cmd->relay_state[i] + } irqrestore(flags); } From e787fa5bce52e10179cae33df56caa765bfa75e2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 3 Jan 2013 00:33:22 -0800 Subject: [PATCH 2/5] Add FIONWRITE to allow applications to sniff the amount of writable space on a descriptor. Implement this for serial devices only. --- nuttx/drivers/serial/serial.c | 21 +++++++++++++++++++++ nuttx/include/nuttx/fs/ioctl.h | 4 ++++ 2 files changed, 25 insertions(+) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index c650da5db318..24744524fad5 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -688,6 +688,27 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) *(int *)arg = count; } + case FIONWRITE: + { + int count; + irqstate_t state = irqsave(); + + /* determine the number of bytes free in the buffer */ + + if (dev->xmit.head <= dev->xmit.tail) + { + count = dev->xmit.tail - dev->xmit.head - 1; + } + else + { + count = dev->xmit.size - (dev->xmit.head - dev->xmit.tail) - 1; + } + + irqrestore(state); + + *(int *)arg = count; + } + #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: { diff --git a/nuttx/include/nuttx/fs/ioctl.h b/nuttx/include/nuttx/fs/ioctl.h index 08f62e16484d..6d60c2ee9752 100644 --- a/nuttx/include/nuttx/fs/ioctl.h +++ b/nuttx/include/nuttx/fs/ioctl.h @@ -110,6 +110,10 @@ * OUT: Bytes readable from this fd */ +#define FIONWRITE _FIOC(0x0005) /* IN: Location to return value (int *) + * OUT: Bytes writable to this fd + */ + /* NuttX file system ioctl definitions **************************************/ #define _DIOCVALID(c) (_IOC_TYPE(c)==_DIOCBASE) From 91ca80e6347feebb1e39b9d16df3f88fb8a68152 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 21:28:26 -0800 Subject: [PATCH 3/5] Fix the handling of FIONREAD/FIONWRITE; thanks Tridge. --- nuttx/drivers/serial/serial.c | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index 24744524fad5..28c657af055e 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -660,9 +660,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) int ret = dev->ops->ioctl(filep, cmd, arg); - /* Append any higher level TTY flags */ + /* If the low-level handler didn't handle the call, see if we can handle it here */ - if (ret == OK) + if (ret == -ENOTTY) { switch (cmd) { @@ -686,6 +686,9 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; + + break; } case FIONWRITE: @@ -695,7 +698,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) /* determine the number of bytes free in the buffer */ - if (dev->xmit.head <= dev->xmit.tail) + if (dev->xmit.head < dev->xmit.tail) { count = dev->xmit.tail - dev->xmit.head - 1; } @@ -707,8 +710,19 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) irqrestore(state); *(int *)arg = count; + ret = 0; + + break; } + } + } + /* Append any higher level TTY flags */ + + else if (ret == OK) + { + switch (cmd) + { #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: { From 69cdab9afc0215e26dc5084e21fd61725acb6c84 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 4 Jan 2013 23:41:21 -0800 Subject: [PATCH 4/5] Fix a typo that caused PWM_SERVO_GET ioctls to fail on the FMU PWM outputs. --- apps/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index a995f6214d23..2e3b130a9cb7 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -500,7 +500,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_SET(0); + channel = cmd - PWM_SERVO_GET(0); *(servo_position_t *)arg = up_pwm_servo_get(channel); break; } From 76277ec622742388aade35ef9d439d42ea6caad7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 5 Jan 2013 22:37:26 -0800 Subject: [PATCH 5/5] Ignore more. --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index ea416fae488c..d0d84b98546c 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ Make.dep *.o *.a *~ +*.dSYM Images/*.bin Images/*.px4 nuttx/Make.defs @@ -40,3 +41,4 @@ nsh_romfsimg.h cscope.out .configX-e nuttx-export.zip +dot.gdbinit