Fixed formatting

src/drivers/actuators/voxl_esc/CMakeLists.txt

@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		crc16.c
 		crc16.h
-
 		voxl_esc_serial.cpp
 		voxl_esc_serial.hpp
 		voxl_esc.cpp

src/drivers/actuators/voxl_esc/voxl_esc.cpp

@@ -94,24 +94,145 @@ VoxlEsc::~VoxlEsc()
 
 int VoxlEsc::init()
 {
+	PX4_INFO("VOXL_ESC: Starting VOXL ESC driver");
 
 	/* Getting initial parameter values */
 	int ret = update_params();
 
 	if (ret != OK) {
+		PX4_ERR("VOXL_ESC: Failed to update params during init");
 		return ret;
 	}
 
 	_uart_port = new VoxlEscSerial();
-	memset(&_esc_chans, 0x00, sizeof(_esc_chans));
 
-	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
-		_version_info[esc_id].sw_version = UINT16_MAX;
-		_version_info[esc_id].hw_version = UINT16_MAX;
-		_version_info[esc_id].id = esc_id;
+	if (!_uart_port) {
+		PX4_ERR("VOXL_ESC: Failed allocating VoxlEscSerial");
+		return -1;
 	}
 
-	//get_instance()->ScheduleOnInterval(10000); //100hz
+	// Open serial port
+	PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %d", _device, (int) _parameters.baud_rate);
+
+	if (!_uart_port->is_open()) {
+		if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) {
+			PX4_INFO("VOXL_ESC: Successfully opened UART ESC device");
+
+		} else {
+			PX4_ERR("VOXL_ESC: Failed openening device");
+			return -1;
+		}
+	}
+
+	// Reset output channel values
+	memset(&_esc_chans, 0x00, sizeof(_esc_chans));
+
+	//reset the ESC version info before requesting
+	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
+		_version_info[esc_id].sw_version = 0;  //invalid
+		_version_info[esc_id].hw_version = 0;  //invalid
+		_version_info[esc_id].id         = esc_id;
+	}
+
+	// Detect ESCs
+	PX4_INFO("VOXL_ESC: Detecting ESCs...");
+	qc_esc_packet_init(&_fb_packet);
+
+	//request extended version info from each ESC and wait for reply
+	for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
+		Command cmd;
+		cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
+
+		if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
+			PX4_ERR("VOXL_ESC: Could not write version request packet to UART port");
+			return -1;
+		}
+
+		hrt_abstime t_request = hrt_absolute_time();
+		hrt_abstime t_timeout = 50000; //50ms timeout for version info response
+		bool got_response     = false;
+
+		while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) {
+			px4_usleep(100); //sleep a bit while waiting for ESC to respond
+
+			int nread = _uart_port->uart_read(_read_buf, sizeof(_read_buf));
+
+			for (int i = 0; i < nread; i++) {
+				int16_t parse_ret = qc_esc_packet_process_char(_read_buf[i], &_fb_packet);
+
+				if (parse_ret > 0) {
+					hrt_abstime response_time = hrt_elapsed_time(&t_request);
+					//PX4_INFO("got packet of length %i",ret);
+					_rx_packet_count++;
+					uint8_t packet_type = qc_esc_packet_get_type(&_fb_packet);
+					uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet);
+
+					if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) {
+						QC_ESC_EXTENDED_VERSION_INFO ver;
+						memcpy(&ver, _fb_packet.buffer, packet_size);
+
+						PX4_INFO("VOXL_ESC: \tESC ID     : %i", ver.id);
+						PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version));
+
+						uint8_t *u = &ver.unique_id[0];
+						PX4_INFO("VOXL_ESC: \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("VOXL_ESC: \tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
+						PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
+						PX4_INFO("VOXL_ESC: \tReply time : %d us", (int)response_time);
+						PX4_INFO("VOXL_ESC:");
+
+						if (ver.id == esc_id) {
+							memcpy(&_version_info[esc_id], &ver, sizeof(ver));
+							got_response = true;
+						}
+					}
+				}
+			}
+		}
+
+		if (!got_response) {
+			PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id);
+		}
+	}
+
+	//check the SW version of the ESCs
+	bool esc_detection_fault = false;
+
+	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
+		if (_version_info[esc_id].sw_version == 0) {
+			PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id);
+			esc_detection_fault = true;
+		}
+	}
+
+	//check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*"
+	for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
+		if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) {
+			PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash:  (%.12s) != (%.12s)",
+				esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version);
+			esc_detection_fault = true;
+		}
+	}
+
+	//if firmware version is equal or greater than VOXL_ESC_EXT_RPM, ESC packet with extended rpm range is supported. use it
+	_extended_rpm      = true;
+
+	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
+		if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) {
+			_extended_rpm = false;
+		}
+	}
+
+	PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm);
+
+	if (esc_detection_fault) {
+		PX4_ERR("VOXL_ESC: Critical error during ESC initialization. Exiting");
+		return -1;
+	}
+
+	PX4_INFO("VOXL_ESC: All ESCs successfully detected");
 
 	ScheduleNow();
 
