diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1e799721ce..542edce0ab 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -129,4 +129,5 @@
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         },
+        "C_Cpp.errorSquiggles": "disabled",
 }
diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
index 92fec8e76e..33e714f1fd 100644
--- a/src/drivers/dshot/DShot.cpp
+++ b/src/drivers/dshot/DShot.cpp
@@ -38,7 +38,6 @@
 #include <px4_platform_common/sem.hpp>
 
 char DShot::_telemetry_device[] {};
-bool DShot::_telemetry_swap_rxtx{false};
 px4::atomic_bool DShot::_request_telemetry_init{false};
 
 DShot::DShot() :
@@ -58,9 +57,6 @@ DShot::~DShot()
 	up_dshot_arm(false);
 
 	perf_free(_cycle_perf);
-	perf_free(_bdshot_rpm_perf);
-	perf_free(_dshot_telem_perf);
-
 	delete _telemetry;
 }
 
@@ -101,7 +97,7 @@ int DShot::task_spawn(int argc, char *argv[])
 
 void DShot::enable_dshot_outputs(const bool enabled)
 {
-	if (enabled && !_outputs_initialized) {
+	if (enabled && !_outputs_initialized && esc_flasher_flashing_state == 0) {
 		unsigned int dshot_frequency = 0;
 		uint32_t dshot_frequency_param = 0;
 
@@ -147,6 +143,9 @@ void DShot::enable_dshot_outputs(const bool enabled)
 
 		_bidirectional_dshot_enabled = _param_bidirectional_enable.get();
 
+		// Get ESC_TYPE parameter at startup
+		_esc_type = (ESCType)_param_esc_type.get();
+
 		int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled);
 
 		if (ret < 0) {
@@ -193,7 +192,7 @@ void DShot::update_num_motors()
 	_num_motors = motor_count;
 }
 
-void DShot::init_telemetry(const char *device, bool swap_rxtx)
+void DShot::init_telemetry(const char *device)
 {
 	if (!_telemetry) {
 		_telemetry = new DShotTelemetry{};
@@ -205,7 +204,7 @@ void DShot::init_telemetry(const char *device, bool swap_rxtx)
 	}
 
 	if (device != NULL) {
-		int ret = _telemetry->init(device, swap_rxtx);
+		int ret = _telemetry->init(device);
 
 		if (ret != 0) {
 			PX4_ERR("telemetry init failed (%i)", ret);
@@ -247,12 +246,27 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem
 		++esc_status.counter;
 
 		ret = 1; // Indicate we wrapped, so we publish data
+#if 1
+		// Flag get_esc_info state machine to start ESC detection
+		if (get_esc_info_boot == 0 && get_esc_info_start == 0) {
+			// Use get_esc_info_boot so we only start ESC detection at boot ONCE
+			// It can be restarted via ESC_Flasher.cpp, using get_esc_info_start = 1
+			get_esc_info_boot = 1;
+			get_esc_info_start = 1;
+			get_esc_info_time = hrt_absolute_time() + 3_s;
+			// Set _esc_type_temp to the first enum, AM32
+			if (_esc_type == ESCType::Unknown) {
+				_esc_type_temp = ESCType::AM32;
+			}
+			else {
+				_esc_type_temp = _esc_type;
+			}
+		}
+#endif
 	}
 
 	_last_telemetry_index = telemetry_index;
 
-	perf_count(_dshot_telem_perf);
-
 	return ret;
 }
 
@@ -260,14 +274,39 @@ void DShot::publish_esc_status(void)
 {
 	esc_status_s &esc_status = esc_status_pub.get();
 	int telemetry_index = 0;
+	int num_active_motors = _num_motors;
 
 	// clear data of the esc that are offline
 	for (int index = 0; (index < _last_telemetry_index); index++) {
 		if ((esc_status.esc_online_flags & (1 << index)) == 0) {
 			memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s));
+			num_active_motors--;
 		}
 	}
 
+	if (num_active_motors != _num_motors && esc_flasher_flashing_state == 0) {
+		_broken_esc++;
+		// 50 times will take 2 seconds, since this runs at 25 Hz
+		if (_broken_esc >= 50 && _broken_esc % 50 == 0) {
+			//PX4_INFO("Broken ESC detected! Hardware fix required.");
+			if (!_broken_esc_flag) {
+				PX4_INFO("Broken ESC detected! Hardware fix required.");
+				_broken_esc_flag = true;
+
+				// Send esc_versions uOrb with result BROKEN
+				esc_flasher_versions_s esc_flasher_versions{0};
+				esc_flasher_versions.result = esc_flasher_versions_s::RESULT_BROKEN_ESC;
+				// Flag versions_valid so the message gets sent out in PRE_SHOW_STATUS.hpp
+				esc_flasher_versions.versions_valid = 1;
+				esc_flasher_versions.timestamp = hrt_absolute_time();
+				_esc_flasher_versions_pub.publish(esc_flasher_versions);
+			}
+		}
+	}
+	else {
+		_broken_esc = 0;
+	}
+
 	// FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout
 	esc_status.esc_count = _num_motors;
 	esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
@@ -288,6 +327,12 @@ void DShot::publish_esc_status(void)
 		}
 	}
 
+	// If ESC flashing is in progress, mark all ESCs as offline so vehicle cannot arm
+	if (esc_flasher_flashing_state > 0) {
+		esc_status.esc_online_flags = 0;
+		esc_status.esc_armed_flags = 0;
+	}
+
 	if (!esc_status_pub.advertised()) {
 		esc_status_pub.advertise();
 
@@ -328,9 +373,9 @@ int DShot::handle_new_bdshot_erpm(void)
 
 			++telemetry_index;
 		}
-	}
 
-	perf_count(_bdshot_rpm_perf);
+
+	}
 
 	return num_erpms;
 }
@@ -347,6 +392,13 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num
 		cmd.motor_mask = 1 << motor_index;
 	}
 
+	ESCType temp_esc_type = _esc_type;
+	if (get_esc_info_state != 0) {
+		temp_esc_type = _esc_type_temp;
+	}
+	if (temp_esc_type == ESCType::AM32) {
+		if (cmd.command == 9 || cmd.command == 10) cmd.save = true;
+	}
 	cmd.num_repetitions = num_repetitions;
 	_new_command.store(&cmd);
 
@@ -367,13 +419,128 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num
 	return 0;
 }
 
