delete PWM_SERVO_CLEAR_ARM_OK

src/drivers/drv_pwm_output.h

@@ -137,12 +137,6 @@ typedef uint16_t	servo_position_t;
  */
 #define _PWM_SERVO_BASE		0x2a00
 
-/** arm all servo outputs handle by this driver */
-#define PWM_SERVO_ARM		_PX4_IOC(_PWM_SERVO_BASE, 0)
-
-/** disarm all servo outputs (stop generating pulses) */
-#define PWM_SERVO_DISARM	_PX4_IOC(_PWM_SERVO_BASE, 1)
-
 /** get default servo update rate */
 #define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2)
 
@@ -161,12 +155,6 @@ typedef uint16_t	servo_position_t;
 /** check the selected update rates */
 #define PWM_SERVO_GET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 7)
 
-/** set the 'ARM ok' bit, which activates the safety switch */
-#define PWM_SERVO_SET_ARM_OK	_PX4_IOC(_PWM_SERVO_BASE, 8)
-
-/** clear the 'ARM ok' bit, which deactivates the safety switch */
-#define PWM_SERVO_CLEAR_ARM_OK	_PX4_IOC(_PWM_SERVO_BASE, 9)
-
 /** start DSM bind */
 #define DSM_BIND_START	_PX4_IOC(_PWM_SERVO_BASE, 10)
 
@@ -203,23 +191,6 @@ typedef uint16_t	servo_position_t;
 /** get the lockdown override flag to enable outputs in HIL */
 #define PWM_SERVO_GET_DISABLE_LOCKDOWN		_PX4_IOC(_PWM_SERVO_BASE, 24)
 
-/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
-
-/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
-#define PWM_SERVO_SET_FORCE_FAILSAFE		_PX4_IOC(_PWM_SERVO_BASE, 26)
-
-/** make failsafe non-recoverable (termination) if it occurs */
-#define PWM_SERVO_SET_TERMINATION_FAILSAFE	_PX4_IOC(_PWM_SERVO_BASE, 27)
-
-/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
-
-/** set auxillary output mode */
-#define PWM_SERVO_ENTER_TEST_MODE  18
-#define PWM_SERVO_EXIT_TEST_MODE   19
-#define PWM_SERVO_SET_MODE         _PX4_IOC(_PWM_SERVO_BASE, 34)
-
 /*
  *
  *

src/drivers/pca9685_pwm_out/main.cpp

@@ -488,14 +488,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 
 		break;
 
-	case PWM_SERVO_SET_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
-	case PWM_SERVO_ARM:
-	case PWM_SERVO_DISARM:
-		break;
-
 	default:
 		ret = -ENOTTY;
 		break;

src/drivers/pwm_out/PWMOut.cpp

@@ -411,10 +411,6 @@ bool PWMOut::update_pwm_out_state(bool on)
 bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	if (_test_mode) {
-		return false;
-	}
-
 	/* output to the servos */
 	if (_pwm_initialized) {
 		for (size_t i = 0; i < num_outputs; i++) {
@@ -449,47 +445,97 @@ void PWMOut::Run()
 		return;
 	}
 
-	perf_begin(_cycle_perf);
+	// ESC calibration
+	if (_pwm_initialized && !_mixing_output.armed().armed) {
+		if (_mixing_output.armed().in_esc_calibration_mode) {
+			_esc_calibration_mode = true;
+			_esc_calibration_last = _mixing_output.armed().timestamp;
+
+			// set outputs to maximum
+			for (size_t i = 0; i < _num_outputs; i++) {
+				// TODO: ESCs only
+				//   - PWM only (no oneshot)
+				if (_pwm_mask & (1 << (i + _output_base))) {
+					up_pwm_servo_set(_output_base + i, PWM_DEFAULT_MAX);
+				}
+			}
+
+			ScheduleDelayed(10_ms);
+			return;
+
+		} else if (_esc_calibration_mode) {
+			if (hrt_elapsed_time(&_esc_calibration_last) < 3_s) {
+				// set outputs to minimum
+				for (size_t i = 0; i < _num_outputs; i++) {
+					// TODO: ESCs only
+					if (_pwm_mask & (1 << (i + _output_base))) {
+						up_pwm_servo_set(_output_base + i, PWM_DEFAULT_MIN);
+					}
+				}
+
+				ScheduleDelayed(10_ms);
+				return;
+
+			} else {
+				// calibration finished
+				_esc_calibration_mode = false;
+				_esc_calibration_last = 0;
+			}
+		}
+	}
+
+
+
 	perf_count(_interval_perf);
 
-	if (!_mixing_output.useDynamicMixing()) {
-		// push backup schedule
-		ScheduleDelayed(_backup_schedule_interval_us);
-	}
+	if (_mixing_output.armed().in_esc_calibration_mode) {
+		// do calibration
 
-	_mixing_output.update();
 
-	/* update PWM status if armed or if disarmed PWM values are set */
-	bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
-		      || _mixing_output.armed().in_esc_calibration_mode;
 
-	if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
+	} else {
+		perf_begin(_cycle_perf);
 
-		if (update_pwm_out_state(pwm_on)) {
-			_pwm_on = pwm_on;
+		if (!_mixing_output.useDynamicMixing()) {
+			// push backup schedule
+			ScheduleDelayed(_backup_schedule_interval_us);
 		}
-	}
 
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
+		_mixing_output.update();
 
-		// update parameters from storage
-		if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
-			update_params();
+		/* update PWM status if armed or if disarmed PWM values are set */
+		bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing()
+			      || _mixing_output.armed().in_esc_calibration_mode;
+
+		if (_pwm_on != pwm_on || _require_arming[_instance].load()) {
+
+			if (update_pwm_out_state(pwm_on)) {
+				_pwm_on = pwm_on;
+			}
 		}
+
+		// check for parameter updates
+		if (_parameter_update_sub.updated()) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings)
+				update_params();
+			}
+		}
+
+		if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
+			update_current_rate();
+		}
+
+		// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+		_mixing_output.updateSubscriptions(true, true);
+		perf_end(_cycle_perf);
 	}
 