@@ -155,45 +276,48 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
 	param_get(param_find("VOXL_ESC_VLOG"),    &params->verbose_logging);
 	param_get(param_find("VOXL_ESC_PUB_BST"), &params->publish_battery_status);
 
+	param_get(param_find("VOXL_ESC_T_WARN"), &params->esc_warn_temp_threshold);
+	param_get(param_find("VOXL_ESC_T_OVER"), &params->esc_over_temp_threshold);
+
 	if (params->rpm_min >= params->rpm_max) {
-		PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN.  Please verify parameters.");
 		params->rpm_min = 0;
 		ret = PX4_ERROR;
 	}
 
 	if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) {
-		PX4_ERR("Invalid parameter VOXL_ESC_T_PERC.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_PERC.  Please verify parameters.");
 		params->turtle_motor_percent = 0;
 		ret = PX4_ERROR;
 	}
 
 	if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) {
-		PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_DEAD.  Please verify parameters.");
 		params->turtle_motor_deadband = 0;
 		ret = PX4_ERROR;
 	}
 
 	if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) {
-		PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_EXPO.  Please verify parameters.");
 		params->turtle_motor_expo = 0;
 		ret = PX4_ERROR;
 	}
 
 	if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) {
-		PX4_ERR("Invalid parameter VOXL_ESC_T_MINF.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_MINF.  Please verify parameters.");
 		params->turtle_stick_minf = 0.0f;
 		ret = PX4_ERROR;
 	}
 
 	if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) {
-		PX4_ERR("Invalid parameter VOXL_ESC_T_COSP.  Please verify parameters.");
+		PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_COSP.  Please verify parameters.");
 		params->turtle_cosphi = 0.0f;
 		ret = PX4_ERROR;
 	}
 
 	for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) {
 		if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) {
-			PX4_ERR("Invalid parameter VOXL_ESC_FUNCX.  Only supports motors 1-4.  Please verify parameters.");
+			PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX.  Only supports motors 1-4.  Please verify parameters.");
 			params->function_map[i] = 0;
 			ret = PX4_ERROR;
 
@@ -211,7 +335,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
 		if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED ||
 		    params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) ||
 		    params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_ERR("Invalid parameter VOXL_ESC_MOTORX.  Please verify parameters.");
+			PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_MOTORX.  Please verify parameters.");
 			params->motor_map[i] = 0;
 			ret = PX4_ERROR;
 		}
@@ -271,23 +395,6 @@ int VoxlEsc::flush_uart_rx()
 	return 0;
 }
 
