Voxl ESC driver update (#23022)

* Made Serial API open the UART in NON BLOCKING mode
* Updated voxl_esc driver to latest from ModalAI fork
* Ported voxl_esc driver over to new Serial UART API
* Removed voxl_esc serial abstraction since new Serial API is already a serial abstraction

platforms/common/include/px4_platform_common/Serial.hpp

@@ -55,6 +55,7 @@ public:
 	virtual ~Serial();
 
 	// Open sets up the port and gets it configured based on desired configuration
+	// The port is always opened in NON BLOCKING mode.
 	bool open();
 	bool isOpen() const;
 

platforms/nuttx/src/px4/common/SerialImpl.cpp

@@ -181,7 +181,7 @@ bool SerialImpl::open()
 	}
 
 	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 	if (serial_fd < 0) {
 		PX4_ERR("failed to open %s err: %d", _port, errno);

platforms/nuttx/src/px4/common/include/SerialImpl.hpp

@@ -114,6 +114,7 @@ private:
 	bool _single_wire_mode{false};
 	bool _swap_rx_tx_mode{false};
 	bool _inverted_mode{false};
+
 };
 
 } // namespace device

platforms/posix/include/SerialImpl.hpp

@@ -114,6 +114,7 @@ private:
 	bool _single_wire_mode{false};
 	bool _swap_rx_tx_mode{false};
 	bool _inverted_mode{false};
+
 };
 
 } // namespace device

platforms/posix/src/px4/common/SerialImpl.cpp

@@ -179,7 +179,7 @@ bool SerialImpl::open()
 	}
 
 	// Open the serial port
-	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+	int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 	if (serial_fd < 0) {
 		PX4_ERR("failed to open %s err: %d", _port, errno);

platforms/qurt/src/px4/SerialImpl.cpp

@@ -163,7 +163,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 		return -1;
 	}
 
-	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 500);
+	int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 100);
 
 	if (ret_read < 0) {
 		PX4_DEBUG("%s read error %d", _port, ret_read);

src/drivers/actuators/voxl_esc/CMakeLists.txt

@@ -38,8 +38,6 @@ px4_add_module(
 		crc16.c
 		crc16.h
 
-		voxl_esc_serial.cpp
-		voxl_esc_serial.hpp
 		voxl_esc.cpp
 		voxl_esc.hpp
 		qc_esc_packet_types.h

src/drivers/actuators/voxl_esc/voxl_esc.cpp

@@ -36,7 +36,6 @@
 #include <px4_platform_common/getopt.h>
 
 #include "voxl_esc.hpp"
-#include "voxl_esc_serial.hpp"
 
 // future use:
 #define MODALAI_PUBLISH_ESC_STATUS	0
@@ -83,10 +82,7 @@ VoxlEsc::~VoxlEsc()
 {
 	_outputs_on = false;
 
-	if (_uart_port) {
+	_uart_port.close();
-		_uart_port->uart_close();
-		_uart_port = nullptr;
-	}
 
 	perf_free(_cycle_perf);
 	perf_free(_output_update_perf);
@@ -94,30 +90,185 @@ 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();
+	print_params();
-	memset(&_esc_chans, 0x00, sizeof(_esc_chans));
 
-	for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
+	//WARING: uart port initialization and device detection does not happen here
-		_version_info[esc_id].sw_version = UINT16_MAX;
+	//because init() is called from a different thread from Run(), so fd opened in init() cannot be used in Run()
-		_version_info[esc_id].hw_version = UINT16_MAX;
+	//this is an issue (feature?) specific to nuttx where each thread group gets separate set of fds
-		_version_info[esc_id].id = esc_id;
+	//https://cwiki.apache.org/confluence/display/NUTTX/Detaching+File+Descriptors
-	}
+	//detaching file descriptors is not implemented in the current version of nuttx that px4 uses
-
+	//
-	//get_instance()->ScheduleOnInterval(10000); //100hz
+	//There is no problem when running on VOXL2, but in order to have the same logical flow on both systems,
+	//we will initialize uart and query the device in Run()
 
 	ScheduleNow();
 
 	return 0;
 }
 
+int VoxlEsc::device_init()
+{
+	if (_device_initialized) {
+		return 0;
+	}
+
+	// Open serial port
+	if (!_uart_port.isOpen()) {
+		PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate);
+#ifndef __PX4_QURT
+
+		//warn user that unless DMA is enabled for UART RX, data can be lost due to high frequency of per char cpu interrupts
+		//at least at 2mbit, there are definitely losses, did not test other baud rates to find the cut off
+		if (_parameters.baud_rate > 250000) {
+			PX4_WARN("VOXL_ESC: Baud rate is too high for non-DMA based UART, this can lead to loss of RX data");
+		}
+
+#endif
+
+		// 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;
+		}
+	}
+
+	// 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) {
+		memset(&(_version_info[esc_id]), 0, sizeof(_version_info[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.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.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 : %" PRIu32 "us", (uint32_t)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;
+		}
+	}
+
+	if (esc_detection_fault) {
+		PX4_ERR("VOXL_ESC: Critical error during ESC initialization");
+		return -1;
+	}
+
+	PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm);
+
+	PX4_INFO("VOXL_ESC: All ESCs successfully detected");
+
+	_device_initialized =  true;
+
+	return 0;
+}
+
 int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
 {
 	int ret = PX4_OK;
@@ -155,45 +306,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 +365,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;
 		}
