Merge remote-tracking branch 'tridge/pullrequest-force-safety'

5 files changed, 16 insertions, 0 deletions

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index f66ec7c95..6873f24b6 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm);
 /** make failsafe non-recoverable (termination) if it occurs */
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+
 /*
  *
  *
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 122a3cd17..3d3e1b0eb 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		// these are no-ops, as no safety switch
 		break;
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 3871b4a2c..fd9eb4170 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2278,6 +2278,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
+		/* force safety switch on */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+		break;
+
 	case PWM_SERVO_SET_FORCE_FAILSAFE:
 		/* force failsafe mode instantly */
 		if (arg == 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index a3e8a58d3..9b2e047cb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -221,6 +221,7 @@ enum {							/* DSM bind states */
                                                            hence index 12 can safely be used. */
 #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON		14	/* force safety switch into 'disarmed' (PWM disabled state) */
 #define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 7f19e983f..49c2a9f56 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
+		case PX4IO_P_SETUP_FORCE_SAFETY_ON:
+			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
+				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			}
+			break;
+
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;