-bool VoxlEsc::check_versions_updated()
-{
-	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
-		if (_version_info[esc_id].sw_version == UINT16_MAX) { return false; }
-	}
-
-	// PX4_INFO("Got all ESC Version info!");
-	_extended_rpm = true;
-	_need_version_info = false;
-
-	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
-		if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) { _extended_rpm = false; }
-	}
-
-	return true;
-}
-
 int VoxlEsc::read_response(Command *out_cmd)
 {
 	px4_usleep(_current_cmd.resp_delay_us);
@@ -341,7 +448,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 						uint32_t voltage     = fb.voltage;
 						int32_t  current     = fb.current * 8;
 						int32_t  temperature = fb.temperature / 100;
-						PX4_INFO("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, motor_idx + 1,
+						PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id,
+							 motor_idx + 1,
 							 (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature);
 					}
 
@@ -382,6 +490,19 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 					_esc_status.timestamp = _esc_status.esc[id].timestamp;
 					_esc_status.counter++;
 
+
+					if ((_parameters.esc_over_temp_threshold > 0)
+					    && (_esc_status.esc[id].esc_temperature > _parameters.esc_over_temp_threshold)) {
+						_esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_OVER_ESC_TEMPERATURE);
+					}
+
+					//TODO: do we also issue a warning if over-temperature threshold is exceeded?
+					if ((_parameters.esc_warn_temp_threshold > 0)
+					    && (_esc_status.esc[id].esc_temperature > _parameters.esc_warn_temp_threshold)) {
+						_esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_WARN_ESC_TEMPERATURE);
+					}
+
+
 					//print ESC status just for debugging
 					/*
 					PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, CNT %d, FAIL %d",
@@ -397,30 +518,24 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
 				QC_ESC_VERSION_INFO ver;
 				memcpy(&ver, _fb_packet.buffer, packet_size);
 
-				if (_need_version_info) {
-					memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO));
-					check_versions_updated();
-					break;
-				}
-
-				PX4_INFO("ESC ID: %i", ver.id);
-				PX4_INFO("HW Version: %i", ver.hw_version);
-				PX4_INFO("SW Version: %i", ver.sw_version);
-				PX4_INFO("Unique ID: %i", (int)ver.unique_id);
+				PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id);
+				PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version);
+				PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version);
+				PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id);
 
 			} else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) {
 				QC_ESC_EXTENDED_VERSION_INFO ver;
 				memcpy(&ver, _fb_packet.buffer, packet_size);
-				PX4_INFO("\tESC ID     : %i", ver.id);
-				PX4_INFO("\tBoard      : %i", ver.hw_version);
-				PX4_INFO("\tSW Version : %i", ver.sw_version);
+				PX4_INFO("VOXL_ESC: \tESC ID     : %i", ver.id);
+				PX4_INFO("VOXL_ESC: \tBoard      : %i", ver.hw_version);
+				PX4_INFO("VOXL_ESC: \tSW Version : %i", ver.sw_version);
 
 				uint8_t *u = &ver.unique_id[0];
-				PX4_INFO("\tUnique ID  : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
+				PX4_INFO("VOXL_ESC: \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("\tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
-				PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
+				PX4_INFO("VOXL_ESC: \tFirmware   : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
+				PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
 
 			} else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) {
 				QC_ESC_FB_POWER_STATUS packet;
@@ -533,7 +648,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 	}
 
 	if (!is_running()) {
-		PX4_INFO("Not running");
+		PX4_INFO("VOXL_ESC:Not running");
 		return -1;
 
 	}
@@ -592,7 +707,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	if (!strcmp(verb, "reset")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Reset ESC: %i", esc_id);
+			PX4_INFO("VOXL_ESC: Reset ESC: %i", esc_id);
 			cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);
@@ -604,7 +719,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "version")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Request version for ESC: %i", esc_id);
+			PX4_INFO("VOXL_ESC: Request version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
 			cmd.resp_delay_us = 2000;
@@ -617,7 +732,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "version-ext")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Request extended version for ESC: %i", esc_id);
+			PX4_INFO("VOXL_ESC: Request extended version for ESC: %i", esc_id);
 			cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = true;
 			cmd.resp_delay_us = 5000;
@@ -630,7 +745,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "tone")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Request tone for ESC mask: %i", esc_id);
+			PX4_INFO("VOXL_ESC: Request tone for ESC mask: %i", esc_id);
 			cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf));
 			cmd.response = false;
 			return get_instance()->send_cmd_thread_safe(&cmd);
@@ -644,7 +759,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 		if (led_mask <= 0x0FFF) {
 			get_instance()->_led_rsc.test = true;
 			get_instance()->_led_rsc.breath_en = false;
-			PX4_INFO("Request LED control for ESCs with mask: %i", led_mask);
+			PX4_INFO("VOXL_ESC: Request LED control for ESCs with mask: %i", led_mask);
 
 			get_instance()->_esc_chans[0].led = (led_mask & 0x0007);
 			get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3;
@@ -659,7 +774,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	}  else if (!strcmp(verb, "rpm")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
+			PX4_INFO("VOXL_ESC: Request RPM for ESC ID: %i - RPM: %i", esc_id, rate);
 			int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;
 
@@ -693,8 +808,8 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;
 
-			PX4_INFO("feedback id debug: %i", id_fb);
-			PX4_INFO("Sending UART ESC RPM command %i", rate);
+			PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb);
+			PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate);
 
 			return get_instance()->send_cmd_thread_safe(&cmd);
 
@@ -705,7 +820,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "pwm")) {
 		if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) {
-			PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
+			PX4_INFO("VOXL_ESC: Request PWM for ESC ID: %i - PWM: %i", esc_id, rate);
 			int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0};
 			uint8_t id_fb = 0;
 
@@ -738,8 +853,8 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 			cmd.repeat_delay_us = repeat_delay_us;
 			cmd.print_feedback  = true;
 
-			PX4_INFO("feedback id debug: %i", id_fb);
-			PX4_INFO("Sending UART ESC power command %i", rate);
+			PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb);
+			PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate);
 
 			return get_instance()->send_cmd_thread_safe(&cmd);
 
@@ -1104,7 +1219,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 					       _extended_rpm);
 
 	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send packet");
+		PX4_ERR("VOXL_ESC: Failed to send packet");
 		return false;
 	}
 
@@ -1145,7 +1260,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 	_esc_status_pub.publish(_esc_status);
 
-	// If any extra external modal io data has been received then
+	// If any extra external voxl2 io data has been received then
 	// send it over as well
 	while (_voxl2_io_data_sub.updated()) {
 		buffer128_s io_data{};
@@ -1156,7 +1271,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		// 		 io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3],
 		// 		 io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]);
 		if (_uart_port->uart_write(io_data.data, io_data.len) != io_data.len) {
-			PX4_ERR("Failed to send modal io data to esc");
+			PX4_ERR("VOXL_ESC: Failed to send modal io data to esc");
 			return false;
 		}
 	}
@@ -1179,32 +1294,6 @@ void VoxlEsc::Run()
 
 	perf_begin(_cycle_perf);
 
-	/* Open serial port in this thread */
-	if (!_uart_port->is_open()) {
-		if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) {
-			PX4_INFO("Opened UART ESC device");
-
-		} else {
-			PX4_ERR("Failed openening device");
-			return;
-		}
-	}
-
-	/* Get ESC FW version info */
-	if (_need_version_info) {
-		for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
-			Command cmd;
-			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
-
-			if (_uart_port->uart_write(cmd.buf, cmd.len) == cmd.len) {
-				if (read_response(&_current_cmd) != 0) { PX4_ERR("Failed to parse version request response packet!"); }
-
-			} else {
-				PX4_ERR("Failed to send version request packet!");
-			}
-		}
-	}
-
 	_mixing_output.update();  //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module
 
 	/* update output status if armed */
