2015-10-20 14:04:59 -03:00
|
|
|
#include "RPIOUARTDriver.h"
|
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
#include <stdlib.h>
|
|
|
|
#include <cstdio>
|
2015-10-20 14:04:59 -03:00
|
|
|
|
2016-05-17 23:26:57 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2016-10-05 12:56:34 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2015-08-18 00:29:58 -03:00
|
|
|
|
|
|
|
#include "px4io_protocol.h"
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
#define RPIOUART_DEBUG 0
|
|
|
|
|
|
|
|
#include <cassert>
|
|
|
|
|
|
|
|
#if RPIOUART_DEBUG
|
|
|
|
#define debug(fmt, args ...) do {hal.console->printf("[RPIOUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
|
|
#define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
|
|
#else
|
2016-05-17 23:26:57 -03:00
|
|
|
#define debug(fmt, args ...)
|
|
|
|
#define error(fmt, args ...)
|
2015-08-18 00:29:58 -03:00
|
|
|
#endif
|
|
|
|
|
|
|
|
using namespace Linux;
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
RPIOUARTDriver::RPIOUARTDriver() :
|
|
|
|
UARTDriver(false),
|
2016-04-26 15:17:35 -03:00
|
|
|
_dev(nullptr),
|
2015-08-18 00:29:58 -03:00
|
|
|
_external(false),
|
|
|
|
_need_set_baud(false),
|
|
|
|
_baudrate(0)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
bool RPIOUARTDriver::isExternal()
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
return _external;
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
//hal.console->printf("[RPIOUARTDriver]: begin \n");
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (device_path != nullptr) {
|
2015-10-20 18:13:25 -03:00
|
|
|
UARTDriver::begin(b,rxS,txS);
|
2015-08-18 00:29:58 -03:00
|
|
|
if ( is_initialized()) {
|
|
|
|
_external = true;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (rxS < 1024) {
|
|
|
|
rxS = 2048;
|
|
|
|
}
|
|
|
|
if (txS < 1024) {
|
|
|
|
txS = 2048;
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_initialised = false;
|
|
|
|
while (_in_timer) hal.scheduler->delay(1);
|
|
|
|
|
2016-07-29 21:40:56 -03:00
|
|
|
_readbuf.set_size(rxS);
|
|
|
|
_writebuf.set_size(txS);
|
2015-08-18 00:29:58 -03:00
|
|
|
|
2016-11-17 21:05:30 -04:00
|
|
|
if (!_registered_callback) {
|
|
|
|
_dev = hal.spi->get_device("raspio");
|
|
|
|
_registered_callback = true;
|
2017-01-13 15:26:14 -04:00
|
|
|
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&RPIOUARTDriver::_bus_timer, void));
|
2016-11-17 21:05:30 -04:00
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
/* set baudrate */
|
|
|
|
_baudrate = b;
|
|
|
|
_need_set_baud = true;
|
|
|
|
|
2016-07-29 21:40:56 -03:00
|
|
|
if (_writebuf.get_size() && _readbuf.get_size()) {
|
2015-08-18 00:29:58 -03:00
|
|
|
_initialised = true;
|
|
|
|
}
|
2016-11-17 21:05:30 -04:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
if (_external) {
|
2015-10-20 18:13:25 -03:00
|
|
|
return UARTDriver::_write_fd(buf, n);
|
2016-05-17 23:26:57 -03:00
|
|
|
}
|
2015-08-18 00:29:58 -03:00
|
|
|
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
int RPIOUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
if (_external) {
|
2015-10-20 18:13:25 -03:00
|
|
|
return UARTDriver::_read_fd(buf, n);
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2015-10-20 18:13:25 -03:00
|
|
|
void RPIOUARTDriver::_timer_tick(void)
|
2015-08-18 00:29:58 -03:00
|
|
|
{
|
|
|
|
if (_external) {
|
2015-10-20 18:13:25 -03:00
|
|
|
UARTDriver::_timer_tick();
|
2015-08-18 00:29:58 -03:00
|
|
|
return;
|
|
|
|
}
|
2016-11-17 21:05:30 -04:00
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2017-01-13 15:26:14 -04:00
|
|
|
void RPIOUARTDriver::_bus_timer(void)
|
2016-11-17 21:05:30 -04:00
|
|
|
{
|
2015-08-18 00:29:58 -03:00
|
|
|
/* set the baudrate of raspilotio */
|
|
|
|
if (_need_set_baud) {
|
|
|
|
if (_baudrate != 0) {
|
2016-10-27 07:06:34 -03:00
|
|
|
struct IOPacket _packet_tx = {0}, _packet_rx = {0};
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
_packet_tx.count_code = 2 | PKT_CODE_WRITE;
|
|
|
|
_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
|
|
|
|
_packet_tx.regs[0] = _baudrate & 0xffff;
|
|
|
|
_packet_tx.regs[1] = _baudrate >> 16;
|
|
|
|
_packet_tx.crc = crc_packet(&_packet_tx);
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
|
|
|
|
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_need_set_baud = false;
|
|
|
|
}
|
|
|
|
/* finish set */
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-11-17 21:05:30 -04:00
|
|
|
if (!_initialised) {
|
2017-01-13 15:26:14 -04:00
|
|
|
return;
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_in_timer = true;
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
struct IOPacket _packet_tx = {0}, _packet_rx = {0};
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
/* get write_buf bytes */
|
2016-11-16 13:52:40 -04:00
|
|
|
uint32_t max_size = MIN((uint32_t) PKT_MAX_REGS * 2,
|
2016-11-17 21:05:30 -04:00
|
|
|
_baudrate / 10 / (1000000 / 100000));
|
2016-10-05 12:56:34 -03:00
|
|
|
uint32_t n = MIN(_writebuf.available(), max_size);
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
_writebuf.read(&((uint8_t *)_packet_tx.regs)[0], n);
|
|
|
|
_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART;
|
|
|
|
_packet_tx.page = PX4IO_PAGE_UART_BUFFER;
|
|
|
|
_packet_tx.offset = n;
|
|
|
|
_packet_tx.crc = crc_packet(&_packet_tx);
|
2015-08-18 00:29:58 -03:00
|
|
|
/* end get write_buf bytes */
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
/* set raspilotio to read uart data */
|
2016-10-27 07:06:34 -03:00
|
|
|
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
|
|
|
|
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
hal.scheduler->delay_microseconds(100);
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
/* get uart data from raspilotio */
|
2016-10-27 07:06:34 -03:00
|
|
|
memset(&_packet_tx, 0, sizeof(_packet_tx));
|
|
|
|
_packet_tx.count_code = PKT_CODE_READ;
|
|
|
|
_packet_tx.crc = crc_packet(&_packet_tx);
|
|
|
|
_dev->transfer((uint8_t *)&_packet_tx, sizeof(_packet_tx),
|
|
|
|
(uint8_t *)&_packet_rx, sizeof(_packet_rx));
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
hal.scheduler->delay_microseconds(100);
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
if (_packet_rx.page == PX4IO_PAGE_UART_BUFFER) {
|
2016-10-05 12:56:34 -03:00
|
|
|
/* add bytes to read buf */
|
2016-10-27 07:06:34 -03:00
|
|
|
max_size = MIN(_packet_rx.offset, PKT_MAX_REGS * 2);
|
2016-10-05 12:56:34 -03:00
|
|
|
n = MIN(_readbuf.space(), max_size);
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2016-10-27 07:06:34 -03:00
|
|
|
_readbuf.write(&((uint8_t *)_packet_rx.regs)[0], n);
|
2015-08-18 00:29:58 -03:00
|
|
|
}
|
2016-05-17 23:26:57 -03:00
|
|
|
|
2015-08-18 00:29:58 -03:00
|
|
|
_in_timer = false;
|
|
|
|
}
|