-	if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) {
-		update_current_rate();
-	}
 
-	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
-	_mixing_output.updateSubscriptions(true, true);
-
-	perf_end(_cycle_perf);
 }
 
 void PWMOut::update_params()
@@ -711,25 +757,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 	lock();
 
 	switch (cmd) {
-	case PWM_SERVO_ARM:
-		update_pwm_out_state(true);
-		break;
-
-	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:
-		break;
-
-	case PWM_SERVO_DISARM:
-
-		/* Ignore disarm if disarmed PWM is set already. */
-		if (_num_disarmed_set == 0) {
-			update_pwm_out_state(false);
-		}
-
-		break;
-
 	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
 		*(uint32_t *)arg = _pwm_default_rate;
 		break;
@@ -1048,23 +1075,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		*(unsigned *)arg = _num_outputs;
 		break;
 
-	case PWM_SERVO_SET_MODE: {
-			switch (arg) {
-			case PWM_SERVO_ENTER_TEST_MODE:
-				_test_mode = true;
-				break;
-
-			case PWM_SERVO_EXIT_TEST_MODE:
-				_test_mode = false;
-				break;
-
-			default:
-				ret = -EINVAL;
-			}
-
-			break;
-		}
-
 	case MIXERIOCRESET:
 		_mixing_output.resetMixerThreadSafe();
 
@@ -1089,165 +1099,8 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
-int PWMOut::test(const char *dev)
-{
-	int	 fd;
-	unsigned servo_count = 0;
-	unsigned pwm_value = 1000;
-	int	 direction = 1;
-	int  ret;
-	int   rv = -1;
-
-	fd = ::open(dev, O_RDWR);
-
-	if (fd < 0) {
-		PX4_ERR("open fail");
-		return -1;
-	}
-
-	if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
-		PX4_ERR("Failed to Enter pwm test mode");
-		goto err_out_no_test;
-	}
-
-	if (::ioctl(fd, PWM_SERVO_ARM, 0) < 0) {
-		PX4_ERR("servo arm failed");
-		goto err_out;
-	}
-
-	if (::ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
-		PX4_ERR("Unable to get servo count");
-		goto err_out;
-	}
-
-	PX4_INFO("Testing %u servos", servo_count);
-
-	struct pollfd fds;
-
-	fds.fd = 0; /* stdin */
-
-	fds.events = POLLIN;
-
-	PX4_INFO("Press CTRL-C or 'c' to abort.");
-
-	for (;;) {
-		/* sweep all servos between 1000..2000 */
-		servo_position_t servos[servo_count];
-
-		for (unsigned i = 0; i < servo_count; i++) {
-			servos[i] = pwm_value;
-		}
-
-		for (unsigned i = 0; i < servo_count;	i++) {
-			if (::ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
-				PX4_ERR("servo %u set failed", i);
-				goto err_out;
-			}
-		}
-
-		if (direction > 0) {
-			if (pwm_value < 2000) {
-				pwm_value++;
-
-			} else {
-				direction = -1;
-			}
-
-		} else {
-			if (pwm_value > 1000) {
-				pwm_value--;
-
-			} else {
-				direction = 1;
-			}
-		}
-
-		/* readback servo values */
-		for (unsigned i = 0; i < servo_count; i++) {
-			servo_position_t value;
-
-			if (::ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
-				PX4_ERR("error reading PWM servo %u", i);
-				goto err_out;
-			}
-
-			if (value != servos[i]) {
-				PX4_ERR("servo %u readback error, got %" PRIu16 " expected %" PRIu16, i, value, servos[i]);
-				goto err_out;
-			}
-		}
-
-		/* Check if user wants to quit */
-		char c;
-		ret = ::poll(&fds, 1, 0);
-
-		if (ret > 0) {
-
-			::read(0, &c, 1);
-
-			if (c == 0x03 || c == 0x63 || c == 'q') {
-				PX4_INFO("User abort");
-				break;
-			}
-		}
-	}
-
-	rv = 0;
-
-err_out:
-
-	if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
-		PX4_ERR("Failed to Exit pwm test mode");
-	}
-
-err_out_no_test:
-	::close(fd);
-	return rv;
-}
-
 int PWMOut::custom_command(int argc, char *argv[])
 {
-
-	int ch = 0;
-	int myoptind = 0;
-	const char *myoptarg = nullptr;
-	const char *dev = PX4FMU_DEVICE_PATH;
-
-	while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'd':
-			if (nullptr == strstr(myoptarg, "/dev/")) {
-				PX4_WARN("device %s not valid", myoptarg);
-				print_usage(nullptr);
-				return 1;
-			}
-
-			dev = myoptarg;
-			break;
-		}
-	}
-
-	if (myoptind >= argc) {
-		print_usage(nullptr);
-		return 1;
-	}
-
-	const char *verb = argv[myoptind];
-
-	/* start pwm_out if not running */
-	if (!is_running()) {
-
-		int ret = PWMOut::task_spawn(argc, argv);
-
-		if (ret) {
-			return ret;
-		}
-	}
-
-	if (!strcmp(verb, "test")) {
-		return test(dev);
-	}
-
 	return print_usage("unknown command");
 }
 