@@ -1296,19 +1385,19 @@ void VoxlEsc::Run()
 				} else {
 					if (_current_cmd.retries == 0) {
 						_current_cmd.clear();
-						PX4_ERR("Failed to send command, errno: %i", errno);
+						PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno);
 
 					} else {
 						_current_cmd.retries--;
-						PX4_ERR("Failed to send command, errno: %i", errno);
+						PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno);
 					}
 				}
 
 				px4_usleep(_current_cmd.repeat_delay_us);
 			} while (_current_cmd.repeats-- > 0);
 
-			PX4_INFO("RX packet count: %d", (int)_rx_packet_count);
-			PX4_INFO("CRC error count: %d", (int)_rx_crc_error_count);
+			PX4_INFO("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count);
+			PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count);
 
 		} else {
 			Command *new_cmd = _pending_cmd.load();
@@ -1432,6 +1521,25 @@ int VoxlEsc::print_status()
 	return 0;
 }
 
+const char *VoxlEsc::board_id_to_name(int board_id)
+{
+	switch(board_id){
+		case 31: return "ModalAi 4-in-1 ESC V2 RevB (M0049)";
+		case 32: return "Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)";
+		case 33: return "Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)";
+		case 34: return "ModalAi 4-in-1 ESC (M0117-1)";
+		case 35: return "ModalAi I/O Expander (M0065)";
+		case 36: return "ModalAi 4-in-1 ESC (M0117-3)";
+		case 37: return "ModalAi 4-in-1 ESC (M0134-1)";
+		case 38: return "ModalAi 4-in-1 ESC (M0134-3)";
+		case 39: return "ModalAi 4-in-1 ESC (M0129-1)";
+		case 40: return "ModalAi 4-in-1 ESC (M0129-3)";
+		case 41: return "ModalAi 4-in-1 ESC (M0134-6)";
+		case 42: return "ModalAi 4-in-1 ESC (M0138-1)";
+		default: return "Unknown Board";
+	}
+}
+
 extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]);
 
 int voxl_esc_main(int argc, char *argv[])

src/drivers/actuators/voxl_esc/voxl_esc.hpp

@@ -125,7 +125,8 @@ private:
 	static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1;
 	static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2;
 
-	static constexpr uint16_t VOXL_ESC_EXT_RPM = 39;
+	static constexpr uint16_t VOXL_ESC_EXT_RPM =
+		39;                // minimum firmware version for extended RPM command support
 	static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX -
 			1;		// 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
 	static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX -
@@ -152,6 +153,8 @@ private:
 		int32_t		direction_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1};
 		int32_t		verbose_logging{0};
 		int32_t 	publish_battery_status{0};