@@ -264,35 +418,11 @@ int VoxlEsc::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-int VoxlEsc::flush_uart_rx()
-{
-	while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {}
-
-	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);
 
-	int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf));
+	int res = _uart_port.read(_read_buf, sizeof(_read_buf));
 
 	if (res > 0) {
 		//PX4_INFO("read %i bytes",res);
@@ -341,7 +471,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 +513,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 +541,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) {
+				PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id);
-					memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO));
+				PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version);
-					check_versions_updated();
+				PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version);
-					break;
+				PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id);
-				}
-
-				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);
 
 			} 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("VOXL_ESC: \tESC ID     : %i", ver.id);
-				PX4_INFO("\tBoard      : %i", ver.hw_version);
+				PX4_INFO("VOXL_ESC: \tBoard      : %i", ver.hw_version);
-				PX4_INFO("\tSW Version : %i", ver.sw_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("VOXL_ESC: \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: \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;
@@ -525,7 +663,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
 
 	const char *verb = argv[argc - 1];
 
-	/* start the FMU if not running */
+	/* start the driver if not running */
 	if (!strcmp(verb, "start")) {
 		if (!is_running()) {
 			return VoxlEsc::task_spawn(argc, argv);
@@ -533,7 +671,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 +730,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 +742,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 +755,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 +768,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 +782,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 +797,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 +831,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("VOXL_ESC: Feedback id debug: %i", id_fb);
-			PX4_INFO("Sending UART ESC RPM command %i", rate);
+			PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate);
 
 			return get_instance()->send_cmd_thread_safe(&cmd);
 
@@ -705,7 +843,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 +876,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("VOXL_ESC: Feedback id debug: %i", id_fb);
-			PX4_INFO("Sending UART ESC power command %i", rate);
+			PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate);
 
 			return get_instance()->send_cmd_thread_safe(&cmd);
 
@@ -1103,8 +1241,8 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 					       sizeof(cmd.buf),
 					       _extended_rpm);
 
-	if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
+	if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
-		PX4_ERR("Failed to send packet");
+		PX4_ERR("VOXL_ESC: Failed to send packet");
 		return false;
 	}
 
@@ -1118,7 +1256,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	 * uart_read is non-blocking and we will just parse whatever bytes came in up until this point
 	 */
 
-	int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf));
+	int res = _uart_port.read(_read_buf, sizeof(_read_buf));
 
 	if (res > 0) {
 		parse_response(_read_buf, res, false);
@@ -1155,8 +1293,8 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 		// PX4_INFO("   0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x",
 		// 		 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) {
+		if (_uart_port.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;
 		}
 	}
