fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 09:07:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bringing in latest voxl_esc driver updates from ModalAI fork and porting to new Serial UART API

Browse Source
This commit is contained in:
Eric Katzfey
2024-04-02 09:33:07 -07:00
parent de7352c153
commit ccbb7b6c21
8 changed files with 135 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/actuators/voxl_esc/CMakeLists.txt
-1
View File
@@ -37,7 +37,6 @@ px4_add_module(
SRCS
crc16.c
crc16.h
voxl_esc_serial.cpp
voxl_esc_serial.hpp
voxl_esc.cpp
src/drivers/actuators/voxl_esc/qc_esc_packet.c
+8 -2
View File
@@ -137,9 +137,13 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i
int32_t min = ext_rpm > 0 ? ESC_RPM_MIN_EXT : ESC_RPM_MIN;
// Limit RPMs to prevent overflow when converting to int16_t
if (rpm0 > max) { rpm0 = max; } if (rpm0 < min) { rpm0 = min; }
if (rpm1 > max) { rpm1 = max; } if (rpm1 < min) { rpm1 = min; }
if (rpm2 > max) { rpm2 = max; } if (rpm2 < min) { rpm2 = min; }
if (rpm3 > max) { rpm3 = max; } if (rpm3 < min) { rpm3 = min; }
if (fb_id != -1) { fb_id = fb_id % 4; }
@@ -149,13 +153,14 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i
leds |= ((uint16_t)(led2 & 0b00000111)) << 6;
leds |= ((uint16_t)(led3 & 0b00000111)) << 9;
if (ext_rpm > 0){
if (ext_rpm > 0) {
cmd = ESC_PACKET_TYPE_RPM_DIV2_CMD;
data[0] = ((rpm0 / 4) * 2);
data[1] = ((rpm1 / 4) * 2);
data[2] = ((rpm2 / 4) * 2);
data[3] = ((rpm3 / 4) * 2);
data[4]= leds;
data[4] = leds;
} else {
data[0] = rpm0; data[1] = rpm1; data[2] = rpm2; data[3] = rpm3; data[4] = leds;
}
@@ -164,6 +169,7 @@ int32_t qc_esc_create_rpm_packet4_fb(int32_t rpm0, int32_t rpm1, int32_t rpm2, i
data[0] &= ~(0x0001); data[1] &= ~(0x0001); data[2] &= ~(0x0001); data[3] &= ~(0x0001);
if (fb_id == 0) { data[0] |= 0x0001; } if (fb_id == 1) { data[1] |= 0x0001; }
if (fb_id == 2) { data[2] |= 0x0001; } if (fb_id == 3) { data[3] |= 0x0001; }
return qc_esc_create_packet(cmd, (uint8_t *) & (data[0]), 10, out, out_size);
src/drivers/actuators/voxl_esc/qc_esc_packet.h
+9 -10
View File
@@ -148,16 +148,15 @@ typedef struct {
// Definition of the feedback response packet from ESC, which contains battery voltage and total current
typedef struct
{
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id; //ESC Id (could be used as system ID elsewhere)
uint16_t voltage; //Input voltage (Millivolts)
int16_t current; //Total Current (8mA resolution)
uint16_t crc;
} __attribute__ ((__packed__)) QC_ESC_FB_POWER_STATUS;
typedef struct {
uint8_t header;
uint8_t length;
uint8_t type;
uint8_t id; //ESC Id (could be used as system ID elsewhere)
uint16_t voltage; //Input voltage (Millivolts)
int16_t current; //Total Current (8mA resolution)
uint16_t crc;
} __attribute__((__packed__)) QC_ESC_FB_POWER_STATUS;
//-------------------------------------------------------------------------
src/drivers/actuators/voxl_esc/voxl_esc.cpp
+78 -68
View File
@@ -94,7 +94,7 @@ VoxlEsc::~VoxlEsc()
int VoxlEsc::init()
{
PX4_INFO("VOXL_ESC: Starting VOXL ESC driver");
PX4_INFO("VOXL_ESC: Starting VOXL ESC driver");
/* Getting initial parameter values */
int ret = update_params();
@@ -105,13 +105,15 @@ int VoxlEsc::init()
}
_uart_port = new VoxlEscSerial();
if (!_uart_port){
if (!_uart_port) {
PX4_ERR("VOXL_ESC: Failed allocating VoxlEscSerial");
return -1;
}
// Open serial port
PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %d", _device, _parameters.baud_rate);
PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %d", _device, (int) _parameters.baud_rate);
if (!_uart_port->is_open()) {
if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) {
PX4_INFO("VOXL_ESC: Successfully opened UART ESC device");
@@ -126,7 +128,7 @@ int VoxlEsc::init()
memset(&_esc_chans, 0x00, sizeof(_esc_chans));
//reset the ESC version info before requesting
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id){
for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) {
_version_info[esc_id].sw_version = 0; //invalid
_version_info[esc_id].hw_version = 0; //invalid
_version_info[esc_id].id = esc_id;
@@ -136,13 +138,12 @@ int VoxlEsc::init()
PX4_INFO("VOXL_ESC: Detecting ESCs...");
qc_esc_packet_init(&_fb_packet);
//request extended version info from each ESC and wait for reply
for (uint8_t esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++){
//request extended version info from each ESC and wait for reply
for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
Command cmd;
cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf));
if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len)
{
if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
PX4_ERR("VOXL_ESC: Could not write version request packet to UART port");
return -1;
}
@@ -151,7 +152,7 @@ int VoxlEsc::init()
hrt_abstime t_timeout = 50000; //50ms timeout for version info response
bool got_response = false;
while( (!got_response) && (hrt_elapsed_time(&t_request) < t_timeout) ){
while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) {
px4_usleep(100); //sleep a bit while waiting for ESC to respond
int nread = _uart_port->uart_read(_read_buf, sizeof(_read_buf));
@@ -169,9 +170,9 @@ int VoxlEsc::init()
if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) {
QC_ESC_EXTENDED_VERSION_INFO ver;
memcpy(&ver, _fb_packet.buffer, packet_size);
PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id);
PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str());
PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version));
uint8_t *u = &ver.unique_id[0];
PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X",
@@ -179,11 +180,11 @@ int VoxlEsc::init()
PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
PX4_INFO("VOXL_ESC: \tReply time : %uus",(uint32_t)response_time);
PX4_INFO("VOXL_ESC: \tReply time : %d us", (int)response_time);
PX4_INFO("VOXL_ESC:");
if (ver.id == esc_id){
memcpy(&_version_info[esc_id],&ver,sizeof(ver));
if (ver.id == esc_id) {
memcpy(&_version_info[esc_id], &ver, sizeof(ver));
got_response = true;
}
}
@@ -191,24 +192,25 @@ int VoxlEsc::init()
}
}
if (!got_response){
if (!got_response) {
PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id);
}
}
//check the SW version of the ESCs
bool esc_detection_fault = false;
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++){
if (_version_info[esc_id].sw_version == 0){
for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
if (_version_info[esc_id].sw_version == 0) {
PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id);
esc_detection_fault = true;
}
}
//check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*"
for (int esc_id=1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++){
if (strncmp(_version_info[0].firmware_git_version,_version_info[esc_id].firmware_git_version, 9) != 0) {
PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)",
for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) {
PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)",
esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version);
esc_detection_fault = true;
}
@@ -216,18 +218,20 @@ int VoxlEsc::init()
//if firmware version is equal or greater than VOXL_ESC_EXT_RPM, ESC packet with extended rpm range is supported. use it
_extended_rpm = true;
for (int esc_id=0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++){
if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM){
for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) {
if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) {
_extended_rpm = false;
}
}
PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm);
if (esc_detection_fault){
if (esc_detection_fault) {
PX4_ERR("VOXL_ESC: Critical error during ESC initialization. Exiting");
return -1;
}
PX4_INFO("VOXL_ESC: All ESCs successfully detected");
ScheduleNow();
@@ -271,7 +275,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map)
param_get(param_find("VOXL_ESC_VLOG"), &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);
@@ -444,7 +448,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
uint32_t voltage = fb.voltage;
int32_t current = fb.current * 8;
int32_t temperature = fb.temperature / 100;
PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, motor_idx + 1,
PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id,
motor_idx + 1,
(int)rpm, (int)power, (int)voltage, (int)current, (int)temperature);
}
@@ -484,19 +489,19 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_esc_status.timestamp = _esc_status.esc[id].timestamp;
_esc_status.counter++;
if ((_parameters.esc_over_temp_threshold > 0) && (_esc_status.esc[id].esc_temperature > _parameters.esc_over_temp_threshold))
{
_esc_status.esc[id].failures |= 1<<(esc_report_s::FAILURE_OVER_ESC_TEMPERATURE);
if ((_parameters.esc_over_temp_threshold > 0)
&& (_esc_status.esc[id].esc_temperature > _parameters.esc_over_temp_threshold)) {
_esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_OVER_ESC_TEMPERATURE);
}
//TODO: do we also issue a warning if over-temperature threshold is exceeded?
if ((_parameters.esc_warn_temp_threshold > 0) && (_esc_status.esc[id].esc_temperature > _parameters.esc_warn_temp_threshold))
{
_esc_status.esc[id].failures |= 1<<(esc_report_s::FAILURE_WARN_ESC_TEMPERATURE);
if ((_parameters.esc_warn_temp_threshold > 0)
&& (_esc_status.esc[id].esc_temperature > _parameters.esc_warn_temp_threshold)) {
_esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_WARN_ESC_TEMPERATURE);
}
//print ESC status just for debugging
/*
@@ -512,7 +517,7 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
else if (packet_type == ESC_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(QC_ESC_VERSION_INFO)) {
QC_ESC_VERSION_INFO ver;
memcpy(&ver, _fb_packet.buffer, packet_size);
PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id);
PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version);
PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version);
@@ -531,10 +536,11 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version);
PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version);
} else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) {
QC_ESC_FB_POWER_STATUS packet;
memcpy(&packet,_fb_packet.buffer, packet_size);
memcpy(&packet, _fb_packet.buffer, packet_size);
float voltage = packet.voltage * 0.001f; // Voltage is reported at 1 mV resolution
float current = packet.current * 0.008f; // Total current is reported at 8mA resolution
@@ -545,12 +551,13 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback)
_battery.updateCurrent(current);
hrt_abstime current_time = hrt_absolute_time();
if ((current_time - _last_battery_report_time) >= _battery_report_interval) {
_last_battery_report_time = current_time;
_battery.updateAndPublishBatteryStatus(current_time);
}
}
}
} else { //parser error
@@ -793,7 +800,7 @@ int VoxlEsc::custom_command(int argc, char *argv[])
id_fb,
cmd.buf,
sizeof(cmd.buf),
get_instance()->_extended_rpm);
get_instance()->_extended_rpm);
cmd.response = true;
cmd.repeats = repeat_count;
@@ -1180,10 +1187,12 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
} else {
if (_extended_rpm) {
if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) outputs[i] = VOXL_ESC_RPM_MAX_EXT;
if (outputs[i] > VOXL_ESC_RPM_MAX_EXT) { outputs[i] = VOXL_ESC_RPM_MAX_EXT; }
} else {
if (outputs[i] > VOXL_ESC_RPM_MAX) outputs[i] = VOXL_ESC_RPM_MAX;
if (outputs[i] > VOXL_ESC_RPM_MAX) { outputs[i] = VOXL_ESC_RPM_MAX; }
}
if (!_turtle_mode_en) {
_esc_chans[i].rate_req = outputs[i] * _output_map[i].direction;
@@ -1193,21 +1202,21 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
}
}
}
Command cmd;
cmd.len = qc_esc_create_rpm_packet4_fb(_esc_chans[0].rate_req,
_esc_chans[1].rate_req,
_esc_chans[2].rate_req,
_esc_chans[3].rate_req,
_esc_chans[0].led,
_esc_chans[1].led,
_esc_chans[2].led,
_esc_chans[3].led,
_fb_idx,
cmd.buf,
sizeof(cmd.buf),
_extended_rpm);
_esc_chans[1].rate_req,
_esc_chans[2].rate_req,
_esc_chans[3].rate_req,
_esc_chans[0].led,
_esc_chans[1].led,
_esc_chans[2].led,
_esc_chans[3].led,
_fb_idx,
cmd.buf,
sizeof(cmd.buf),
_extended_rpm);
if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) {
PX4_ERR("VOXL_ESC: Failed to send packet");
@@ -1256,6 +1265,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
while (_voxl2_io_data_sub.updated()) {
buffer128_s io_data{};
_voxl2_io_data_sub.copy(&io_data);
// PX4_INFO("Got Modal IO data: %u bytes", io_data.len);
// PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x",
// io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3],
@@ -1511,22 +1521,22 @@ int VoxlEsc::print_status()
return 0;
}
std::string VoxlEsc::board_id_to_name(int board_id)
const char *VoxlEsc::board_id_to_name(int board_id)
{
switch(board_id){
case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)");
case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)");
case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)");
case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)");
case 35: return std::string("ModalAi I/O Expander (M0065)");
case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)");
case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)");
case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)");
case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)");
case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)");
case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)");
case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)");
default: return std::string("Unknown Board");
case 31: return "ModalAi 4-in-1 ESC V2 RevB (M0049)";
case 32: return "Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)";
case 33: return "Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)";
case 34: return "ModalAi 4-in-1 ESC (M0117-1)";
case 35: return "ModalAi I/O Expander (M0065)";
case 36: return "ModalAi 4-in-1 ESC (M0117-3)";
case 37: return "ModalAi 4-in-1 ESC (M0134-1)";
case 38: return "ModalAi 4-in-1 ESC (M0134-3)";
case 39: return "ModalAi 4-in-1 ESC (M0129-1)";
case 40: return "ModalAi 4-in-1 ESC (M0129-3)";
case 41: return "ModalAi 4-in-1 ESC (M0134-6)";
case 42: return "ModalAi 4-in-1 ESC (M0138-1)";
default: return "Unknown Board";
}
}
src/drivers/actuators/voxl_esc/voxl_esc.hpp
+7 -6
View File
@@ -56,8 +56,6 @@
#include "qc_esc_packet.h"
#include "qc_esc_packet_types.h"
#include <string>
class VoxlEsc : public ModuleBase<VoxlEsc>, public OutputModuleInterface
{
public:
@@ -127,9 +125,12 @@ private:
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1;
static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2;
static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; // minimum firmware version for extended RPM command support
static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX-1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX-5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
static constexpr uint16_t VOXL_ESC_EXT_RPM =
39; // minimum firmware version for extended RPM command support
static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX -
1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t)
static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX -
5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t)
//static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); }
//static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); }
@@ -210,7 +211,7 @@ private:
voxl_esc_params_t _parameters;
int update_params();
int load_params(voxl_esc_params_t *params, ch_assign_t *map);
std::string board_id_to_name(int board_id);
const char* board_id_to_name(int board_id);
bool _turtle_mode_en{false};
int32_t _rpm_turtle_min{0};
src/drivers/actuators/voxl_esc/voxl_esc_params.c
+3 -3
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
@@ -236,7 +236,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1);
/**
* UART ESC Temperature Warning Threshold (Degrees C)
*
*
* Only applicable to ESCs that report temperature
*
* @reboot_required true
@@ -251,7 +251,7 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0);
/**
* UART ESC Over-Temperature Threshold (Degrees C)
*
*
* Only applicable to ESCs that report temperature
*
* @reboot_required true
src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp
+19 -110
View File
@@ -40,152 +40,61 @@ VoxlEscSerial::VoxlEscSerial()
VoxlEscSerial::~VoxlEscSerial()
{
if (_uart_fd >= 0) {
uart_close();
if (_uart.isOpen()) {
_uart.close();
}
}
int VoxlEscSerial::uart_open(const char *dev, speed_t speed)
int VoxlEscSerial::uart_open(const char *dev, uint32_t speed)
{
if (_uart_fd >= 0) {
PX4_ERR("Port in use: %s (%i)", dev, errno);
if (_uart.isOpen()) {
PX4_ERR("Port in use: %s", dev);
return -1;
}
/* Open UART */
#ifdef __PX4_QURT
_uart_fd = qurt_uart_open(dev, speed);
#else
_uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
#endif
if (_uart_fd < 0) {
PX4_ERR("Error opening port: %s (%i)", dev, errno);
// Configure UART port
if (! _uart.setPort(dev)) {
PX4_ERR("Error configuring serial device on port %s", dev);
return -1;
}
#ifndef __PX4_QURT
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) {
PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state);
uart_close();
if (! _uart.setBaudrate(speed)) {
PX4_ERR("Error setting baudrate to %d on %s", (int) speed, dev);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(_uart_fd, &_cfg);
/* Disable output post-processing */
_cfg.c_oflag &= ~OPOST;
_cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
_cfg.c_cflag &= ~CSIZE;
_cfg.c_cflag |= CS8; /* 8-bit characters */
_cfg.c_cflag &= ~PARENB; /* no parity bit */
_cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
_cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
_cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
_cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) {
PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state);
uart_close();
// Open the UART. If this is successful then the UART is ready to use.
if (! _uart.open()) {
PX4_ERR("Error opening serial device %s", dev);
return -1;
}
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) {
PX4_ERR("Error configuring port: %s (tcsetattr)", dev);
uart_close();
return -1;
}
#endif
_speed = speed;
return 0;
}
int VoxlEscSerial::uart_set_baud(speed_t speed)
int VoxlEscSerial::uart_set_baud(uint32_t speed)
{
#ifndef __PX4_QURT
if (_uart_fd < 0) {
if (! _uart.setBaudrate(speed)) {
PX4_ERR("Error setting baudrate to %d on %s", (int) speed, _uart.getPort());
return -1;
}
if (cfsetispeed(&_cfg, speed) < 0) {
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) {
return -1;
}
_speed = speed;
return 0;
#endif
return -1;
}
int VoxlEscSerial::uart_close()
{
#ifndef __PX4_QURT
if (_uart_fd < 0) {
PX4_ERR("invalid state for closing");
return -1;
}
if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) {
PX4_ERR("failed restoring uart to original state");
}
if (close(_uart_fd)) {
PX4_ERR("error closing uart");
}
#endif
_uart_fd = -1;
_uart.close();
return 0;
}
int VoxlEscSerial::uart_write(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for writing or buffer");
return -1;
}
#ifdef __PX4_QURT
return qurt_uart_write(_uart_fd, (const char *) buf, len);
#else
return write(_uart_fd, buf, len);
#endif
return _uart.write(buf, len);
}
int VoxlEscSerial::uart_read(FAR void *buf, size_t len)
{
if (_uart_fd < 0 || buf == NULL) {
PX4_ERR("invalid state for reading or buffer");
return -1;
}
#ifdef __PX4_QURT
#define ASYNC_UART_READ_WAIT_US 2000
// The UART read on SLPI is via an asynchronous service so specify a timeout
// for the return. The driver will poll periodically until the read comes in
// so this may block for a while. However, it will timeout if no read comes in.
return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US);
#else
return read(_uart_fd, buf, len);
#endif
return _uart.read((uint8_t *) buf, len);
}
src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp
+11 -19
View File
@@ -34,36 +34,28 @@
#pragma once
#include <px4_log.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <px4_platform_common/Serial.hpp>
#ifdef __PX4_QURT
#include <drivers/device/qurt/uart.h>
#define FAR
#endif
using namespace device;
class VoxlEscSerial
{
public:
VoxlEscSerial();
virtual ~VoxlEscSerial();
int uart_open(const char *dev, speed_t speed);
int uart_set_baud(speed_t speed);
int uart_close();
int uart_write(FAR void *buf, size_t len);
int uart_read(FAR void *buf, size_t len);
bool is_open() { return _uart_fd >= 0; };
int uart_get_baud() {return _speed; }
int uart_open(const char *dev, uint32_t speed);
int uart_set_baud(uint32_t speed);
int uart_close();
int uart_write(FAR void *buf, size_t len);
int uart_read(FAR void *buf, size_t len);
bool is_open() { return _uart.isOpen(); };
uint32_t uart_get_baud() { return _uart.getBaudrate(); }
private:
int _uart_fd = -1;
#if ! defined(__PX4_QURT)
struct termios _orig_cfg;
struct termios _cfg;
#endif
int _speed = -1;
Serial _uart {};
};