drivers: allow forcing the safety switch on

This allows forcing the safety switch to the on position from software which stops the pwm outputs
ArduPilot · Sep 18, 2014 · 4cc6120 · 4cc6120
1 parent 23a11fc
commit 4cc6120
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 0 deletions.
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
@@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
 /** force safety switch off (to disable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
 
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 24)
+
 /*
  *
  *

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
@@ -737,6 +737,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
@@ -2128,6 +2128,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 DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
@@ -211,6 +211,7 @@ enum {							/* DSM bind states */
 
 #define PX4IO_P_SETUP_FORCE_SAFETY_OFF	       12	/* force safety switch into
                                                            'armed' (PWM enabled) state */
+#define PX4IO_P_SETUP_FORCE_SAFETY_ON          13   /* 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
@@ -569,6 +569,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;