2016-02-17 21:25:28 -04:00
|
|
|
#pragma once
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2015-05-04 03:15:12 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2012-12-12 18:04:27 -04:00
|
|
|
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <stdarg.h>
|
2015-05-04 03:15:12 -03:00
|
|
|
#include "AP_HAL_SITL_Namespace.h"
|
2015-11-03 16:59:43 -04:00
|
|
|
#include <AP_HAL/utility/Socket.h>
|
2016-01-10 02:17:32 -04:00
|
|
|
#include <AP_HAL/utility/RingBuffer.h>
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
|
2012-12-12 18:04:27 -04:00
|
|
|
public:
|
2015-05-04 03:15:12 -03:00
|
|
|
friend class HALSITL::SITL_State;
|
2012-12-13 18:57:01 -04:00
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
UARTDriver(const uint8_t portNumber, SITL_State *sitlState) {
|
2015-05-04 21:59:07 -03:00
|
|
|
_portNumber = portNumber;
|
2012-12-17 22:34:13 -04:00
|
|
|
_sitlState = sitlState;
|
2015-05-04 21:59:07 -03:00
|
|
|
|
2012-12-17 22:34:13 -04:00
|
|
|
_fd = -1;
|
|
|
|
_listen_fd = -1;
|
2015-05-04 21:59:07 -03:00
|
|
|
}
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2016-01-10 02:23:32 -04:00
|
|
|
static UARTDriver *from(AP_HAL::UARTDriver *uart) {
|
|
|
|
return static_cast<UARTDriver*>(uart);
|
2016-01-10 02:17:32 -04:00
|
|
|
}
|
|
|
|
|
2012-12-12 18:04:27 -04:00
|
|
|
/* Implementations of UARTDriver virtual methods */
|
2018-11-07 06:59:48 -04:00
|
|
|
void begin(uint32_t b) override {
|
2015-05-04 21:59:07 -03:00
|
|
|
begin(b, 0, 0);
|
|
|
|
}
|
2018-11-07 06:59:48 -04:00
|
|
|
void begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
|
|
|
|
void end() override;
|
|
|
|
void flush() override;
|
|
|
|
bool is_initialized() override {
|
2015-05-04 21:59:07 -03:00
|
|
|
return true;
|
|
|
|
}
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2018-11-07 06:59:48 -04:00
|
|
|
void set_blocking_writes(bool blocking) override
|
2012-12-12 18:04:27 -04:00
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
_nonblocking_writes = !blocking;
|
2012-12-12 18:04:27 -04:00
|
|
|
}
|
|
|
|
|
2018-11-07 06:59:48 -04:00
|
|
|
bool tx_pending() override {
|
2015-05-04 21:59:07 -03:00
|
|
|
return false;
|
2012-12-12 18:04:27 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Implementations of Stream virtual methods */
|
2016-08-02 10:42:50 -03:00
|
|
|
uint32_t available() override;
|
|
|
|
uint32_t txspace() override;
|
|
|
|
int16_t read() override;
|
2012-12-12 18:04:27 -04:00
|
|
|
|
|
|
|
/* Implementations of Print virtual methods */
|
2018-11-07 06:59:48 -04:00
|
|
|
size_t write(uint8_t c) override;
|
|
|
|
size_t write(const uint8_t *buffer, size_t size) override;
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2012-12-18 03:12:05 -04:00
|
|
|
// file descriptor, exposed so SITL_State::loop_hook() can use it
|
2015-05-04 21:59:07 -03:00
|
|
|
int _fd;
|
2012-12-18 03:12:05 -04:00
|
|
|
|
2017-11-22 18:21:50 -04:00
|
|
|
bool _unbuffered_writes;
|
|
|
|
|
2018-11-07 06:59:48 -04:00
|
|
|
enum flow_control get_flow_control(void) override { return FLOW_CONTROL_ENABLE; }
|
2016-01-10 02:17:32 -04:00
|
|
|
|
2017-11-30 16:36:51 -04:00
|
|
|
void configure_parity(uint8_t v) override;
|
|
|
|
void set_stop_bits(int n) override;
|
|
|
|
bool set_unbuffered_writes(bool on) override;
|
2017-11-22 18:21:50 -04:00
|
|
|
|
2018-11-07 06:59:48 -04:00
|
|
|
void _timer_tick(void) override;
|
2018-05-15 21:52:08 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
return timestamp estimate in microseconds for when the start of
|
|
|
|
a nbytes packet arrived on the uart. This should be treated as a
|
|
|
|
time constraint, not an exact time. It is guaranteed that the
|
|
|
|
packet did not start being received after this time, but it
|
|
|
|
could have been in a system buffer before the returned time.
|
|
|
|
|
|
|
|
This takes account of the baudrate of the link. For transports
|
|
|
|
that have no baudrate (such as USB) the time estimate may be
|
|
|
|
less accurate.
|
|
|
|
|
|
|
|
A return value of zero means the HAL does not support this API
|
|
|
|
*/
|
2018-05-16 18:01:14 -03:00
|
|
|
uint64_t receive_time_constraint_us(uint16_t nbytes) override;
|
2016-01-06 04:05:42 -04:00
|
|
|
|
2012-12-12 18:04:27 -04:00
|
|
|
private:
|
|
|
|
uint8_t _portNumber;
|
2015-11-03 16:59:43 -04:00
|
|
|
bool _connected = false; // true if a client has connected
|
|
|
|
bool _use_send_recv = false;
|
2015-05-04 21:59:07 -03:00
|
|
|
int _listen_fd; // socket we are listening on
|
|
|
|
int _serial_port;
|
|
|
|
static bool _console;
|
|
|
|
bool _nonblocking_writes;
|
2016-01-10 02:17:32 -04:00
|
|
|
ByteBuffer _readbuffer{16384};
|
|
|
|
ByteBuffer _writebuffer{16384};
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2016-08-22 04:11:48 -03:00
|
|
|
const char *_uart_path;
|
|
|
|
uint32_t _uart_baudrate;
|
|
|
|
|
2015-05-10 02:36:18 -03:00
|
|
|
// IPv4 address of target for uartC
|
|
|
|
const char *_tcp_client_addr;
|
|
|
|
|
2015-11-03 16:59:43 -04:00
|
|
|
void _tcp_start_connection(uint16_t port, bool wait_for_connection);
|
2016-08-22 04:11:48 -03:00
|
|
|
void _uart_start_connection(void);
|
|
|
|
void _check_reconnect();
|
2015-11-03 16:59:43 -04:00
|
|
|
void _tcp_start_client(const char *address, uint16_t port);
|
2012-12-12 18:04:27 -04:00
|
|
|
void _check_connection(void);
|
|
|
|
static bool _select_check(int );
|
|
|
|
static void _set_nonblocking(int );
|
2017-11-30 16:36:51 -04:00
|
|
|
bool set_speed(int speed);
|
2012-12-12 18:04:27 -04:00
|
|
|
|
2012-12-17 22:34:13 -04:00
|
|
|
SITL_State *_sitlState;
|
2018-05-15 21:52:08 -03:00
|
|
|
uint64_t _receive_timestamp;
|
2012-12-12 18:04:27 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
#endif
|