@@ -1170,6 +1308,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 void VoxlEsc::Run()
 {
 	if (should_exit()) {
+		PX4_ERR("VOXL_ESC: Stopping the module");
 		ScheduleClear();
 		_mixing_output.unregister();
 
@@ -1179,30 +1318,26 @@ void VoxlEsc::Run()
 
 	perf_begin(_cycle_perf);
 
-	/* Open serial port in this thread */
+	//check to see if we need to open uart port and query the device
-	if (!_uart_port->is_open()) {
+	//see comment in init() regarding why we do not initialize the device there
-		if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) {
-			PX4_INFO("Opened UART ESC device");
 
-		} else {
+	int retries_left = VOXL_ESC_NUM_INIT_RETRIES;
-			PX4_ERR("Failed openening device");
+
-			return;
+	while ((!_device_initialized) && (retries_left > 0)) {
+		retries_left--;
+		int dev_init_ret = device_init();
+
+		if (dev_init_ret != 0) {
+			PX4_WARN("VOXL_ESC: Failed to initialize device, retries left %d", retries_left);
 		}
 	}
 
-	/* Get ESC FW version info */
+	if (!_device_initialized) {
-	if (_need_version_info) {
+		PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module");
-		for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
+		ScheduleClear();
-			Command cmd;
+		_mixing_output.unregister();
-			cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
+		exit_and_cleanup();
-
+		return;
-			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
@@ -1278,11 +1413,11 @@ void VoxlEsc::Run()
 	if (!_outputs_on) {
 		if (_current_cmd.valid()) {
 			//PX4_INFO("sending %d commands with delay %dus",_current_cmd.repeats,_current_cmd.repeat_delay_us);
-			flush_uart_rx();
+			_uart_port.flush();
 
 			do {
 				//PX4_INFO("CMDs left %d",_current_cmd.repeats);
-				if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) {
+				if (_uart_port.write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) {
 					if (_current_cmd.repeats == 0) {
 						_current_cmd.clear();
 					}
@@ -1296,19 +1431,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("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count);
-			PX4_INFO("CRC error count: %d", (int)_rx_crc_error_count);
+			PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count);
 
 		} else {
 			Command *new_cmd = _pending_cmd.load();
@@ -1385,16 +1520,10 @@ $ todo
 	return 0;
 }
 
-int VoxlEsc::print_status()
+void VoxlEsc::print_params()
 {
-	PX4_INFO("Max update rate: %i Hz", _current_update_rate);
-	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
-	PX4_INFO("UART port: %s", _device);
-	PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no");
-
-	PX4_INFO("");
-
 	PX4_INFO("Params: VOXL_ESC_CONFIG: %" PRId32, _parameters.config);
+	PX4_INFO("Params: VOXL_ESC_MODE: %" PRId32, _parameters.mode);
 	PX4_INFO("Params: VOXL_ESC_BAUD: %" PRId32, _parameters.baud_rate);
 
 	PX4_INFO("Params: VOXL_ESC_FUNC1: %" PRId32, _parameters.function_map[0]);
@@ -1410,6 +1539,28 @@ int VoxlEsc::print_status()
 	PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min);
 	PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max);
 
+	PX4_INFO("Params: VOXL_ESC_T_PERC: %" PRId32, _parameters.turtle_motor_percent);
+	PX4_INFO("Params: VOXL_ESC_T_DEAD: %" PRId32, _parameters.turtle_motor_deadband);
+	PX4_INFO("Params: VOXL_ESC_T_EXPO: %" PRId32, _parameters.turtle_motor_expo);
+	PX4_INFO("Params: VOXL_ESC_T_MINF: %f",       (double)_parameters.turtle_stick_minf);
+	PX4_INFO("Params: VOXL_ESC_T_COSP: %f",       (double)_parameters.turtle_cosphi);
+
+	PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32,    _parameters.verbose_logging);
+	PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status);
+	
+	PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold);
+	PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold);
+}
+
+int VoxlEsc::print_status()
+{
+	PX4_INFO("Max update rate: %i Hz", _current_update_rate);
+	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
+	PX4_INFO("UART port: %s", _device);
+	PX4_INFO("UART open: %s", _uart_port.isOpen() ? "yes" : "no");
+
+	PX4_INFO("");
+	print_params();
 	PX4_INFO("");
 
 	for( int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++){
@@ -1432,6 +1583,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

@@ -51,11 +51,13 @@
 #include <uORB/topics/actuator_test.h>
 #include <uORB/topics/buffer128.h>
 
-#include "voxl_esc_serial.hpp"
+#include <px4_platform_common/Serial.hpp>
 
 #include "qc_esc_packet.h"
 #include "qc_esc_packet_types.h"
 
+using namespace device;
+
 class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
 {
 public:
@@ -76,16 +78,18 @@ public:
 
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
+	void print_params();
 
 	/** @see OutputModuleInterface */
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 	virtual int	init();
+	int device_init(); // function where uart port is opened and ESC queried
 
 	struct Command {
 		uint16_t	id                 = 0;
-		uint8_t 	len                = 0;
+		uint8_t		len                = 0;
 		uint16_t	repeats            = 0;
 		uint16_t	repeat_delay_us    = 2000;
 		uint8_t		retries            = 0;
@@ -94,7 +98,7 @@ public:
 		bool		print_feedback     = false;
 
 		static const uint8_t BUF_SIZE = 128;
-		uint8_t 	buf[BUF_SIZE];
+		uint8_t		buf[BUF_SIZE];
 
 		bool valid() const { return len > 0; }
 		void clear() { len = 0; }
@@ -125,16 +129,19 @@ 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 -
 			5;	// 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
 
+	static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3;
+
 	//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); }
 	//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); }
 
-	VoxlEscSerial 		*_uart_port;
+	Serial			_uart_port{};
 
 	typedef struct {
 		int32_t		config{VOXL_ESC_UART_CONFIG};
@@ -151,7 +158,9 @@ private:
 		int32_t		motor_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4};
 		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		publish_battery_status{0};
+		int32_t		esc_warn_temp_threshold{0};
+		int32_t		esc_over_temp_threshold{0};
 	} voxl_esc_params_t;
 
 	struct EscChan {
@@ -161,7 +170,7 @@ private:
 		uint8_t		power_applied;
 		uint8_t		led;
 		uint8_t		cmd_counter;
-		float 		voltage;  //Volts
+		float		voltage;  //Volts
 		float		current;  //Amps
 		float		temperature; //deg C
 		hrt_abstime 	feedback_time;
@@ -182,7 +191,7 @@ private:
 	} led_rsc_t;
 
 	ch_assign_t		_output_map[VOXL_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
-	MixingOutput 		_mixing_output;
+	MixingOutput		_mixing_output;
 
 	perf_counter_t		_cycle_perf;
 	perf_counter_t		_output_update_perf;
@@ -193,8 +202,8 @@ private:
 
 	uORB::Subscription	_vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription	_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription 	_parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription 	_actuator_test_sub{ORB_ID(actuator_test)};
+	uORB::Subscription	_actuator_test_sub{ORB_ID(actuator_test)};
 	uORB::Subscription	_led_update_sub{ORB_ID(led_control)};
 	uORB::Subscription	_voxl2_io_data_sub{ORB_ID(voxl2_io_data)};
 
@@ -203,12 +212,12 @@ private:
 
 	bool _extended_rpm{false};
 	bool _need_version_info{true};
-	QC_ESC_VERSION_INFO _version_info[4];
+	QC_ESC_EXTENDED_VERSION_INFO _version_info[VOXL_ESC_OUTPUT_CHANNELS];
-	bool check_versions_updated();
 
 	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};
@@ -216,7 +225,7 @@ private:
 	manual_control_setpoint_s _manual_control_setpoint{};
 
 	uint16_t		_cmd_id{0};
-	Command 		_current_cmd;
+	Command			_current_cmd;
 	px4::atomic<Command *>	_pending_cmd{nullptr};
 
 	EscChan			_esc_chans[VOXL_ESC_OUTPUT_CHANNELS];
@@ -224,24 +233,25 @@ private:
 	esc_status_s		_esc_status;
 	EscPacket		_fb_packet;
 
-	led_rsc_t	 	_led_rsc;
+	led_rsc_t		_led_rsc;
 	int			_fb_idx;
 	uint32_t		_rx_crc_error_count{0};
 	uint32_t		_rx_packet_count{0};
 
-	static const uint8_t 	READ_BUF_SIZE = 128;
+	static const uint8_t	READ_BUF_SIZE = 128;
 	uint8_t			_read_buf[READ_BUF_SIZE];
 
-	Battery 		_battery;
+	Battery			_battery;
 	static constexpr unsigned _battery_report_interval{100_ms};
 	hrt_abstime		_last_battery_report_time;
 
-	void 			update_leds(vehicle_control_mode_s mode, led_control_s control);
+	bool			_device_initialized{false};
 
-	int 			read_response(Command *out_cmd);
+	void			update_leds(vehicle_control_mode_s mode, led_control_s control);
-	int 			parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
+
-	int			flush_uart_rx();
+	int				read_response(Command *out_cmd);
-	int			check_for_esc_timeout();
+	int				parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
+	int				check_for_esc_timeout();
 	void			mix_turtle_mode(uint16_t outputs[]);
 	void			handle_actuator_test();
 };

src/drivers/actuators/voxl_esc/voxl_esc_params.c

@@ -232,3 +232,34 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0);
  * @max 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

@@ -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 "voxl_esc_serial.hpp"
-
-VoxlEscSerial::VoxlEscSerial()
-{
-}
-
-VoxlEscSerial::~VoxlEscSerial()
-{
-	if (_uart_fd >= 0) {
-		uart_close();
-	}
-}
-
-int VoxlEscSerial::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 VoxlEscSerial::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 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;
-
-	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
-}
-
-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
-}

src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp

@@ -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 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; }
-
-private:
-	int			_uart_fd = -1;
-
-#if ! defined(__PX4_QURT)
-	struct termios		_orig_cfg;
-	struct termios		_cfg;
-#endif
-
-	int   _speed = -1;
-};