Breakout px4io.hpp and delete unneeded #includes.

WIP
2026-05-25 03:37:34 +08:00 · 2022-09-15 13:07:51 -04:00 · 2022-09-09 15:12:03 -04:00
42 changed files with 453 additions and 1748 deletions
@@ -159,12 +159,6 @@
 #define BOARD_HAS_ON_RESET 1


-/* Internal IMU Heater
- *
- * Connected to the IO MCU; tell compiler to enable support
- */
-#define PX4IO_HEATER_ENABLED
-
 __BEGIN_DECLS

 /****************************************************************************************************
@@ -2,7 +2,6 @@ uint64 timestamp	# time since system start (microseconds)

 bool armed		# Set to true if system is armed
 bool prearmed		# Set to true if the actuator safety is disabled but motors are not armed
-bool ready_to_arm	# Set to true if system is ready to be armed
 bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
 bool manual_lockdown    # Set to true if manual throttle kill switch is engaged
 bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
@@ -14,7 +14,3 @@ uint32 controller_time_on_usec
 float32 proportional_value
 float32 integrator_value
 float32 feed_forward_value
-
-uint8 MODE_GPIO  = 1
-uint8 MODE_PX4IO = 2
-uint8 mode
@@ -6,19 +6,16 @@ float32 voltage_v		# Servo rail voltage in volts
 float32 rssi_v			# RSSI pin voltage in volts

 # PX4IO status flags (PX4IO_P_STATUS_FLAGS)
-bool status_arm_sync
-bool status_failsafe
-bool status_fmu_initialized
 bool status_fmu_ok
-bool status_init_ok
-bool status_outputs_armed
 bool status_raw_pwm
+
 bool status_rc_ok
 bool status_rc_dsm
 bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
+
 bool status_safety_button_event # px4io safety button was pressed for longer than 1 second

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
@@ -26,18 +23,11 @@ bool alarm_pwm_error
 bool alarm_rc_lost

 # PX4IO arming (PX4IO_P_SETUP_ARMING)
-bool arming_failsafe_custom
 bool arming_fmu_armed
-bool arming_fmu_prearmed
-bool arming_force_failsafe
 bool arming_io_arm_ok
-bool arming_lockdown
-bool arming_termination_failsafe

 uint16[8] pwm
-uint16[8] pwm_disarmed
-uint16[8] pwm_failsafe

-uint16[8] pwm_rate_hz
+uint16[4] pwm_group_rate_hz

 uint16[18] raw_inputs
@@ -117,7 +117,6 @@ public:
 	virtual ~ArchPX4IOSerial();

 	virtual int	init();
-	virtual int	ioctl(unsigned operation, unsigned &arg);

 protected:
 	/**
@@ -158,70 +158,6 @@ ArchPX4IOSerial::init()
 	return 0;
 }

-int
-ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
-{
-	switch (operation) {
-
-	case 1:		/* XXX magic number - test operation */
-		switch (arg) {
-		case 0:
-			syslog(LOG_INFO, "test 0\n");
-
-			/* kill DMA, this is a PIO test */
-			stm32_dmastop(_tx_dma);
-			stm32_dmastop(_rx_dma);
-			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
-
-			for (;;) {
-				while (!(rSR & USART_SR_TXE))
-					;
-
-				rDR = 0x55;
-			}
-
-			return 0;
-
-		case 1: {
-				unsigned fails = 0;
-
-				for (unsigned count = 0;; count++) {
-					uint16_t value = count & 0xffff;
-
-					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
-						fails++;
-					}
-
-					if (count >= 5000) {
-						syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
-						perf_print_counter(_pc_txns);
-						perf_print_counter(_pc_retries);
-						perf_print_counter(_pc_timeouts);
-						perf_print_counter(_pc_crcerrs);
-						perf_print_counter(_pc_dmaerrs);
-						perf_print_counter(_pc_protoerrs);
-						perf_print_counter(_pc_uerrs);
-						perf_print_counter(_pc_idle);
-						perf_print_counter(_pc_badidle);
-						count = 0;
-					}
-				}
-
-				return 0;
-			}
-
-		case 2:
-			syslog(LOG_INFO, "test 2\n");
-			return 0;
-		}
-
-	default:
-		break;
-	}
-
-	return -1;
-}
-
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
@@ -170,70 +170,6 @@ ArchPX4IOSerial::init()
 	return 0;
 }

-int
-ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
-{
-	switch (operation) {
-
-	case 1:		/* XXX magic number - test operation */
-		switch (arg) {
-		case 0:
-			syslog(LOG_INFO, "test 0\n");
-
-			/* kill DMA, this is a PIO test */
-			stm32_dmastop(_tx_dma);
-			stm32_dmastop(_rx_dma);
-			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
-
-			for (;;) {
-				while (!(rISR & USART_ISR_TXE))
-					;
-
-				rTDR = 0x55;
-			}
-
-			return 0;
-
-		case 1: {
-				unsigned fails = 0;
-
-				for (unsigned count = 0;; count++) {
-					uint16_t value = count & 0xffff;
-
-					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
-						fails++;
-					}
-
-					if (count >= 5000) {
-						syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
-						perf_print_counter(_pc_txns);
-						perf_print_counter(_pc_retries);
-						perf_print_counter(_pc_timeouts);
-						perf_print_counter(_pc_crcerrs);
-						perf_print_counter(_pc_dmaerrs);
-						perf_print_counter(_pc_protoerrs);
-						perf_print_counter(_pc_uerrs);
-						perf_print_counter(_pc_idle);
-						perf_print_counter(_pc_badidle);
-						count = 0;
-					}
-				}
-
-				return 0;
-			}
-
-		case 2:
-			syslog(LOG_INFO, "test 2\n");
-			return 0;
-		}
-
-	default:
-		break;
-	}
-
-	return -1;
-}
-
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
@@ -208,70 +208,6 @@ ArchPX4IOSerial::init()
 	return 0;
 }

-int
-ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
-{
-	switch (operation) {
-
-	case 1:   /* XXX magic number - test operation */
-		switch (arg) {
-		case 0:
-			syslog(LOG_INFO, "test 0\n");
-
-			/* kill DMA, this is a PIO test */
-			stm32_dmastop(_tx_dma);
-			stm32_dmastop(_rx_dma);
-			rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT);
-
-			for (;;) {
-				while (!(rISR & USART_ISR_TXE))
-					;
-
-				rTDR = 0x55;
-			}
-
-			return 0;
-
-		case 1: {
-				unsigned fails = 0;
-
-				for (unsigned count = 0;; count++) {
-					uint16_t value = count & 0xffff;
-
-					if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
-						fails++;
-					}
-
-					if (count >= 5000) {
-						syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails);
-						perf_print_counter(_pc_txns);
-						perf_print_counter(_pc_retries);
-						perf_print_counter(_pc_timeouts);
-						perf_print_counter(_pc_crcerrs);
-						perf_print_counter(_pc_dmaerrs);
-						perf_print_counter(_pc_protoerrs);
-						perf_print_counter(_pc_uerrs);
-						perf_print_counter(_pc_idle);
-						perf_print_counter(_pc_badidle);
-						count = 0;
-					}
-				}
-
-				return 0;
-			}
-
-		case 2:
-			syslog(LOG_INFO, "test 2\n");
-			return 0;
-		}
-
-	default:
-		break;
-	}
-
-	return -1;
-}
-
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
@@ -1,65 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_io_heater.h
- *
- * IO IMU heater control interface (PX4IO)
- */
-
-#ifndef _DRV_IO_HEATER_H
-#define _DRV_IO_HEATER_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-/*
- * ioctl() definitions
- */
-
-#define IO_HEATER_DEVICE_PATH "/dev/px4io"
-
-#define _IO_HEATER_BASE       (0x2e00)
-
-#define PX4IO_HEATER_CONTROL  _PX4_IOC(_IO_HEATER_BASE, 0)
-
-/* ... to IOX_SET_VALUE + 8 */
-
-/* enum passed to IO_HEATER_CONTROL ioctl()*/
-enum IO_HEATER_MODE {
-	HEATER_MODE_OFF = 0,
-	HEATER_MODE_ON = 1,
-	HEATER_MODE_DISABLED = -1
-};
-
-#endif
@@ -111,9 +111,6 @@ __BEGIN_DECLS
 */
 #define _PWM_SERVO_BASE		0x2a00

-/** start DSM bind */
-#define DSM_BIND_START	_PX4_IOC(_PWM_SERVO_BASE, 10)
-
 /** specific rates for configuring the timer for OneShot or PWM */
 #define	PWM_RATE_ONESHOT			0u
 #define	PWM_RATE_LOWER_LIMIT		1u
@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_sbus.h
- *
- * Futaba S.BUS / S.BUS 2 compatible interface.
- */
-
-#ifndef _DRV_SBUS_H
-#define _DRV_SBUS_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-#include "drv_orb_dev.h"
-
-/**
- * Path for the default S.BUS device
- */
-#define SBUS0_DEVICE_PATH	"/dev/sbus0"
-
-#define _SBUS_BASE		0x2c00
-
-/** Enable S.BUS version 1 / 2 output (0 to disable) */
-#define SBUS_SET_PROTO_VERSION		_IOC(_SBUS_BASE, 0)
-
-#endif /* _DRV_SBUS_H */
@@ -46,34 +46,11 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_io_heater.h>
-
-#if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED)
-// Heater on some boards is on IO MCU
-// Use ioctl calls to IO driver to turn heater on/off
-#  define HEATER_PX4IO
-#else
-// Use direct calls to turn GPIO pin on/off
-#  ifndef GPIO_HEATER_OUTPUT
-#  error "To use the heater driver, the board_config.h must define and initialize GPIO_HEATER_OUTPUT"
-#  endif
-#  define HEATER_GPIO
-#endif

 Heater::Heater() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
 {
-#ifdef HEATER_PX4IO
-	_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
-
-	if (_io_fd < 0) {
-		PX4_ERR("Unable to open heater device path");
-		return;
-	}
-
-#endif
-
 	_heater_status_pub.advertise();
 }