+void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index)
+{
+	if (_request_esc_info.load() != nullptr) {
+		// already in progress (not expected to ever happen)
+		return;
+	}
+
+	DShotTelemetry::OutputBuffer output_buffer{};
+	output_buffer.motor_index = motor_index;
+
+	// start the request
+	_request_esc_info.store(&output_buffer);
+
+	// wait until processed
+	int max_time = 1000;
+
+	while (_request_esc_info.load() != nullptr && max_time-- > 0) {
+		px4_usleep(1000);
+	}
+
+	_request_esc_info.store(nullptr); // just in case we time out...
+	_waiting_for_esc_info = false;
+
+	if (output_buffer.buf_pos == 0) {
+		PX4_ERR("No data received. If telemetry is setup correctly, try again");
+		return;
+	}
+
+	DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer);
+}
+
+int DShot::retrieve_and_print_esc_info_non_blocking(const int motor_index)
+{
+	if (_request_esc_info.load() != nullptr) {
+		// already in progress (not expected to ever happen)
+		return -1;
+	}
+
+	esc_flasher_output_buffer.buf_pos = 0;
+	esc_flasher_output_buffer.motor_index = motor_index;
+
+	// start the request
+	_request_esc_info.store(&esc_flasher_output_buffer);
+
+	return 0;
+}
+
+int DShot::retrieve_and_print_esc_info_check_result(uint8_t* fw_ver_major, uint8_t* fw_ver_minor)
+{
+	// Return 0 only if AM32 ESC
+	// Return -1 if still waiting for data
+	// Return -2 for timeout or CRC error or other error
+	// Return -3 for BLHeli ESC
+
+	if (_request_esc_info.load() != nullptr) {
+		// Data not ready yet
+		return -1;
+	}
+
+	_request_esc_info.store(nullptr); // just in case we time out...
+	_waiting_for_esc_info = false;
+
+	if (esc_flasher_output_buffer.buf_pos == 0) {
+		PX4_ERR("No data received. If telemetry is setup correctly, try again");
+		return -2;
+	}
+
+	int retval = DShotTelemetry::decodeEscInfoPacketFwVersion(esc_flasher_output_buffer, fw_ver_major, fw_ver_minor);
+	if (retval == 0) {
+		// AM32 ESC success
+		return retval;
+	}
+	else if (retval == -1) {
+		// No data or CRC error or other error
+		return -2;
+	}
+	else if (retval == -2) {
+		// BLHeli ESC success
+		//PX4_ERR("AM32 ESC not found");
+		return -3;
+	}
+
+	return retval;
+}
+
+int DShot::request_esc_info()
+{
+	_telemetry->redirectOutput(*_request_esc_info.load());
+	_waiting_for_esc_info = true;
+
+	int motor_index = _request_esc_info.load()->motor_index;
+
+	_current_command.motor_mask = 1 << motor_index;
+
+	ESCType temp_esc_type = _esc_type;
+	if (get_esc_info_state != 0) {
+		temp_esc_type = _esc_type_temp;
+	}
+	if (temp_esc_type == ESCType::AM32 || temp_esc_type == ESCType::AM32_Old) _current_command.num_repetitions = 6;
+	else _current_command.num_repetitions = 1;
+
+	_current_command.command = DShot_cmd_esc_info;
+	_current_command.save = false;
+
+	PX4_DEBUG("Requesting ESC info for motor %i", motor_index);
+	return motor_index;
+}
+
 void DShot::mixerChanged()
 {
 	update_num_motors();
+
 }
 
 bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
+{
+	return false;
+}
+
+bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
 	if (!_outputs_on) {
 		return false;
@@ -382,47 +549,99 @@ bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 	int requested_telemetry_index = -1;
 
 	if (_telemetry) {
-		requested_telemetry_index = _telemetry->getRequestMotorIndex();
+		// check for an ESC info request. We only process it when we're not expecting other telemetry data
+		if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors
+		    && !_telemetry->expectingData() && !_current_command.valid()) {
+			requested_telemetry_index = request_esc_info();
+		} else {
+			requested_telemetry_index = _telemetry->getRequestMotorIndex();
+		}
 	}
 
-	int telemetry_index = 0;
+	if (stop_motors) {
 
-	for (int i = 0; i < (int)num_outputs; i++) {
-
-		uint16_t output = outputs[i];
-
-		if (output == DSHOT_DISARM_VALUE) {
+		int telemetry_index = 0;
 
+		// when motors are stopped we check if we have other commands to send
+		for (int i = 0; i < (int)num_outputs; i++) {
 			if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) {
-				up_dshot_motor_command(i, _current_command.command, true);
-
+				// for some reason we need to always request telemetry when sending a command
+				// unless command is esc_info
+				ESCType temp_esc_type = _esc_type;
+				if (get_esc_info_state != 0) {
+					temp_esc_type = _esc_type_temp;
+				}
+				if ((temp_esc_type == ESCType::AM32 || temp_esc_type == ESCType::AM32_Old) && _current_command.command == 6) {
+					up_dshot_motor_command(i, _current_command.command, false);
+				}
+				else up_dshot_motor_command(i, _current_command.command, true);
 			} else {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
 			}
 
-		} else {
+			telemetry_index += _mixing_output.isFunctionSet(i);
+		}
 
-			if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
-				output = convert_output_to_3d_scaling(output);
+		if (_current_command.valid()) {
+			--_current_command.num_repetitions;
+
+			if (_current_command.num_repetitions == 0 && _current_command.save) {
+				_current_command.save = false;
+				_current_command.num_repetitions = 10;
+				_current_command.command = dshot_command_t::DShot_cmd_save_settings;
+			}
+		}
+
+	} else {
+		int telemetry_index = 0;
+
+		for (int i = 0; i < (int)num_outputs; i++) {
+
+			uint16_t output = outputs[i];
+
+			if (output == DSHOT_DISARM_VALUE) {
+				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
+
+			} else {
+
+				// DShot 3D splits the throttle ranges in two.
+				// This is in terms of DShot values, code below is in terms of actuator_output
+				// Direction 1) 48 is the slowest, 1047 is the fastest.
+				// Direction 2) 1049 is the slowest, 2047 is the fastest.
+				if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
+					if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
+						output = DSHOT_DISARM_VALUE;
+
+					} else {
+						bool upper_range = output >= 1000;
+
+						if (upper_range) {
+							output -= 1000;
+
+						} else {
+							output = 999 - output; // lower range is inverted
+						}
+
+						float max_output = 999.f;
+						float min_output = max_output * _param_dshot_min.get();
+						output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
+
+						if (upper_range) {
+							output += 1000;
+						}
+
+					}
+				}
+
+				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
+							telemetry_index == requested_telemetry_index);
 			}
 
-			up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
-						telemetry_index == requested_telemetry_index);
+			telemetry_index += _mixing_output.isFunctionSet(i);
 		}
 
-		telemetry_index += _mixing_output.isFunctionSet(i);
-	}
-
-	// Decrement the command counter
-	if (_current_command.valid()) {
-		--_current_command.num_repetitions;
-
-		// Queue a save command after the burst if save has been requested
-		if (_current_command.num_repetitions == 0 && _current_command.save) {
-			_current_command.save = false;
-			_current_command.num_repetitions = 10;
-			_current_command.command = dshot_command_t::DShot_cmd_save_settings;
-		}
+		// clear commands when motors are running
+		_current_command.clear();
 	}
 
 	up_dshot_trigger();
@@ -430,36 +649,6 @@ bool DShot::updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 	return true;
 }
 
