mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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
This commit is contained in:
Eric Katzfey
GitHub
parent
a8a67fbf8f
commit
2dccd6cacb
@ -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;
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -114,6 +114,7 @@ private:
|
||||
bool _single_wire_mode{false};
|
||||
bool _swap_rx_tx_mode{false};
|
||||
bool _inverted_mode{false};
|
||||
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
||||
@ -114,6 +114,7 @@ private:
|
||||
bool _single_wire_mode{false};
|
||||
bool _swap_rx_tx_mode{false};
|
||||
bool _inverted_mode{false};
|
||||
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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->uart_close();
|
||||
_uart_port = nullptr;
|
||||
}
|
||||
_uart_port.close();
|
||||
|
||||
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();
|
||||
memset(&_esc_chans, 0x00, sizeof(_esc_chans));
|
||||
print_params();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//get_instance()->ScheduleOnInterval(10000); //100hz
|
||||
//WARING: uart port initialization and device detection does not happen here
|
||||
//because init() is called from a different thread from Run(), so fd opened in init() cannot be used in Run()
|
||||
//this is an issue (feature?) specific to nuttx where each thread group gets separate set of fds
|
||||
//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
|
||||
//
|
||||
//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"), ¶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 +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) {
|
||||
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;
|
||||
@ -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("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 +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("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);
|
||||
|
||||
@ -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) {
|
||||
PX4_ERR("Failed to send packet");
|
||||
if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) {
|
||||
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) {
|
||||
PX4_ERR("Failed to send modal io data to esc");
|
||||
if (_uart_port.write(io_data.data, io_data.len) != io_data.len) {
|
||||
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 */
|
||||
if (!_uart_port->is_open()) {
|
||||
if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) {
|
||||
PX4_INFO("Opened UART ESC device");
|
||||
//check to see if we need to open uart port and query the device
|
||||
//see comment in init() regarding why we do not initialize the device there
|
||||
|
||||
} else {
|
||||
PX4_ERR("Failed openening device");
|
||||
return;
|
||||
int retries_left = VOXL_ESC_NUM_INIT_RETRIES;
|
||||
|
||||
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 (_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!");
|
||||
}
|
||||
}
|
||||
if (!_device_initialized) {
|
||||
PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module");
|
||||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
_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("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();
|
||||
@ -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[])
|
||||
|
||||
@ -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 _actuator_test_sub{ORB_ID(actuator_test)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
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];
|
||||
bool check_versions_updated();
|
||||
QC_ESC_EXTENDED_VERSION_INFO _version_info[VOXL_ESC_OUTPUT_CHANNELS];
|
||||
|
||||
voxl_esc_params_t _parameters;
|
||||
int update_params();
|
||||
int load_params(voxl_esc_params_t *params, ch_assign_t *map);
|
||||
const char *board_id_to_name(int board_id);
|
||||
|
||||
bool _turtle_mode_en{false};
|
||||
int32_t _rpm_turtle_min{0};
|
||||
@ -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);
|
||||
int parse_response(uint8_t *buf, uint8_t len, bool print_feedback);
|
||||
int flush_uart_rx();
|
||||
int check_for_esc_timeout();
|
||||
void update_leds(vehicle_control_mode_s mode, led_control_s control);
|
||||
|
||||
int read_response(Command *out_cmd);
|
||||
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();
|
||||
};
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
|
||||
}
|
||||
@ -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;
|
||||
};
|
||||
Loading…
x
Reference in New Issue
Block a user