@@ -1317,7 +1170,6 @@ By default the module runs on a work queue with a callback on the uORB actuator_
 
 	PRINT_MODULE_USAGE_NAME("pwm_out", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test outputs");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;

src/drivers/pwm_out/PWMOut.hpp

@@ -101,8 +101,6 @@ public:
 	static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); }
 	static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); }
 
-	static int test(const char *dev);
-
 	int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
 
 	int init() override;
@@ -147,7 +145,9 @@ private:
 	bool		_pwm_on{false};
 	uint32_t	_pwm_mask{0};
 	bool		_pwm_initialized{false};
-	bool		_test_mode{false};
+
+	bool           _esc_calibration_mode{false};
+	hrt_abstime    _esc_calibration_last{0};
 
 	unsigned	_num_disarmed_set{0};
 

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -94,12 +94,6 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 	lock();
 
 	switch (cmd) {
-	case PWM_SERVO_ARM:
-		break;
-
-	case PWM_SERVO_DISARM:
-		break;
-
 	case PWM_SERVO_SET_MIN_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 

src/drivers/px4io/px4io.cpp

@@ -218,6 +218,9 @@ private:
 
 	hrt_abstime             _last_status_publish{0};
 
+	bool           _esc_calibration_mode{false};
+	hrt_abstime    _esc_calibration_last{0};
+
 	bool			_param_update_force{true};	///< force a parameter update
 	bool			_timer_rates_configured{false};
 
@@ -235,7 +238,6 @@ private:
 	float			_analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage
 
 	bool			_test_fmu_fail{false}; ///< To test what happens if IO loses FMU
-	bool			_in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active
 
 	MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
 
@@ -430,7 +432,7 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 {
 	SmartLock lock_guard(_lock);
 
-	if (!_test_fmu_fail && !_in_test_mode) {
+	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
 	}
@@ -579,6 +581,52 @@ void PX4IO::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
+	// ESC calibration
+	if (!_mixing_output.armed().armed && !_test_fmu_fail) {
+		if (_mixing_output.armed().in_esc_calibration_mode) {
+			_esc_calibration_mode = true;
+			_esc_calibration_last = _mixing_output.armed().timestamp;
+
+			// set outputs to maximum
+			uint16_t outputs[MAX_ACTUATORS];
+
+			for (unsigned i = 0; i < _max_actuators; i++) {
+				outputs[i] = PWM_DEFAULT_MAX;
+			}
+
+			// TODO: ESCs only
+			//   - PWM only (no oneshot)
+			io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, _max_actuators);
+
+			ScheduleDelayed(10_ms);
+			return;
+
+		} else if (_esc_calibration_mode) {
+			if (hrt_elapsed_time(&_esc_calibration_last) < 3_s) {
+				// set outputs to minimum
+				uint16_t outputs[MAX_ACTUATORS];
+
+				for (unsigned i = 0; i < _max_actuators; i++) {
+					outputs[i] = PWM_DEFAULT_MIN;
+				}
+
+				// TODO: ESCs only
+				io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, _max_actuators);
+
+				ScheduleDelayed(10_ms);
+				return;
+
+			} else {
+				// calibration finished
+				_esc_calibration_mode = false;
+				_esc_calibration_last = 0;
+			}
+		}
+	}
+
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
@@ -592,7 +640,7 @@ void PX4IO::Run()
 		_mixing_output.update();
 	}
 
