fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 04:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Brought in latest version of the voxl_esc driver

Browse Source
This commit is contained in:
Eric Katzfey
2024-04-01 17:35:31 -07:00
parent 8579175013
commit de7352c153
5 changed files with 270 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/voxl_esc/qc_esc_packet.c
+2 -8
View File
@@ -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);
src/drivers/actuators/voxl_esc/qc_esc_packet.h
+10 -9
View File
@@ -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;
//-------------------------------------------------------------------------
src/drivers/actuators/voxl_esc/voxl_esc.cpp
+216 -118
View File
@@ -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"), &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 +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[])
src/drivers/actuators/voxl_esc/voxl_esc.hpp
+9 -7
View File
@@ -56,6 +56,8 @@
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
#include <string>
class VoxlEsc : public ModuleBase<VoxlEsc>, 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};
src/drivers/actuators/voxl_esc/voxl_esc_params.c
+33 -2
View File
@@ -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);
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);