diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board
index db58d4e28a..1aec34124a 100644
--- a/boards/modalai/voxl2-slpi/default.px4board
+++ b/boards/modalai/voxl2-slpi/default.px4board
@@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
 CONFIG_DRIVERS_RC_CRSF_RC=y
+CONFIG_DRIVERS_VOXL2_IO=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_EKF2=y
diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt
index 57a5f4af21..b6e3d8a3c2 100644
--- a/src/drivers/voxl2_io/CMakeLists.txt
+++ b/src/drivers/voxl2_io/CMakeLists.txt
@@ -37,15 +37,12 @@ px4_add_module(
 	SRCS
 		voxl2_io_crc16.c
 		voxl2_io_crc16.h
-		voxl2_io_serial.cpp
-		voxl2_io_serial.hpp
 		voxl2_io_packet.c
 		voxl2_io_packet.h
 		voxl2_io_packet_types.h
 		voxl2_io.cpp
 		voxl2_io.hpp
 	DEPENDS
-		rc
 		px4_work_queue
 		mixer_module
 	MODULE_CONFIG
diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig
index 622cad470d..15964deeeb 100644
--- a/src/drivers/voxl2_io/Kconfig
+++ b/src/drivers/voxl2_io/Kconfig
@@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO
 	bool "voxl2_io"
 	default n
 	---help---
-		Enable support for voxl2_io
+		Enable support for voxl2_io
\ No newline at end of file
diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml
index 021305e955..17feaba8c5 100644
--- a/src/drivers/voxl2_io/module.yaml
+++ b/src/drivers/voxl2_io/module.yaml
@@ -1,12 +1,13 @@
 module_name: VOXL2 IO Output
 actuator_output:
-  config_parameters:
-    - param: 'VOXL2_IO_MIN'
-      label: 'PWM min value'
-    - param: 'VOXL2_IO_MAX'
-      label: 'PWM max value'
+#  config_parameters:
+#    - param: 'VOXL2_IO_MIN'
+#      label: 'PWM min value'
+#    - param: 'VOXL2_IO_MAX'
+#      label: 'PWM max value'
   output_groups:
     - param_prefix: VOXL2_IO
       group_label: 'PWMs'
       channel_label: 'PWM Channel'
-      num_channels: 4
+      num_channels: 8
+
diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp
index 352b474906..90adfb8422 100644
--- a/src/drivers/voxl2_io/voxl2_io.cpp
+++ b/src/drivers/voxl2_io/voxl2_io.cpp
@@ -33,8 +33,6 @@
 
 #include "voxl2_io.hpp"
 
-#include <px4_platform_common/sem.hpp>
-
 
 Voxl2IO::Voxl2IO() :
 	OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)),
@@ -43,69 +41,81 @@ Voxl2IO::Voxl2IO() :
 	_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
 {
 	_mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS);
-	_uart_port = new Voxl2IoSerial();
-	voxl2_io_packet_init(&_sbus_packet);
+	voxl2_io_packet_init(&_voxl2_io_packet);
+
+	//set low rate scheduling interval to 200hz so that RC can be updated even if all actuators are disabled
+	//otherwise, the default low rate scheduling interval is 300ms and RC packets are delayed and lost
+	_mixing_output.setLowrateSchedulingInterval(5_ms);
 }
 
 Voxl2IO::~Voxl2IO()
 {
-	/* make sure servos are off */
-	stop_all_pwms();
-
-	if (_uart_port) {
-		_uart_port->uart_close();
-		_uart_port = nullptr;
-	}
+	_uart_port.close();
 
 	perf_free(_cycle_perf);
 	perf_free(_output_update_perf);
 }
 
-
 int Voxl2IO::init()
 {
+	PX4_INFO("VOXL2_IO: Driver starting");
+
 	int ret = PX4_OK;
 
-	/* Open serial port in this thread */
-	if (!_uart_port->is_open()) {
-		if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) {
-			/* Send PWM config to M0065... pwm_min and pwm_max */
-			PX4_INFO("Opened UART connection to M0065 device on port %s", _device);
-
-		} else {
-			PX4_ERR("Failed opening device");
-			return PX4_ERROR;
-		}
-	}
-
-	/* Verify connectivity and protocol version number */
-	if (get_version_info() < 0) {
-		PX4_ERR("Failed to detect voxl2_io protocol version.");
-		return PX4_ERROR;
-
-	} else {
-		if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION
-		    && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) {
-			PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version);
-
-		} else {
-			PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version);
-			return PX4_ERROR;
-		}
-	}
-
-	/* Getting initial parameter values */
+	// Getting initial parameter values
 	ret = update_params();
 
 	if (ret != OK) {
+		PX4_ERR("VOXL2_IO: Failed to update params during init");
 		return ret;
 	}
 
-	/* Send PWM MIN/MAX to M0065 */
-	update_pwm_config();
+	// Print initial param values
+	print_params();
 
-	ScheduleOnInterval(_current_update_interval);
-	// ScheduleNow();
+	PX4_INFO("VOXL2_IO: ");
+
+	// Open serial port
+	if (!_uart_port.isOpen()) {
+		PX4_INFO("VOXL2_IO: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate);
+
+		// Configure UART port
+		if (! _uart_port.setPort(_device)) {
+			PX4_ERR("Error configuring serial device on port %s", _device);
+			return -1;
+		}
+
+		if (! _uart_port.setBaudrate(_parameters.baud_rate)) {
+			PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device);
+			return -1;
+		}
+
+		// Open the UART. If this is successful then the UART is ready to use.
+		if (! _uart_port.open()) {
+			PX4_ERR("Error opening serial device  %s", _device);
+			return -1;
+		}
+	}
+
+	// Detect M0065 board
+	ret = get_version_info();
+
+	if (ret != 0) {
+		PX4_ERR("VOXL2_IO: Could not detect the board");
+		PX4_ERR("VOXL2_IO: Driver initialization failed. Exiting");
+
+		if (_uart_port.open()) {
+			PX4_INFO("VOXL2_IO: Closing uart port");
+			_uart_port.close();
+		}
+
+		return -1;
+	}
+
+	//ScheduleOnInterval(_current_update_interval);
+	ScheduleNow();
+
+	PX4_INFO("VOXL2_IO: Driver initialization succeeded");
 	return ret;
 }
 
@@ -117,11 +127,10 @@ int Voxl2IO::update_params()
 	ret = load_params(&_parameters);
 
 	if (ret == PX4_OK) {
-		_mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED);
+		_mixing_output.setAllDisarmedValues(_parameters.pwm_dis);
 		_mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE);
-		_mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN);
-		_mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX);
-		_pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min;
+		_mixing_output.setAllMinValues(_parameters.pwm_min);
+		_mixing_output.setAllMaxValues(_parameters.pwm_max);
 	}
 
 	return ret;
@@ -130,196 +139,159 @@ int Voxl2IO::update_params()
 int Voxl2IO::load_params(voxl2_io_params_t *params)
 {
 	int ret = PX4_OK;
-	int32_t max = params->pwm_max;
-	int32_t min = params->pwm_min;
 
 	// initialize out
 	for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) {
 		params->function_map[i] = (int)OutputFunction::Disabled;
 	}
 
-	/* UART config, PWM mode, and RC protocol*/
+	// UART config, PWM mode, and RC protocol
 	param_get(param_find("VOXL2_IO_BAUD"),    &params->baud_rate);
-	param_get(param_find("RC_INPUT_PROTO"),    &params->param_rc_input_proto);
+	//param_get(param_find("RC_INPUT_PROTO"),    &params->param_rc_input_proto);
 
-	/* PWM min, max, and failsafe values*/
+	// PWM min, max, and failsafe values
 	param_get(param_find("VOXL2_IO_MIN"),  &params->pwm_min);
 	param_get(param_find("VOXL2_IO_MAX"),  &params->pwm_max);
+	param_get(param_find("VOXL2_IO_DIS"),  &params->pwm_dis);
+	param_get(param_find("VOXL2_IO_CMIN"), &params->pwm_cal_min);
+	param_get(param_find("VOXL2_IO_CMAX"), &params->pwm_cal_max);
 
-	/* PWM output functions */
+	// PWM output functions
+	//0: disabled, 1: constant min, 2: constant max
+	//101-112: motors, 201-208: servos, 402: RC Roll, 403: RC Pitch, 404: RC Throttle,
+	//405: RC Yaw, 406: RC Flaps, 407-412: RC AUX 1-6, 420-422: Gimbal RPY
 	param_get(param_find("VOXL2_IO_FUNC1"),  &params->function_map[0]);
 	param_get(param_find("VOXL2_IO_FUNC2"),  &params->function_map[1]);
 	param_get(param_find("VOXL2_IO_FUNC3"),  &params->function_map[2]);
 	param_get(param_find("VOXL2_IO_FUNC4"),  &params->function_map[3]);
+	param_get(param_find("VOXL2_IO_FUNC5"),  &params->function_map[4]);
+	param_get(param_find("VOXL2_IO_FUNC6"),  &params->function_map[5]);
+	param_get(param_find("VOXL2_IO_FUNC7"),  &params->function_map[6]);
+	param_get(param_find("VOXL2_IO_FUNC8"),  &params->function_map[7]);
 
-	/* Validate PWM min and max values */
+	// Validate PWM min and max values
 	if (params->pwm_min > params->pwm_max) {
-		PX4_ERR("Invalid parameter VOXL2_IO_MIN.  Please verify parameters.");
+		PX4_ERR("VOXL2_IO: Invalid parameter VOXL2_IO_MIN.  Please verify parameters.");
 		params->pwm_min = 0;
 		ret = PX4_ERROR;
 	}
 
-	if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) {
-		PX4_INFO("Updating PWM params load_params");
-		update_pwm_config();
-	}
-
 	return ret;
 }
 
-void Voxl2IO::update_pwm_config()
-{
-	Command cmd;
-	uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast<uint8_t>((_parameters.pwm_min & 0xFF00) >> 8), static_cast<uint8_t>(_parameters.pwm_min & 0xFF),
-						    static_cast<uint8_t>((_parameters.pwm_max & 0xFF00) >> 8), static_cast<uint8_t>(_parameters.pwm_max & 0xFF)
-						   };
-	cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE,	cmd.buf,
-					 sizeof(cmd.buf));
-
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send config packet");
-
-	} else {
-		_bytes_sent += cmd.len;
-		_packets_sent++;
-	}
-}
-
 int Voxl2IO::get_version_info()
 {
-	int res = 0 ;
-	int header = -1 ;
-	int info_packet = -1;
-	int read_retries = 3;
-	int read_succeeded = 0;
+	PX4_INFO("VOXL2_IO: Detecting M0065 board...");
+	voxl2_io_packet_init(&_voxl2_io_packet);
+
 	Command cmd;
+	cmd.len = voxl2_io_create_extended_version_request_packet(0, cmd.buf, sizeof(cmd.buf));
 
-	/* Request protocol version info from M0065 */
-	cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE);
+	int retries_left  = _board_detect_retries;
+	bool got_response = false;
 
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send version info packet");
+	while ((got_response == false) && (retries_left > 0)) {
+		retries_left--;
+
+		//send the version request command to the board
+		if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
+			PX4_ERR("VOXL2_IO: Could not write version request packet to UART port");
+			return -1;
+		}
 
-	} else {
 		_bytes_sent += cmd.len;
 		_packets_sent++;
-	}
 
-	/* Read response */
-	px4_usleep(10000);
-	memset(&_read_buf, 0x00, READ_BUF_SIZE);
-	res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE);
+		hrt_abstime t_request = hrt_absolute_time();
+		hrt_abstime t_timeout = 50000; //50ms timeout for version info response
 
-	while (read_retries) {
-		if (res) {
-			/* Get index of packer header */
-			for (int index = 0; index < READ_BUF_SIZE; ++index) {
-				if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) {
-					info_packet = index;
-					break;
-				}
 
-				if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) {
-					header = index;
+		//wait for the response to come back
+		while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) {
+			px4_usleep(500); //sleep a bit while waiting for the board to respond
+
+			int nread = _uart_port.read(_read_buf, sizeof(_read_buf));
+
+			for (int i = 0; i < nread; i++) {
+				int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet);
+
+				if (parse_ret > 0) {
+					hrt_abstime response_time = hrt_elapsed_time(&t_request);
+					//PX4_INFO("got packet of length %i",ret);
+					_packets_received++;
+					uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet);
+					uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet);
+
+					if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(VOXL2_IO_EXTENDED_VERSION_INFO)) {
+						VOXL2_IO_EXTENDED_VERSION_INFO ver;
+						memcpy(&ver, _voxl2_io_packet.buffer, packet_size);
+
+						PX4_INFO("VOXL2_IO: \tVOXL2_IO ID: %i", ver.id);
+						PX4_INFO("VOXL2_IO: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str());
+
+						uint8_t *u = &ver.unique_id[0];
+						PX4_INFO("VOXL2_IO: \tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
+							 u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]);
+
+						PX4_INFO("VOXL2_IO: \tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
+						PX4_INFO("VOXL2_IO: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
+						PX4_INFO("VOXL2_IO: \tReply time : %uus", (uint32_t)response_time);
+
+						//we requested response from ID 0, so it should match
+						if (ver.id != 0) {
+							PX4_ERR("VOXL2_IO: Invalid id: %d", ver.id);
+						}
+
+						//check HW (board version)
+						else if (ver.hw_version != VOXL2_IO_HW_VERSION) {
+							PX4_ERR("VOXL2_IO: Invalid HW version : %d (expected %d)", ver.hw_version, VOXL2_IO_HW_VERSION);
+							return -1;
+						}
+
+						//check firmware version running on the board
+						else if (ver.sw_version != VOXL2_IO_SW_VERSION) {
+							PX4_ERR("VOXL2_IO: Invalid FW version : %d (expected %d)", ver.sw_version, VOXL2_IO_SW_VERSION);
+							return -1;
+
+						} else {
+							got_response = true;
+							memcpy(&_version_info, &ver, sizeof(_version_info)); //store the version info only if it is valid
+						}
+					}
 				}
 			}
 
-			/* Try again in a bit if packet header not present yet... */
-			if (header == -1 || info_packet == -1) {
-				if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); }
-
-				if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); }
-
-				read_retries--;
-				flush_uart_rx();
-
-				if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-					PX4_ERR("Failed to send version info packet");
-
-				} else {
-					_bytes_sent += cmd.len;
-					_packets_sent++;
-					px4_usleep(2000);
-				}
-
-				continue;
-			}
-
-			/* Check if we got a valid packet...*/
-			if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) {
-				if (_debug) {
-					PX4_ERR("Error parsing version info packet");
-					PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n",
-						     _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4],
-						     _read_buf[header + 5],
-						     _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10],
-						     _read_buf[header + 11],
-						     _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16],
-						     _read_buf[header + 17],
-						     _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22],
-						     _read_buf[header + 23],
-						     _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28],
-						     _read_buf[header + 29]
-						    );
-				}
-
-				read_retries--;
-				flush_uart_rx();
-
-				if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-					PX4_ERR("Failed to send version info packet");
-
-				} else {
-					_bytes_sent += cmd.len;
-					_packets_sent++;
-					px4_usleep(2000);
-				}
-
-				break;
-
-			}  else {
-				memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO));
-				read_succeeded = 1;
+			//break out of the loop waiting for a response
+			if (got_response) {
 				break;
 			}
+		}
 
-		} else {
-			read_retries--;
 
-			if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-				PX4_ERR("Failed to send version info packet");
-
-			} else {
-				_bytes_sent += cmd.len;
-				_packets_sent++;
-				px4_usleep(2000);
-			}
+		if (!got_response) {
+			PX4_ERR("VOXL2_IO: Board version info response timeout (%d retries left)", retries_left);
 		}
 	}
 
-	if (! read_succeeded) {
-		return -EIO;
-	}
-
-	return 0;
+	return (got_response == true ? 0 : -1);
 }
 
 bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS],
 			    unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	/* Stop Mixer while ESCs are being calibrated */
+	// Stop Mixer while ESCs are being calibrated
 	if (_outputs_disabled) {
 		return 0;
 	}
 
+	//PX4_INFO("VOXL2_IO: Mixer output: %u, %u, %u, %u", outputs[0], outputs[1], outputs[2], outputs[3]);
+
 	//in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function)
 	//So, if Run() is blocked by a custom command, this function will not be called until Run is running again
-	int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
-	uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
-	int32_t _fb_idx = -1;
+	uint32_t output_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0};
 
 	if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) {
-		PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!");
+		PX4_ERR("VOXL2_IO: Num outputs != VOXL2_IO_OUTPUT_CHANNELS!");
 		return false;
 	}
 
@@ -333,43 +305,20 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN
 			_pwm_on = true;
 		}
 
-		if (!_pwm_on || stop_motors) {
-			_rate_req[i] = 0;
+		//Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right?
+		if (0) { //(!_pwm_on || stop_motors) {
+			output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns
 
 		} else {
-			_rate_req[i] = outputs[i];
+			output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE;  //convert to ns
 		}
-
-		_pwm_values[i] = _rate_req[i];
 	}
 
 	Command cmd;
-	cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3],
-			_led_req[0], _led_req[1], _led_req[2], _led_req[3],
-			_fb_idx, cmd.buf, sizeof(cmd.buf));
+	cmd.len = voxl2_io_create_hires_pwm_packet(output_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf));
 
-	if (_pwm_on && _debug) {
-		PX4_INFO("Mixer outputs");
-		PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]",
-			 outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5],
-			 outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11],
-			 outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17]
-			);
-
-		// Debug messages for PWM 400Hz values sent to M0065
-		uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS;
-		PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1);
-		uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS;
-		PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2);
-		uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS;
-		PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3);
-		uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS;
-		PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4);
-		PX4_INFO("");
-	}
-
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send packet");
+	if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
+		PX4_ERR("VOXL2_IO: Failed to send packet");
 		return false;
 
 	} else {
@@ -377,40 +326,39 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN
 		_packets_sent++;
 	}
 
+	//if (_pwm_on && _debug){
+	if (_debug) {
+		PX4_INFO("VOXL2_IO: Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]",
+			 outputs[0], outputs[1], outputs[2], outputs[3],
+			 outputs[4], outputs[5], outputs[6], outputs[7]);
+	}
+
 	perf_count(_output_update_perf);
 
 	return true;
 }
 
-int Voxl2IO::flush_uart_rx()
-{
-	while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {}
-
-	return 0;
-}
-
 static bool valid_port(int port)
 {
-	if (port == 2 || port == 6 || port == 7) { return true; }
+	if (port == 2 || port == 6 || port == 7) {
+		return true;
+	}
 
 	return false;
 }
 
+/*
 int Voxl2IO::parse_response(uint8_t *buf, uint8_t len)
 {
 	for (int i = 0; i < len; i++) {
-		int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet);
+		int16_t ret = voxl2_io_packet_process_char(buf[i], &_voxl2_io_packet);
 
 		if (ret > 0) {
-			uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet);
-			uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet);
+			uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet);
+			uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet);
 
 			if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) {
 				return 0;
-
-			} else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) {
-				return 0;
-
 			} else {
 				return -1;
 			}
@@ -418,36 +366,31 @@ int Voxl2IO::parse_response(uint8_t *buf, uint8_t len)
 		} else { //parser error
 			switch (ret) {
 			case VOXL2_IO_ERROR_BAD_CHECKSUM:
-				if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); }
-
+				if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet checksum");
 				break;
 
 			case VOXL2_IO_ERROR_BAD_LENGTH:
-				if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); }
-
+				if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet length");
 				break;
 
 			case VOXL2_IO_ERROR_BAD_HEADER:
-				if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); }
-
+				if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet header");
 				break;
 
 			case VOXL2_IO_NO_PACKET:
-				// if(_pwm_on) PX4_WARN("NO packet");
+				// if(_pwm_on) PX4_WARN("VOXL2_IO: NO packet");
 				break;
 
 			default:
-				if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); }
-
+				if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: Unknown error: %i", ret);
 				break;
 			}
-
-			return ret;
 		}
 	}
 
 	return 0;
 }
+*/
 
 void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local,
 			 uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
@@ -455,6 +398,8 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local,
 			 unsigned frame_drops, int rssi, input_rc_s &input_rc)
 {
 	// fill rc_in struct for publishing
+	memset(&input_rc, 0, sizeof(input_rc));  //zero out the struct first
+
 	input_rc.channel_count = raw_rc_count_local;
 
 	if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
@@ -474,9 +419,9 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local,
 		_raw_rc_values[i] = UINT16_MAX;
 	}
 
-	input_rc.timestamp = now;
+	input_rc.timestamp             = now;
 	input_rc.timestamp_last_signal = input_rc.timestamp;
-	input_rc.rc_ppm_frame_length = 0;
+	input_rc.rc_ppm_frame_length   = 0;
 
 	/* fake rssi if no value was provided */
 	if (rssi == -1) {
@@ -494,131 +439,98 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local,
 		_sbus_frame_drops++;
 	}
 
-	input_rc.rc_failsafe = failsafe;
-	input_rc.rc_lost = input_rc.rc_failsafe;
-	input_rc.rc_lost_frame_count = _sbus_frame_drops;
+	input_rc.rssi_dbm             = 0.0f;
+	input_rc.rc_failsafe          = failsafe;
+	input_rc.rc_lost              = input_rc.rc_failsafe;
+	input_rc.rc_lost_frame_count  = _sbus_frame_drops;
 	input_rc.rc_total_frame_count = ++_sbus_total_frames;
 }
 
-int Voxl2IO::receive_sbus()
+
+int Voxl2IO::parse_sbus_packet(uint8_t *raw_data, uint32_t data_len)
 {
-	int res = 0;
-	int header = -1;
-	int read_retries = 3;
-	int read_succeeded = 0;
-	voxl2_io_packet_init(&_sbus_packet);
+	input_rc_s input_rc;
+	uint16_t num_values;
+	bool sbus_failsafe    = false;
+	bool sbus_frame_drop  = false;
+	uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]);
+	hrt_abstime t_now     = hrt_absolute_time();
 
-	while (read_retries) {
-		memset(&_read_buf, 0x00, READ_BUF_SIZE);
-		res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE);
 
-		if (res) {
-			/* Get index of packer header */
-			for (int index = 0; index < READ_BUF_SIZE; ++index) {
-				if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) {
-					header = index;
-					break;
-				}
-			}
+	bool rc_updated = sbus_parse(t_now, raw_data, data_len, _raw_rc_values, &num_values,
+				     &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels);
 
-			/* Try again in a bit if packet header not present yet... */
-			if (header == -1) {
-				if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); }
+	if (rc_updated) {
+		//if (_pwm_on && _debug){
+		if (_debug) {
+			//PX4_INFO("VOXL2_IO: Decoded packet, header pos: %i", header);
+			PX4_INFO("VOXL2_IO: [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]",
+				 _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2],
+				 _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5],
+				 _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8],
+				 _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11],
+				 _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14],
+				 _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17]
+				);
 
-				read_retries--;
-				continue;
-			}
-
-			/* Check if we got a valid packet...*/
-			if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) {
-				if (_pwm_on && _debug) {
-					PX4_ERR("Error parsing QC RAW SBUS packet");
-					PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n",
-						     _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4],
-						     _read_buf[header + 5],
-						     _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10],
-						     _read_buf[header + 11],
-						     _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16],
-						     _read_buf[header + 17],
-						     _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22],
-						     _read_buf[header + 23],
-						     _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28],
-						     _read_buf[header + 29]
-						    );
-				}
-
-				read_retries--;
-				break;
-			}
-
-			input_rc_s input_rc;
-			uint16_t num_values;
-			bool sbus_failsafe = false;
-			bool sbus_frame_drop = false;
-			uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]);
-			hrt_abstime now = hrt_absolute_time();
-			bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values,
-						     &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels);
-
-			if (rc_updated) {
-				if (_pwm_on && _debug) {
-					PX4_INFO("Decoded packet, header pos: %i", header);
-					PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]",
-						 _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2],
-						 _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5],
-						 _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8],
-						 _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11],
-						 _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14],
-						 _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17]
-						);
-				}
-
-				input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
-				fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc);
-
-				if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
-					_rc_last_valid = input_rc.timestamp;
-				}
-
-				input_rc.timestamp_last_signal = _rc_last_valid;
-				_rc_pub.publish(input_rc);
-
-				_bytes_received += res;
-				_packets_received++;
-				read_succeeded = 1;
-				break;
-
-			} else if (_pwm_on && _debug) {
-				PX4_ERR("Failed to decode SBUS packet, header pos: %i", header);
-
-				if (sbus_frame_drop) {
-					PX4_WARN("SBUS frame dropped");
-				}
-
-				PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]",
-					_read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4],
-					_read_buf[header + 5],
-					_read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10],
-					_read_buf[header + 11],
-					_read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16],
-					_read_buf[header + 17],
-					_read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22],
-					_read_buf[header + 23],
-					_read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28],
-					_read_buf[header + 29]
-				       );
+			if (sbus_frame_drop) {
+				PX4_WARN("VOXL2_IO: SBUS frame dropped");
 			}
 		}
 
-		read_retries--;
+		input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
+		fill_rc_in(num_values, _raw_rc_values, t_now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc);
+
+		if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
+			_rc_last_valid_time = input_rc.timestamp;
+		}
+
+		input_rc.timestamp_last_signal = _rc_last_valid_time;
+		_rc_pub.publish(input_rc);
+
+		if (_rc_mode == RC_MODE::SCAN) {
+			PX4_INFO("VOXL2_IO: Detected VOXL2 IO SBUS RC");
+			_rc_mode = RC_MODE::SBUS;
+		}
 	}
 
-	if (! read_succeeded) {
-		_new_packet = false;
-		return -EIO;
+	return 0;
+}
+
+
+int Voxl2IO::receive_uart_packets()
+{
+	int nread = _uart_port.read(_read_buf, READ_BUF_SIZE);
+
+	if (nread > 0) {
+		if (_debug) {
+			PX4_INFO("VOXL2_IO: receive_uart_packets read %d bytes", nread);
+		}
+
+		_bytes_received += nread;
+
+		for (int i = 0; i < nread; i++) {
+			int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet);
+
+			if (parse_ret > 0) {
+				_packets_received++;
+
+				uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet);
+				uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet);
+
+				if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) {
+
+					//parse SBUS packet only if configured to do so
+					if ((_rc_mode == RC_MODE::SCAN) || (_rc_mode == RC_MODE::SBUS)) {
+						parse_sbus_packet(&_voxl2_io_packet.buffer[SBUS_PAYLOAD], SBUS_FRAME_SIZE);
+					}
+				}
+
+				//parse other packets (future use)
+			}
+		}
 	}
 
-	_new_packet = true;
 	return 0;
 }
 
@@ -635,26 +547,14 @@ void Voxl2IO::Run()
 
 	perf_begin(_cycle_perf);
 
-	/* Handle RC */
-	if (_rc_mode == RC_MODE::SCAN) {
-		if (receive_sbus() == PX4_OK) {
-			PX4_INFO("Found M0065 SBUS RC.");
-			_rc_mode = RC_MODE::SBUS;
-		}	// Add more cases here for other protocols in the future..
-
-	} else if (_rc_mode == RC_MODE::SBUS) {
-		receive_sbus();
-	}
-
-	/* Only update outputs if we have new values from RC */
-	if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) {
-		_mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module
-		_new_packet = false;
-	}
+	_mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module
 
 	/* update PWM status if armed or if disarmed PWM values are set */
 	_pwm_on = _mixing_output.armed().armed;
 
+	//receive packets from voxl_io board
+	receive_uart_packets();
+
 	/* check for parameter updates */
 	if (!_pwm_on && _parameter_update_sub.updated()) {
 		/* clear update */
@@ -665,48 +565,6 @@ void Voxl2IO::Run()
 		update_params();
 	}
 
-	/* Don't process commands if pwm on */
-	if (!_pwm_on) {
-		if (_current_cmd.valid()) {
-			PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us);
-			flush_uart_rx();
-
-			do {
-				PX4_INFO("CMDs left %d", _current_cmd.repeats);
-
-				if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) {
-					if (_current_cmd.repeats == 0) {
-						_current_cmd.clear();
-					}
-
-
-				} else {
-					_bytes_sent += _current_cmd.len;
-					_packets_sent++;
-
-					if (_current_cmd.retries == 0) {
-						_current_cmd.clear();
-						PX4_ERR("Failed to send command, errno: %i", errno);
-
-					} else {
-						_current_cmd.retries--;
-						PX4_ERR("Failed to send command, errno: %i", errno);
-					}
-				}
-
-				px4_usleep(_current_cmd.repeat_delay_us);
-			} while (_current_cmd.repeats-- > 0);
-
-		} else {
-			Command *new_cmd = _pending_cmd.load();
-
-			if (new_cmd) {
-				_current_cmd = *new_cmd;
-				_pending_cmd.store(nullptr);
-			}
-		}
-	}
-
 	/* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */
 	_mixing_output.updateSubscriptions(true);
 	perf_end(_cycle_perf);
@@ -728,17 +586,17 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
 		while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) {
 			switch (ch) {
 			case 'v':
-				PX4_INFO("Verbose mode enabled");
+				PX4_INFO("VOXL2_IO: Verbose mode enabled");
 				get_instance()->_debug = true;
 				break;
 
 			case 'd':
-				PX4_INFO("M0065 PWM outputs disabled");
+				PX4_INFO("VOXL2_IO: M0065 PWM outputs disabled");
 				get_instance()->_outputs_disabled = true;
 				break;
 
 			case 'e':
-				PX4_INFO("M0065 using external RC");
+				PX4_INFO("VOXL2_IO: M0065 using external RC");
 				get_instance()->_rc_mode = RC_MODE::EXTERNAL;
 				break;
 
@@ -747,7 +605,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
 					snprintf(get_instance()->_device, 2, "%s", myoptarg);
 
 				} else {
-					PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg);
+					PX4_ERR("VOXL2_IO: Bad UART port number: %s (must be 2, 6, or 7).", myoptarg);
 					_object.store(nullptr);
 					_task_id = -1;
 					return PX4_ERROR;
@@ -756,7 +614,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
 				break;
 
 			default:
-				print_usage("Unknown command, parsing flags");
+				print_usage("VOXL2_IO: Unknown command, parsing flags");
 				break;
 			}
 		}
@@ -766,7 +624,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
 		}
 
 	} else {
-		PX4_ERR("alloc failed");
+		PX4_ERR("VOXL2_IO: alloc failed");
 	}
 
 	_object.store(nullptr);
@@ -776,280 +634,131 @@ int Voxl2IO::task_spawn(int argc, char *argv[])
 }
 
 
-bool Voxl2IO::stop_all_pwms()
-{
-	int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
-	int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
-	uint8_t _fb_idx = 0;
-
-	Command cmd;
-	cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3],
-			_led_req[0], _led_req[1], _led_req[2], _led_req[3],
-			_fb_idx, cmd.buf, sizeof(cmd.buf));
-
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send packet");
-		return false;
-
-	} else {
-		_bytes_sent += cmd.len;
-		_packets_sent++;
-	}
-
-	return true;
-}
-
-int Voxl2IO::send_cmd_thread_safe(Command *cmd)
-{
-	cmd->id = _cmd_id++;
-	_pending_cmd.store(cmd);
-
-	/* wait until main thread processed it */
-	while (_pending_cmd.load()) {
-		px4_usleep(1000);
-	}
-
-	return 0;
-}
-
 int Voxl2IO::calibrate_escs()
 {
 
-	/* Disable outputs so Mixer isn't being a PITA while we calibrate */
+	// Disable outputs so Mixer isn't being a PITA while we calibrate
 	_outputs_disabled = true;
 
 	Command cmd;
-	int32_t fb_idx = -1;
-	uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0};
-	cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf,
-					 sizeof(cmd.buf));
 
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet");
-		_outputs_disabled = false;
-		return -1;
+	PX4_INFO("VOXL2_IO: PWM ESC calibration is starting!");
+
+	// Give user 10 seconds to plug in PWM cable for ESCs
+	PX4_INFO("VOXL2_IO: MIN PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_min);
+	PX4_INFO("VOXL2_IO: MAX PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_max);
+	PX4_INFO("VOXL2_IO:");
+	PX4_INFO("VOXL2_IO: Connect your ESCs! (Calibration will start in ~10 seconds)");
+
+	px4_usleep(10000000);
+
+	uint32_t max_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0};
+	uint32_t min_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0};
+
+	for (int idx = 0; idx < VOXL2_IO_OUTPUT_CHANNELS; idx++) {
+		//only send out calibration pulse if the actuator is a motor
+		if ((_parameters.function_map[idx] >= 101) && (_parameters.function_map[idx] <= 112)) {
+			max_pwm_cmds[idx] = _parameters.pwm_cal_max * MIXER_OUTPUT_TO_CMD_SCALE;
+			min_pwm_cmds[idx] = _parameters.pwm_cal_min * MIXER_OUTPUT_TO_CMD_SCALE;
+		}
 	}
 
-	/* Give user 10 seconds to plug in PWM cable for ESCs */
-	PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)");
-	hrt_abstime start_cal = hrt_absolute_time();
-
-	while (hrt_elapsed_time(&start_cal) < 10000000) {
-		continue;
+	if (_debug) {
+		PX4_INFO("VOXL2_IO: Scaled max pwms: %u %u %u %u %u %u %u %u",
+			 max_pwm_cmds[0], max_pwm_cmds[1], max_pwm_cmds[2], max_pwm_cmds[3],
+			 max_pwm_cmds[4], max_pwm_cmds[5], max_pwm_cmds[6], max_pwm_cmds[7]);
 	}
 
-	/* PWM MAX 3 seconds */
-	PX4_INFO("Writing PWM MAX for 3 seconds!");
-	int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX};
+	hrt_abstime start;
 
-	if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); }
+	// Send PWM max every 2.5ms for 5 seconds
+	PX4_INFO("VOXL2_IO: Sending PWM MAX (%dus) for 5 seconds!", _parameters.pwm_cal_max);
+	cmd.len = voxl2_io_create_hires_pwm_packet(max_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf));
+	start = hrt_absolute_time();
 
-	int16_t led_cmd[4] {0, 0, 0, 0};
-	cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3],
-			led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3],
-			fb_idx, cmd.buf, sizeof(cmd.buf));
+	while (hrt_elapsed_time(&start) < 5000000) {
+		if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
+			PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MAX packet");
+			_outputs_disabled = false;
+			return -1;
+		}
 
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet");
-		_outputs_disabled = false;
-		return -1;
-
-	} else {
-		cmd.clear();
+		px4_usleep(2500);
 	}
 
-	hrt_abstime start_pwm_max = hrt_absolute_time();
+	// Send PWM min every 2.5ms for 5 seconds
+	PX4_INFO("VOXL2_IO: Sending PWM MIN (%dus) for 5 seconds!", _parameters.pwm_cal_min);
 
-	while (hrt_elapsed_time(&start_pwm_max) < 3000000) {
-		continue;
+	if (_debug) {
+		PX4_INFO("VOXL2_IO: Scaled min pwms: %u %u %u %u %u %u %u %u",
+			 min_pwm_cmds[0], min_pwm_cmds[1], min_pwm_cmds[2], min_pwm_cmds[3],
+			 min_pwm_cmds[4], min_pwm_cmds[5], min_pwm_cmds[6], min_pwm_cmds[7]);
 	}
 
-	/* PWM MIN 4 seconds */
-	PX4_INFO("Writing PWM MIN for 4 seconds!");
-	int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN};
+	cmd.len = voxl2_io_create_hires_pwm_packet(min_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf));
 
-	if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); }
 
-	cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3],
-			led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3],
-			fb_idx, cmd.buf, sizeof(cmd.buf));
+	start = hrt_absolute_time();
 
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet");
-		_outputs_disabled = false;
-		return -1;
+	while (hrt_elapsed_time(&start) < 5000000) {
+		if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
+			PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MIN packet");
+			_outputs_disabled = false;
+			return -1;
+		}
+
+		px4_usleep(2500);
 	}
 
-	hrt_abstime start_pwm_min = hrt_absolute_time();
-
-	while (hrt_elapsed_time(&start_pwm_min) < 4000000) {
-		continue;
-	}
-
-	PX4_INFO("ESC Calibration complete");
+	PX4_INFO("VOXL2_IO: Waiting 5 seconds to finish the calibration (no PWM output)");
+	px4_usleep(5000000);
 
+	PX4_INFO("VOXL2_IO: ESC Calibration complete");
 	_outputs_disabled = false;
 	return 0;
 }
 
 int Voxl2IO::custom_command(int argc, char *argv[])
 {
-	int myoptind = 0;
-	int ch;
-	const char *myoptarg = nullptr;
-
-	Command  cmd;
-	uint8_t  output_channel   = 0xF;
-	int16_t  rate     = 0;
-
-	uint32_t repeat_count    = 100;
-	uint32_t repeat_delay_us = 10000;
-
 	const char *verb = argv[argc - 1];
 
-	if ((strcmp(verb, "pwm")) == 0 && argc < 3) {
-		return print_usage("pwm command: missing args");
-
-	} else if (argc < 1) {
+	if (argc < 1) {
 		return print_usage("unknown command: missing args");
 	}
 
-	PX4_INFO("Executing the following command: %s", verb);
+	PX4_INFO("VOXL2_IO: Executing the following command: %s", verb);
 
-	/* start the FMU if not running */
+	/* start if not running */
 	if (!strcmp(verb, "start")) {
 		if (!is_running()) {
 			return Voxl2IO::task_spawn(argc, argv);
 		}
-	}
 
-	if (!strcmp(verb, "status")) {
-		if (!is_running()) {
-			PX4_INFO("Not running");
-			return -1;
-		}
-
-		return get_instance()->print_status();
+		PX4_INFO("VOXL2_IO: Already running");
+		return 0;
 	}
 
 	if (!is_running()) {
-		PX4_INFO("Not running");
+		PX4_INFO("VOXL2_IO: Not running");
 		return -1;
 	}
 
+	if (!strcmp(verb, "status")) {
+		return get_instance()->print_status();
+	}
+
+
 	if (!strcmp(verb, "calibrate_escs")) {
 		if (get_instance()->_outputs_disabled) {
-			PX4_WARN("Can't calibrate ESCs while outputs are disabled.");
+			PX4_WARN("VOXL2_IO: Can't calibrate ESCs while outputs are disabled.");
 			return -1;
 		}
 
 		return get_instance()->calibrate_escs();
 	}
 
-	while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'c':
-			output_channel = atoi(myoptarg);
-
-			if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) {
-				char reason[50];
-				sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1);
-				print_usage(reason);
-				return 0;
-			}
-
-			break;
-
-		case 'n':
-			repeat_count = atoi(myoptarg);
-
-			if (repeat_count < 1) {
-				print_usage("bad repeat_count");
-				return 0;
-			}
-
-			break;
-
-		case 't':
-			repeat_delay_us = atoi(myoptarg);
-
-			if (repeat_delay_us < 1) {
-				print_usage("bad repeat delay");
-				return 0;
-			}
-
-			break;
-
-		case 'r':
-			rate = atoi(myoptarg);
-			break;
-
-		case 'p':
-			if (valid_port(atoi(myoptarg))) {
-				snprintf(get_instance()->_device, 2, "%s", myoptarg);
-
-			} else {
-				PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg);
-				return 0;
-			}
-
-			break;
-
-		default:
-			print_usage("Unknown command, parsing flags");
-			return 0;
-		}
-	}
-
-	if (!strcmp(verb, "pwm")) {
-		PX4_INFO("Output channel: %i", output_channel);
-		PX4_INFO("Repeat count: %i", repeat_count);
-		PX4_INFO("Repeat delay (us): %i", repeat_delay_us);
-		PX4_INFO("Rate: %i", rate);
-
-		if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) {
-			PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate);
-			int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
-			uint8_t id_fb = 0;
-
-			if (output_channel ==
-			    0xFF) {  //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'.
-				rate_req[0] = rate;
-				rate_req[1] = rate;
-				rate_req[2] = rate;
-				rate_req[3] = rate;
-
-			} else {
-				rate_req[output_channel] = rate;
-				id_fb = output_channel;
-			}
-
-			cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3],
-					0, 0, 0, 0,
-					id_fb, cmd.buf, sizeof(cmd.buf));
-
-			cmd.response        = false;
-			cmd.repeats         = repeat_count;
-			cmd.resp_delay_us   = 1000;
-			cmd.repeat_delay_us = repeat_delay_us;
-			cmd.print_feedback  = false;
-
-			PX4_INFO("feedback id debug: %i", id_fb);
-			PX4_INFO("Sending UART M0065 power command %i", rate);
-
-			if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-				PX4_ERR("Failed to send packet: stop PWMs");
-				return -1;
-
-			} else {
-				get_instance()->_bytes_sent += cmd.len;
-				get_instance()->_packets_sent++;
-			}
-
-		} else {
-			print_usage("Invalid Output Channel, use 0-3");
-			return 0;
-		}
+	if (!strcmp(verb, "enable_debug")) {
+		get_instance()->_debug = true;
 	}
 
 	return print_usage("unknown custom command");
@@ -1057,22 +766,20 @@ int Voxl2IO::custom_command(int argc, char *argv[])
 
 int Voxl2IO::print_status()
 {
-	PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval);
-	PX4_INFO("PWM Rate: 400 Hz");	// Only support 400 Hz for now
-	PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no");
-	PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version);
-	PX4_INFO("RC Type: SBUS");		// Only support SBUS through M0065 for now
-	PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes");
-	PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames);
-	PX4_INFO("UART port: %s", _device);
-	PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no");
-	PX4_INFO("Packets sent: %" PRIu32, _packets_sent);
-	PX4_INFO("");
-	PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate);
-	PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]);
-	PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]);
-	PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]);
-	PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]);
+	//PX4_INFO("VOXL2_IO: Max update rate: %u Hz", 1000000/_current_update_interval);
+	//PX4_INFO("VOXL2_IO: PWM Rate: 400 Hz");	// Only support 400 Hz for now
+	PX4_INFO("VOXL2_IO: Outputs on     : %s", _pwm_on ? "yes" : "no");
+	PX4_INFO("VOXL2_IO: SW version     : %u", _version_info.sw_version);
+	PX4_INFO("VOXL2_IO: HW version     : %u: %s", _version_info.hw_version,
+		 board_id_to_name(_version_info.hw_version).c_str());
+	PX4_INFO("VOXL2_IO: RC Type        : SBUS");		// Only support SBUS through M0065 for now
+	PX4_INFO("VOXL2_IO: RC Connected   : %s",  hrt_absolute_time() - _rc_last_valid_time > 500000 ? "no" : "yes");
+	PX4_INFO("VOXL2_IO: RC Packets Rxd : %"    PRIu16, _sbus_total_frames);
+	PX4_INFO("VOXL2_IO: UART port      : %s", _device);
+	PX4_INFO("VOXL2_IO: UART open      : %s", _uart_port.open() ? "yes" : "no");
+	PX4_INFO("VOXL2_IO: Packets sent   : %"    PRIu32, _packets_sent);
+	PX4_INFO("VOXL2_IO: ");
+	print_params();
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_output_update_perf);
@@ -1103,17 +810,48 @@ px4io driver is used for main ones.
 	PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false);
 	PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request");
-	PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false);
-	PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false);
-	PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false);
-	PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
 }
 
+std::string Voxl2IO::board_id_to_name(int board_id)
+{
+	switch(board_id){
+		case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)");
+		case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)");
+		case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)");
+		case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)");
+		case 35: return std::string("ModalAi I/O Expander (M0065)");
+		case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)");
+		case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)");
+		case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)");
+		case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)");
+		case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)");
+		case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)");
+		case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)");
+		default: return std::string("Unknown Board");
+	}
+}
+
+void Voxl2IO::print_params()
+{
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_BAUD  : %" PRId32, _parameters.baud_rate);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC1 : %" PRId32, _parameters.function_map[0]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC2 : %" PRId32, _parameters.function_map[1]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC3 : %" PRId32, _parameters.function_map[2]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC4 : %" PRId32, _parameters.function_map[3]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC5 : %" PRId32, _parameters.function_map[4]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC6 : %" PRId32, _parameters.function_map[5]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC7 : %" PRId32, _parameters.function_map[6]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC8 : %" PRId32, _parameters.function_map[7]);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_DIS   : %" PRId32, _parameters.pwm_dis);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MIN   : %" PRId32, _parameters.pwm_min);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MAX   : %" PRId32, _parameters.pwm_max);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMIN  : %" PRId32, _parameters.pwm_cal_min);
+	PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMAX  : %" PRId32, _parameters.pwm_cal_max);
+}
+
 extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]);
 
 int voxl2_io_main(int argc, char *argv[])
diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp
index 6ac46251bc..b3458456a0 100644
--- a/src/drivers/voxl2_io/voxl2_io.hpp
+++ b/src/drivers/voxl2_io/voxl2_io.hpp
@@ -35,6 +35,7 @@
 
 #include <float.h>
 #include <math.h>
+#include <string>
 
 #include <board_config.h>
 
@@ -49,16 +50,17 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module.h>
+#include <px4_platform_common/Serial.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/input_rc.h>
 
-#include "voxl2_io_serial.hpp"
-
 #include "voxl2_io_packet.h"
 #include "voxl2_io_packet_types.h"
 
+using namespace device;
+
 using namespace time_literals;
 
 class Voxl2IO final : public ModuleBase<Voxl2IO>, public OutputModuleInterface
@@ -85,8 +87,8 @@ public:
 
 	virtual int	init();
 
-	void update_pwm_config();
 	int get_version_info();
+	void print_params();
 
 	struct Command {
 		uint16_t	id                 = 0;
@@ -105,8 +107,8 @@ public:
 		void clear() { len = 0; }
 	};
 
-	int send_cmd_thread_safe(Command *cmd);
-	int receive_sbus();
+	int receive_uart_packets();
+	int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len);
 
 	void fill_rc_in(uint16_t raw_rc_count_local,
 			uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
@@ -114,53 +116,42 @@ public:
 			unsigned frame_drops, int rssi, input_rc_s &input_rc);
 private:
 	void Run() override;
-	bool stop_all_pwms();
 
 	/* PWM Parameters */
-	static constexpr uint32_t VOXL2_IO_CONFIG = 0;				// Default to off
-	static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4;	// PWM_MIN, PWM_MAX, 4 bytes
-	static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1;
-	static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600;
-	static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4;
-	static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0;
-
-	static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200;
-	static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000;
-
-	static constexpr uint16_t DISARMED_VALUE = 0;
-
-	static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0;
-	static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800;
-	static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0;
-	static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0;
-
-	static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000;
-	static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000;
-	static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900;
-	static constexpr int32_t VOXL2_IO_TICS = 24;	// 24 tics per us on M0065 timer clks
+	static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD    = 921600;
+	static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8;
+	static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE  =
+		UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function
 
 	/* SBUS */
 	static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30;
 	static constexpr uint16_t SBUS_PAYLOAD = 3;
 
+	//packet packing function accepts pulse width in nanoseconds
+	//assuming the mixer outputs in microseconds, need to convert
+	static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000;  //standard pwm
+	//static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125;   //oneshot125
+
 	/* M0065 version info */
-	static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6;
-	static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1;
-	static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35;
-	VOXL2_IO_VERSION_INFO _version_info;
+	static constexpr uint16_t VOXL2_IO_SW_VERSION = 2;
+	static constexpr uint16_t VOXL2_IO_HW_VERSION = 35;  //this is the version of the HW
+	int  _board_detect_retries{3};
+	VOXL2_IO_EXTENDED_VERSION_INFO _version_info;
 
 	/* Module update interval */
 	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
 
 	typedef struct {
-		int32_t		config{VOXL2_IO_CONFIG};
 		int32_t		baud_rate{VOXL2_IO_DEFAULT_BAUD};
-		int32_t		pwm_min{VOXL2_IO_DEFAULT_MIN};
-		int32_t		pwm_max{VOXL2_IO_DEFAULT_MAX};
-		int32_t		pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE};
-		int32_t 	param_rc_input_proto{0};
+		int32_t		pwm_min{0};
+		int32_t		pwm_max{0};
+		int32_t     pwm_dis{0};
+		int32_t     pwm_cal_min{0};
+		int32_t     pwm_cal_max{0};
+		//int32_t		pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE};
+		//int32_t 	param_rc_input_proto{0};
 		int32_t		param_rc_rssi_pwm_chan{0};
-		int32_t		function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
+		int32_t		function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0};
 		int32_t		verbose_logging{0};
 	} voxl2_io_params_t;
 	voxl2_io_params_t	_parameters;
@@ -179,20 +170,18 @@ private:
 		SCAN
 	} _rc_mode{RC_MODE::SCAN};
 
-	/* QUP7, VOXL2 J19, /dev/slpi-uart-7*/
 	char 				_device[10] {VOXL2_IO_DEFAULT_PORT};
-	Voxl2IoSerial 		*_uart_port;
+	Serial				_uart_port{};
 
 	/* Mixer output */
 	MixingOutput 	_mixing_output;
 
 	/* RC input */
-	VOXL2_IOPacket _sbus_packet;
-	uint64_t _rc_last_valid;
+	VOXL2_IOPacket _voxl2_io_packet;
+	uint64_t _rc_last_valid_time;
 	uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX};
-	unsigned _sbus_frame_drops{0};
-	uint16_t _sbus_total_frames{0};
-	bool	 _new_packet{false};
+	unsigned int _sbus_frame_drops{0};
+	uint32_t _sbus_total_frames{0};
 
 	/* Publications */
 	uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
@@ -201,17 +190,12 @@ private:
 	uORB::Subscription 	_parameter_update_sub{ORB_ID(parameter_update)};
 
 	bool		_pwm_on{false};
-	int32_t		_pwm_fullscale{0};
-	int16_t 	_pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 	bool		_outputs_disabled{false};
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;
 
 	bool 			_debug{false};
-	uint16_t		_cmd_id{0};
-	Command 		_current_cmd;
-	px4::atomic<Command *>	_pending_cmd{nullptr};
 
 	static const uint8_t 	READ_BUF_SIZE = 128;
 	uint8_t			_read_buf[READ_BUF_SIZE];
@@ -220,9 +204,8 @@ private:
 	uint32_t		_packets_sent{0};
 	uint32_t		_packets_received{0};
 
-	int parse_response(uint8_t *buf, uint8_t len);
 	int	load_params(voxl2_io_params_t *params);
 	int update_params();
-	int	flush_uart_rx();
 	int calibrate_escs();
+	std::string board_id_to_name(int board_id);
 };
diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c
index 3de26acbfe..99340fa974 100644
--- a/src/drivers/voxl2_io/voxl2_io_packet.c
+++ b/src/drivers/voxl2_io/voxl2_io_packet.c
@@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2,
 	return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
 }
 
+typedef struct {
+	uint8_t  command_type;
+	//uint8_t  channel_offset;
+	uint16_t vals[8]; //could be 1,2,4,6,8
+} __attribute__((__packed__)) pwm_hires_cmd_t;
+
+pwm_hires_cmd_t hires_cmd;
+
+int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size)
+{
+	if (cmd_cnt > 8) {
+		return -1;
+	}
+
+	hires_cmd.command_type   = 0;
+	//hires_cmd.channel_offset = 0;
+
+	//resolution of commands in the packet is 0.05us = 50ns
+	for (uint32_t idx = 0; idx < cmd_cnt; idx++) {
+		hires_cmd.vals[idx] = pwm_val_ns[idx] / 50;
+	}
+
+	return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out,
+				      out_size);
+}
+
 int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size)
 {
 	uint16_t packet_size = size + 5;
diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h
index 7e6237017f..1c4d242814 100644
--- a/src/drivers/voxl2_io/voxl2_io_packet.h
+++ b/src/drivers/voxl2_io/voxl2_io_packet.h
@@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in
 				    uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
 				    uint8_t *out, uint16_t out_size);
 
+int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size);
+
+
 // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
 // Return value is the length of generated packet (if positive), otherwise error code
 int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h
index 150982eca4..12766fb0ef 100644
--- a/src/drivers/voxl2_io/voxl2_io_packet_types.h
+++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h
@@ -46,6 +46,7 @@
 #define VOXL2_IO_PACKET_TYPE_SOUND_CMD       3
 #define VOXL2_IO_PACKET_TYPE_STEP_CMD        4
 #define VOXL2_IO_PACKET_TYPE_LED_CMD         5
+#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD   6
 #define VOXL2_IO_PACKET_TYPE_RESET_CMD       10
 #define VOXL2_IO_PACKET_TYPE_SET_ID_CMD      11
 #define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD     12
diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c
index 704cca989e..ac99723b8d 100644
--- a/src/drivers/voxl2_io/voxl2_io_params.c
+++ b/src/drivers/voxl2_io/voxl2_io_params.c
@@ -32,34 +32,73 @@
  ****************************************************************************/
 
 /**
- * UART M0065 baud rate
+ * VOXL2_IO UART baud rate
  *
- * Default rate is 921600, which is used for communicating with M0065.
+ * Default rate is 921600, which is used for communicating with VOXL2_IO board
  *
  * @group VOXL2 IO
  * @unit bit/s
  */
 PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600);
 
+
 /**
- * M0065 PWM Min
+ * VOXL2_IO Disabled PWM
  *
- * Minimum duration (microseconds) for M0065 PWM
+ * Pulse duration in disabled state (microseconds) for VOXL2_IO board
  *
  * @min 0
  * @max 2000
  * @group VOXL2 IO
  * @unit us
  */
-PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000);
+PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000);
 
 /**
- * M0065 PWM Max
+ * VOXL2_IO Min PWM
  *
- * Maximum duration (microseconds) for M0065 PWM
+ * Minimum duration (microseconds) for VOXL2_IO board
+ *
+ * @min 0
+ * @max 2000
+ * @group VOXL2 IO
+ * @unit us
+ */
+
+PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100);
+
+/**
+ * VOXL2_IO Max PWM
+ *
+ * Maximum duration (microseconds) for VOXL2_IO board
  * @min 0
  * @max 2000
  * @group VOXL2 IO
  * @unit us
  */
 PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000);
+
+
+/**
+ * VOXL2_IO Calibration Min PWM
+ *
+ * Minimum duration (microseconds) for VOXL2_IO board
+ *
+ * @min 0
+ * @max 2000
+ * @group VOXL2 IO
+ * @unit us
+ */
+
+PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050);
+
+/**
+ * VOXL2_IO Calibration Max PWM
+ *
+ * Maximum duration (microseconds) for VOXL2_IO board
+ * @min 0
+ * @max 2000
+ * @group VOXL2 IO
+ * @unit us
+ */
+PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000);
diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp
deleted file mode 100644
index cee0923ece..0000000000
--- a/src/drivers/voxl2_io/voxl2_io_serial.cpp
+++ /dev/null
@@ -1,191 +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.
- *
- ****************************************************************************/
-
-#include "string.h"
-#include "voxl2_io_serial.hpp"
-
-Voxl2IoSerial::Voxl2IoSerial()
-{
-}
-
-Voxl2IoSerial::~Voxl2IoSerial()
-{
-	if (_uart_fd >= 0) {
-		uart_close();
-	}
-}
-
-int Voxl2IoSerial::uart_open(const char *dev, speed_t speed)
-{
-	if (_uart_fd >= 0) {
-		PX4_ERR("Port in use: %s (%i)", dev, errno);
-		return -1;
-	}
-
-	/* Open UART */
-#ifdef __PX4_QURT
-	_uart_fd = qurt_uart_open(dev, speed);
-#else
-	_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
-#endif
-
-	if (_uart_fd < 0) {
-		PX4_ERR("Error opening port: %s (%i)", dev, errno);
-		return -1;
-	}
-
-#ifndef __PX4_QURT
-	/* Back up the original UART configuration to restore it after exit */
-	int termios_state;
-
-	if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) {
-		PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state);
-		uart_close();
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(_uart_fd, &_cfg);
-
-	/* Disable output post-processing */
-	_cfg.c_oflag &= ~OPOST;
-
-	_cfg.c_cflag |= (CLOCAL | CREAD);    /* ignore modem controls */
-	_cfg.c_cflag &= ~CSIZE;
-	_cfg.c_cflag |= CS8;                 /* 8-bit characters */
-	_cfg.c_cflag &= ~PARENB;             /* no parity bit */
-	_cfg.c_cflag &= ~CSTOPB;             /* only need 1 stop bit */
-	_cfg.c_cflag &= ~CRTSCTS;            /* no hardware flowcontrol */
-
-	/* setup for non-canonical mode */
-	_cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
-	_cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
-
-	if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) {
-		PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state);
-		uart_close();
-		return -1;
-	}
-
-	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) {
-		PX4_ERR("Error configuring port: %s (tcsetattr)", dev);
-		uart_close();
-		return -1;
-	}
-
-#endif
-
-	_speed =  speed;
-
-	return 0;
-}
-
-int Voxl2IoSerial::uart_set_baud(speed_t speed)
-{
-#ifndef __PX4_QURT
-
-	if (_uart_fd < 0) {
-		return -1;
-	}
-
-	if (cfsetispeed(&_cfg, speed) < 0) {
-		return -1;
-	}
-
-	if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) {
-		return -1;
-	}
-
-	_speed = speed;
-
-	return 0;
-#endif
-
-	return -1;
-}
-
-int Voxl2IoSerial::uart_close()
-{
-#ifndef __PX4_QURT
-
-	if (_uart_fd < 0) {
-		PX4_ERR("invalid state for closing");
-		return -1;
-	}
-
-	if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) {
-		PX4_ERR("failed restoring uart to original state");
-	}
-
-	if (close(_uart_fd)) {
-		PX4_ERR("error closing uart");
-	}
-
-#endif
-
-	_uart_fd = -1;
-
-	return 0;
-}
-
-int Voxl2IoSerial::uart_write(FAR void *buf, size_t len)
-{
-	if (_uart_fd < 0 || buf == NULL) {
-		PX4_ERR("invalid state for writing or buffer");
-		return -1;
-	}
-
-#ifdef __PX4_QURT
-	return qurt_uart_write(_uart_fd, (const char *) buf, len);
-#else
-	return write(_uart_fd, buf, len);
-#endif
-}
-
-int Voxl2IoSerial::uart_read(FAR void *buf, size_t len)
-{
-	if (_uart_fd < 0 || buf == NULL) {
-		PX4_ERR("invalid state for reading or buffer");
-		return -1;
-	}
-
-#ifdef __PX4_QURT
-#define ASYNC_UART_READ_WAIT_US 2000
-	// The UART read on SLPI is via an asynchronous service so specify a timeout
-	// for the return. The driver will poll periodically until the read comes in
-	// so this may block for a while. However, it will timeout if no read comes in.
-	return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US);
-#else
-	return read(_uart_fd, buf, len);
-#endif
-}
diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/src/drivers/voxl2_io/voxl2_io_serial.hpp
deleted file mode 100644
index 638bdef288..0000000000
--- a/src/drivers/voxl2_io/voxl2_io_serial.hpp
+++ /dev/null
@@ -1,69 +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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_log.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <termios.h>
-
-#ifdef __PX4_QURT
-#include <drivers/device/qurt/uart.h>
-#define FAR
-#endif
-
-class Voxl2IoSerial
-{
-public:
-	Voxl2IoSerial();
-	virtual ~Voxl2IoSerial();
-
-	int		uart_open(const char *dev, speed_t speed);
-	int		uart_set_baud(speed_t speed);
-	int		uart_close();
-	int		uart_write(FAR void *buf, size_t len);
-	int		uart_read(FAR void *buf, size_t len);
-	bool		is_open() { return _uart_fd >= 0; };
-	int		uart_get_baud() {return _speed; }
-
-private:
-	int			_uart_fd = -1;
-
-#if ! defined(__PX4_QURT)
-	struct termios		_orig_cfg;
-	struct termios		_cfg;
-#endif
-
-	int   _speed = -1;
-};