-	SmartLock lock_guard(_lock);
+
 
 	if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
 		/* run at 50 */
@@ -1573,30 +1621,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	/* regular ioctl? */
 	switch (cmd) {
-	case PWM_SERVO_ARM:
-		PX4_DEBUG("PWM_SERVO_ARM");
-		/* set the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
-		break;
-
-	case PWM_SERVO_SET_ARM_OK:
-		PX4_DEBUG("PWM_SERVO_SET_ARM_OK");
-		/* set the 'OK to arm' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
-		break;
-
-	case PWM_SERVO_CLEAR_ARM_OK:
-		PX4_DEBUG("PWM_SERVO_CLEAR_ARM_OK");
-		/* clear the 'OK to arm' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
-		break;
-
-	case PWM_SERVO_DISARM:
-		PX4_DEBUG("PWM_SERVO_DISARM");
-		/* clear the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
-		break;
-
 	case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
 		PX4_DEBUG("PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
 		/* get the default update rate */
@@ -1808,48 +1832,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _lockdown_override;
 		break;
 
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		PX4_DEBUG("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 PWM_SERVO_SET_FORCE_SAFETY_ON:
-		PX4_DEBUG("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:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_FAILSAFE");
-
-		/* force failsafe mode instantly */
-		if (arg == 0) {
-			/* clear force failsafe flag */
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
-
-		} else {
-			/* set force failsafe flag */
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
-		}
-
-		break;
-
-	case PWM_SERVO_SET_TERMINATION_FAILSAFE:
-		PX4_DEBUG("PWM_SERVO_SET_TERMINATION_FAILSAFE");
-
-		/* if failsafe occurs, do not allow the system to recover */
-		if (arg == 0) {
-			/* clear termination failsafe flag */
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
-
-		} else {
-			/* set termination failsafe flag */
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
-		}
-
-		break;
-
 	case DSM_BIND_START:
 		/* bind a DSM receiver */
 		ret = dsm_bind_ioctl(arg);
@@ -1867,7 +1849,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 				ret = -EINVAL;
 
 			} else {
-				if (!_test_fmu_fail && _in_test_mode) {
+				if (!_test_fmu_fail) {
 					/* send a direct PWM value */
 					ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
 
@@ -1916,22 +1898,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}
 
-	case PWM_SERVO_SET_MODE: {
-			// reset all channels to disarmed when entering/leaving test mode, so that we don't
-			// accidentially use values from previous tests
-			pwm_output_values pwm_disarmed;
-
-			if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) {
-				for (unsigned i = 0; i < _max_actuators; ++i) {
-					io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]);
-				}
-			}
-
-			_in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE);
-			ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL;
-		}
-		break;
-
 	case MIXERIOCRESET:
 		PX4_DEBUG("MIXERIOCRESET");
 		_mixing_output.resetMixerThreadSafe();
@@ -2418,29 +2384,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return 1;
 	}
 
-
-	if (!strcmp(verb, "safety_off")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to disable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "safety_on")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to enable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
 	if (!strcmp(verb, "debug")) {
 		if (argc <= 1) {
 			PX4_ERR("usage: px4io debug LEVEL");
@@ -2531,8 +2474,6 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware");
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level");
 	PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("monitor", "continuously monitor status");

src/drivers/uavcan/uavcan_main.cpp

@@ -866,12 +866,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 	lock();
 
 	switch (cmd) {
-	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;
-
 	case MIXERIOCRESET:
 		_mixing_interface_esc.mixingOutput().resetMixerThreadSafe();
 

src/lib/mixer_module/mixer_module.cpp

@@ -683,19 +683,6 @@ bool MixingOutput::updateStaticMixer()
 			if (_control_subs[i].copy(&_controls[i])) {
 				n_updates++;
 			}
-
-			/* During ESC calibration, we overwrite the throttle value. */
-			if (i == 0 && _armed.in_esc_calibration_mode) {
-
-				/* Set all controls to 0 */
-				memset(&_controls[i], 0, sizeof(_controls[i]));
-
-				/* except thrust to maximum. */
-				_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
-
-				/* Switch off the output limit ramp for the calibration. */
-				_output_limit.state = OUTPUT_LIMIT_STATE_ON;
-			}
 		}
 	}
 
@@ -1043,7 +1030,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
 	}
 
 	/* throttle not arming - mark throttle input as invalid */