-uint16_t DShot::convert_output_to_3d_scaling(uint16_t output)
-{
-	// DShot 3D splits the throttle ranges in two.
-	// This is in terms of DShot values, code below is in terms of actuator_output
-	// Direction 1) 48 is the slowest, 1047 is the fastest.
-	// Direction 2) 1049 is the slowest, 2047 is the fastest.
-	if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
-		return DSHOT_DISARM_VALUE;
-	}
-
-	bool upper_range = output >= 1000;
-
-	if (upper_range) {
-		output -= 1000;
-
-	} else {
-		output = 999 - output; // lower range is inverted
-	}
-
-	float max_output = 999.f;
-	float min_output = max_output * _param_dshot_min.get();
-	output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
-
-	if (upper_range) {
-		output += 1000;
-	}
-
-	return output;
-}
-
 void DShot::Run()
 {
 	if (should_exit()) {
@@ -484,7 +673,14 @@ void DShot::Run()
 	if (_telemetry) {
 		const int telem_update = _telemetry->update(_num_motors);
 
-		if (telem_update >= 0) {
+		// Are we waiting for ESC info?
+		if (_waiting_for_esc_info) {
+			if (telem_update != -1) {
+				_request_esc_info.store(nullptr);
+				_waiting_for_esc_info = false;
+			}
+
+		} else if (telem_update >= 0) {
 			const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(),
 						    _bidirectional_dshot_enabled);
 
@@ -510,7 +706,7 @@ void DShot::Run()
 
 	// telemetry device update request?
 	if (_request_telemetry_init.load()) {
-		init_telemetry(_telemetry_device, _telemetry_swap_rxtx);
+		init_telemetry(_telemetry_device);
 		_request_telemetry_init.store(false);
 	}
 
@@ -526,6 +722,10 @@ void DShot::Run()
 
 	handle_vehicle_commands();
 
+	handle_esc_flasher_requests();
+
+	run_get_esc_info();
+
 	if (!_mixing_output.armed().armed) {
 		if (_reversible_outputs != _mixing_output.reversibleOutputs()) {
 			_reversible_outputs = _mixing_output.reversibleOutputs();
@@ -615,6 +815,438 @@ void DShot::handle_vehicle_commands()
 	}
 }
 
+void DShot::handle_esc_flasher_requests()
+{
+	static uint8_t fw_version_major;
+	static uint8_t fw_version_minor;
+	static uint32_t gpio;
+	int retval = 0;
+
+	while (!_current_command.valid() && _esc_flasher_request_sub.update(&_esc_flasher_request)) {
+		PX4_INFO("DShot received request from ESC Flasher");
+
+		if (_esc_flasher_request.request == esc_flasher_request_s::REQUEST_CANCEL ||
+			_esc_flasher_request.request == esc_flasher_request_s::REQUEST_FLASHING_COMPLETE) {
+			// Cancel any requests, re-enable DShot
+			esc_flasher_esc_info_state = 0;
+			esc_flasher_esc_info_motor_index = 0;
+			esc_flasher_flashing_state = 0;
+			enable_dshot_outputs(true);
+
+			memset(&_esc_flasher_response, 0, sizeof(_esc_flasher_response));
+			_esc_flasher_response.msg_id = _esc_flasher_request.msg_id;
+			_esc_flasher_response.request = _esc_flasher_request.request;
+			_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_ACCEPTED;
+			_esc_flasher_response.timestamp = hrt_absolute_time();
+
+			if (_esc_flasher_request.request == esc_flasher_request_s::REQUEST_CANCEL) {
+				PX4_WARN("Canceling ESC Flasher requests and re-enabling DShot");
+			}
+			else {
+				PX4_INFO("Flashing completed by ESC Flasher, re-enabling DShot");
+				PX4_INFO("Restarting ESC detection to update ESC versions");
+
+				// Restart ESC detection, so new version is saved and send via esc_versions uOrb
+				get_esc_info_start = 1;
+				get_esc_info_time = hrt_absolute_time() + 2_s;
+				_esc_type_temp = _esc_type;
+			}
+			_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+			// Clear broken esc flags on CANCEL or COMPLETE
+			_broken_esc = 0;
+			_broken_esc_flag = false;
+
+			break;
+		}
+
+		if (esc_flasher_esc_info_state) {
+			// ESC_INFO request is in progress
+			memset(&_esc_flasher_response, 0, sizeof(_esc_flasher_response));
+			_esc_flasher_response.msg_id = _esc_flasher_request.msg_id;
+			_esc_flasher_response.request = _esc_flasher_request.request;
+			_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_IN_PROGRESS;
+			_esc_flasher_response.timestamp = hrt_absolute_time();
+
+			PX4_ERR("ESC INFO request already in progress");
+			_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+			break;
+		}
+		if (esc_flasher_flashing_state) {
+			// FLASHING request is in progress
+			memset(&_esc_flasher_response, 0, sizeof(_esc_flasher_response));
+			_esc_flasher_response.msg_id = _esc_flasher_request.msg_id;
+			_esc_flasher_response.request = _esc_flasher_request.request;
+			_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_IN_PROGRESS;
+			_esc_flasher_response.timestamp = hrt_absolute_time();
+
+			PX4_ERR("FLASHING request already in progress");
+			_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+			break;
+		}
+
+		if (_esc_flasher_request.request == esc_flasher_request_s::REQUEST_ESC_INFO) {
+			if (telemetry_enabled()) {
+				esc_flasher_esc_info_state = 1;
+				esc_flasher_esc_info_motor_index = 0;
+				memset(&_esc_flasher_response, 0, sizeof(_esc_flasher_response));
+				_esc_flasher_response.msg_id = _esc_flasher_request.msg_id;
+				_esc_flasher_response.request = _esc_flasher_request.request;
+			}
+			else {
+				PX4_ERR("Telemetry is not enabled, but required to get ESC info");
+			}
+		}
+		else if (_esc_flasher_request.request == esc_flasher_request_s::REQUEST_FLASHING) {
+			// When ESC Flasher requests flashing, disable all DShot outputs until
+			// ESC Flasher notifies completion or cancellation
+			// Return via uORB all GPIO pin definitions, since ESC Flasher will need those for bit-banging UART
+			esc_flasher_flashing_state = 1;
+			memset(&_esc_flasher_response, 0, sizeof(_esc_flasher_response));
+			_esc_flasher_response.msg_id = _esc_flasher_request.msg_id;
+			_esc_flasher_response.request = _esc_flasher_request.request;
+		}
+	}
+
+	switch (esc_flasher_esc_info_state) {
+		case 0:
+			break;
+		case 1:
+			// Get ESC_INFO from the requested motors
+			if (_esc_flasher_request.motor_flags & (1 << esc_flasher_esc_info_motor_index)) {
+				retrieve_and_print_esc_info_non_blocking(esc_flasher_esc_info_motor_index);
+				esc_flasher_esc_info_state = 2;
+			}
+			else {
+				// Move on to next motor
+				esc_flasher_esc_info_motor_index++;
+				if (esc_flasher_esc_info_motor_index >= 4) {
+					esc_flasher_esc_info_state = 3;
+				}
+			}
+
+			break;
+		case 2:
+			// Wait for ESC_INFO to finish
+			retval = retrieve_and_print_esc_info_check_result(&fw_version_major, &fw_version_minor);
+			if (retval == 0) {
+
+				// Save the versions to our response struct
+				_esc_flasher_response.fw_flags |= (1 << esc_flasher_esc_info_motor_index);
+				_esc_flasher_response.fw_major[esc_flasher_esc_info_motor_index] = fw_version_major;
+				_esc_flasher_response.fw_minor[esc_flasher_esc_info_motor_index] = fw_version_minor;
+
+				// Move on to next motor
+				esc_flasher_esc_info_motor_index++;
+				if (esc_flasher_esc_info_motor_index >= 4) {
+					esc_flasher_esc_info_state = 3;
+				}
+				else {
+					esc_flasher_esc_info_state = 1;
+				}
+			}
+			else if (retval == -1) {
+				// Still waiting for response or timeout
+			}
+			else if (retval == -2) {
+				// No valid response from ESC
+				// Move on to next motor
+				esc_flasher_esc_info_motor_index++;
+				if (esc_flasher_esc_info_motor_index >= 4) {
+					esc_flasher_esc_info_state = 3;
+				}
+				else {
+					esc_flasher_esc_info_state = 1;
+				}
+			}
+			else if (retval == -3) {
+				// Non-AM32 ESC detected
+				// Publish response message
+				_esc_flasher_response.fw_flags = 0;
+				_esc_flasher_response.timestamp = hrt_absolute_time();
+				_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_UNSUPPORTED;
+
+				_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+				esc_flasher_esc_info_state = 0;
+				esc_flasher_esc_info_motor_index = 0;
+			}
+
+			break;
+		case 3:
+			// Publish response message
+			_esc_flasher_response.timestamp = hrt_absolute_time();
+			_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_ACCEPTED;
+
+			_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+			esc_flasher_esc_info_state = 0;
+			esc_flasher_esc_info_motor_index = 0;
+
+			break;
+	}
+
+	switch (esc_flasher_flashing_state) {
+		case 0:
+			break;
+		case 1:
+			// Disable all DShot outputs
+			enable_dshot_outputs(false);
+			_outputs_initialized = false;
+			esc_flasher_flashing_state = 2;
+
+			break;
+		case 2:
+			// Send GPIO pins to ESC Flasher
+			for (uint32_t i = 0; i < 16; i++) {
+				if (_esc_flasher_request.motor_flags & (1 << i)) {
+					gpio = io_timer_channel_get_gpio_output(i);
+					// Clear unwanted flags from GPIO pin definition
+					gpio &= (GPIO_PORT_MASK | GPIO_PIN_MASK);
+					_esc_flasher_response.gpio_pins[i] = gpio;
+					_esc_flasher_response.gpio_flags |= (1 << i);
+				}
+			}
+
+			esc_flasher_flashing_state = 3;
+
+			break;
+		case 3:
+			// Publish response message
+			_esc_flasher_response.timestamp = hrt_absolute_time();
+			_esc_flasher_response.result = esc_flasher_request_ack_s::ESC_FLASHER_CMD_RESULT_ACCEPTED;
+
+			_esc_flasher_request_ack_pub.publish(_esc_flasher_response);
+
+			esc_flasher_flashing_state = 4;
+
+			break;
+		case 4:
+			// Wait state for ESC Flasher, ensure DShot outputs stay off
+
+			break;
+	}
+
+	// Update ESC status if flashing is active, so the vehicle cannot arm
+	if (esc_flasher_flashing_state > 0) {
+		publish_esc_status();
+	}
+}
+
+void DShot::run_get_esc_info()
+{
+	int retval = 0;
+
+	if (_num_motors && get_esc_info_start && (esc_flasher_flashing_state == 0) && (esc_flasher_esc_info_state == 0)) {
+		// Once we have _num_motors, mixers have been initialized, try to get esc_info from all motors
+
+		if (get_esc_info_state == 0 && (hrt_absolute_time() > get_esc_info_time)) {
+			// Start state machine
+			PX4_INFO("Starting ESC detection, attempting ESC type %d", (int)_esc_type_temp);
+			get_esc_info_state = 1;
+			get_esc_info_motor_index = 0;
+
+			memset(&_esc_info_save, 0, sizeof(_esc_info_save));
+
+			// This ESC_TYPE test is setup to run shortly after bootup
+			// It will start out at the param ESC_TYPE and move up the list until the end, no loop
+			// If DShot gets a response to esc_info command, then it is a success and the new ESC_TYPE is saved in the param
+			// If no response then no change to ESC_TYPE param
+			// To restart the test, reset the param ESC_TYPE to 0 and reboot PX4
+		}
+
+		switch (get_esc_info_state) {
+			case 0:
+				// Idle state
+				break;
+			case 1:
+				// Check motor_index against _num_motors
+				if (get_esc_info_motor_index < (uint32_t)_num_motors) {
+					get_esc_info_state = 2;
+				}
+				else {
+					// Done, count how many motors gave the esc_info
+					int motor_count = 0;
+					for (int i = 0; i < _num_motors; i++) {
+						if (_esc_info_save[i].type != ESCType::Unknown) {
+							motor_count++;
+						}
+					}
+
+					if (motor_count == _num_motors) {
+						get_esc_info_state = 10;
+						PX4_INFO("ESC_INFO received for %d out of %d motors", (int)motor_count, (int)_num_motors);
+					}
+					else if (motor_count == 0) {
+
+						// Move to next ESC_TYPE during the bootup test
+						int temp_type = (int)_esc_type_temp;
+						temp_type++;
+						_esc_type_temp = (ESCType)temp_type;
+
+						if (_esc_type_temp == ESCType::BlueJay) {
+							// End of test, fail
+							get_esc_info_state = 11;
+							PX4_ERR("No ESC_INFO received, please set param ESC_TYPE to 0 and reboot");
+							break;
+						}
+
+						// No data received, try again in 1 second
+						if (get_esc_info_tries++ < 3) {
+							get_esc_info_time = hrt_absolute_time() + 1_s;
+							get_esc_info_state = 0;
+							PX4_WARN("No ESC_INFO received, trying again in 1 second with ESC type %d", (int)_esc_type_temp);
+						}
+						else {
+							// After 3 tries, no data received, end state-machine
+							get_esc_info_state = 11;
+							PX4_ERR("No ESC_INFO received");
+						}
+					}
+					else {
+						// Only received data from some motors
+						get_esc_info_state = 10;
+						PX4_INFO("ESC_INFO received for %d out of %d motors", (int)motor_count, (int)_num_motors);
+					}
+				}
+				break;
+			case 2:
+				// Get next motor esc_info
+				retrieve_and_print_esc_info_non_blocking(get_esc_info_motor_index);
+				get_esc_info_state = 3;
+				break;
+			case 3:
+				// Wait for ESC_INFO to finish
+				retval = retrieve_and_print_esc_info_check_result(&_esc_info_save[get_esc_info_motor_index].fw_major, &_esc_info_save[get_esc_info_motor_index].fw_minor);
+				if (retval == 0) {
+					// AM32 ESC detected
+					_esc_info_save[get_esc_info_motor_index].type = ESCType::AM32;
+					get_esc_info_motor_index++;
+					get_esc_info_state = 1;
+					break;
+				}
+				else if (retval == -1) {
+					// Still waiting for response or timeout
+				}
+				else if (retval == -2) {
+					// No valid response from ESC
+					// Move on to next motor
+					get_esc_info_motor_index++;
+					get_esc_info_state = 1;
+					break;
+				}
+				else if (retval == -3) {
+					// Non-AM32 ESC detected
+					_esc_info_save[get_esc_info_motor_index].type = ESCType::BLHELI32;
+					get_esc_info_motor_index++;
+					get_esc_info_state = 1;
+					break;
+				}
+
+				break;
+			case 10:
+			{
+				// Done with all ESCs, publish uOrb message
+				esc_flasher_versions_s esc_flasher_versions{0};
+				int motor_count = 0;
+				bool same_type = true;
+				bool same_versions = true;
+				ESCType type = ESCType::Unknown;
+				uint8_t version_major = 0;
+				uint8_t version_minor = 0;
+
+				for (int i = 0; i < _num_motors; i++) {
+					if (_esc_info_save[i].type != ESCType::Unknown) {
+						if (i == 0) {
+							// First motor, set type
+							type = _esc_info_save[i].type;
+							version_major = _esc_info_save[i].fw_major;
+							version_minor = _esc_info_save[i].fw_minor;
+						}
+						else {
+							// Subsequent motors, check type and versions against first motor
+							if (_esc_info_save[i].type != type) {
+								same_type = false;
+							}
+							if (_esc_info_save[i].fw_major != version_major || _esc_info_save[i].fw_minor != version_minor) {
+								same_versions = false;
+							}
+						}
+
+						motor_count++;
+						esc_flasher_versions.versions_valid |= (1 << i);
+						esc_flasher_versions.esc_type[i] = (uint8_t)_esc_info_save[i].type;
+						esc_flasher_versions.versions_major[i] = _esc_info_save[i].fw_major;
+						esc_flasher_versions.versions_minor[i] = _esc_info_save[i].fw_minor;
+					}
+				}
+
+				esc_flasher_versions.result = esc_flasher_versions_s::RESULT_SUCCESS;
+				if (motor_count != _num_motors) {
+					esc_flasher_versions.result = esc_flasher_versions_s::RESULT_MISMATCH;
+				}
+				if (!same_type) {
+					esc_flasher_versions.result = esc_flasher_versions_s::RESULT_MISMATCH;
+				}
+				if (!same_versions) {
+					esc_flasher_versions.result = esc_flasher_versions_s::RESULT_MISMATCH;
+				}
+
+				if (same_type) {
+					// If all types agree, set param
+					_param_esc_type.set((int32_t)type);
+					_param_esc_type.commit_no_notification();
+					PX4_INFO("ESC Type detected: %s, saving to param ESC_TYPE", esc_types_strings[(uint32_t)type]);
+				}
+
+				// Compare AM32 to our PX4-internal flashable AM32 version
+				esc_flasher_status_s esc_flasher_status;
+				if (_esc_flasher_status_sub.copy(&esc_flasher_status)) {
+					if ((esc_flasher_versions.result == esc_flasher_versions_s::RESULT_SUCCESS) &&
+						(type == ESCType::AM32 || type == ESCType::AM32_Old)) {
+
+#if 0
+						// Debug testing hack
+						// REMOVE when done
+						// First bootup, give false version so it thinks it needs to update
+						if (get_esc_info_debug == 0) {
+							esc_versions.versions_minor[0]--;
+							get_esc_info_debug = 1;
+						}
+						// REMOVE WHEN DONE
+						// REMOVE WHEN DONE
+						//
+#endif
+
+						if (esc_flasher_versions.versions_major[0] != esc_flasher_status.version_major ||
+							esc_flasher_versions.versions_minor[0] != esc_flasher_status.version_minor) {
+							// The fw version of AM32 detected on the ESCs does not match our internal flashable version
+							esc_flasher_versions.result = esc_flasher_versions_s::RESULT_NEEDS_UPDATE;
+						}
+					}
+				}
+
+				esc_flasher_versions.timestamp = hrt_absolute_time();
+
+				if (!_broken_esc_flag) {
+					_esc_flasher_versions_pub.publish(esc_flasher_versions);
+				}
+
+				get_esc_info_state = 11;
+			}
+				break;
+			case 11:
+				// Done, return to idle state
+				get_esc_info_state = 0;
+				get_esc_info_start = 0;
+
+				break;
+		}
+	}
+}
+
 void DShot::update_params()
 {
 	parameter_update_s pupdate;
@@ -633,50 +1265,41 @@ void DShot::update_params()
 			_mixing_output.minValue(i) = DSHOT_MIN_THROTTLE;
 		}
 	}
+
+	_esc_type = (ESCType)_param_esc_type.get();
 }
 
 int DShot::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];
 
+	if (!strcmp(verb, "telemetry")) {
+		if (argc > 1) {
+			// telemetry can be requested before the module is started
+			strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1);
+			_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
+			_request_telemetry_init.store(true);
+		}
+
+		return 0;
+	}
+
 	int motor_index = -1; // select motor index, default: -1=all
 	int myoptind = 1;
