From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Thu, 20 Dec 2012 21:31:02 -0800
Subject: 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 97033f2cd..b2fee65ac 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 456564ba7..d662e634f 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -61,6 +61,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mixer.h>
 
@@ -114,6 +115,8 @@ private:
 
 	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 40ea38cf7..ffb7fd679 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);
 }
-- 
cgit v1.2.3