-	if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) {
+	if (output->armNoThrottle()) {
 		if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
 		     control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
 		    control_index == actuator_controls_s::INDEX_THROTTLE) {

src/modules/commander/Commander.cpp

@@ -1300,14 +1300,25 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param7) == 1) {
 					/* do esc calibration */
-					if (check_battery_disconnected(&_mavlink_log_pub)) {
-						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
-						_status_flags.condition_calibration_enabled = true;
-						_armed.in_esc_calibration_mode = true;
-						_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
+
+					// check safety
+					uORB::SubscriptionData<safety_s> safety_sub{ORB_ID(safety)};
+					safety_sub.update();
+
+					if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) {
+						//mavlink_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first"); // TODO
+						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
 
 					} else {
-						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+						if (check_battery_disconnected(&_mavlink_log_pub)) {
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+							_status_flags.condition_calibration_enabled = true;
+							_armed.in_esc_calibration_mode = true;
+							_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
+
+						} else {
+							answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+						}
 					}
 
 				} else if ((int)(cmd.param4) == 0) {

src/modules/commander/esc_calibration.cpp

@@ -78,42 +78,14 @@ bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
 	return false;
 }
 
-static void set_motor_actuators(uORB::Publication<actuator_test_s> &publisher, float value, bool release_control)
+int do_esc_calibration(orb_advert_t *mavlink_log_pub)
 {
-	actuator_test_s actuator_test{};
-	actuator_test.timestamp = hrt_absolute_time();
-	actuator_test.value = value;
-	actuator_test.action = release_control ? actuator_test_s::ACTION_RELEASE_CONTROL : actuator_test_s::ACTION_DO_CONTROL;
-	actuator_test.timeout_ms = 0;
+	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
+	px4_usleep(100000);
 
-	for (int i = 0; i < actuator_test_s::MAX_NUM_MOTORS; ++i) {
-		actuator_test.function = actuator_test_s::FUNCTION_MOTOR1 + i;
-		publisher.publish(actuator_test);
-	}
-}
-
-int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
-{
-	// check safety
-	uORB::SubscriptionData<safety_s> safety_sub{ORB_ID(safety)};
-	safety_sub.update();
-
-	if (safety_sub.get().safety_switch_available && !safety_sub.get().safety_off) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disable safety first");
-		return PX4_ERROR;
-	}
-
-	int	return_code = PX4_OK;
-	uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
-	// since we publish multiple at once, make sure the output driver subscribes before we publish
-	actuator_test_pub.advertise();
-	px4_usleep(10000);
-
-	// set motors to high
-	set_motor_actuators(actuator_test_pub, 1.f, false);
+	int return_code = PX4_OK;
 	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
 
-
 	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
 	const battery_status_s &battery = batt_sub.get();
 	batt_sub.update();
@@ -121,8 +93,7 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
 	hrt_abstime timeout_start = hrt_absolute_time();
 
 	while (true) {
-		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
-		// sit high.
+		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM sit high.
 		static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
 		static constexpr hrt_abstime pwm_high_timeout{3_s};
 		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
@@ -152,134 +123,8 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub)
 	}
 
 	if (return_code == PX4_OK) {
-		// set motors to low
-		set_motor_actuators(actuator_test_pub, 0.f, false);
-		px4_usleep(4000000);
-
-		// release control
-		set_motor_actuators(actuator_test_pub, 0.f, true);
-
 		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
 	}
 
 	return return_code;
 }
-
-static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub)
-{
-	int	return_code = PX4_OK;
-	hrt_abstime timeout_start = 0;
-
-	uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
-	const battery_status_s &battery = batt_sub.get();
-	batt_sub.update();
-	bool batt_connected = battery.connected;
-
-	int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
-
-	if (fd < 0) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
-	/* tell IO/FMU that its ok to disable its safety with the switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
-	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
-	/* tell IO to switch off safety without using the safety switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != PX4_OK) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
-		return_code = PX4_ERROR;
-		goto Out;
-	}
-
-	calibration_log_info(mavlink_log_pub, "[cal] Connect battery now");
-
-	timeout_start = hrt_absolute_time();
-
-	while (true) {
-		// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
-		// sit high.
-		static constexpr hrt_abstime battery_connect_wait_timeout{20_s};
-		static constexpr hrt_abstime pwm_high_timeout{3_s};
-		hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
-
-		if (hrt_elapsed_time(&timeout_start) > timeout_wait) {
-			if (!batt_connected) {
-				calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
-				return_code = PX4_ERROR;
-				goto Out;
-			}
-
-			// PWM was high long enough
-			break;
-		}
-
-		if (!batt_connected) {
-			if (batt_sub.update()) {
-				if (battery.connected) {
-					// Battery is connected, signal to user and start waiting again
-					batt_connected = true;
-					timeout_start = hrt_absolute_time();
-					calibration_log_info(mavlink_log_pub, "[cal] Battery connected");
-				}
-			}
-		}
-
-		px4_usleep(50000);
-	}
-
-Out:
-
-	if (fd != -1) {
-		if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
-			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
-		}
-
-		if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
-			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
-		}
-
-		if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
-			calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
-		}
-
-		px4_close(fd);
-	}
-
-	if (return_code == PX4_OK) {
-		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
-	}
-
-	return return_code;
-}
-
-int do_esc_calibration(orb_advert_t *mavlink_log_pub)
-{
-	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
-
-	param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
-	int32_t ctrl_alloc = 0;
-
-	if (p_ctrl_alloc != PARAM_INVALID) {
-		param_get(p_ctrl_alloc, &ctrl_alloc);
-	}
-
-	if (ctrl_alloc == 1) {
-		return do_esc_calibration_ctrl_alloc(mavlink_log_pub);
-
-	} else {
-		return do_esc_calibration_ioctl(mavlink_log_pub);
-	}
-}

src/systemcmds/esc_calib/esc_calib.cpp

@@ -270,24 +270,6 @@ extern "C" __EXPORT int esc_calib_main(int argc, char *argv[])
 		goto cleanup;
 	}
 
-	/* tell IO/FMU that its ok to disable its safety with the switch */
-	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-
-	if (ret != OK) {
-		PX4_ERR("PWM_SERVO_SET_ARM_OK");
-		goto cleanup;
-	}
-
-	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	ret = ioctl(fd, PWM_SERVO_ARM, 0);
-
-	if (ret != OK) {
-		PX4_ERR("PWM_SERVO_ARM");
-		goto cleanup;
-	}
-
-	printf("Outputs armed");
-
 
 	/* wait for user confirmation */
 	printf("\nHigh PWM set: %d\n"
@@ -371,14 +353,6 @@ extern "C" __EXPORT int esc_calib_main(int argc, char *argv[])
 		px4_usleep(50000);
 	}
 
-	/* disarm */
-	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
-
-	if (ret != OK) {
-		PX4_ERR("PWM_SERVO_DISARM");
-		goto cleanup;
-	}
-
 	printf("Outputs disarmed");
 
 	printf("ESC calibration finished\n");

src/systemcmds/motor_ramp/motor_ramp.cpp

@@ -338,24 +338,6 @@ int prepare(int fd, unsigned long *max_channels)
 		return 1;
 	}
 
-	/* tell IO/FMU that its ok to disable its safety with the switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
-		PX4_ERR("PWM_SERVO_SET_ARM_OK");
-		return 1;
-	}
-
-	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
-	if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
-		PX4_ERR("PWM_SERVO_ARM");
-		return 1;
-	}
-
-	/* tell IO to switch off safety without using the safety switch */
-	if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
-		PX4_ERR("PWM_SERVO_SET_FORCE_SAFETY_OFF");
-		return 1;
-	}
-
 	return 0;
 }
 
@@ -404,13 +386,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
 		return 1;
 	}
 
-	if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
-		PX4_ERR("Failed to Enter pwm test mode");
-		px4_close(fd);
-		_thread_running = false;
-		return 1;
-	}
-
 	if (prepare(fd, &max_channels) != OK) {
 		_thread_should_exit = true;
 	}
@@ -517,13 +492,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
-			PX4_ERR("Failed to Exit pwm test mode");
-			px4_close(fd);
-			_thread_running = false;
-			return 1;
-		}
-
 		px4_close(fd);
 	}
 

src/systemcmds/pwm/pwm.cpp

@@ -103,25 +103,12 @@ Note that in OneShot mode, the PWM range [1000, 2000] is automatically mapped to
 Set the PWM rate for all channels to 400 Hz:
 $ pwm rate -a -r 400
 
-Test the outputs of eg. channels 1 and 3, and set the PWM value to 1200 us:
-$ pwm arm
-$ pwm test -c 13 -p 1200
-
 )DESCR_STR");
 
 
 	PRINT_MODULE_USAGE_NAME("pwm", "command");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("arm", "Arm output");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("disarm", "Disarm output");
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print current configuration of all channels");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("forcefail", "Force Failsafe mode. "
-                                         "PWM outputs are set to failsafe values.");
-	PRINT_MODULE_USAGE_ARG("on|off", "Turn on or off", false);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("terminatefail", "Enable Termination Failsafe mode. "
-                                         "While this is true, "
-                                         "any failsafe that occurs will be unrecoverable (even if recovery conditions are met).");
-	PRINT_MODULE_USAGE_ARG("on|off", "Turn on or off", false);
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("rate", "Configure PWM rates");
 	PRINT_MODULE_USAGE_PARAM_INT('r', -1, 50, 400, "PWM Rate in Hz (0 = Oneshot, otherwise 50 to 400Hz)", false);