-	bool swap_rxtx = false;
-	const char *device_name = nullptr;
 	int ch;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "m:xd:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'm':
 			motor_index = strtol(myoptarg, nullptr, 10) - 1;
 			break;
 
-		case 'x':
-			swap_rxtx = true;
-			break;
-
-		case 'd':
-			device_name = myoptarg;
-			break;
-
 		default:
 			return print_usage("unrecognized flag");
 		}
 	}
 
-	if (!strcmp(verb, "telemetry")) {
-		if (device_name) {
-			// telemetry can be requested before the module is started
-			strncpy(_telemetry_device, device_name, sizeof(_telemetry_device) - 1);
-			_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
-			_telemetry_swap_rxtx = swap_rxtx;
-			_request_telemetry_init.store(true);
-		}
-
-		return 0;
-	}
-
 	struct VerbCommand {
 		const char *name;
 		dshot_command_t command;
@@ -707,6 +1330,26 @@ int DShot::custom_command(int argc, char *argv[])
 		}
 	}
 
+	if (!strcmp(verb, "esc_info")) {
+		if (!is_running()) {
+			PX4_ERR("module not running");
+			return -1;
+		}
+
+		if (motor_index == -1) {
+			PX4_ERR("No motor index specified");
+			return -1;
+		}
+
+		if (!get_instance()->telemetry_enabled()) {
+			PX4_ERR("Telemetry is not enabled, but required to get ESC info");
+			return -1;
+		}
+
+		get_instance()->retrieve_and_print_esc_info_thread_safe(motor_index);
+		return 0;
+	}
+
 	if (!is_running()) {
 		int ret = DShot::task_spawn(argc, argv);
 
@@ -724,9 +1367,6 @@ int DShot::print_status()
 	PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
 	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
 	perf_print_counter(_cycle_perf);
-	perf_print_counter(_bdshot_rpm_perf);
-	perf_print_counter(_dshot_telem_perf);
-
 	_mixing_output.printStatus();
 
 	if (_telemetry) {
@@ -773,8 +1413,7 @@ After saving, the reversed direction will be regarded as the normal one. So to r
 	PRINT_MODULE_USAGE_COMMAND("start");
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<device>", "UART device", false);
-	PRINT_MODULE_USAGE_PARAM_FLAG('x', "Swap RX/TX pins", true);
+	PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);
 
 	// DShot commands
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction");
@@ -798,6 +1437,9 @@ After saving, the reversed direction will be regarded as the normal one. So to r
 	PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5");
 	PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
 
+	PRINT_MODULE_USAGE_COMMAND_DESCR("esc_info", "Request ESC information");
+	PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false);
+
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h
index 8137e461e2..4e0728af87 100644
--- a/src/drivers/dshot/DShot.h
+++ b/src/drivers/dshot/DShot.h
@@ -42,6 +42,11 @@
 
 #include "DShotTelemetry.h"
 