@@ -96,65 +73,23 @@ int Heater::custom_command(int argc, char *argv[])
 void Heater::disable_heater()
 {
 	// Reset heater to off state.
-#ifdef HEATER_PX4IO
-	if (_io_fd >= 0) {
-		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
-	}
-
-#endif
-
-#ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
-#endif
 }

 void Heater::initialize_heater_io()
 {
 	// Initialize heater to off state.
-#ifdef HEATER_PX4IO
-	if (_io_fd < 0) {
-		_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
-	}
-
-	if (_io_fd >= 0) {
-		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
-	}
-
-#endif
-
-#ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
-#endif
 }

 void Heater::heater_off()
 {
-#ifdef HEATER_PX4IO
-
-	if (_io_fd >= 0) {
-		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
-	}
-
-#endif
-
-#ifdef HEATER_GPIO
 	HEATER_OUTPUT_EN(false);
-#endif
 }

 void Heater::heater_on()
 {
-#ifdef HEATER_PX4IO
-
-	if (_io_fd >= 0) {
-		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
-	}
-
-#endif
-
-#ifdef HEATER_GPIO
 	HEATER_OUTPUT_EN(true);
-#endif
 }

 bool Heater::initialize_topics()
@@ -182,14 +117,6 @@ bool Heater::initialize_topics()
 void Heater::Run()
 {
 	if (should_exit()) {
-#if defined(HEATER_PX4IO)
-
-		// must be closed from wq thread
-		if (_io_fd >= 0) {
-			px4_close(_io_fd);
-		}
-
-#endif
 		exit_and_cleanup();
 		return;
 	}
@@ -260,14 +187,6 @@ void Heater::publish_status()
 	status.proportional_value      = _proportional_value;
 	status.integrator_value        = _integrator_value;
 	status.feed_forward_value      = _param_sens_imu_temp_ff.get();
-
-#ifdef HEATER_PX4IO
-	status.mode = heater_status_s::MODE_PX4IO;
-#endif
-#ifdef HEATER_GPIO
-	status.mode = heater_status_s::MODE_GPIO;
-#endif
-
 	status.timestamp = hrt_absolute_time();
 	_heater_status_pub.publish(status);
 }
@@ -130,14 +130,6 @@ private:
 	 */
 	void update_params(const bool force = false);

-	/** Work queue struct for the scheduler. */
-	static struct work_s _work;
-
-	/** File descriptor for PX4IO for heater ioctl's */
-#if defined(PX4IO_HEATER_ENABLED)
-	int _io_fd {-1};
-#endif
-
 	bool _heater_initialized     = false;
 	bool _heater_on              = false;
 	bool _temperature_target_met = false;
@@ -37,312 +37,7 @@
 *
 * PX4IO is connected via DMA enabled high-speed UART.
 */
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/events.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/posix.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <px4_platform_common/sem.hpp>
-
-#include <crc32.h>
-
-#include <drivers/device/device.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_io_heater.h>
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_sbus.h>
-
-#include <lib/circuit_breaker/circuit_breaker.h>
-#include <lib/mathlib/mathlib.h>
-#include <lib/mixer_module/mixer_module.hpp>
-#include <lib/parameters/param.h>
-#include <lib/perf/perf_counter.h>
-#include <lib/rc/dsm.h>
-#include <lib/systemlib/mavlink_log.h>
-#include <lib/button/ButtonPublisher.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/input_rc.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/px4io_status.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <debug.h>
-
-#include <modules/px4iofirmware/protocol.h>
-
-#include "uploader.h"
-
-#include "modules/dataman/dataman.h"
-
-#include "px4io_driver.h"
-
-#define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
-#define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 1)
-#define PX4IO_CHECK_CRC			_IOC(0xff00, 2)
-
-static constexpr unsigned MIN_TOPIC_UPDATE_INTERVAL = 2500; // 2.5 ms -> 400 Hz
-
-using namespace time_literals;
-
-/**
- * The PX4IO class.
- *
- * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
- */
-class PX4IO : public cdev::CDev, public ModuleBase<PX4IO>, public OutputModuleInterface
-{
-public:
-	/**
-	 * Constructor.
-	 *
-	 * Initialize all class variables.
-	 */
-	PX4IO() = delete;
-	explicit PX4IO(device::Device *interface);
-
-	~PX4IO() override;
-
-	/**
-	 * Initialize the PX4IO class.
-	 *
-	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
-	 */
-	int		init() override;
-
-	/**
-	 * IO Control handler.
-	 *
-	 * Handle all IOCTL calls to the PX4IO file descriptor.
-	 *
-	 * @param[in] filp file handle (not used). This function is always called directly through object reference
-	 * @param[in] cmd the IOCTL command
-	 * @param[in] the IOCTL command parameter (optional)
-	 */
-	int		ioctl(file *filp, int cmd, unsigned long arg) override;
-
-	/**
-	 * Print IO status.
-	 *
-	 * Print all relevant IO status information
-	 *
-	 */
-	int			print_status();
-
-	static int custom_command(int argc, char *argv[]);
-
-	static int print_usage(const char *reason = nullptr);
-
-	static int task_spawn(int argc, char *argv[]);
-
-	/*
-	 * To test what happens if IO stops receiving updates from FMU.
-	 *
-	 * @param is_fail	true for failure condition, false for normal operation.
-	 */
-	void			test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; };
-
-	uint16_t		system_status() const { return _status; }
-
-	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-			   unsigned num_control_groups_updated) override;
-
-private:
-	void Run() override;
-
-	void updateDisarmed();
-	void updateFailsafe();
-	void updateTimerRateGroups();
-
-	static int checkcrc(int argc, char *argv[]);
-	static int bind(int argc, char *argv[]);
-
-	static constexpr int PX4IO_MAX_ACTUATORS = 8;
-
-	device::Device *const _interface;
-
-	unsigned		_hardware{0};		///< Hardware revision
-	unsigned		_max_actuators{0};		///< Maximum # of actuators supported by PX4IO
-	unsigned		_max_controls{0};		///< Maximum # of controls supported by PX4IO
-	unsigned		_max_rc_input{0};		///< Maximum receiver channels supported by PX4IO
-	unsigned		_max_transfer{16};		///< Maximum number of I2C transfers supported by PX4IO
-
-	bool			_first_param_update{true};
-	uint32_t    		_group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
-
-	hrt_abstime		_poll_last{0};
-
-	orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
-
-	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-	perf_counter_t	_interface_read_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface read")};
-	perf_counter_t	_interface_write_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface write")};
-
-	/* cached IO state */
-	uint16_t		_status{0};		///< Various IO status flags
-	uint16_t		_alarms{0};		///< Various IO alarms
-	uint16_t		_setup_arming{0};	///< last arming setup state
-	uint16_t		_last_written_arming_s{0};	///< the last written arming state reg
-	uint16_t		_last_written_arming_c{0};	///< the last written arming state reg
-
-	uORB::Subscription	_t_actuator_armed{ORB_ID(actuator_armed)};		///< system armed control topic
-	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};	///< vehicle command topic
-	uORB::Subscription	_t_vehicle_status{ORB_ID(vehicle_status)};		///< vehicle status topic
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	hrt_abstime             _last_status_publish{0};
-
-	uint16_t 		_rc_valid_update_count{0};
-
-	bool			_param_update_force{true};	///< force a parameter update
-	bool			_timer_rates_configured{false};
-
-	/* advertised topics */
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
-	uORB::Publication<px4io_status_s>	_px4io_status_pub{ORB_ID(px4io_status)};
-
-	ButtonPublisher	_button_publisher;
-	bool _previous_safety_off{false};
-
-	bool			_lockdown_override{false};	///< override the safety lockdown
-
-	int32_t		_thermal_control{-1}; ///< thermal control state
-	bool			_analog_rc_rssi_stable{false}; ///< true when analog RSSI input is stable
-	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};
-
-	bool _pwm_min_configured{false};
-	bool _pwm_max_configured{false};
-	bool _pwm_fail_configured{false};
-	bool _pwm_dis_configured{false};
-	bool _pwm_rev_configured{false};
-
-	/**
-	 * Update IO's arming-related state
-	 */
-	int			io_set_arming_state();
-
-	/**
-	 * Fetch status and alarms from IO
-	 *
-	 * Also publishes battery voltage/current.
-	 */
-	int			io_get_status();
-
-	/**
-	 * Fetch RC inputs from IO.
-	 *
-	 * @param input_rc	Input structure to populate.
-	 * @return		OK if data was returned.
-	 */
-	int			io_publish_raw_rc();
-
-	/**
-	 * write register(s)
-	 *
-	 * @param page		Register page to write to.
-	 * @param offset	Register offset to start writing at.
-	 * @param values	Pointer to array of values to write.
-	 * @param num_values	The number of values to write.
-	 * @return		OK if all values were successfully written.
-	 */
-	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
-
-	/**
-	 * write a register
-	 *
-	 * @param page		Register page to write to.
-	 * @param offset	Register offset to write to.
-	 * @param value		Value to write.
-	 * @return		OK if the value was written successfully.
-	 */
-	int			io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
-
-	/**
-	 * read register(s)
-	 *
-	 * @param page		Register page to read from.
-	 * @param offset	Register offset to start reading from.
-	 * @param values	Pointer to array where values should be stored.
-	 * @param num_values	The number of values to read.
-	 * @return		OK if all values were successfully read.
-	 */
-	int			io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
-
-	/**
-	 * read a register
-	 *
-	 * @param page		Register page to read from.
-	 * @param offset	Register offset to start reading from.
-	 * @return		Register value that was read, or _io_reg_get_error on error.
-	 */
-	uint32_t		io_reg_get(uint8_t page, uint8_t offset);
-	static const uint32_t	_io_reg_get_error = 0x80000000;
-
-	/**
-	 * modify a register
-	 *
-	 * @param page		Register page to modify.
-	 * @param offset	Register offset to modify.
-	 * @param clearbits	Bits to clear in the register.
-	 * @param setbits	Bits to set in the register.
-	 */
-	int			io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
-
-	/**
-	 * Handle a status update from IO.
-	 *
-	 * Publish IO status information if necessary.
-	 *
-	 * @param status	The status register
-	 */
-	int			io_handle_status(uint16_t status);
-
-	/**
-	 * Handle issuing dsm bind ioctl to px4io.
-	 *
-	 * @param dsmMode	DSM2_BIND_PULSES, DSMX_BIND_PULSES, DSMX8_BIND_PULSES
-	 */
-	int			dsm_bind_ioctl(int dsmMode);
-
-	/**
-	 * Respond to a vehicle command with an ACK message
-	 *
-	 * @param cmd		The command that was executed or denied (inbound)
-	 * @param result	The command result
-	 */
-	void			answer_command(const vehicle_command_s &cmd, uint8_t result);
-
-	void update_params();
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::PWM_SBUS_MODE>) _param_pwm_sbus_mode,
-		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
-		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
-		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal,
-		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
-		(ParamInt<px4::params::SYS_USE_IO>) _param_sys_use_io
-	)
-};
-
-#define PX4IO_DEVICE_PATH	"/dev/px4io"
+#include "px4io.hpp"

 PX4IO::PX4IO(device::Device *interface) :
 	CDev(PX4IO_DEVICE_PATH),