@@ -132,15 +119,11 @@ $ pwm test -c 13 -p 1200
 	PRINT_MODULE_USAGE_COMMAND_DESCR("disarmed", "Set Disarmed PWM value");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("min", "Set Minimum PWM value");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("max", "Set Maximum PWM value");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set Output to a specific value until 'q' or 'c' or 'ctrl-c' pressed");
 
-	PRINT_MODULE_USAGE_COMMAND_DESCR("steps", "Run 5 steps from 0 to 100%");
-
-
-	PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'failsafe', 'disarmed', 'min', 'max' and 'test' require a PWM value:");
+	PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'failsafe', 'disarmed', 'min', 'max' require a PWM value:");
 	PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 4000, "PWM value (eg. 1100)", false);
 
-	PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'failsafe', 'disarmed', 'min', 'max', 'test' and 'steps' "
+	PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'failsafe', 'disarmed', 'min', 'max' "
 					 "additionally require to specify the channels with one of the following commands:");
 	PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, nullptr, "select channels in the form: 1234 (1 digit per channel, 1=first)",
 					true);
@@ -168,7 +151,6 @@ pwm_main(int argc, char *argv[])
 	bool oneshot = false;
 	int ch;
 	int ret;
-	int rv = 1;
 	char *ep;
 	uint32_t set_mask = 0;
 	unsigned group;
@@ -306,42 +288,7 @@ pwm_main(int argc, char *argv[])
 
 	oneshot = !strcmp(command, "oneshot");
 