+#include <uORB/topics/esc_flasher_request.h>
+#include <uORB/topics/esc_flasher_request_ack.h>
+#include <uORB/topics/esc_flasher_status.h>
+#include <uORB/topics/esc_flasher_versions.h>
+
 using namespace time_literals;
 
 #if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
@@ -76,6 +81,8 @@ public:
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
+	void retrieve_and_print_esc_info_thread_safe(const int motor_index);
+
 	/**
 	 * Send a dshot command to one or all motors
 	 * This is expected to be called from another thread.
@@ -93,6 +100,9 @@ public:
 	bool updateOutputs(uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated);// override;
+
 private:
 
 	/** Disallow copy construction and move assignment. */
@@ -121,7 +131,7 @@ private:
 
 	void enable_dshot_outputs(const bool enabled);
 
-	void init_telemetry(const char *device, bool swap_rxtx);
+	void init_telemetry(const char *device);
 
 	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm);
 
@@ -129,6 +139,8 @@ private:
 
 	int handle_new_bdshot_erpm(void);
 
+	int request_esc_info();
+
 	void Run() override;
 
 	void update_params();
@@ -137,7 +149,13 @@ private:
 
 	void handle_vehicle_commands();
 
-	uint16_t convert_output_to_3d_scaling(uint16_t output);
+	void handle_esc_flasher_requests();
+
+	void run_get_esc_info();
+
+	int retrieve_and_print_esc_info_non_blocking(const int motor_index);
+
+	int retrieve_and_print_esc_info_check_result(uint8_t* fw_ver_major, uint8_t* fw_ver_minor);
 
 	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};
@@ -147,31 +165,78 @@ private:
 	uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
 
 	static char _telemetry_device[20];
-	static bool _telemetry_swap_rxtx;
 	static px4::atomic_bool _request_telemetry_init;
 
 	px4::atomic<Command *> _new_command{nullptr};
 
+	px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
 
 	bool _outputs_initialized{false};
 	bool _outputs_on{false};
+	bool _waiting_for_esc_info{false};
 	bool _bidirectional_dshot_enabled{false};
 
+	const char *esc_type_unknown = "Unknown";
+	const char *esc_type_am32 = "AM32";
+	const char *esc_type_blheli32 = "BLHeli32";
+	const char *esc_type_am32_old = "AM32_Old";
+	const char *esc_type_bluejay = "BlueJay";
+
+	const char* esc_types_strings[5] = {esc_type_unknown, esc_type_am32, esc_type_blheli32, esc_type_am32_old, esc_type_bluejay};
+
+	enum class ESCType {
+		Unknown = 0,
+		AM32 = 1,
+		BLHELI32 = 2,
+		AM32_Old = 3,
+		BlueJay = 4
+	};
+	ESCType _esc_type{ESCType::Unknown};
+	ESCType _esc_type_temp{ESCType::Unknown};
+
+	esc_flasher_request_s _esc_flasher_request{0};
+	esc_flasher_request_ack_s _esc_flasher_response{0};
+	uint32_t esc_flasher_esc_info_state{0};
+	uint32_t esc_flasher_esc_info_motor_index;
+	uint32_t esc_flasher_flashing_state{0};
+	DShotTelemetry::OutputBuffer esc_flasher_output_buffer{};
+
 	static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 	uint32_t _output_mask{0};
 
 	int _num_motors{0};
