mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 07:28:54 +08:00
voxl2_io: Updated to latest version from ModalAI fork
- Updated to the latest version of the voxl2_io driver from the ModalAI fork. - Moved to the platform independent Serial driver - Added voxl2_io driver to the SLPI DSP build
This commit is contained in:
Eric Katzfey
@@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||
CONFIG_DRIVERS_RC_CRSF_RC=y
|
||||
CONFIG_DRIVERS_VOXL2_IO=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
|
||||
@@ -37,15 +37,12 @@ px4_add_module(
|
||||
SRCS
|
||||
voxl2_io_crc16.c
|
||||
voxl2_io_crc16.h
|
||||
voxl2_io_serial.cpp
|
||||
voxl2_io_serial.hpp
|
||||
voxl2_io_packet.c
|
||||
voxl2_io_packet.h
|
||||
voxl2_io_packet_types.h
|
||||
voxl2_io.cpp
|
||||
voxl2_io.hpp
|
||||
DEPENDS
|
||||
rc
|
||||
px4_work_queue
|
||||
mixer_module
|
||||
MODULE_CONFIG
|
||||
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO
|
||||
bool "voxl2_io"
|
||||
default n
|
||||
---help---
|
||||
Enable support for voxl2_io
|
||||
Enable support for voxl2_io
|
||||
@@ -1,12 +1,13 @@
|
||||
module_name: VOXL2 IO Output
|
||||
actuator_output:
|
||||
config_parameters:
|
||||
- param: 'VOXL2_IO_MIN'
|
||||
label: 'PWM min value'
|
||||
- param: 'VOXL2_IO_MAX'
|
||||
label: 'PWM max value'
|
||||
# config_parameters:
|
||||
# - param: 'VOXL2_IO_MIN'
|
||||
# label: 'PWM min value'
|
||||
# - param: 'VOXL2_IO_MAX'
|
||||
# label: 'PWM max value'
|
||||
output_groups:
|
||||
- param_prefix: VOXL2_IO
|
||||
group_label: 'PWMs'
|
||||
channel_label: 'PWM Channel'
|
||||
num_channels: 4
|
||||
num_channels: 8
|
||||
|
||||
|
||||
+391
-653
File diff suppressed because it is too large
Load Diff
@@ -35,6 +35,7 @@
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
@@ -49,16 +50,17 @@
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/Serial.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
|
||||
#include "voxl2_io_serial.hpp"
|
||||
|
||||
#include "voxl2_io_packet.h"
|
||||
#include "voxl2_io_packet_types.h"
|
||||
|
||||
using namespace device;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Voxl2IO final : public ModuleBase<Voxl2IO>, public OutputModuleInterface
|
||||
@@ -85,8 +87,8 @@ public:
|
||||
|
||||
virtual int init();
|
||||
|
||||
void update_pwm_config();
|
||||
int get_version_info();
|
||||
void print_params();
|
||||
|
||||
struct Command {
|
||||
uint16_t id = 0;
|
||||
@@ -105,8 +107,8 @@ public:
|
||||
void clear() { len = 0; }
|
||||
};
|
||||
|
||||
int send_cmd_thread_safe(Command *cmd);
|
||||
int receive_sbus();
|
||||
int receive_uart_packets();
|
||||
int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len);
|
||||
|
||||
void fill_rc_in(uint16_t raw_rc_count_local,
|
||||
uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
|
||||
@@ -114,53 +116,42 @@ public:
|
||||
unsigned frame_drops, int rssi, input_rc_s &input_rc);
|
||||
private:
|
||||
void Run() override;
|
||||
bool stop_all_pwms();
|
||||
|
||||
/* PWM Parameters */
|
||||
static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off
|
||||
static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes
|
||||
static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1;
|
||||
static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600;
|
||||
static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4;
|
||||
static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0;
|
||||
|
||||
static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200;
|
||||
static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000;
|
||||
|
||||
static constexpr uint16_t DISARMED_VALUE = 0;
|
||||
|
||||
static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0;
|
||||
static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800;
|
||||
static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0;
|
||||
static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0;
|
||||
|
||||
static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000;
|
||||
static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000;
|
||||
static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900;
|
||||
static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks
|
||||
static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600;
|
||||
static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8;
|
||||
static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE =
|
||||
UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function
|
||||
|
||||
/* SBUS */
|
||||
static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30;
|
||||
static constexpr uint16_t SBUS_PAYLOAD = 3;
|
||||
|
||||
//packet packing function accepts pulse width in nanoseconds
|
||||
//assuming the mixer outputs in microseconds, need to convert
|
||||
static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000; //standard pwm
|
||||
//static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125; //oneshot125
|
||||
|
||||
/* M0065 version info */
|
||||
static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6;
|
||||
static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1;
|
||||
static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35;
|
||||
VOXL2_IO_VERSION_INFO _version_info;
|
||||
static constexpr uint16_t VOXL2_IO_SW_VERSION = 2;
|
||||
static constexpr uint16_t VOXL2_IO_HW_VERSION = 35; //this is the version of the HW
|
||||
int _board_detect_retries{3};
|
||||
VOXL2_IO_EXTENDED_VERSION_INFO _version_info;
|
||||
|
||||
/* Module update interval */
|
||||
static constexpr unsigned _current_update_interval{4000}; // 250 Hz
|
||||
|
||||
typedef struct {
|
||||
int32_t config{VOXL2_IO_CONFIG};
|
||||
int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD};
|
||||
int32_t pwm_min{VOXL2_IO_DEFAULT_MIN};
|
||||
int32_t pwm_max{VOXL2_IO_DEFAULT_MAX};
|
||||
int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE};
|
||||
int32_t param_rc_input_proto{0};
|
||||
int32_t pwm_min{0};
|
||||
int32_t pwm_max{0};
|
||||
int32_t pwm_dis{0};
|
||||
int32_t pwm_cal_min{0};
|
||||
int32_t pwm_cal_max{0};
|
||||
//int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE};
|
||||
//int32_t param_rc_input_proto{0};
|
||||
int32_t param_rc_rssi_pwm_chan{0};
|
||||
int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0};
|
||||
int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0};
|
||||
int32_t verbose_logging{0};
|
||||
} voxl2_io_params_t;
|
||||
voxl2_io_params_t _parameters;
|
||||
@@ -179,20 +170,18 @@ private:
|
||||
SCAN
|
||||
} _rc_mode{RC_MODE::SCAN};
|
||||
|
||||
/* QUP7, VOXL2 J19, /dev/slpi-uart-7*/
|
||||
char _device[10] {VOXL2_IO_DEFAULT_PORT};
|
||||
Voxl2IoSerial *_uart_port;
|
||||
Serial _uart_port{};
|
||||
|
||||
/* Mixer output */
|
||||
MixingOutput _mixing_output;
|
||||
|
||||
/* RC input */
|
||||
VOXL2_IOPacket _sbus_packet;
|
||||
uint64_t _rc_last_valid;
|
||||
VOXL2_IOPacket _voxl2_io_packet;
|
||||
uint64_t _rc_last_valid_time;
|
||||
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX};
|
||||
unsigned _sbus_frame_drops{0};
|
||||
uint16_t _sbus_total_frames{0};
|
||||
bool _new_packet{false};
|
||||
unsigned int _sbus_frame_drops{0};
|
||||
uint32_t _sbus_total_frames{0};
|
||||
|
||||
/* Publications */
|
||||
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
|
||||
@@ -201,17 +190,12 @@ private:
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
bool _pwm_on{false};
|
||||
int32_t _pwm_fullscale{0};
|
||||
int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0};
|
||||
bool _outputs_disabled{false};
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _output_update_perf;
|
||||
|
||||
bool _debug{false};
|
||||
uint16_t _cmd_id{0};
|
||||
Command _current_cmd;
|
||||
px4::atomic<Command *> _pending_cmd{nullptr};
|
||||
|
||||
static const uint8_t READ_BUF_SIZE = 128;
|
||||
uint8_t _read_buf[READ_BUF_SIZE];
|
||||
@@ -220,9 +204,8 @@ private:
|
||||
uint32_t _packets_sent{0};
|
||||
uint32_t _packets_received{0};
|
||||
|
||||
int parse_response(uint8_t *buf, uint8_t len);
|
||||
int load_params(voxl2_io_params_t *params);
|
||||
int update_params();
|
||||
int flush_uart_rx();
|
||||
int calibrate_escs();
|
||||
std::string board_id_to_name(int board_id);
|
||||
};
|
||||
|
||||
@@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2,
|
||||
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size);
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
uint8_t command_type;
|
||||
//uint8_t channel_offset;
|
||||
uint16_t vals[8]; //could be 1,2,4,6,8
|
||||
} __attribute__((__packed__)) pwm_hires_cmd_t;
|
||||
|
||||
pwm_hires_cmd_t hires_cmd;
|
||||
|
||||
int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size)
|
||||
{
|
||||
if (cmd_cnt > 8) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
hires_cmd.command_type = 0;
|
||||
//hires_cmd.channel_offset = 0;
|
||||
|
||||
//resolution of commands in the packet is 0.05us = 50ns
|
||||
for (uint32_t idx = 0; idx < cmd_cnt; idx++) {
|
||||
hires_cmd.vals[idx] = pwm_val_ns[idx] / 50;
|
||||
}
|
||||
|
||||
return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out,
|
||||
out_size);
|
||||
}
|
||||
|
||||
int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size)
|
||||
{
|
||||
uint16_t packet_size = size + 5;
|
||||
|
||||
@@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in
|
||||
uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3,
|
||||
uint8_t *out, uint16_t out_size);
|
||||
|
||||
int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size);
|
||||
|
||||
|
||||
// Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id)
|
||||
// Return value is the length of generated packet (if positive), otherwise error code
|
||||
int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3,
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
#define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3
|
||||
#define VOXL2_IO_PACKET_TYPE_STEP_CMD 4
|
||||
#define VOXL2_IO_PACKET_TYPE_LED_CMD 5
|
||||
#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD 6
|
||||
#define VOXL2_IO_PACKET_TYPE_RESET_CMD 10
|
||||
#define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11
|
||||
#define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12
|
||||
|
||||
@@ -32,34 +32,73 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* UART M0065 baud rate
|
||||
* VOXL2_IO UART baud rate
|
||||
*
|
||||
* Default rate is 921600, which is used for communicating with M0065.
|
||||
* Default rate is 921600, which is used for communicating with VOXL2_IO board
|
||||
*
|
||||
* @group VOXL2 IO
|
||||
* @unit bit/s
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600);
|
||||
|
||||
|
||||
/**
|
||||
* M0065 PWM Min
|
||||
* VOXL2_IO Disabled PWM
|
||||
*
|
||||
* Minimum duration (microseconds) for M0065 PWM
|
||||
* Pulse duration in disabled state (microseconds) for VOXL2_IO board
|
||||
*
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @group VOXL2 IO
|
||||
* @unit us
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000);
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000);
|
||||
|
||||
/**
|
||||
* M0065 PWM Max
|
||||
* VOXL2_IO Min PWM
|
||||
*
|
||||
* Maximum duration (microseconds) for M0065 PWM
|
||||
* Minimum duration (microseconds) for VOXL2_IO board
|
||||
*
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @group VOXL2 IO
|
||||
* @unit us
|
||||
*/
|
||||
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100);
|
||||
|
||||
/**
|
||||
* VOXL2_IO Max PWM
|
||||
*
|
||||
* Maximum duration (microseconds) for VOXL2_IO board
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @group VOXL2 IO
|
||||
* @unit us
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000);
|
||||
|
||||
|
||||
/**
|
||||
* VOXL2_IO Calibration Min PWM
|
||||
*
|
||||
* Minimum duration (microseconds) for VOXL2_IO board
|
||||
*
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @group VOXL2 IO
|
||||
* @unit us
|
||||
*/
|
||||
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050);
|
||||
|
||||
/**
|
||||
* VOXL2_IO Calibration Max PWM
|
||||
*
|
||||
* Maximum duration (microseconds) for VOXL2_IO board
|
||||
* @min 0
|
||||
* @max 2000
|
||||
* @group VOXL2 IO
|
||||
* @unit us
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000);
|
||||
|
||||
@@ -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 "voxl2_io_serial.hpp"
|
||||
|
||||
Voxl2IoSerial::Voxl2IoSerial()
|
||||
{
|
||||
}
|
||||
|
||||
Voxl2IoSerial::~Voxl2IoSerial()
|
||||
{
|
||||
if (_uart_fd >= 0) {
|
||||
uart_close();
|
||||
}
|
||||
}
|
||||
|
||||
int Voxl2IoSerial::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 Voxl2IoSerial::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 Voxl2IoSerial::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 Voxl2IoSerial::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 Voxl2IoSerial::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 Voxl2IoSerial
|
||||
{
|
||||
public:
|
||||
Voxl2IoSerial();
|
||||
virtual ~Voxl2IoSerial();
|
||||
|
||||
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;
|
||||
};
|
||||
Reference in New Issue
Block a user