mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_PX4: support SBusOut
add unbuffered writes and uart config
This commit is contained in:
parent
66c4b7d986
commit
12ea8efa84
|
@ -574,7 +574,7 @@ void PX4RCOutput::timer_tick(void)
|
||||||
/*
|
/*
|
||||||
enable sbus output
|
enable sbus output
|
||||||
*/
|
*/
|
||||||
bool PX4RCOutput::enable_sbus_out(uint16_t rate_hz)
|
bool PX4RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
|
||||||
{
|
{
|
||||||
int fd = open("/dev/px4io", 0);
|
int fd = open("/dev/px4io", 0);
|
||||||
if (fd == -1) {
|
if (fd == -1) {
|
||||||
|
|
|
@ -38,7 +38,7 @@ public:
|
||||||
void set_output_mode(enum output_mode mode) override;
|
void set_output_mode(enum output_mode mode) override;
|
||||||
|
|
||||||
void timer_tick(void) override;
|
void timer_tick(void) override;
|
||||||
bool enable_sbus_out(uint16_t rate_hz) override;
|
bool enable_px4io_sbus_out(uint16_t rate_hz) override;
|
||||||
|
|
||||||
// set default output update rate
|
// set default output update rate
|
||||||
void set_default_rate(uint16_t rate_hz) override;
|
void set_default_rate(uint16_t rate_hz) override;
|
||||||
|
|
|
@ -28,7 +28,8 @@ PX4UARTDriver::PX4UARTDriver(const char *devpath, const char *perf_name) :
|
||||||
_in_timer(false),
|
_in_timer(false),
|
||||||
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
|
_perf_uart(perf_alloc(PC_ELAPSED, perf_name)),
|
||||||
_os_start_auto_space(-1),
|
_os_start_auto_space(-1),
|
||||||
_flow_control(FLOW_CONTROL_DISABLE)
|
_flow_control(FLOW_CONTROL_DISABLE),
|
||||||
|
_unbuffered_writes(false)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -153,6 +154,44 @@ void PX4UARTDriver::set_flow_control(enum flow_control fcontrol)
|
||||||
_flow_control = fcontrol;
|
_flow_control = fcontrol;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PX4UARTDriver::configure_parity(uint8_t v) {
|
||||||
|
if (_fd == -1) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
struct termios t;
|
||||||
|
tcgetattr(_fd, &t);
|
||||||
|
if (v != 0) {
|
||||||
|
// enable parity
|
||||||
|
t.c_cflag |= PARENB;
|
||||||
|
if (v == 1) {
|
||||||
|
t.c_cflag |= PARODD;
|
||||||
|
} else {
|
||||||
|
t.c_cflag &= ~PARODD;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// disable parity
|
||||||
|
t.c_cflag &= ~PARENB;
|
||||||
|
}
|
||||||
|
tcsetattr(_fd, TCSANOW, &t);
|
||||||
|
}
|
||||||
|
|
||||||
|
void PX4UARTDriver::set_stop_bits(int n) {
|
||||||
|
if (_fd == -1) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
struct termios t;
|
||||||
|
tcgetattr(_fd, &t);
|
||||||
|
if (n > 1) t.c_cflag |= CSTOPB;
|
||||||
|
else t.c_cflag &= ~CSTOPB;
|
||||||
|
tcsetattr(_fd, TCSANOW, &t);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PX4UARTDriver::set_unbuffered_writes(bool on) {
|
||||||
|
_unbuffered_writes = on;
|
||||||
|
return _unbuffered_writes;
|
||||||
|
}
|
||||||
|
|
||||||
void PX4UARTDriver::begin(uint32_t b)
|
void PX4UARTDriver::begin(uint32_t b)
|
||||||
{
|
{
|
||||||
begin(b, 0, 0);
|
begin(b, 0, 0);
|
||||||
|
@ -256,7 +295,7 @@ int16_t PX4UARTDriver::read()
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
write one byte to the buffer
|
write one byte
|
||||||
*/
|
*/
|
||||||
size_t PX4UARTDriver::write(uint8_t c)
|
size_t PX4UARTDriver::write(uint8_t c)
|
||||||
{
|
{
|
||||||
|
@ -268,6 +307,11 @@ size_t PX4UARTDriver::write(uint8_t c)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_unbuffered_writes) {
|
||||||
|
// write one byte to the file descriptor
|
||||||
|
return _write_fd(&c, 1);
|
||||||
|
}
|
||||||
|
|
||||||
while (_writebuf.space() == 0) {
|
while (_writebuf.space() == 0) {
|
||||||
if (_nonblocking_writes) {
|
if (_nonblocking_writes) {
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -278,14 +322,14 @@ size_t PX4UARTDriver::write(uint8_t c)
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
write size bytes to the write buffer
|
* write size bytes
|
||||||
*/
|
*/
|
||||||
size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
|
size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
|
||||||
{
|
{
|
||||||
if (_uart_owner_pid != getpid()){
|
if (_uart_owner_pid != getpid()){
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
if (!_initialised) {
|
if (!_initialised) {
|
||||||
try_initialise();
|
try_initialise();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -302,6 +346,11 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_unbuffered_writes) {
|
||||||
|
// write buffer straight to the file descriptor
|
||||||
|
return _write_fd(buffer, size);
|
||||||
|
}
|
||||||
|
|
||||||
return _writebuf.write(buffer, size);
|
return _writebuf.write(buffer, size);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -39,6 +39,10 @@ public:
|
||||||
void set_flow_control(enum flow_control flow_control);
|
void set_flow_control(enum flow_control flow_control);
|
||||||
enum flow_control get_flow_control(void) { return _flow_control; }
|
enum flow_control get_flow_control(void) { return _flow_control; }
|
||||||
|
|
||||||
|
void configure_parity(uint8_t v);
|
||||||
|
void set_stop_bits(int n);
|
||||||
|
bool set_unbuffered_writes(bool on);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const char *_devpath;
|
const char *_devpath;
|
||||||
int _fd;
|
int _fd;
|
||||||
|
@ -47,6 +51,7 @@ private:
|
||||||
volatile bool _in_timer;
|
volatile bool _in_timer;
|
||||||
|
|
||||||
bool _nonblocking_writes;
|
bool _nonblocking_writes;
|
||||||
|
bool _unbuffered_writes;
|
||||||
|
|
||||||
// we use in-task ring buffers to reduce the system call cost
|
// we use in-task ring buffers to reduce the system call cost
|
||||||
// of ::read() and ::write() in the main loop
|
// of ::read() and ::write() in the main loop
|
||||||
|
|
Loading…
Reference in New Issue