@@ -412,7 +107,6 @@ int PX4IO::init()

 	_hardware      = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
 	_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
-	_max_controls  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
 	_max_transfer  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
 	_max_rc_input  = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);

@@ -452,8 +146,7 @@ int PX4IO::init()
 	}

 	/* dis-arm IO before touching anything */
-	io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN,
-		      0);
+	io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);

 	if (ret != OK) {
 		mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t");
@@ -482,28 +175,6 @@ int PX4IO::init()
 	return OK;
 }

-void PX4IO::updateDisarmed()
-{
-	uint16_t values[PX4IO_MAX_ACTUATORS] {};
-
-	for (unsigned i = 0; i < _max_actuators; i++) {
-		values[i] = _mixing_output.disarmedValue(i);
-	}
-
-	io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators);
-}
-
-void PX4IO::updateFailsafe()
-{
-	uint16_t values[PX4IO_MAX_ACTUATORS] {};
-
-	for (unsigned i = 0; i < _max_actuators; i++) {
-		values[i] = _mixing_output.actualFailsafeValue(i);
-	}
-
-	io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators);
-}
-
 void PX4IO::Run()
 {
 	if (should_exit()) {
@@ -519,13 +190,29 @@ void PX4IO::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

+	if (_thermal_control) {
+		if (_heater_status_sub.updated()) {
+			heater_status_s heater_status;
+
+			if (_heater_status_sub.copy(&heater_status)) {
+
+				if (heater_status.heater_on) {
+					io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL);
+
+				} else {
+					io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF);
+				}
+			}
+		}
+	}
+
 	/* if we have new control data from the ORB, handle it */
 	if (_param_sys_hitl.get() <= 0) {
 		_mixing_output.update();
 	}

 	if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
-		/* run at 50 */
+		/* run at 50Hz */
 		_poll_last = hrt_absolute_time();

 		/* pull status and alarms from IO */
@@ -537,7 +224,7 @@ void PX4IO::Run()

 	if (_param_sys_hitl.get() <= 0) {
 		/* check updates on uORB topics and handle it */
-		if (_t_actuator_armed.updated()) {
+		if (_actuator_armed_sub.updated()) {
 			io_set_arming_state();

 			// TODO: throttle
@@ -546,9 +233,9 @@ void PX4IO::Run()

 	if (!_mixing_output.armed().armed) {
 		/* vehicle command */
-		if (_t_vehicle_command.updated()) {
+		if (_vehicle_command_sub.updated()) {
 			vehicle_command_s cmd{};
-			_t_vehicle_command.copy(&cmd);
+			_vehicle_command_sub.copy(&cmd);

 			// Check for a DSM pairing command
 			if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
@@ -595,11 +282,6 @@ void PX4IO::Run()

 			update_params();

-			/* Check if the flight termination circuit breaker has been updated */
-			bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
-			/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !disable_flighttermination);
-
 			if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) {

 				_thermal_control = _param_sens_en_themal.get();
@@ -615,21 +297,6 @@ void PX4IO::Run()

 				io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl);
 			}
-
-			/* S.BUS output */
-			if (_param_pwm_sbus_mode.get() == 1) {
-				/* enable S.BUS 1 */
-				io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
-
-			} else if (_param_pwm_sbus_mode.get() == 2) {
-				/* enable S.BUS 2 */
-				io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
-
-			} else {
-				/* disable S.BUS */
-				io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES,
-					      (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
-			}
 		}
 	}

@@ -738,9 +405,8 @@ void PX4IO::update_params()

 		// sync params to IO
 		updateTimerRateGroups();
-		updateFailsafe();
-		updateDisarmed();
 		_first_param_update = false;
+
 		return;
 	}

@@ -768,7 +434,7 @@ PX4IO::io_set_arming_state()

 	actuator_armed_s armed;

-	if (_t_actuator_armed.copy(&armed)) {
+	if (_actuator_armed_sub.copy(&armed)) {
 		if (armed.armed || armed.in_esc_calibration_mode) {
 			set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;

@@ -776,43 +442,7 @@ PX4IO::io_set_arming_state()
 			clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 		}

-		if (armed.prearmed) {
-			set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
-
-		} else {
-			clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED;
-		}
-
-		if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) {
-			set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
-			_lockdown_override = true;
-
-		} else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) {
-			clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
-			_lockdown_override = false;
-		}
-
-		if (armed.force_failsafe) {
-			set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
-
-		} else {
-			clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
-		}
-
-		// XXX this is for future support in the commander
-		// but can be removed if unneeded
-		// if (armed.termination_failsafe) {
-		// 	set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
-		// } else {
-		// 	clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
-		// }
-
-		if (armed.ready_to_arm) {
-			set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-
-		} else {
-			clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-		}
+		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}

 	if (_last_written_arming_s != set || _last_written_arming_c != clear) {
@@ -824,60 +454,6 @@ PX4IO::io_set_arming_state()
 	return 0;
 }

-int PX4IO::io_handle_status(uint16_t status)
-{
-	int ret = 1;
-	/**
-	 * WARNING: This section handles in-air resets.
-	 */
-
-	/* check for IO reset - force it back to armed if necessary */
-	if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
-		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
-
-		/* set new status */
-		_status = status;
-
-	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
-
-		/* set the sync flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
-		/* set new status */
-		_status = status;
-
-	} else {
-		ret = 0;
-
-		/* set new status */
-		_status = status;
-	}
-
-	/**
-	 * Get and handle the safety button status
-	 */
-	const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
-
-	if (safety_button_pressed) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
-		_button_publisher.safetyButtonTriggerEvent();
-	}
-
-	/**
-	 * Inform PX4IO board about safety_off state for LED control
-	 */
-	vehicle_status_s vehicle_status;
-
-	if (_t_vehicle_status.update(&vehicle_status)) {
-		if (_previous_safety_off != vehicle_status.safety_off) {
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
-			_previous_safety_off = vehicle_status.safety_off;
-		}
-	}
-
-	return ret;
-}
-
 int PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	// Do not bind if invalid pulses are provided
@@ -933,7 +509,30 @@ int PX4IO::io_get_status()
 	const uint16_t STATUS_VSERVO = regs[4];
 	const uint16_t STATUS_VRSSI  = regs[5];

-	io_handle_status(STATUS_FLAGS);
+	/* set new status */
+	_status = STATUS_FLAGS;
+
+	/**
+	 * Get and handle the safety button status
+	 */
+	const bool safety_button_pressed = _status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
+
+	if (safety_button_pressed) {
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
+		_button_publisher.safetyButtonTriggerEvent();
+	}
+
+	/**
+	 * Inform PX4IO board about safety_off state for LED control
+	 */
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		if (_previous_safety_off != vehicle_status.safety_off) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
+			_previous_safety_off = vehicle_status.safety_off;
+		}
+	}

 	const float rssi_v = STATUS_VRSSI * 0.001f; // voltage is scaled to mV

@@ -963,48 +562,32 @@ int PX4IO::io_get_status()
 		status.free_memory_bytes = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM);

 		// PX4IO_P_STATUS_FLAGS
-		status.status_outputs_armed   = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
 		status.status_rc_ok           = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_OK;
 		status.status_rc_ppm          = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_PPM;
 		status.status_rc_dsm          = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_DSM;
 		status.status_rc_sbus         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SBUS;
 		status.status_rc_st24         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24;
 		status.status_rc_sumd         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD;
-		status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
 		status.status_fmu_ok          = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_OK;
 		status.status_raw_pwm         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RAW_PWM;
-		status.status_arm_sync        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
-		status.status_init_ok         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
-		status.status_failsafe        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
 		status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;

 		// PX4IO_P_STATUS_ALARMS
-		status.alarm_rc_lost       = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
-		status.alarm_pwm_error     = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR;
+		status.alarm_rc_lost   = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
+		status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR;

 		// PX4IO_P_SETUP_ARMING
-		status.arming_io_arm_ok            = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-		status.arming_fmu_armed            = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED;
-		status.arming_fmu_prearmed         = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_PREARMED;
-		status.arming_failsafe_custom      = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
-		status.arming_lockdown             = SETUP_ARMING & PX4IO_P_SETUP_ARMING_LOCKDOWN;
-		status.arming_force_failsafe       = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
-		status.arming_termination_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
+		status.arming_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+		status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED;

 		for (unsigned i = 0; i < _max_actuators; i++) {
-			status.pwm[i] = io_reg_get(PX4IO_PAGE_SERVOS, i);
-			status.pwm_disarmed[i] = io_reg_get(PX4IO_PAGE_DISARMED_PWM, i);
-			status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i);
+			status.pwm[i] = io_reg_get(PX4IO_PAGE_DIRECT_PWM, i);
 		}

-		{
-			int i = 0;
-
-			for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) {
-				// This is a bit different than below, setting the groups, not the channels
-				status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset);
-			}
-		}
+		status.pwm_group_rate_hz[0] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0);
+		status.pwm_group_rate_hz[1] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP1);
+		status.pwm_group_rate_hz[2] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP2);
+		status.pwm_group_rate_hz[3] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP3);

 		uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);

@@ -1233,8 +816,7 @@ int PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC),
 	       io_reg_get(PX4IO_PAGE_SETUP,  PX4IO_P_SETUP_CRC + 1));

-	printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n",
-	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
+	printf("%" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n",
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
 	       io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT));
@@ -1269,14 +851,10 @@ int PX4IO::print_status()

 	/* setup and state */
 	uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
-	printf("features 0x%04" PRIx16 "%s%s%s\n", features,
-	       ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
-	       ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
+	printf("features 0x%04" PRIx16 "%s\n", features,
 	       ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 	      );

-	printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE));
-
 	printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));

 	/* IMU heater (Pixhawk 2.1) */
@@ -1304,11 +882,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)

 	/* regular ioctl? */
 	switch (cmd) {
-	case DSM_BIND_START:
-		/* bind a DSM receiver */
-		ret = dsm_bind_ioctl(arg);
-		break;
-
 	case PX4IO_SET_DEBUG:
 		PX4_DEBUG("PX4IO_SET_DEBUG");

@@ -1317,7 +890,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;

 	case PX4IO_REBOOT_BOOTLOADER:
-		if (system_status() & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
+		if (_status & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			PX4_ERR("not upgrading IO firmware, system is armed");
 			return -EINVAL;

@@ -1357,36 +930,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 			break;
 		}

-	case SBUS_SET_PROTO_VERSION:
-		PX4_DEBUG("SBUS_SET_PROTO_VERSION");
-
-		if (arg == 1) {
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
-
-		} else if (arg == 2) {
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
-
-		} else {
-			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES,
-					    (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
-		}
-
-		break;
-
-	case PX4IO_HEATER_CONTROL:
-		if (arg == (unsigned long)HEATER_MODE_DISABLED) {
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_IGNORE);
-
-		} else if (arg == 1) {
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL);
-
-		} else {
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF);
-
-		}
-
-		break;
-
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filep, cmd, arg);
@@ -1507,7 +1050,7 @@ int PX4IO::bind(int argc, char *argv[])
 		pulses = atoi(argv[1]);
 	}

-	get_instance()->ioctl(nullptr, DSM_BIND_START, pulses);
+	get_instance()->dsm_bind_ioctl(pulses);
 	return 0;
 }

@@ -1678,28 +1221,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return bind(argc - 1, argv + 1);
 	}

-	if (!strcmp(verb, "sbus1_out")) {
-		int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
-
-		if (ret != 0) {
-			PX4_ERR("S.BUS v1 failed (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "sbus2_out")) {
-		int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
-
-		if (ret != 0) {
-			PX4_ERR("S.BUS v2 failed (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
 	if (!strcmp(verb, "test_fmu_fail")) {
 		get_instance()->test_fmu_fail(true);
 		return 0;
@@ -1736,8 +1257,6 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");
 	PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_ok", "re-enable IO updates");

@@ -0,0 +1,304 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4io.hpp
+ * Driver for the PX4IO board.
+ *
+ * PX4IO is connected via DMA enabled high-speed UART.
+ */
+
+#include <px4_platform_common/events.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/sem.hpp>
+
+#include <crc32.h>
+
+#include <debug.h>
+
+#include <lib/circuit_breaker/circuit_breaker.h>
+#include <lib/button/ButtonPublisher.hpp>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <lib/rc/dsm.h>
+#include <lib/systemlib/mavlink_log.h>
+
+#include <modules/px4iofirmware/protocol.h>
+
+#include <uORB/topics/heater_status.h>
+#include <uORB/topics/input_rc.h>
+#include <uORB/topics/px4io_status.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "modules/dataman/dataman.h"
+
+#include "px4io_driver.h"
+
+#include "uploader.h"
+
+#define PX4IO_DEVICE_PATH               "/dev/px4io"
+
+#define PX4IO_SET_DEBUG                 _IOC(0xff00, 0)
+#define PX4IO_REBOOT_BOOTLOADER         _IOC(0xff00, 1)
+#define PX4IO_CHECK_CRC                 _IOC(0xff00, 2)
+
+static constexpr unsigned MIN_TOPIC_UPDATE_INTERVAL = 2500; // 2.5 ms -> 400 Hz
+
+using namespace time_literals;
+
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
+class PX4IO : public cdev::CDev, public ModuleBase<PX4IO>, public OutputModuleInterface
+{
+public:
+	/**
+	 * Constructor.
+	 *
+	 * Initialize all class variables.
+	 */
+	PX4IO() = delete;
+
+	explicit PX4IO(device::Device *interface);
+
+	~PX4IO() override;
+
+	/**
+	 * Initialize the PX4IO class.
+	 *
+	 * Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
+	int init() override;
+
+	/**
+	 * IO Control handler.
+	 *
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
+	int ioctl(file *filp, int cmd, unsigned long arg) override;
+
+	/**
+	 * Print IO status.
+	 *
+	 * Print all relevant IO status information
+	 *
+	 */
+	int print_status();
+
+	static int custom_command(int argc, char *argv[]);
+
+	static int print_usage(const char *reason = nullptr);
+
+	static int task_spawn(int argc, char *argv[]);
+
+	/*
+	 * To test what happens if IO stops receiving updates from FMU.
+	 *
+	 * @param is_fail       true for failure condition, false for normal operation.
+	 */
+	void test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; };
+
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+			   unsigned num_control_groups_updated) override;
+
+private:
+	void Run() override;
+
+	/**
+	 * Respond to a vehicle command with an ACK message
+	 *
+	 * @param cmd           The command that was executed or denied (inbound)
+	 * @param result        The command result
+	 */
+	void answer_command(const vehicle_command_s &cmd, uint8_t result);
+
+	/**
+	 * Handle issuing dsm bind ioctl to px4io.
+	 *
+	 * @param dsmMode       DSM2_BIND_PULSES, DSMX_BIND_PULSES, DSMX8_BIND_PULSES
+	 */
+	int dsm_bind_ioctl(int dsmMode);
+
+	/**
+	 * Update IO's arming-related state
+	 */
+	int io_set_arming_state();
+
+	/**
+	 * Fetch status and alarms from IO
+	 *
+	 * Also publishes battery voltage/current.
+	 */
+	int io_get_status();
+
+	/**
+	 * Fetch RC inputs from IO.
+	 *
+	 * @param input_rc      Input structure to populate.
+	 * @return              OK if data was returned.
+	 */
+	int io_publish_raw_rc();
+
+	/**
+	 * write register(s)
+	 *
+	 * @param page          Register page to write to.
+	 * @param offset        Register offset to start writing at.
+	 * @param values        Pointer to array of values to write.
+	 * @param num_values    The number of values to write.
+	 * @return              OK if all values were successfully written.
+	 */
+	int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+
+	/**
+	 * write a register
+	 *
+	 * @param page          Register page to write to.
+	 * @param offset        Register offset to write to.
+	 * @param value         Value to write.
+	 * @return              OK if the value was written successfully.
+	 */
+	int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
+
+	/**
+	 * read register(s)
+	 *
+	 * @param page          Register page to read from.
+	 * @param offset        Register offset to start reading from.
+	 * @param values        Pointer to array where values should be stored.
+	 * @param num_values    The number of values to read.
+	 * @return              OK if all values were successfully read.
+	 */
+	int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
+
+	/**
+	 * read a register
+	 *
+	 * @param page          Register page to read from.
+	 * @param offset        Register offset to start reading from.
+	 * @return              Register value that was read, or _io_reg_get_error on error.
+	 */
+	uint32_t io_reg_get(uint8_t page, uint8_t offset);
+
+	/**
+	 * modify a register
+	 *
+	 * @param page          Register page to modify.
+	 * @param offset        Register offset to modify.
+	 * @param clearbits     Bits to clear in the register.
+	 * @param setbits       Bits to set in the register.
+	 */
+	int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
+
+	void update_params();
+
+	void updateTimerRateGroups();
+
+	static int checkcrc(int argc, char *argv[]);
+
+	static int bind(int argc, char *argv[]);
+
+	static constexpr int PX4IO_MAX_ACTUATORS = 8;
+
+	static const uint32_t   _io_reg_get_error = 0x80000000;
+
+	device::Device *const _interface;
+
+	ButtonPublisher _button_publisher;
+	MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
+
+	hrt_abstime _last_status_publish{0};
+	hrt_abstime _poll_last{0};
+
+	orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t _interface_read_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface read")};
+	perf_counter_t _interface_write_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface write")};
+
+	/* advertised topics */
+	uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};
+	uORB::Publication<px4io_status_s>  _px4io_status_pub{ORB_ID(px4io_status)};
+
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};   ///< system armed control topic
+	uORB::Subscription _heater_status_sub{ORB_ID(heater_status)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command topic
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};   ///< vehicle status topic
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage
+
+	int32_t _thermal_control{-1};   ///< thermal control state
+
+	unsigned _hardware{0};          ///< Hardware revision
+	unsigned _max_actuators{0};     ///< Maximum # of actuators supported by PX4IO
+	unsigned _max_rc_input{0};      ///< Maximum receiver channels supported by PX4IO
+	unsigned _max_transfer{16};     ///< Maximum number of I2C transfers supported by PX4IO
+
+	uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
+
+	/* cached IO state */
+	uint16_t _alarms{0};                    ///< Various IO alarms
+	uint16_t _last_written_arming_s{0};     ///< the last written arming state reg
+	uint16_t _last_written_arming_c{0};     ///< the last written arming state reg
+	uint16_t _rc_valid_update_count{0};
+	uint16_t _setup_arming{0};              ///< last arming setup state
+	uint16_t _status{0};                    ///< Various IO status flags
+
+	bool _analog_rc_rssi_stable{false};     ///< true when analog RSSI input is stable
+
+	bool _first_param_update{true};
+	bool _param_update_force{true};         ///< force a parameter update
+	bool _previous_safety_off{false};
+
+	bool _test_fmu_fail{false};             ///< To test what happens if IO loses FMU
+	bool _timer_rates_configured{false};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
+		(ParamInt<px4::params::RC_RSSI_PWM_MAX>)  _param_rc_rssi_pwm_max,
+		(ParamInt<px4::params::RC_RSSI_PWM_MIN>)  _param_rc_rssi_pwm_min,
+		(ParamInt<px4::params::SENS_EN_THERMAL>)  _param_sens_en_themal,
+		(ParamInt<px4::params::SYS_HITL>)         _param_sys_hitl,
+		(ParamInt<px4::params::SYS_USE_IO>)       _param_sys_use_io
+	)
+};
@@ -53,13 +53,3 @@
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_USE_IO, 0);
-
-/**
- * S.BUS out
- *
- * Set to 1 to enable S.BUS version 1 output instead of RSSI.
- *
- * @boolean
- * @group PWM Outputs
- */
-PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0);
@@ -66,7 +66,6 @@ int ex_hwtest_main(int argc, char *argv[])
 	memset(&arm, 0, sizeof(arm));

 	arm.timestamp = hrt_absolute_time();
-	arm.ready_to_arm = true;
 	arm.armed = true;
 	orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm);
 	orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm);
@@ -75,13 +74,6 @@ int ex_hwtest_main(int argc, char *argv[])
 	int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
 	orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);

-	if (arm.ready_to_arm && arm.armed) {
-		warnx("Actuator armed");
-
-	} else {
-		errx(1, "Arming actuators failed");
-	}
-
 	hrt_abstime stime;

 	int count = 0;
@@ -79,7 +79,6 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou
 	/* Safely initialize armed flags */
 	_armed.armed = false;
 	_armed.prearmed = false;
-	_armed.ready_to_arm = false;
 	_armed.lockdown = false;
 	_armed.force_failsafe = false;
 	_armed.in_esc_calibration_mode = false;
@@ -93,7 +93,6 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
 {
 	return (a.armed == b.armed &&
 		a.prearmed == b.prearmed &&
-		a.ready_to_arm == b.ready_to_arm &&
 		a.lockdown == b.lockdown &&
 		a.manual_lockdown == b.manual_lockdown &&
 		a.force_failsafe == b.force_failsafe &&
@@ -2232,7 +2231,6 @@ Commander::run()

 			// publish actuator_armed first (used by output modules)
 			_actuator_armed.armed = _arm_state_machine.isArmed();
-			_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
 			_actuator_armed.timestamp = hrt_absolute_time();
 			_actuator_armed_pub.publish(_actuator_armed);

@@ -31,12 +31,9 @@
 #
 ############################################################################

-option(PX4IO_PERF "Enable px4io perf counters" OFF)
-
 add_library(px4iofirmware
 	adc.cpp
 	controls.cpp
-	mixer.cpp
 	px4io.cpp
 	registers.c
 	safety_button.cpp
@@ -54,8 +51,3 @@ target_link_libraries(px4iofirmware
 		nuttx_mm
 		rc
 )
-
-if(PX4IO_PERF)
-	target_compile_definitions(px4iofirmware PRIVATE PX4IO_PERF)
-	target_link_libraries(px4iofirmware PRIVATE perf)
-endif()
@@ -45,10 +45,6 @@

 #include <drivers/drv_hrt.h>

-#if defined(PX4IO_PERF)
-# include <perf/perf_counter.h>
-#endif
-
 #define DEBUG
 #include "px4io.h"

@@ -79,17 +75,9 @@
 #define rJDR4		REG(STM32_ADC_JDR4_OFFSET)
 #define rDR		REG(STM32_ADC_DR_OFFSET)

-#if defined(PX4IO_PERF)
-perf_counter_t		adc_perf;
-#endif
-
 int
 adc_init(void)
 {
-#if defined(PX4IO_PERF)
-	adc_perf = perf_alloc(PC_ELAPSED, "adc");
-#endif
-
 	/* put the ADC into power-down mode */
 	rCR2 &= ~ADC_CR2_ADON;
 	up_udelay(10);
@@ -143,10 +131,6 @@ adc_init(void)
 uint16_t
 adc_measure(unsigned channel)
 {
-#if defined(PX4IO_PERF)
-	perf_begin(adc_perf);
-#endif
-
 	/* clear any previous EOC */
 	rSR = 0;
 	(void)rDR;
@@ -162,9 +146,6 @@ adc_measure(unsigned channel)

 		/* never spin forever - this will give a bogus result though */
 		if (hrt_elapsed_time(&now) > 100) {
-#if defined(PX4IO_PERF)
-			perf_end(adc_perf);
-#endif
 			return 0xffff;
 		}
 	}
@@ -173,8 +154,5 @@ adc_measure(unsigned channel)
 	uint16_t result = rDR;
 	rSR = 0;

-#if defined(PX4IO_PERF)
-	perf_end(adc_perf);
-#endif
 	return result;
 }
@@ -50,21 +50,11 @@
 #include <rc/dsm.h>
 #include <uORB/topics/input_rc.h>

-#if defined(PX4IO_PERF)
-# include <perf/perf_counter.h>
-#endif
-
 #include "px4io.h"

 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
 static bool	dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);

-#if defined(PX4IO_PERF)
-static perf_counter_t c_gather_dsm;
-static perf_counter_t c_gather_sbus;
-static perf_counter_t c_gather_ppm;
-#endif
-
 static int _dsm_fd = -1;
 int _sbus_fd = -1;

@@ -79,9 +69,6 @@ static unsigned _frame_drops = 0;

 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
-#if defined(PX4IO_PERF)
-	perf_begin(c_gather_dsm);
-#endif
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
 	bool dsm_11_bit;
@@ -118,10 +105,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		}
 	}

-#if defined(PX4IO_PERF)
-	perf_end(c_gather_dsm);
-#endif
-
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
 	uint8_t st24_rssi, lost_count;
 	uint16_t st24_channel_count = 0;
@@ -198,12 +181,6 @@ controls_init(void)

 	/* S.bus input (USART3) */
 	_sbus_fd = sbus_init("/dev/ttyS2", false);
-
-#if defined(PX4IO_PERF)
-	c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
-	c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
-	c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
-#endif
 }

 void
@@ -244,10 +221,6 @@ controls_tick()
 		_rssi = 0;
 	}

-#if defined(PX4IO_PERF)
-	perf_begin(c_gather_sbus);
-#endif
-
 	bool sbus_updated = false;

 	if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
@@ -282,18 +255,11 @@ controls_tick()
 		}
 	}

-#if defined(PX4IO_PERF)
-	perf_end(c_gather_sbus);
-#endif
-
 	/*
 	 * XXX each S.bus frame will cause a PPM decoder interrupt
 	 * storm (lots of edges).  It might be sensible to actually
 	 * disable the PPM decoder completely if we have S.bus signal.
 	 */
-#if defined(PX4IO_PERF)
-	perf_begin(c_gather_ppm);
-#endif
 	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);

 	if (ppm_updated) {
@@ -303,16 +269,9 @@ controls_tick()
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}

-#if defined(PX4IO_PERF)
-	perf_end(c_gather_ppm);
-#endif
-
 	bool dsm_updated = false, st24_updated = false, sumd_updated = false;

 	if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) {
-#if defined(PX4IO_PERF)
-		perf_begin(c_gather_dsm);
-#endif

 		(void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated);

@@ -327,10 +286,6 @@ controls_tick()
 		if (sumd_updated) {
 			atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD);
 		}
-
-#if defined(PX4IO_PERF)
-		perf_end(c_gather_dsm);
-#endif
 	}

 	/* limit number of channels to allowable data size */
@@ -1,251 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mixer.cpp
- *
- * Control channel input/output mixer and failsafe.
- *
- * @author Lorenz Meier <lorenz@px4.io>
- */
-
-#include <px4_platform_common/px4_config.h>
-#include <syslog.h>
-
-#include <sys/types.h>
-#include <stdbool.h>
-#include <float.h>
-#include <string.h>
-#include <math.h>
-
-#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_hrt.h>
-
-#include <rc/sbus.h>
-
-extern "C" {
-	/* #define DEBUG */
-#include "px4io.h"
-}
-
-/*
- * Maximum interval in us before FMU signal is considered lost
- */
-#define FMU_INPUT_DROP_LIMIT_US		500000
-
-/* current servo arm/disarm state */
-static volatile bool mixer_servos_armed = false;
-static volatile bool should_arm = false;
-static volatile bool should_arm_nothrottle = false;
-static volatile bool should_always_enable_pwm = false;
-
-static bool new_fmu_data = false;
-static uint64_t last_fmu_update = 0;
-
-extern int _sbus_fd;
-
-/* selected control values and count for mixing */
-enum mixer_source {
-	MIX_NONE,
-	MIX_DISARMED,
-	MIX_FAILSAFE,
-};
-
-void
-mixer_tick()
-{
-	/* check that we are receiving fresh data from the FMU */
-	irqstate_t irq_flags = enter_critical_section();
-	const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
-	leave_critical_section(irq_flags);
-
-	if ((fmu_data_received_time == 0) ||
-	    hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
-
-		/* too long without FMU input, time to go to failsafe */
-		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
-			isr_debug(1, "AP RX timeout");
-		}
-
-		atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
-
-	} else {
-		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
-
-		/* this flag is never cleared once OK */
-		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED);
-
-		if (fmu_data_received_time > last_fmu_update) {
-			new_fmu_data = true;
-			last_fmu_update = fmu_data_received_time;
-		}
-	}
-
-	/* default to disarmed mixing */
-	mixer_source source = MIX_DISARMED;
-
-	/*
-	 * Decide which set of controls we're using.
-	 */
-
-	/* Do not mix if we have raw PWM and FMU is ok. */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
-
-		/* don't actually mix anything - copy values from r_page_direct_pwm */
-		source = MIX_NONE;
-		memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
-	}
-
-	/*
-	 * Decide whether the servos should be armed right now.
-	 *
-	 * We must be armed, and we must have a PWM source; either raw from
-	 * FMU or from the mixer.
-	 *
-	 */
-	should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)		/* IO initialised without error */
-		     && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		/* and FMU is armed */
-		     ;
-
-	should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)  /* IO initialised without error */
-				 && ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED)     /* and FMU is prearmed */
-				     || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)       /* or direct PWM is set */
-				    ));
-
-	/* we enable PWM output always on the IO side if FMU is up and running
-	 * as zero-outputs can be controlled by FMU by sending a 0 PWM command
-	 */
-	should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
-				    && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK));
-
-	/*
-	 * Check if FMU is still alive, if not, terminate the flight
-	 */
-	if (REG_TO_BOOL(r_setup_flighttermination) && 			/* Flight termination is allowed */
-	    (source == MIX_DISARMED) && 				/* and if we ended up not changing the default mixer */
-	    should_arm && 						/* and we should be armed, so we intended to provide outputs */
-	    (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { 	/* and FMU is initialized */
-		atomic_modify_or(&r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */
-	}
-
-	/*
-	 * Check if we should force failsafe - and do it if we have to
-	 */
-	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
-		source = MIX_FAILSAFE;
-	}
-
-	/*
-	 * Set failsafe status flag depending on mixing source
-	 */
-	if (source == MIX_FAILSAFE) {
-		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE);
-
-	} else {
-		atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE));
-	}
-
-	/*
-	 * Run the mixers.
-	 */
-	if (source == MIX_FAILSAFE) {
-		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_failsafe[i];
-		}
-
-	} else if (source == MIX_DISARMED) {
-		/* copy disarmed values to the servo outputs */
-		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			r_page_servos[i] = r_page_servo_disarmed[i];
-		}
-	}
-
-	/* set arming */
-	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
-
-	/* lockdown means to send a valid pulse which disables the outputs */
-	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
-		needs_to_arm = true;
-	}
-
-	if (needs_to_arm && !mixer_servos_armed) {
-		/* need to arm, but not armed */
-		up_pwm_servo_arm(true, 0);
-		mixer_servos_armed = true;
-		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
-		isr_debug(5, "> PWM enabled");
-
-	} else if (!needs_to_arm && mixer_servos_armed) {
-		/* armed but need to disarm */
-		up_pwm_servo_arm(false, 0);
-		mixer_servos_armed = false;
-		atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED));
-		isr_debug(5, "> PWM disabled");
-	}
-
-	if (mixer_servos_armed
-	    && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE))
-	    && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
-		/* update the servo outputs. */
-		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			up_pwm_servo_set(i, r_page_servos[i]);
-		}
-
-		/* set S.BUS1 or S.BUS2 outputs */
-		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
-
-		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
-		}
-
-	} else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
-		/* set the disarmed servo outputs. */
-		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
-			/* copy values into reporting register */
-			r_page_servos[i] = r_page_servo_disarmed[i];
-		}
-
-		/* set S.BUS1 or S.BUS2 outputs */
-		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
-		}
-
-		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
-		}
-	}
-}
@@ -79,16 +79,13 @@

 #define PX4IO_PROTOCOL_VERSION		5

-/* maximum allowable sizes on this protocol version */
-#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT	8	/**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
-
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG			0
 #define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
 #define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
-#define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
+
 #define PX4IO_P_CONFIG_ACTUATOR_COUNT		5	/* hardcoded max actuator output count */
 #define PX4IO_P_CONFIG_RC_INPUT_COUNT		6	/* hardcoded max R/C input count supported */
 #define PX4IO_P_CONFIG_ADC_INPUT_COUNT		7	/* hardcoded max ADC inputs */
@@ -97,21 +94,20 @@
 /* dynamic status page */
 #define PX4IO_PAGE_STATUS			1
 #define PX4IO_P_STATUS_FREEMEM			0
-#define PX4IO_P_STATUS_CPULOAD			1

 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
+/*                                              (1 << 0) */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 1) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 2) /* PPM input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_DSM		(1 << 3) /* DSM input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SBUS		(1 << 4) /* SBUS input is valid */
 #define PX4IO_P_STATUS_FLAGS_FMU_OK		(1 << 5) /* controls from FMU are valid */
 #define PX4IO_P_STATUS_FLAGS_RAW_PWM		(1 << 6) /* raw PWM from FMU */
-#define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 7) /* the arming state between IO and FMU is in sync */
-#define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 8) /* initialisation of the IO completed without error */
-#define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 9) /* failsafe is active */
+/* UNUSED                                       (1 << 7) */
+/* UNUSED                                       (1 << 8) */
+/* UNUSED                                       (1 << 9) */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF         (1 << 10) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED	(1 << 11) /* FMU was initialized and OK once */
+/* UNUSED                                       (1 << 11) */
 #define PX4IO_P_STATUS_FLAGS_RC_ST24		(1 << 12) /* ST24 input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_SUMD		(1 << 13) /* SUMD input is valid */
 #define PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT	(1 << 14) /* px4io safety button was pressed for longer than 1 second */
@@ -123,9 +119,6 @@
 #define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
 #define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */

-/* array of PWM servo output values, microseconds */
-#define PX4IO_PAGE_SERVOS			3	/* 0..CONFIG_ACTUATOR_COUNT-1 */
-
 /* array of raw RC input values, microseconds */
 #define PX4IO_PAGE_RAW_RC_INPUT			4
 #define PX4IO_P_RAW_RC_COUNT			0	/* number of valid channels */
@@ -152,22 +145,14 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP			50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+/* UNUSED                                       (1 << 0) */
+/* UNUSED                                       (1 << 1) */
 #define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 2) /**< enable ADC RSSI parsing */

 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK            (1 << 0) /* OK to arm the IO side */
 #define PX4IO_P_SETUP_ARMING_FMU_ARMED            (1 << 1) /* FMU is already armed */
-#define PX4IO_P_SETUP_ARMING_FMU_PREARMED         (1 << 2) /* FMU is already prearmed */
-#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM      (1 << 3) /* use custom failsafe values */
-#define PX4IO_P_SETUP_ARMING_LOCKDOWN             (1 << 4) /* If set, the system operates normally, but won't actuate any servos */
-#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE       (1 << 5) /* If set, the system will always output the failsafe values */
-#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */

-#define PX4IO_P_SETUP_PWM_RATES                 2	/* bitmask, 0 = low rate, 1 = high rate */
-#define PX4IO_P_SETUP_PWM_DEFAULTRATE           3	/* 'low' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_PWM_ALTRATE               4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_VSERVO_SCALE              5	/* hardware rev [2] servo voltage correction factor (float) */
 #define PX4IO_P_SETUP_DSM                       6	/* DSM bind state */
 enum {							/* DSM bind states */
@@ -187,9 +172,9 @@ enum {							/* DSM bind states */
 /* storage space of 12 occupied by CRC */
 #define PX4IO_P_SETUP_SAFETY_BUTTON_ACK	14	/**< ACK from FMU when it gets safety button pressed status */
 #define PX4IO_P_SETUP_SAFETY_OFF		15	/**< FMU inform PX4IO about safety_off for LED indication*/
-#define PX4IO_P_SETUP_SBUS_RATE			16	/**< frame rate of SBUS1 output in Hz */
+/* UNUSED                                       16      */
 #define PX4IO_P_SETUP_THERMAL			17	/**< thermal management */
-#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION	18	/**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
+/* UNUSED                                       18      */
 #define PX4IO_P_SETUP_PWM_RATE_GROUP0            19	/* Configure timer group 0 update rate in Hz */
 #define PX4IO_P_SETUP_PWM_RATE_GROUP1            20	/* Configure timer group 1 update rate in Hz */
 #define PX4IO_P_SETUP_PWM_RATE_GROUP2            21	/* Configure timer group 2 update rate in Hz */
@@ -202,16 +187,6 @@ enum {							/* DSM bind states */
 /* PWM output */
 #define PX4IO_PAGE_DIRECT_PWM			54		/**< 0..CONFIG_ACTUATOR_COUNT-1 */

-/* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
-
-/* Debug and test page - not used in normal operation */
-#define PX4IO_PAGE_TEST				127
-#define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
-
-/* PWM disarmed values that are active */
-#define PX4IO_PAGE_DISARMED_PWM			109		/* 0..CONFIG_ACTUATOR_COUNT-1 */
-
 /**
 * Serial protocol encapsulation.
 */
@@ -55,10 +55,6 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_watchdog.h>

-#if defined(PX4IO_PERF)
-# include <lib/perf/perf_counter.h>
-#endif
-
 #include <stm32_uart.h>

 #define DEBUG
@@ -77,6 +73,20 @@ static volatile uint32_t last_msg_counter;
 static volatile uint8_t msg_next_out;
 static volatile uint8_t msg_next_in;

+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
+/*
+ * Maximum interval in us before FMU signal is considered lost
+ */
+#define FMU_INPUT_DROP_LIMIT_US		500000
+
+/* current servo arm/disarm state */
+static volatile bool mixer_servos_armed = false;
+
+static bool new_fmu_data = false;
+static uint64_t last_fmu_update = 0;
+
 /*
 * WARNING: too large buffers here consume the memory required
 * for mixer handling. Do not allocate more than 80 bytes for
@@ -272,6 +282,64 @@ calculate_fw_crc(void)
 	r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16;
 }

+static void mixer_tick()
+{
+	/* check that we are receiving fresh data from the FMU */
+	irqstate_t irq_flags = enter_critical_section();
+	const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
+	leave_critical_section(irq_flags);
+
+	if ((fmu_data_received_time == 0) ||
+	    hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+
+		/* too long without FMU input, time to go to failsafe */
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+			isr_debug(1, "AP RX timeout");
+		}
+
+		atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	} else {
+		atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+		if (fmu_data_received_time > last_fmu_update) {
+			new_fmu_data = true;
+			last_fmu_update = fmu_data_received_time;
+		}
+	}
+
+	/*
+	 * Decide whether the servos should be armed right now.
+	 *
+	 * We must be armed, and we must have a PWM source; either raw from
+	 * FMU or from the mixer.
+	 *
+	 */
+	const bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
+				&& (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+				&& (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM);
+
+	if (should_arm && !mixer_servos_armed) {
+		/* need to arm, but not armed */
+		up_pwm_servo_arm(true, 0);
+		mixer_servos_armed = true;
+		isr_debug(5, "> PWM enabled");
+
+	} else if (!should_arm && mixer_servos_armed) {
+		/* armed but need to disarm */
+		up_pwm_servo_arm(false, 0);
+		mixer_servos_armed = false;
+		isr_debug(5, "> PWM disabled");
+	}
+
+	if (mixer_servos_armed && should_arm) {
+		/* update the servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			up_pwm_servo_set(i, r_page_direct_pwm[i]);
+		}
+	}
+}
+
 extern "C" __EXPORT int user_start(int argc, char *argv[])
 {
 	/* configure the first 8 PWM outputs (i.e. all of them) */
@@ -307,11 +375,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
 	LED_GREEN(false);
 #endif /* LED_GREEN */

-	/* turn off S.Bus out (if supported) */
-#ifdef ENABLE_SBUS_OUT
-	ENABLE_SBUS_OUT(false);
-#endif
-
 	/* start the safety button handler */
 	safety_button_init();

@@ -324,17 +387,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
 	/* start the FMU interface */
 	interface_init();

-#if defined(PX4IO_PERF)
-	/* add a performance counter for mixing */
-	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
-
-	/* add a performance counter for controls */
-	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
-
-	/* and one for measuring the loop rate */
-	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
-#endif
-
 	struct mallinfo minfo = mallinfo();
 	r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
 	syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
@@ -354,30 +406,9 @@ extern "C" __EXPORT int user_start(int argc, char *argv[])
 	for (;;) {
 		watchdog_pet();

-#if defined(PX4IO_PERF)
-		/* track the rate at which the loop is running */
-		perf_count(loop_perf);
-
-		/* kick the mixer */
-		perf_begin(mixer_perf);
-#endif
-
 		mixer_tick();
-
-#if defined(PX4IO_PERF)
-		perf_end(mixer_perf);
-
-		/* kick the control inputs */
-		perf_begin(controls_perf);
-#endif
-
 		controls_tick();

-#if defined(PX4IO_PERF)
-		perf_end(controls_perf);
-#endif
-
-
 		/*
 		blink blue LED at 4Hz in normal operation. When in
 		override blink 4x faster so the user can clearly see
@@ -77,13 +77,10 @@ __BEGIN_DECLS
 * Registers.
 */
 extern volatile uint16_t	r_page_status[];	/* PX4IO_PAGE_STATUS */
-extern uint16_t			r_page_servos[];	/* PX4IO_PAGE_SERVOS */
 extern uint16_t			r_page_direct_pwm[];	/* PX4IO_PAGE_DIRECT_PWM */
 extern uint16_t			r_page_raw_rc_input[];	/* PX4IO_PAGE_RAW_RC_INPUT */

 extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
-extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
-extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */

 /*
 * Register aliases.
@@ -99,13 +96,6 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */

 #define r_setup_features	r_page_setup[PX4IO_P_SETUP_FEATURES]
 #define r_setup_arming		r_page_setup[PX4IO_P_SETUP_ARMING]
-#define r_setup_pwm_rates	r_page_setup[PX4IO_P_SETUP_PWM_RATES]
-#define r_setup_pwm_defaultrate	r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
-#define r_setup_pwm_altrate	r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
-
-#define r_setup_sbus_rate	r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
-
-#define r_setup_flighttermination	r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]

 /*
 * System state structure.
@@ -123,8 +113,6 @@ struct sys_state_s {

 extern struct sys_state_s system_state;

-# define ENABLE_SBUS_OUT(_s)		px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
-
 # define VDD_SERVO_FAULT		(!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))

 # define PX4IO_ADC_CHANNEL_COUNT	2
@@ -137,11 +125,6 @@ void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
 void atomic_modify_clear(volatile uint16_t *target, uint16_t modification);
 void atomic_modify_and(volatile uint16_t *target, uint16_t modification);

-/*
- * Mixer
- */
-extern void	mixer_tick(void);
-
 /**
 * Safety button/LED.
 */
@@ -55,7 +55,6 @@
 #include "protocol.h"

 static int	registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
-static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);

 /**
 * PAGE 0
@@ -67,7 +66,6 @@ static const uint16_t	r_page_config[] = {
 	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 2,
 	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= PX4IO_BL_VERSION,
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= PX4IO_MAX_TRANSFER_LEN,
-	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
 	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
 	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_RC_INPUT_CHANNELS,
 	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
@@ -80,20 +78,12 @@ static const uint16_t	r_page_config[] = {
 */
 volatile uint16_t	r_page_status[] = {
 	[PX4IO_P_STATUS_FREEMEM]		= 0,
-	[PX4IO_P_STATUS_CPULOAD]		= 0,
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VSERVO]			= 0,
 	[PX4IO_P_STATUS_VRSSI]			= 0,
 };

-/**
- * PAGE 3
- *
- * Servo PWM values
- */
-uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
-
 /**
 * PAGE 4
 *
@@ -133,51 +123,19 @@ volatile uint16_t	r_page_setup[] = {
 	/* default to RSSI ADC functionality */
 	[PX4IO_P_SETUP_FEATURES]		= PX4IO_P_SETUP_FEATURES_ADC_RSSI,
 	[PX4IO_P_SETUP_ARMING]			= 0,
-	[PX4IO_P_SETUP_PWM_RATES]		= 0,
-	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
-	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
-	[PX4IO_P_SETUP_SBUS_RATE]		= 72,
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
 	[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
-	[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0,
 	[PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 0
 };

-#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
+#define PX4IO_P_SETUP_FEATURES_VALID	(PX4IO_P_SETUP_FEATURES_ADC_RSSI)

-#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
-		PX4IO_P_SETUP_ARMING_FMU_PREARMED | \
-		PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
-		PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
-		PX4IO_P_SETUP_ARMING_LOCKDOWN | \
-		PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
-		PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_IO_ARM_OK)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)

-/*
- * PAGE 104 uses r_page_servos.
- */
-
-/**
- * PAGE 105
- *
- * Failsafe servo PWM values
- *
- * Disable pulses as default.
- */
-uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-
-/**
- * PAGE 109
- *
- * disarmed PWM values for difficult ESCs
- *
- */
-uint16_t		r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-
 int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
@@ -208,60 +166,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num

 		break;

-	/* handle setup for servo failsafe values */
-	case PX4IO_PAGE_FAILSAFE_PWM:
-
-		/* copy channel data */
-		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
-
-			if (*values == 0) {
-				/* ignore 0 */
-			} else if (*values < PWM_LOWEST_MIN) {
-				r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
-
-			} else if (*values > PWM_HIGHEST_MAX) {
-				r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
-
-			} else {
-				r_page_servo_failsafe[offset] = *values;
-			}
-
-			/* flag the failsafe values as custom */
-			r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
-
-			offset++;
-			num_values--;
-			values++;
-		}
-
-		break;
-
-	case PX4IO_PAGE_DISARMED_PWM: {
-			/* copy channel data */
-			while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
-				if (*values == 0) {
-					/* 0 means disabling always PWM */
-					r_page_servo_disarmed[offset] = 0;
-
-				} else if (*values < PWM_LOWEST_MIN) {
-					r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
-
-				} else if (*values > PWM_HIGHEST_MAX) {
-					r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
-
-				} else {
-					r_page_servo_disarmed[offset] = *values;
-				}
-
-				offset++;
-				num_values--;
-				values++;
-			}
-
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
-		}
-		break;
-
 	default:

 		/* avoid offset wrap */
@@ -297,19 +201,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			r_status_alarms &= ~value;
 			break;

-		case PX4IO_P_STATUS_FLAGS:
-
-			/*
-			 * Allow FMU override of arming state (to allow in-air restores),
-			 * but only if the arming state is not in sync on the IO side.
-			 */
-			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
-				r_status_flags = value;
-
-			}
-
-			break;
-
 		default:
 			/* just ignore writes to other registers in this page */
 			break;
@@ -323,29 +214,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)

 			value &= PX4IO_P_SETUP_FEATURES_VALID;

-			/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
-
-			/* switch S.Bus output pin as needed */
-#ifdef ENABLE_SBUS_OUT
-			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
-
-			/* disable the conflicting options with SBUS 1 */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
-			}
-
-			/* disable the conflicting options with SBUS 2 */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
-			}
-
-#endif
-
-			/* disable the conflicting options with ADC RSSI */
-			if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
-			}
-
 			/* apply changes */
 			r_setup_features = value;

@@ -355,58 +223,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)

 			value &= PX4IO_P_SETUP_ARMING_VALID;

-			/*
-			 * If the failsafe termination flag is set, do not allow the autopilot to unset it
-			 */
-			value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
-
-			/*
-			 * If failsafe termination is enabled and force failsafe bit is set, do not allow
-			 * the autopilot to clear it.
-			 */
-			if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
-				value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
-			}
-
 			r_setup_arming = value;

 			break;

-		case PX4IO_P_SETUP_PWM_RATES:
-			value &= PX4IO_P_SETUP_RATES_VALID;
-			pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
-			break;
-
-		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
-			if (value < 25) {
-				value = 25;
-			}
-
-			if (value > 400) {
-				value = 400;
-			}
-
-			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
-			break;
-
-		case PX4IO_P_SETUP_PWM_ALTRATE:
-
-			/* For PWM constrain to [25,400]Hz
-			 * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode
-			 */
-			if (value != 0) {
-				if (value < 25) {
-					value = 25;
-				}
-
-				if (value > 400) {
-					value = 400;
-				}
-			}
-
-			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
-			break;
-
 		case PX4IO_P_SETUP_SET_DEBUG:
 			r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
@@ -446,13 +266,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)

 			break;

-		case PX4IO_P_SETUP_SBUS_RATE:
-			r_page_setup[offset] = value;
-			sbus1_set_output_rate_hz(value);
-			break;
-
 		case PX4IO_P_SETUP_THERMAL:
-		case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION:
 			r_page_setup[offset] = value;
 			break;

@@ -489,15 +303,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)

 		break;

-	case PX4IO_PAGE_TEST:
-		switch (offset) {
-		case PX4IO_P_TEST_LED:
-			LED_AMBER(value & 1);
-			break;
-		}
-
-		break;
-
 	default:
 		return -1;
 	}
@@ -525,8 +330,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_STATUS:
 		/* PX4IO_P_STATUS_FREEMEM */

-		/* XXX PX4IO_P_STATUS_CPULOAD */
-
 		/* PX4IO_P_STATUS_FLAGS maintained externally */

 		/* PX4IO_P_STATUS_ALARMS maintained externally */
@@ -592,10 +395,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		SELECT_PAGE(r_page_config);
 		break;

-	case PX4IO_PAGE_SERVOS:
-		SELECT_PAGE(r_page_servos);
-		break;
-
 	case PX4IO_PAGE_RAW_RC_INPUT:
 		SELECT_PAGE(r_page_raw_rc_input);
 		break;
@@ -609,14 +408,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		SELECT_PAGE(r_page_direct_pwm);
 		break;

-	case PX4IO_PAGE_FAILSAFE_PWM:
-		SELECT_PAGE(r_page_servo_failsafe);
-		break;
-
-	case PX4IO_PAGE_DISARMED_PWM:
-		SELECT_PAGE(r_page_servo_disarmed);
-		break;
-
 	default:
 		return -1;
 	}
@@ -638,51 +429,3 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val

 	return 0;
 }
-
-/*
- * Helper function to handle changes to the PWM rate control registers.
- */
-static void
-pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
-{
-	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
-
-			/* get the channel mask for this rate group */
-			uint32_t mask = up_pwm_servo_get_rate_group(group);
-
-			if (mask == 0) {
-				continue;
-			}
-
-			/* all channels in the group must be either default or alt-rate */
-			uint32_t alt = map & mask;
-
-			if (pass == 0) {
-				/* preflight */
-				if ((alt != 0) && (alt != mask)) {
-					/* not a legal map, bail with an alarm */
-					r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
-					return;
-				}
-
-			} else {
-				/* set it - errors here are unexpected */
-				if (alt != 0) {
-					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) {
-						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
-					}
-
-				} else {
-					if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) {
-						r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
-					}
-				}
-			}
-		}
-	}
-
-	r_setup_pwm_rates = map;
-	r_setup_pwm_defaultrate = defaultrate;
-	r_setup_pwm_altrate = altrate;
-}
@@ -136,12 +136,6 @@ safety_button_check(void *arg)
 static void
 failsafe_blink(void *arg)
 {
-	/* indicate that a serious initialisation error occured */
-	if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
-		LED_AMBER(true);
-		return;
-	}
-
 	static bool failsafe = false;

 	/* blink the failsafe LED if we don't have FMU input */
@@ -55,20 +55,6 @@
 //#define DEBUG
 #include "px4io.h"

-#if defined(PX4IO_PERF)
-# include <perf/perf_counter.h>
-
-static perf_counter_t	pc_txns;
-static perf_counter_t	pc_errors;
-static perf_counter_t	pc_ore;
-static perf_counter_t	pc_fe;
-static perf_counter_t	pc_ne;
-static perf_counter_t	pc_idle;
-static perf_counter_t	pc_badidle;
-static perf_counter_t	pc_regerr;
-static perf_counter_t	pc_crcerr;
-#endif
-
 static void		rx_handle_packet(void);
 static void		rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
 static DMA_HANDLE	tx_dma;
@@ -92,18 +78,6 @@ static struct IOPacket	dma_packet;
 void
 interface_init(void)
 {
-#if defined(PX4IO_PERF)
-	pc_txns = perf_alloc(PC_ELAPSED, "txns");
-	pc_errors = perf_alloc(PC_COUNT, "errors");
-	pc_ore = perf_alloc(PC_COUNT, "overrun");
-	pc_fe = perf_alloc(PC_COUNT, "framing");
-	pc_ne = perf_alloc(PC_COUNT, "noise");
-	pc_idle = perf_alloc(PC_COUNT, "idle");
-	pc_badidle = perf_alloc(PC_COUNT, "badidle");
-	pc_regerr = perf_alloc(PC_COUNT, "regerr");
-	pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
-#endif
-
 	/* allocate DMA */
 	tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
 	rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
@@ -165,10 +139,6 @@ rx_handle_packet(void)
 	dma_packet.crc = 0;

 	if (crc != crc_packet(&dma_packet)) {
-#if defined(PX4IO_PERF)
-		perf_count(pc_crcerr);
-#endif
-
 		/* send a CRC error reply */
 		dma_packet.count_code = PKT_CODE_CORRUPT;
 		dma_packet.page = 0xff;
@@ -181,10 +151,6 @@ rx_handle_packet(void)

 		/* it's a blind write - pass it on */
 		if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
-#if defined(PX4IO_PERF)
-			perf_count(pc_regerr);
-#endif
-
 			dma_packet.count_code = PKT_CODE_ERROR;

 		} else {
@@ -201,10 +167,6 @@ rx_handle_packet(void)
 		uint16_t *registers;

 		if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
-#if defined(PX4IO_PERF)
-			perf_count(pc_regerr);
-#endif
-
 			dma_packet.count_code = PKT_CODE_ERROR;

 		} else {
@@ -238,9 +200,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	 * We are here because DMA completed, or UART reception stopped and
 	 * we think we have a packet in the buffer.
 	 */
-#if defined(PX4IO_PERF)
-	perf_begin(pc_txns);
-#endif

 	/* disable UART DMA */
 	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
@@ -265,10 +224,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 		DMA_CCR_MSIZE_8BITS);
 	stm32_dmastart(tx_dma, NULL, NULL, false);
 	rCR3 |= USART_CR3_DMAT;
-
-#if defined(PX4IO_PERF)
-	perf_end(pc_txns);
-#endif
 }

 static int
@@ -283,23 +238,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
 		  USART_SR_NE |		/* noise error - we have lost a byte due to noise */
 		  USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */

-#if defined(PX4IO_PERF)
-		perf_count(pc_errors);
-
-		if (sr & USART_SR_ORE) {
-			perf_count(pc_ore);
-		}
-
-		if (sr & USART_SR_NE) {
-			perf_count(pc_ne);
-		}
-
-		if (sr & USART_SR_FE) {
-			perf_count(pc_fe);
-		}
-
-#endif
-
 		/* send a line break - this will abort transmission/reception on the other end */
 		rCR1 |= USART_CR1_SBK;

@@ -331,9 +269,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
 		if ((length < 1) || (length < PKT_SIZE(dma_packet))) {

 			/* it was too short - possibly truncated */
-#if defined(PX4IO_PERF)
-			perf_count(pc_badidle);
-#endif
 			dma_reset();
 			return 0;
 		}
@@ -342,9 +277,6 @@ serial_interrupt(int irq, void *context, FAR void *arg)
 		 * Looks like we received a packet. Stop the DMA and go process the
 		 * packet.
 		 */
-#if defined(PX4IO_PERF)
-		perf_count(pc_idle);
-#endif
 		stm32_dmastop(rx_dma);
 		rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
 	}
Author	SHA1	Message	Date
mcsauder	a01b029578	Breakout px4io.hpp and delete unneeded #includes.	2022-09-15 13:07:51 -04:00
Daniel Agar	23011ac404	WIP	2022-09-09 15:12:03 -04:00