+	uint32_t _broken_esc{0};
+	bool _broken_esc_flag{0};
+
+	// run_get_esc_info variables
+	struct EscInfoSave {
+		ESCType type;
+		uint8_t fw_major;
+		uint8_t fw_minor;
+	};
+
+	EscInfoSave _esc_info_save[esc_status_s::CONNECTED_ESC_MAX];
+
+	uint64_t get_esc_info_time{0};
+	uint32_t get_esc_info_state{0};
+	uint32_t get_esc_info_motor_index{0};
+	uint32_t get_esc_info_start{0};
+	uint32_t get_esc_info_boot{0};
+	uint32_t get_esc_info_tries{0};
+	//uint32_t get_esc_info_debug{0};
 
 	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-	perf_counter_t	_bdshot_rpm_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot rpm")};
-	perf_counter_t	_dshot_telem_perf{perf_alloc(PC_COUNT, MODULE_NAME": dshot telem")};
 
 	Command _current_command{};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Subscription _esc_flasher_request_sub{ORB_ID(esc_flasher_request)};
+	uORB::Subscription _esc_flasher_status_sub{ORB_ID(esc_flasher_status)};
+	uORB::Publication<esc_flasher_request_ack_s> _esc_flasher_request_ack_pub{ORB_ID(esc_flasher_request_ack)};
 	uint16_t _esc_status_counter{0};
