aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/drivers/drv_pwm_output.h3
-rw-r--r--src/drivers/px4fmu/fmu.cpp1
-rw-r--r--src/drivers/px4io/px4io.cpp4
3 files changed, 8 insertions, 0 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 7a93e513e..972573f9f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm);
/** get the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
+/** force safety switch off (to disable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
+
/*
*
*
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a70ff6c5c..7258d5f9e 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -736,6 +736,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:
// these are no-ops, as no safety switch
break;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index e318e206a..e5a39ffb0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2129,6 +2129,10 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
case PWM_SERVO_GET_DISABLE_LOCKDOWN:
*(unsigned *)arg = _lockdown_override;
+
+ case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+ /* force safety swith off */
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case DSM_BIND_START: