Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-23 12:18:12 +08:00
Code Issues Actions 41 Packages Projects Releases Wiki Activity

Voxl ESC driver update (#23022)

Browse Source 
* 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
2024-04-17 15:09:35 -04:00
committed by GitHub
parent a8a67fbf8f
commit 2dccd6cacb
12 changed files with 355 additions and 403 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/include/px4_platform_common/Serial.hpp
+1
View File
@@ -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;
platforms/nuttx/src/px4/common/SerialImpl.cpp
+1 -1
View File
@@ -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);
platforms/nuttx/src/px4/common/include/SerialImpl.hpp
+1
View File
@@ -114,6 +114,7 @@ private:
bool _single_wire_mode{false};
bool _swap_rx_tx_mode{false};
bool _inverted_mode{false};
};
} // namespace device
platforms/posix/include/SerialImpl.hpp
+1
View File
@@ -114,6 +114,7 @@ private:
bool _single_wire_mode{false};
bool _swap_rx_tx_mode{false};
bool _inverted_mode{false};
};
} // namespace device
platforms/posix/src/px4/common/SerialImpl.cpp
+1 -1
View File
@@ -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);
platforms/qurt/src/px4/SerialImpl.cpp
+1 -1
View File
@@ -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);
src/drivers/actuators/voxl_esc/CMakeLists.txt
-2
View File
@@ -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
src/drivers/actuators/voxl_esc/voxl_esc.cpp
+286 -116
View File
File diff suppressed because it is too large Load Diff
src/drivers/actuators/voxl_esc/voxl_esc.hpp
+31 -21
View File
@@ -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();
};
src/drivers/actuators/voxl_esc/voxl_esc_params.c
+32 -1
View File
@@ -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);
src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp
-191
View File
@@ -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
}
src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp
-69
View File
@@ -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;
};