+	uORB::Publication<esc_flasher_versions_s> _esc_flasher_versions_pub{ORB_ID(esc_flasher_versions)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
@@ -179,6 +244,7 @@ private:
 		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
 		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
-		(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
+		(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable,
+		(ParamInt<px4::params::ESC_TYPE>) _param_esc_type
 	)
 };
diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp
index 0567759226..33f0b328ba 100644
--- a/src/drivers/dshot/DShotTelemetry.cpp
+++ b/src/drivers/dshot/DShotTelemetry.cpp
@@ -46,38 +46,53 @@ using namespace time_literals;
 
 DShotTelemetry::~DShotTelemetry()
 {
-	_uart.close();
+	deinit();
 }
 
-int DShotTelemetry::init(const char *port, bool swap_rxtx)
+int DShotTelemetry::init(const char *uart_device)
 {
-	if (!_uart.setPort(port)) {
-		PX4_ERR("Error configuring port %s", port);
-		return PX4_ERROR;
+	deinit();
+	_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
+
+	if (_uart_fd < 0) {
+		PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno);
+		return -errno;
 	}
 
-	if (!_uart.setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE)) {
-		PX4_ERR("Error setting baudrate on %s", port);
-		return PX4_ERROR;
+	_num_timeouts = 0;
+	_num_successful_responses = 0;
+	_current_motor_index_request = -1;
+	return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
+}
+
+void DShotTelemetry::deinit()
+{
+	if (_uart_fd >= 0) {
+		close(_uart_fd);
+		_uart_fd = -1;
+	}
+}
+
+int DShotTelemetry::redirectOutput(OutputBuffer &buffer)
+{
+	if (expectingData()) {
+		// Error: cannot override while we already expect data
+		return -EBUSY;
 	}
 
-	if (swap_rxtx) {
-		if (!_uart.setSwapRxTxMode()) {
-			PX4_ERR("Error swapping TX/RX");
-			return PX4_ERROR;
-		}
-	}
-
-	if (! _uart.open()) {
-		PX4_ERR("Error opening %s", port);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
+	_current_motor_index_request = buffer.motor_index;
+	_current_request_start = hrt_absolute_time();
+	_redirect_output = &buffer;
+	_redirect_output->buf_pos = 0;
+	return 0;
 }
 
 int DShotTelemetry::update(int num_motors)
 {
+	if (_uart_fd < 0) {
+		return -1;
+	}
+
 	if (_current_motor_index_request == -1) {
 		// nothing in progress, start a request
 		_current_motor_index_request = 0;
@@ -87,9 +102,10 @@ int DShotTelemetry::update(int num_motors)
 	}
 
 	// read from the uart. This must be non-blocking, so check first if there is data available
-	int bytes_available = _uart.bytesAvailable();
+	int bytes_available = 0;
+	int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available);
 
-	if (bytes_available <= 0) {
+	if (ret != 0 || bytes_available <= 0) {
 		// no data available. Check for a timeout
 		const hrt_abstime now = hrt_absolute_time();
 
@@ -111,12 +127,13 @@ int DShotTelemetry::update(int num_motors)
 		return -1;
 	}
 
-	uint8_t buf[ESC_FRAME_SIZE];
-	int bytes = _uart.read(buf, sizeof(buf));
+	const int buf_length = ESC_FRAME_SIZE;
+	uint8_t buf[buf_length];
 
-	int ret = -1;
+	int num_read = read(_uart_fd, buf, buf_length);
+	ret = -1;
 
-	for (int i = 0; i < bytes && ret == -1; ++i) {
+	for (int i = 0; i < num_read && ret == -1; ++i) {
 		if (_redirect_output) {
 			_redirect_output->buffer[_redirect_output->buf_pos++] = buf[i];
 
@@ -166,9 +183,6 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
 				  _latest_data.erpm);
 			++_num_successful_responses;
 			successful_decoding = true;
-
-		} else {
-			++_num_checksum_errors;
 		}
 
 		return true;
@@ -181,30 +195,32 @@ void DShotTelemetry::printStatus() const
 {
 	PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
 	PX4_INFO("Number of timeouts: %i", _num_timeouts);
-	PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
 }
 
+uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)
+{
+	uint8_t crc_u = crc ^ crc_seed;
+
+	for (int i = 0; i < 8; ++i) {
+		crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
+	}
+
+	return crc_u;
+}
+
+
 uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
 {
-	auto update_crc8 = [](uint8_t crc, uint8_t crc_seed) {
-		uint8_t crc_u = crc ^ crc_seed;
-
-		for (int i = 0; i < 8; ++i) {
-			crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
-		}
-
-		return crc_u;
-	};
-
 	uint8_t crc = 0;
 
 	for (int i = 0; i < len; ++i) {
-		crc = update_crc8(buf[i], crc);
+		crc = updateCrc8(buf[i], crc);
 	}
 
 	return crc;
 }
 
+
 void DShotTelemetry::requestNextMotor(int num_motors)
 {
 	_current_motor_index_request = (_current_motor_index_request + 1) % num_motors;
@@ -237,6 +253,7 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
 		BLHELI32,
 		KissV1,
 		KissV2,
+		AM32
 	};
 	ESCVersionInfo version;
 	int packet_length;
@@ -250,8 +267,21 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
 		packet_length = esc_info_size_kiss_v2;
 
 	} else {
-		version = ESCVersionInfo::KissV1;
-		packet_length = esc_info_size_kiss_v1;
+		if (buffer.buf_pos > esc_info_size_kiss_v1) {
+			version = ESCVersionInfo::AM32;
+			packet_length = esc_info_size_am32;
+		}
+		else {
+			version = ESCVersionInfo::KissV1;
+			packet_length = esc_info_size_kiss_v1;
+		}
+	}
+
+	// Check for AM32 but with bad data
+	if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
+		version = ESCVersionInfo::AM32;
+		packet_length = esc_info_size_am32;
+		PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
 	}
 
 	if (buffer.buf_pos != packet_length) {
@@ -264,116 +294,337 @@ void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
 		return;
 	}
 
-	uint8_t esc_firmware_version = 0;
-	uint8_t esc_firmware_subversion = 0;
-	uint8_t esc_type = 0;
-
-	switch (version) {
-	case ESCVersionInfo::KissV1:
-		esc_firmware_version = data[12];
-		esc_firmware_subversion = (data[13] & 0x1f) + 97;
-		esc_type = (data[13] & 0xe0) >> 5;
-		break;
-
-	case ESCVersionInfo::KissV2:
-	case ESCVersionInfo::BLHELI32:
-		esc_firmware_version = data[13];
-		esc_firmware_subversion = data[14];
-		esc_type = data[15];
-		break;
-	}
-
-	const char *esc_type_str = "";
-
-	switch (version) {
-	case ESCVersionInfo::KissV1:
-	case ESCVersionInfo::KissV2:
-		switch (esc_type) {
-		case 1: esc_type_str = "KISS8A";
-			break;
-
-		case 2: esc_type_str = "KISS16A";
-			break;
-
-		case 3: esc_type_str = "KISS24A";
-			break;
-
-		case 5: esc_type_str = "KISS Ultralite";
-			break;
-
-		default: esc_type_str = "KISS (unknown)";
-			break;
+	if (version == ESCVersionInfo::AM32) {
+		// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
+		if (buffer.buf_pos < packet_length) {
+			PX4_ERR("Not enough data received");
+			return;
 		}
 
-		break;
-
-	case ESCVersionInfo::BLHELI32: {
-			char *esc_type_mutable = (char *)(data + 31);
-			esc_type_mutable[32] = 0;
-			esc_type_str = esc_type_mutable;
-		}
-		break;
-	}
-
-	PX4_INFO("ESC Type: %s", esc_type_str);
-
-	PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x",
-		 data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8],
-		 data[9], data[10], data[11]);
-
-	switch (version) {
-	case ESCVersionInfo::KissV1:
-	case ESCVersionInfo::KissV2:
-		PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100,
-			 (char)esc_firmware_subversion);
-		break;
-
-	case ESCVersionInfo::BLHELI32:
-		PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion);
-		break;
-	}
-
-	if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) {
-		PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal");
-		PX4_INFO("3D Mode: %s", data[17] ? "on" : "off");
-	}
-
-	if (version == ESCVersionInfo::BLHELI32) {
-		uint8_t setting = data[18];
-
-		switch (setting) {
-		case 0:
-			PX4_INFO("Low voltage Limit: off");
-			break;
-
-		case 255:
-			PX4_INFO("Low voltage Limit: unsupported");
-			break;
-
-		default:
-			PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10);
-			break;
+		uint8_t eeprom_version = data[1];
+		if (eeprom_version != 2) {
+			PX4_ERR("Unsupported EEPROM version from ESC");
+			return;
 		}
 
-		setting = data[19];
+		AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
 
-		switch (setting) {
-		case 0:
-			PX4_INFO("Current Limit: off");
+		char firmware_name[sizeof(esc_info->firmware_name) + 1];
+		memcpy(firmware_name, esc_info->firmware_name, sizeof(firmware_name));
+		firmware_name[sizeof(esc_info->firmware_name)] = 0;
+
+		PX4_INFO("Received ESC_INFO packet from motor %d:", buffer.motor_index + 1);
+
+		PX4_INFO("     ESC firmware version: %d.%d - ESC firmware name: %s", esc_info->version.major, esc_info->version.minor, firmware_name);
+
+		if (esc_info->stuck_rotor_protection) PX4_INFO("     Stuck rotor protection is ENABLED");
+		else PX4_INFO("     Stuck rotor protection is DISABLED");
+
+		if (esc_info->telemetry_on_interval) PX4_INFO("     Auto-Telemetry interval is ENABLED");
+		else PX4_INFO("     Auto-Telemetry interval is DISABLED");
+
+		if (esc_info->bi_direction) PX4_INFO("     Bi-Directional DShot is ENABLED");
+		else PX4_INFO("     Bi-Directional DShot is DISABLED");
+	}
+	else {
+		uint8_t esc_firmware_version = 0;
+		uint8_t esc_firmware_subversion = 0;
+		uint8_t esc_type = 0;
+
+		switch (version) {
+		case ESCVersionInfo::KissV1:
+			esc_firmware_version = data[12];
+			esc_firmware_subversion = (data[13] & 0x1f) + 97;
+			esc_type = (data[13] & 0xe0) >> 5;
 			break;
 
-		case 255:
-			PX4_INFO("Current Limit: unsupported");
-			break;
-
-		default:
-			PX4_INFO("Current Limit: %d A", setting);
+		case ESCVersionInfo::KissV2:
+		case ESCVersionInfo::BLHELI32:
+			esc_firmware_version = data[13];
+			esc_firmware_subversion = data[14];
+			esc_type = data[15];
 			break;
+		case ESCVersionInfo::AM32:
+			// AM32 is handled above, should not get to this case
+			return;
 		}
 
-		for (int i = 0; i < 4; ++i) {
-			setting = data[i + 20];
-			PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
+		const char *esc_type_str = "";
+
+		switch (version) {
+		case ESCVersionInfo::KissV1:
+		case ESCVersionInfo::KissV2:
+			switch (esc_type) {
+			case 1: esc_type_str = "KISS8A";
+				break;
+
+			case 2: esc_type_str = "KISS16A";
+				break;
+
+			case 3: esc_type_str = "KISS24A";
+				break;
+
+			case 5: esc_type_str = "KISS Ultralite";
+				break;
+
+			default: esc_type_str = "KISS (unknown)";
+				break;
+			}
+
+			break;
+
+		case ESCVersionInfo::BLHELI32: {
+				char *esc_type_mutable = (char *)(data + 31);
+				esc_type_mutable[32] = 0;
+				esc_type_str = esc_type_mutable;
+			}
+			break;
+		case ESCVersionInfo::AM32:
+			// AM32 is handled above, should not get to this case
+			return;
+		}
+
+		PX4_INFO("ESC Type: %s", esc_type_str);
+
+		PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x",
+			data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8],
+			data[9], data[10], data[11]);
+
+		switch (version) {
+		case ESCVersionInfo::KissV1:
+		case ESCVersionInfo::KissV2:
+			PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100,
+				(char)esc_firmware_subversion);
+			break;
+
+		case ESCVersionInfo::BLHELI32:
+			PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion);
+			break;
+		case ESCVersionInfo::AM32:
+			// AM32 is handled above, should not get to this case
+			return;
+		}
+
+		if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) {
+			PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal");
+			PX4_INFO("3D Mode: %s", data[17] ? "on" : "off");
+		}
+
+		if (version == ESCVersionInfo::BLHELI32) {
+			uint8_t setting = data[18];
+
+			switch (setting) {
+			case 0:
+				PX4_INFO("Low voltage Limit: off");
+				break;
+
+			case 255:
+				PX4_INFO("Low voltage Limit: unsupported");
+				break;
+
+			default:
+				PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10);
+				break;
+			}
+
+			setting = data[19];
+
+			switch (setting) {
+			case 0:
+				PX4_INFO("Current Limit: off");
+				break;
+
+			case 255:
+				PX4_INFO("Current Limit: unsupported");
+				break;
+
+			default:
+				PX4_INFO("Current Limit: %d A", setting);
+				break;
+			}
+
+			for (int i = 0; i < 4; ++i) {
+				setting = data[i + 20];
+				PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off");
+			}
 		}
 	}
 }
