diff --git a/src/drivers/actuators/voxl_esc/CMakeLists.txt b/src/drivers/actuators/voxl_esc/CMakeLists.txt index d588dc29a6..c36711f1d4 100644 --- a/src/drivers/actuators/voxl_esc/CMakeLists.txt +++ b/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 diff --git a/src/drivers/actuators/voxl_esc/qc_esc_packet.c b/src/drivers/actuators/voxl_esc/qc_esc_packet.c index 26d8fca0c3..086622379c 100644 --- a/src/drivers/actuators/voxl_esc/qc_esc_packet.c +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.c @@ -137,9 +137,13 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i int32_t min = ext_rpm > 0 ? ESC_RPM_MIN_EXT : ESC_RPM_MIN; // Limit RPMs to prevent overflow when converting to int16_t + if (rpm0 > max) { rpm0 = max; } if (rpm0 < min) { rpm0 = min; } + if (rpm1 > max) { rpm1 = max; } if (rpm1 < min) { rpm1 = min; } + if (rpm2 > max) { rpm2 = max; } if (rpm2 < min) { rpm2 = min; } + if (rpm3 > max) { rpm3 = max; } if (rpm3 < min) { rpm3 = min; } if (fb_id != -1) { fb_id = fb_id % 4; } @@ -149,13 +153,14 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i leds |= ((uint16_t)(led2 & 0b00000111)) << 6; leds |= ((uint16_t)(led3 & 0b00000111)) << 9; - if (ext_rpm > 0){ + if (ext_rpm > 0) { cmd = ESC_PACKET_TYPE_RPM_DIV2_CMD; data[0] = ((rpm0 / 4) * 2); data[1] = ((rpm1 / 4) * 2); data[2] = ((rpm2 / 4) * 2); data[3] = ((rpm3 / 4) * 2); - data[4]= leds; + data[4] = leds; + } else { data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds; } @@ -164,6 +169,7 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i data[0] &= ~(0x0001); data[1] &= ~(0x0001); data[2] &= ~(0x0001); data[3] &= ~(0x0001); if (fb_id == 0) { data[0] |= 0x0001; } if (fb_id == 1) { data[1] |= 0x0001; } + if (fb_id == 2) { data[2] |= 0x0001; } if (fb_id == 3) { data[3] |= 0x0001; } return qc_esc_create_packet(cmd, (uint8_t *) & (data[0]), 10, out, out_size); diff --git a/src/drivers/actuators/voxl_esc/qc_esc_packet.h b/src/drivers/actuators/voxl_esc/qc_esc_packet.h index bb3b93468b..88ecc8e9f9 100644 --- a/src/drivers/actuators/voxl_esc/qc_esc_packet.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.h @@ -148,16 +148,15 @@ typedef struct { // Definition of the feedback response packet from ESC, which contains battery voltage and total current -typedef struct -{ - uint8_t header; - uint8_t length; - uint8_t type; - uint8_t id; //ESC Id (could be used as system ID elsewhere) - uint16_t voltage; //Input voltage (Millivolts) - int16_t current; //Total Current (8mA resolution) - uint16_t crc; -} __attribute__ ((__packed__)) QC_ESC_FB_POWER_STATUS; +typedef struct { + uint8_t header; + uint8_t length; + uint8_t type; + uint8_t id; //ESC Id (could be used as system ID elsewhere) + uint16_t voltage; //Input voltage (Millivolts) + int16_t current; //Total Current (8mA resolution) + uint16_t crc; +} __attribute__((__packed__)) QC_ESC_FB_POWER_STATUS; //------------------------------------------------------------------------- diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 7df0f692c1..91cea25266 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -94,7 +94,7 @@ VoxlEsc::~VoxlEsc() int VoxlEsc::init() { - PX4_INFO("VOXL_ESC: Starting VOXL ESC driver"); + PX4_INFO("VOXL_ESC: Starting VOXL ESC driver"); /* Getting initial parameter values */ int ret = update_params(); @@ -105,13 +105,15 @@ int VoxlEsc::init() } _uart_port = new VoxlEscSerial(); - if (!_uart_port){ + + if (!_uart_port) { PX4_ERR("VOXL_ESC: Failed allocating VoxlEscSerial"); return -1; } // Open serial port - PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %d", _device, _parameters.baud_rate); + 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"); @@ -126,7 +128,7 @@ int VoxlEsc::init() 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){ + 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; @@ -136,13 +138,12 @@ int VoxlEsc::init() 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++){ + //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) - { + 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; } @@ -151,7 +152,7 @@ int VoxlEsc::init() 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) ){ + 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)); @@ -169,9 +170,9 @@ int VoxlEsc::init() 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).c_str()); + 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", @@ -179,11 +180,11 @@ int VoxlEsc::init() 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 : %uus",(uint32_t)response_time); + 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)); + if (ver.id == esc_id) { + memcpy(&_version_info[esc_id], &ver, sizeof(ver)); got_response = true; } } @@ -191,24 +192,25 @@ int VoxlEsc::init() } } - if (!got_response){ + 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){ + + 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)", + 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; } @@ -216,18 +218,20 @@ int VoxlEsc::init() //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){ + + 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){ + 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(); @@ -271,7 +275,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_VLOG"), ¶ms->verbose_logging); param_get(param_find("VOXL_ESC_PUB_BST"), ¶ms->publish_battery_status); - + param_get(param_find("VOXL_ESC_T_WARN"), ¶ms->esc_warn_temp_threshold); param_get(param_find("VOXL_ESC_T_OVER"), ¶ms->esc_over_temp_threshold); @@ -444,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("VOXL_ESC: [%" 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); } @@ -484,19 +489,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); + + + 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); + 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 /* @@ -512,7 +517,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) { QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - + 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); @@ -531,10 +536,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) 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; - memcpy(&packet,_fb_packet.buffer, packet_size); - + memcpy(&packet, _fb_packet.buffer, packet_size); + float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution float current = packet.current * 0.008f; // Total current is reported at 8mA resolution @@ -545,12 +551,13 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) _battery.updateCurrent(current); hrt_abstime current_time = hrt_absolute_time(); + if ((current_time - _last_battery_report_time) >= _battery_report_interval) { _last_battery_report_time = current_time; _battery.updateAndPublishBatteryStatus(current_time); } } - + } } else { //parser error @@ -793,7 +800,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) id_fb, cmd.buf, sizeof(cmd.buf), - get_instance()->_extended_rpm); + get_instance()->_extended_rpm); cmd.response = true; cmd.repeats = repeat_count; @@ -1180,10 +1187,12 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } else { if (_extended_rpm) { - if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) outputs[i] = VOXL_ESC_RPM_MAX_EXT; + if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; } + } else { - if (outputs[i] > VOXL_ESC_RPM_MAX) outputs[i] = VOXL_ESC_RPM_MAX; + if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; } } + if (!_turtle_mode_en) { _esc_chans[i].rate_req = outputs[i] * _output_map[i].direction; @@ -1193,21 +1202,21 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], } } } - + Command cmd; cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req, - _esc_chans[1].rate_req, - _esc_chans[2].rate_req, - _esc_chans[3].rate_req, - _esc_chans[0].led, - _esc_chans[1].led, - _esc_chans[2].led, - _esc_chans[3].led, - _fb_idx, - cmd.buf, - sizeof(cmd.buf), - _extended_rpm); + _esc_chans[1].rate_req, + _esc_chans[2].rate_req, + _esc_chans[3].rate_req, + _esc_chans[0].led, + _esc_chans[1].led, + _esc_chans[2].led, + _esc_chans[3].led, + _fb_idx, + cmd.buf, + sizeof(cmd.buf), + _extended_rpm); if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { PX4_ERR("VOXL_ESC: Failed to send packet"); @@ -1256,6 +1265,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], while (_voxl2_io_data_sub.updated()) { buffer128_s io_data{}; _voxl2_io_data_sub.copy(&io_data); + // PX4_INFO("Got Modal IO data: %u bytes", io_data.len); // 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], @@ -1511,22 +1521,22 @@ int VoxlEsc::print_status() return 0; } -std::string VoxlEsc::board_id_to_name(int board_id) +const char *VoxlEsc::board_id_to_name(int board_id) { switch(board_id){ - case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); - case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); - case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); - case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); - case 35: return std::string("ModalAi I/O Expander (M0065)"); - case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); - case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); - case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); - case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); - case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); - case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); - case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); - default: return std::string("Unknown Board"); + 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"; } } diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index fcadb3dab4..689bcd2b43 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -56,8 +56,6 @@ #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" -#include - class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: @@ -127,9 +125,12 @@ 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; // 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_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 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); } @@ -210,7 +211,7 @@ private: voxl_esc_params_t _parameters; int update_params(); int load_params(voxl_esc_params_t *params, ch_assign_t *map); - std::string board_id_to_name(int board_id); + const char* board_id_to_name(int board_id); bool _turtle_mode_en{false}; int32_t _rpm_turtle_min{0}; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index 203a538a92..f2719cea04 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -220,7 +220,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0); /** * UART ESC Enable publishing of battery status - * + * * Only applicable to ESCs that report total battery voltage and current * * @reboot_required true @@ -236,7 +236,7 @@ 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 @@ -251,7 +251,7 @@ 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 diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp index 1064dc4365..a60e00ec98 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp +++ b/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); } diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp index 99e918886f..4320f5f421 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp @@ -34,36 +34,28 @@ #pragma once #include -#include -#include -#include +#include #ifdef __PX4_QURT -#include #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 {}; };