+		int32_t 	esc_warn_temp_threshold{0};
+		int32_t 	esc_over_temp_threshold{0};
 	} voxl_esc_params_t;
 
 	struct EscChan {
@@ -203,12 +206,12 @@ private:
 
 	bool _extended_rpm{false};
 	bool _need_version_info{true};
-	QC_ESC_VERSION_INFO _version_info[4];
-	bool check_versions_updated();
+	QC_ESC_EXTENDED_VERSION_INFO _version_info[VOXL_ESC_OUTPUT_CHANNELS];
 
 	voxl_esc_params_t	_parameters;
 	int			update_params();
 	int			load_params(voxl_esc_params_t *params, ch_assign_t *map);
+	const char	*board_id_to_name(int board_id);
 
 	bool			_turtle_mode_en{false};
 	int32_t			_rpm_turtle_min{0};

src/drivers/actuators/voxl_esc/voxl_esc_params.c

@@ -231,4 +231,35 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0);
  * @min 0
  * @max 1
  */
-PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1);
+PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1);
+
+
+/**
+ * UART ESC Temperature Warning Threshold (Degrees C)
+ *
+ * Only applicable to ESCs that report temperature
+ *
+ * @reboot_required true
+ *
+ * @group VOXL ESC
+ * @value 0 - Disabled
+ * @min 0
+ * @max 200
+ */
+PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0);
+
+
+/**
+ * UART ESC Over-Temperature Threshold (Degrees C)
+ *
+ * Only applicable to ESCs that report temperature
+ *
+ * @reboot_required true
+ *
+ * @group VOXL ESC
+ * @value 0 - Disabled
+ * @min 0
+ * @max 200
+ */
+PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0);
+

src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp

@@ -40,152 +40,61 @@ VoxlEscSerial::VoxlEscSerial()
 
 VoxlEscSerial::~VoxlEscSerial()
 {
-	if (_uart_fd >= 0) {
-		uart_close();
+	if (_uart.isOpen()) {
+		_uart.close();
 	}
 }
 
-int VoxlEscSerial::uart_open(const char *dev, speed_t speed)
+int VoxlEscSerial::uart_open(const char *dev, uint32_t speed)
 {
-	if (_uart_fd >= 0) {
-		PX4_ERR("Port in use: %s (%i)", dev, errno);
+	if (_uart.isOpen()) {
+		PX4_ERR("Port in use: %s", dev);
 		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);
+	// Configure UART port
+	if (! _uart.setPort(dev)) {
+		PX4_ERR("Error configuring serial device on port %s", dev);
 		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();
+	if (! _uart.setBaudrate(speed)) {
+		PX4_ERR("Error setting baudrate to %d on %s", (int) speed, dev);
 		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();
+	// Open the UART. If this is successful then the UART is ready to use.
+	if (! _uart.open()) {
+		PX4_ERR("Error opening serial device  %s", dev);
 		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 VoxlEscSerial::uart_set_baud(speed_t speed)
+int VoxlEscSerial::uart_set_baud(uint32_t speed)
 {
-#ifndef __PX4_QURT
-
-	if (_uart_fd < 0) {
+	if (! _uart.setBaudrate(speed)) {
+		PX4_ERR("Error setting baudrate to %d on %s", (int) speed, _uart.getPort());
 		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 VoxlEscSerial::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;
+	_uart.close();
 
 	return 0;
 }
 
 int VoxlEscSerial::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
+	return _uart.write(buf, len);
 }
 
 int VoxlEscSerial::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
+	return _uart.read((uint8_t *) buf, len);
 }

src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp

@@ -34,36 +34,28 @@
 #pragma once
 
 #include <px4_log.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <termios.h>
+#include <px4_platform_common/Serial.hpp>
 
 #ifdef __PX4_QURT
-#include <drivers/device/qurt/uart.h>
 #define FAR
 #endif
 
+using namespace device;
+
 class VoxlEscSerial
 {
 public:
 	VoxlEscSerial();
 	virtual ~VoxlEscSerial();
 
-	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; }
+	int			uart_open(const char *dev, uint32_t speed);
+	int			uart_set_baud(uint32_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.isOpen(); };
+	uint32_t	uart_get_baud() { return _uart.getBaudrate(); }
 
 private:
-	int			_uart_fd = -1;
-
-#if ! defined(__PX4_QURT)
-	struct termios		_orig_cfg;
-	struct termios		_cfg;
-#endif
-
-	int   _speed = -1;
+	Serial	_uart {};
 };