From de7352c153fcc05b7de247665397f09e8113f2da Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 1 Apr 2024 17:35:31 -0700 Subject: [PATCH] Brought in latest version of the voxl_esc driver --- .../actuators/voxl_esc/qc_esc_packet.c | 10 +- .../actuators/voxl_esc/qc_esc_packet.h | 19 +- src/drivers/actuators/voxl_esc/voxl_esc.cpp | 334 +++++++++++------- src/drivers/actuators/voxl_esc/voxl_esc.hpp | 16 +- .../actuators/voxl_esc/voxl_esc_params.c | 35 +- 5 files changed, 270 insertions(+), 144 deletions(-) diff --git a/src/drivers/actuators/voxl_esc/qc_esc_packet.c b/src/drivers/actuators/voxl_esc/qc_esc_packet.c index 086622379c..26d8fca0c3 100644 --- a/src/drivers/actuators/voxl_esc/qc_esc_packet.c +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.c @@ -137,13 +137,9 @@ 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; } @@ -153,14 +149,13 @@ 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; } @@ -169,7 +164,6 @@ 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 88ecc8e9f9..bb3b93468b 100644 --- a/src/drivers/actuators/voxl_esc/qc_esc_packet.h +++ b/src/drivers/actuators/voxl_esc/qc_esc_packet.h @@ -148,15 +148,16 @@ 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 88d070a7cf..7df0f692c1 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -94,24 +94,141 @@ 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, _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).c_str()); + + 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 : %uus",(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; + } + } + + 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(); @@ -154,46 +271,49 @@ 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); 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 +331,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 +391,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 +444,7 @@ 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); } @@ -381,6 +484,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 /* @@ -396,36 +512,29 @@ 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); - - 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; - 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 @@ -436,13 +545,12 @@ 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 @@ -533,7 +641,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 +700,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 +712,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 +725,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 +738,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 +752,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 +767,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; @@ -685,7 +793,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; @@ -693,8 +801,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 +813,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 +846,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); @@ -1072,12 +1180,10 @@ 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; @@ -1087,24 +1193,24 @@ 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("Failed to send packet"); + PX4_ERR("VOXL_ESC: Failed to send packet"); return false; } @@ -1145,18 +1251,17 @@ 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{}; _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], // 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 +1284,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 +1375,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 +1511,25 @@ int VoxlEsc::print_status() return 0; } +std::string 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"); + } +} + extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]); int voxl_esc_main(int argc, char *argv[]) diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index 8cc84435fa..fcadb3dab4 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -56,6 +56,8 @@ #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" +#include + class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: @@ -125,11 +127,9 @@ 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_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); } @@ -152,6 +152,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 +205,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); + std::string 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 8c1d03c6e7..203a538a92 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 @@ -231,4 +231,35 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1); \ No newline at end of file +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); +