Merged relay activation

1 files changed, 36 insertions, 4 deletions

diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 0ea1218e3..99f573d1d 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -62,6 +62,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>
 
@@ -132,6 +133,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?
 
@@ -207,6 +210,7 @@ PX4IO::PX4IO() :
 	_t_outputs(-1),
 	_mixers(nullptr),
 	_primary_pwm_device(false),
+	_relays(0),
 	_switch_armed(false),
 	_send_needed(false),
 	_config_needed(false)
@@ -430,9 +434,6 @@ PX4IO::task_main()
 						}
 					}
 
-					/* publish actuator outputs */
-					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
-
 					/* and flag for update */
 					_send_needed = true;
 				}
@@ -577,7 +578,14 @@ PX4IO::io_send()
 	for (unsigned i = 0; i < _max_actuators; i++)
 		cmd.servo_command[i] = _outputs.output[i];
 
-	// XXX relays
+	/* publish as we send */
+	_outputs.timestamp = hrt_absolute_time();
+	orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
+
+
+	/* 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 -> arming ok */
 	cmd.arm_ok = (_armed.armed && !_armed.lockdown);
@@ -645,6 +653,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;