-	if (!strcmp(command, "arm")) {
-		/* tell safety that its ok to disable it with the switch */
-		ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
-
-		if (ret != OK) {
-			err(1, "PWM_SERVO_SET_ARM_OK");
-		}
-
-		/* tell IO that the system is armed (it will output values if safety is off) */
-		ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
-
-		if (ret != OK) {
-			err(1, "PWM_SERVO_ARM");
-		}
-
-		if (print_verbose) {
-			PX4_INFO("Outputs armed");
-		}
-
-		return 0;
-
-	} else if (!strcmp(command, "disarm")) {
-		/* disarm, but do not revoke the SET_ARM_OK flag */
-		ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
-
-		if (ret != OK) {
-			err(1, "PWM_SERVO_DISARM");
-		}
-
-		if (print_verbose) {
-			PX4_INFO("Outputs disarmed");
-		}
-
-		return 0;
-
-	} else if (oneshot || !strcmp(command, "rate")) {
+	if (oneshot || !strcmp(command, "rate")) {
 
 		/* Change alternate PWM rate or set oneshot
 		 * Either the "oneshot" command was used
@@ -613,237 +560,6 @@ pwm_main(int argc, char *argv[])
 
 		return 0;
 
-	} else if (!strcmp(command, "test")) {
-
-		if (set_mask == 0) {
-			usage("no channels set");
-			return 1;
-		}
-
-		if (pwm_value == 0) {
-			usage("no PWM provided");
-			return 1;
-		}
-
-		/* get current servo values */
-		struct pwm_output_values last_spos;
-
-		for (unsigned i = 0; i < servo_count; i++) {
-
-
-			ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
-
-			if (ret != OK) {
-				PX4_ERR("PWM_SERVO_GET(%d)", i);
-				return 1;
-			}
-		}
-
-		/* perform PWM output */
-
-		/* Open console directly to grab CTRL-C signal */
-		struct pollfd fds;
-		fds.fd = 0; /* stdin */
-		fds.events = POLLIN;
-
-		if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
-				PX4_ERR("Failed to Enter pwm test mode");
-				goto err_out_no_test;
-		}
-
-		PX4_INFO("Press CTRL-C or 'c' to abort.");
-
-		while (1) {
-			for (unsigned i = 0; i < servo_count; i++) {
-				if (set_mask & 1 << i) {
-					ret = px4_ioctl(fd, PWM_SERVO_SET(i), pwm_value);
-
-					if (ret != OK) {
-						PX4_ERR("PWM_SERVO_SET(%d)", i);
-						goto err_out;
-					}
-				}
-			}
-
-			/* abort on user request */
-			char c;
-			ret = poll(&fds, 1, 0);
-
-			if (ret > 0) {
-
-				ret = read(0, &c, 1);
-
-				if (c == 0x03 || c == 0x63 || c == 'q') {
-					/* reset output to the last value */
-					for (unsigned i = 0; i < servo_count; i++) {
-						if (set_mask & 1 << i) {
-							ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
-
-							if (ret != OK) {
-								PX4_ERR("PWM_SERVO_SET(%d)", i);
-								goto err_out;
-							}
-						}
-					}
-
-					PX4_INFO("User abort\n");
-					rv = 0;
-					goto err_out;
-				}
-			}
-
-			/* Delay longer than the max Oneshot duration */
-
-			px4_usleep(2542);
-
-#ifdef __PX4_NUTTX
-			/* Trigger all timer's channels in Oneshot mode to fire
-			 * the oneshots with updated values.
-			 */
-
-			up_pwm_update(0xff);
-#endif
-		}
-		rv = 0;
-err_out:
-			if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
-					rv = 1;
-					PX4_ERR("Failed to Exit pwm test mode");
-			}
-
-err_out_no_test:
-		return rv;
-
-
-	} else if (!strcmp(command, "steps")) {
-
-		if (set_mask == 0) {
-			usage("no channels set");
-			return 1;
-		}
-
-		/* get current servo values */
-		struct pwm_output_values last_spos;
-
-		for (unsigned i = 0; i < servo_count; i++) {
-
-			ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
-
-			if (ret != OK) {
-				PX4_ERR("PWM_SERVO_GET(%d)", i);
-				return 1;
-			}
-		}
-
-		/* perform PWM output */
-
-		/* Open console directly to grab CTRL-C signal */
-		struct pollfd fds;
-		fds.fd = 0; /* stdin */
-		fds.events = POLLIN;
-
-		PX4_WARN("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now.");
-		px4_sleep(5);
-
-		if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
-				PX4_ERR("Failed to Enter pwm test mode");
-				goto err_out_no_test;
-		}
-
-		unsigned off = 900;
-		unsigned idle = 1300;
-		unsigned full = 2000;
-		unsigned steps_timings_us[] = {2000, 5000, 20000, 50000};
-
-		unsigned phase = 0;
-		unsigned phase_counter = 0;
-		unsigned const phase_maxcount = 20;
-
-		for (unsigned steps_timing_index = 0;
-		     steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
-		     steps_timing_index++) {
-
-			PX4_INFO("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
-
-			while (1) {
-				for (unsigned i = 0; i < servo_count; i++) {
-					if (set_mask & 1 << i) {
-
-						unsigned val;
-
-						if (phase == 0) {
-							val = idle;
-
-						} else if (phase == 1) {
-							/* ramp - depending how steep it is this ramp will look instantaneous on the output */
-							val = idle + (full - idle) * ((float)phase_counter / phase_maxcount);
-
-						} else {
-							val = off;
-						}
-
-						ret = px4_ioctl(fd, PWM_SERVO_SET(i), val);
-
-						if (ret != OK) {
-							PX4_ERR("PWM_SERVO_SET(%d)", i);
-							goto err_out;
-						}
-					}
-				}
-
-				/* abort on user request */
-				char c;
-				ret = poll(&fds, 1, 0);
-
-				if (ret > 0) {
-
-					ret = read(0, &c, 1);
-
-					if (ret > 0) {
-						/* reset output to the last value */
-						for (unsigned i = 0; i < servo_count; i++) {
-							if (set_mask & 1 << i) {
-								ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
-
-								if (ret != OK) {
-									PX4_ERR("PWM_SERVO_SET(%d)", i);
-									goto err_out;
-								}
-							}
-						}
-
-						PX4_INFO("User abort\n");
-						rv = 0;
-						goto err_out;
-					}
-				}
-
-				if (phase == 1) {
-					px4_usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
-
-				} else if (phase == 0) {
-					px4_usleep(50000);
-
-				} else if (phase == 2) {
-					px4_usleep(50000);
-
-				} else {
-					break;
-				}
-
-				phase_counter++;
-
-				if (phase_counter > phase_maxcount) {
-					phase++;
-					phase_counter = 0;
-				}
-			}
-		}
-
-		rv = 0;
-		goto err_out;
-
-
 	} else if (!strcmp(command, "status") || !strcmp(command, "info")) {
 
 		printf("device: %s\n", dev);
@@ -968,54 +684,6 @@ err_out_no_test:
 			}
 		}
 
-		return 0;
-
-	} else if (!strcmp(command, "forcefail")) {
-
-		if (argc < 3) {
-			PX4_ERR("arg missing [on|off]");
-			return 1;
-
-		} else {
-
-			if (!strcmp(argv[2], "on")) {
-				/* force failsafe */
-				ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
-
-			} else {
-				/* disable failsafe */
-				ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
-			}
-
-			if (ret != OK) {
-				PX4_ERR("FAILED setting forcefail %s", argv[2]);
-			}
-		}
-
-		return 0;
-
-	} else if (!strcmp(command, "terminatefail")) {
-
-		if (argc < 3) {
-			PX4_ERR("arg missing [on|off]");
-			return 1;
-
-		} else {
-
-			if (!strcmp(argv[2], "on")) {
-				/* force failsafe */
-				ret = px4_ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1);
-
-			} else {
-				/* disable failsafe */
-				ret = px4_ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0);
-			}
-
-			if (ret != OK) {
-				PX4_ERR("FAILED setting termination failsafe %s", argv[2]);
-			}
-		}
-
 		return 0;
 	}