+
+int DShotTelemetry::decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor)
+{
+	// Return 0 for AM32 success
+	// Return -1 for not enough data (unknown)
+	// Return -2 for BLHeli success
+
+	static constexpr int version_position = 12;
+	const uint8_t *data = buffer.buffer;
+
+	if (buffer.buf_pos < version_position) {
+		PX4_ERR("Not enough data received");
+		return -1;
+	}
+
+	enum class ESCVersionInfo {
+		BLHELI32,
+		KissV1,
+		KissV2,
+		AM32
+	};
+	ESCVersionInfo version;
+	int packet_length;
+
+	if (data[version_position] == 254) {
+		version = ESCVersionInfo::BLHELI32;
+		packet_length = esc_info_size_blheli32;
+
+	} else if (data[version_position] == 255) {
+		version = ESCVersionInfo::KissV2;
+		packet_length = esc_info_size_kiss_v2;
+
+	} else {
+		if (buffer.buf_pos > esc_info_size_kiss_v1) {
+			version = ESCVersionInfo::AM32;
+			packet_length = esc_info_size_am32;
+		}
+		else {
+			version = ESCVersionInfo::KissV1;
+			packet_length = esc_info_size_kiss_v1;
+		}
+	}
+
+	// Check for AM32 but with bad data
+	if (version != ESCVersionInfo::AM32 && buffer.buf_pos == esc_info_size_am32) {
+		version = ESCVersionInfo::AM32;
+		packet_length = esc_info_size_am32;
+		PX4_WARN("Packet length mismatch (%i != %i), reverting to AM32 decoding", buffer.buf_pos, packet_length);
+	}
+
+	if (buffer.buf_pos != packet_length) {
+		PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length);
+		return -1;
+	}
+
+	if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) {
+		PX4_ERR("Checksum mismatch");
+		return -1;
+	}
+
+	if (version == ESCVersionInfo::AM32) {
+		// AM32 2.18+ ESC_INFO packet sends bytes 0-47 of its EEPROM struct
+		if (buffer.buf_pos < packet_length) {
+			PX4_ERR("Not enough data received");
+			return -1;
+		}
+
+		uint8_t eeprom_version = data[1];
+		if (eeprom_version != 2) {
+			PX4_ERR("Unsupported EEPROM version from ESC");
+			return -1;
+		}
+
+		AM32_EEprom_st* esc_info = (AM32_EEprom_st*)buffer.buffer;
+
+		*fw_version_major = esc_info->version.major;
+		*fw_version_minor = esc_info->version.minor;
+		return 0;
+	}
+	else {
+		//PX4_ERR("Unsupported ESC type, this function only supports AM32");
+
+		switch (version) {
+		case ESCVersionInfo::KissV1:
+			*fw_version_major = data[12];
+			*fw_version_minor = (data[13] & 0x1f) + 97;
+			break;
+		case ESCVersionInfo::KissV2:
+		case ESCVersionInfo::BLHELI32:
+			*fw_version_major = data[13];
+			*fw_version_minor = data[14];
+			break;
+		case ESCVersionInfo::AM32:
+			// AM32 is handled above, should not get to this case
+			return -1;
+		}
+
+		return -2;
+	}
+}
+
+int DShotTelemetry::setBaudrate(unsigned baud)
+{
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+	default:
+		return -EINVAL;
+	}
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_uart_fd, &uart_config);
+
+	//
+	// Input flags - Turn off input processing
+	//
+	// convert break to null byte, no CR to NL translation,
+	// no NL to CR translation, don't mark parity errors or breaks
+	// no input parity check, don't strip high bit off,
+	// no XON/XOFF software flow control
+	//
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
+				 INLCR | PARMRK | INPCK | ISTRIP | IXON);
+	//
+	// Output flags - Turn off output processing
+	//
+	// no CR to NL translation, no NL to CR-NL translation,
+	// no NL to CR translation, no column 0 CR suppression,
+	// no Ctrl-D suppression, no fill characters, no case mapping,
+	// no local output processing
+	//
+	// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
+	//                     ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
+	uart_config.c_oflag = 0;
+
+	//
+	// No line processing
+	//
+	// echo off, echo newline off, canonical mode off,
+	// extended input processing off, signal chars off
+	//
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	/* no parity, one stop bit, disable flow control */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		return -errno;
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		return -errno;
+	}
+
+	if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
+		return -errno;
+	}
+
+	return 0;
+}
+
diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h
index ccbe7b1ce0..abf1523d17 100644
--- a/src/drivers/dshot/DShotTelemetry.h
+++ b/src/drivers/dshot/DShotTelemetry.h
@@ -33,9 +33,55 @@
 
 #pragma once
 
-#include <px4_platform_common/Serial.hpp>
 #include <drivers/drv_hrt.h>
 
+typedef struct {
+	uint8_t reserved_0; //0
+	uint8_t eeprom_version; //1
+	uint8_t reserved_1; //2
+	struct {
+		uint8_t major; //3
+		uint8_t minor; //4
+	} version;
+	char firmware_name[12]; //5-16
+	uint8_t dir_reversed; // 17
+	uint8_t bi_direction; // 18
+	uint8_t use_sine_start; // 19
+	uint8_t comp_pwm; // 20
+	uint8_t variable_pwm; // 21
+	uint8_t stuck_rotor_protection; // 22
+	uint8_t advance_level; // 23
+	uint8_t pwm_frequency; // 24
+	uint8_t startup_power; // 25
+	uint8_t motor_kv; // 26
+	uint8_t motor_poles; // 27
+	uint8_t brake_on_stop; // 28
+	uint8_t stall_protection; // 29
+	uint8_t beep_volume; // 30
+	uint8_t telemetry_on_interval; // 31
+	struct {
+		uint8_t low_threshold; // 32
+		uint8_t high_threshold; // 33
+		uint8_t neutral; // 34
+		uint8_t dead_band; // 35
+	} servo;
+	uint8_t low_voltage_cut_off; // 36
+	uint8_t low_cell_volt_cutoff; // 37
+	uint8_t rc_car_reverse; // 38
+	uint8_t use_hall_sensors; // 39
+	uint8_t sine_mode_changeover_thottle_level; // 40
+	uint8_t drag_brake_strength; // 41
+	uint8_t driving_brake_strength; // 42
+	struct {
+		uint8_t temperature; // 43
+		uint8_t current; // 44
+	} limits;
+	uint8_t sine_mode_power; // 45
+	uint8_t input_type; // 46
+	uint8_t auto_advance; // 47
+	uint8_t crc; // 48
+} AM32_EEprom_st;
+
 class DShotTelemetry
 {
 public:
@@ -51,6 +97,7 @@ public:
 	static constexpr int esc_info_size_blheli32 = 64;
 	static constexpr int esc_info_size_kiss_v1 = 15;
 	static constexpr int esc_info_size_kiss_v2 = 21;
+	static constexpr int esc_info_size_am32 = 49;
 	static constexpr int max_esc_info_size = esc_info_size_blheli32;
 
 	struct OutputBuffer {
@@ -61,7 +108,9 @@ public:
 
 	~DShotTelemetry();
 
-	int init(const char *uart_device, bool swap_rxtx);
+	int init(const char *uart_device);
+
+	void deinit();
 
 	/**
 	 * Read telemetry from the UART (non-blocking) and handle timeouts.
@@ -70,6 +119,16 @@ public:
 	 */
 	int update(int num_motors);
 
+	/**
+	 * Redirect everything that is read into a different buffer.
+	 * Future calls to @update will write to that instead of an internal buffer, until @update returns
+	 * a value different from -1. No decoding is done.
+	 * The caller must ensure the buffer exists until that point.
+	 * @param buffer
+	 * @return 0 on success <0 on error
+	 */
+	int redirectOutput(OutputBuffer &buffer);
+
 	bool redirectActive() const { return _redirect_output != nullptr; }
 
 	/**
@@ -89,9 +148,18 @@ public:
 
 	static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer);
 
+	static int decodeEscInfoPacketFwVersion(const OutputBuffer &buffer, uint8_t* fw_version_major, uint8_t* fw_version_minor);
+
 private:
 	static constexpr int ESC_FRAME_SIZE = 10;
 
+	/**
+	 * set the Baudrate
+	 * @param baud
+	 * @return 0 on success, <0 on error
+	 */
+	int setBaudrate(unsigned baud);
+
 	void requestNextMotor(int num_motors);
 
 	/**
@@ -102,10 +170,10 @@ private:
 	 */
 	bool decodeByte(uint8_t byte, bool &successful_decoding);
 
+	static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
 	static uint8_t crc8(const uint8_t *buf, uint8_t len);
 
-	device::Serial _uart {};
-
+	int _uart_fd{-1};
 	uint8_t _frame_buffer[ESC_FRAME_SIZE];
 	int _frame_position{0};
 
@@ -119,5 +187,4 @@ private:
 	// statistics
 	int _num_timeouts{0};
 	int _num_successful_responses{0};
-	int _num_checksum_errors{0};
 };