mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
More implementation for the serial side on IO; fix a couple of bugs on the FMU side.
Still needs serial init and some more testing/config on the FMU side, but closer to being ready to test.
This commit is contained in:
px4dev
parent
90c458cb61
commit
d1562f926f
@ -56,6 +56,12 @@
|
||||
******************************************************************************/
|
||||
/* Configuration **************************************************************/
|
||||
|
||||
/******************************************************************************
|
||||
* Serial
|
||||
******************************************************************************/
|
||||
#define SERIAL_BASE STM32_USART1_BASE
|
||||
#define SERIAL_VECTOR STM32_IRQ_USART1
|
||||
|
||||
/******************************************************************************
|
||||
* GPIOS
|
||||
******************************************************************************/
|
||||
|
||||
@ -160,6 +160,8 @@ PX4IO_serial::PX4IO_serial(int port) :
|
||||
return;
|
||||
}
|
||||
|
||||
/* XXX need to configure the port here */
|
||||
|
||||
/* need space for worst-case escapes + hx protocol overhead */
|
||||
/* XXX this is kinda gross, but hx transmits a byte at a time */
|
||||
_tx_buf = new uint8_t[HX_STREAM_MAX_FRAME];
|
||||
@ -257,7 +259,7 @@ PX4IO_serial::get_reg(uint8_t page, uint8_t offset, uint16_t *values, unsigned n
|
||||
return -EIO;
|
||||
} else {
|
||||
/* copy back the result */
|
||||
memcpy(values, &_tx_buf[0], count);
|
||||
memcpy(values, &_rx_buf[0], count);
|
||||
}
|
||||
out:
|
||||
sem_post(&_bus_semaphore);
|
||||
@ -267,16 +269,12 @@ out:
|
||||
int
|
||||
PX4IO_serial::_wait_complete()
|
||||
{
|
||||
/* prepare the stream for transmission */
|
||||
/* prepare the stream for transmission (also discards any received noise) */
|
||||
hx_stream_reset(_stream);
|
||||
hx_stream_start(_stream, _tx_buf, _tx_size);
|
||||
|
||||
/* enable UART */
|
||||
_CR1() |= USART_CR1_RE |
|
||||
USART_CR1_TE |
|
||||
USART_CR1_TXEIE |
|
||||
USART_CR1_RXNEIE |
|
||||
USART_CR1_UE;
|
||||
/* enable transmit-ready interrupt, which will start transmission */
|
||||
_CR1() |= USART_CR1_TXEIE;
|
||||
|
||||
/* compute the deadline for a 5ms timeout */
|
||||
struct timespec abstime;
|
||||
@ -290,13 +288,6 @@ PX4IO_serial::_wait_complete()
|
||||
/* wait for the transaction to complete */
|
||||
int ret = sem_timedwait(&_completion_semaphore, &abstime);
|
||||
|
||||
/* disable the UART */
|
||||
_CR1() &= ~(USART_CR1_RE |
|
||||
USART_CR1_TE |
|
||||
USART_CR1_TXEIE |
|
||||
USART_CR1_RXNEIE |
|
||||
USART_CR1_UE);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -317,7 +308,7 @@ PX4IO_serial::_do_interrupt()
|
||||
if (sr & USART_SR_TXE) {
|
||||
int c = hx_stream_send_next(_stream);
|
||||
if (c == -1) {
|
||||
/* transmit (nearly) done, not interested in TX-ready interrupts now */
|
||||
/* no more bytes to send, not interested in interrupts now */
|
||||
_CR1() &= ~USART_CR1_TXEIE;
|
||||
|
||||
/* was this a tx-only operation? */
|
||||
|
||||
@ -42,7 +42,12 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <drivers/boards/px4io/px4io_internal.h>
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IO
|
||||
# include <drivers/boards/px4io/px4io_internal.h>
|
||||
#endif
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IOV2
|
||||
# include <drivers/boards/px4iov2/px4iov2_internal.h>
|
||||
#endif
|
||||
|
||||
#include "protocol.h"
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include <string.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "px4io.h"
|
||||
#include "protocol.h"
|
||||
@ -349,10 +350,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
||||
case PX4IO_P_SETUP_RELAYS:
|
||||
value &= PX4IO_P_SETUP_RELAYS_VALID;
|
||||
r_setup_relays = value;
|
||||
#ifdef POWER_RELAY1
|
||||
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
|
||||
#endif
|
||||
#ifdef POWER_RELAY2
|
||||
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
|
||||
#endif
|
||||
#ifdef POWER_ACC1
|
||||
POWER_ACC1(value & (1 << 2) ? 1 : 0);
|
||||
#endif
|
||||
#ifdef POWER_ACC2
|
||||
POWER_ACC2(value & (1 << 3) ? 1 : 0);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_SET_DEBUG:
|
||||
|
||||
@ -46,36 +46,40 @@
|
||||
#include <nuttx/arch.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
/* XXX might be able to prune these */
|
||||
#include <chip.h>
|
||||
#include <up_internal.h>
|
||||
#include <up_arch.h>
|
||||
#include <stm32_internal.h>
|
||||
#include <systemlib/hx_stream.h>
|
||||
|
||||
//#define DEBUG
|
||||
#include "px4io.h"
|
||||
|
||||
static uint8_t tx_buf[66]; /* XXX hardcoded magic number */
|
||||
|
||||
static hx_stream_t if_stream;
|
||||
static volatile bool sending = false;
|
||||
|
||||
static int serial_interrupt(int vector, void *context);
|
||||
static void serial_callback(void *arg, const void *data, unsigned length);
|
||||
|
||||
/* serial register accessors */
|
||||
#define REG(_x) (*(volatile uint32_t *)(SERIAL_BASE + _x))
|
||||
#define rSR REG(STM32_USART_SR_OFFSET)
|
||||
#define rDR REG(STM32_USART_DR_OFFSET)
|
||||
#define rBRR REG(STM32_USART_BRR_OFFSET)
|
||||
#define rCR1 REG(STM32_USART_CR1_OFFSET)
|
||||
#define rCR2 REG(STM32_USART_CR2_OFFSET)
|
||||
#define rCR3 REG(STM32_USART_CR3_OFFSET)
|
||||
#define rGTPR REG(STM32_USART_GTPR_OFFSET)
|
||||
|
||||
void
|
||||
interface_init(void)
|
||||
{
|
||||
|
||||
int fd = open("/dev/ttyS1", O_RDWR, O_NONBLOCK);
|
||||
if (fd < 0) {
|
||||
debug("serial fail");
|
||||
return;
|
||||
}
|
||||
/* XXX do serial port init here */
|
||||
|
||||
/* configure serial port - XXX increase port speed? */
|
||||
struct termios t;
|
||||
tcgetattr(fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(fd, TCSANOW, &t);
|
||||
|
||||
/* allocate the HX stream we'll use for communication */
|
||||
if_stream = hx_stream_init(fd, serial_callback, NULL);
|
||||
irq_attach(SERIAL_VECTOR, serial_interrupt);
|
||||
if_stream = hx_stream_init(-1, serial_callback, NULL);
|
||||
|
||||
/* XXX add stream stats counters? */
|
||||
|
||||
@ -85,8 +89,31 @@ interface_init(void)
|
||||
void
|
||||
interface_tick()
|
||||
{
|
||||
/* process incoming bytes */
|
||||
hx_stream_rx(if_stream);
|
||||
/* XXX nothing interesting to do here */
|
||||
}
|
||||
|
||||
static int
|
||||
serial_interrupt(int vector, void *context)
|
||||
{
|
||||
uint32_t sr = rSR;
|
||||
|
||||
if (sr & USART_SR_TXE) {
|
||||
int c = hx_stream_send_next(if_stream);
|
||||
if (c == -1) {
|
||||
/* no more bytes to send, not interested in interrupts now */
|
||||
rCR1 &= ~USART_CR1_TXEIE;
|
||||
sending = false;
|
||||
} else {
|
||||
rDR = c;
|
||||
}
|
||||
}
|
||||
|
||||
if (sr & USART_SR_RXNE) {
|
||||
uint8_t c = rDR;
|
||||
|
||||
hx_stream_rx(if_stream, c);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void
|
||||
@ -98,36 +125,40 @@ serial_callback(void *arg, const void *data, unsigned length)
|
||||
if (length < 2)
|
||||
return;
|
||||
|
||||
/* it's a write operation, pass it to the register API */
|
||||
if (message[0] & PX4IO_PAGE_WRITE) {
|
||||
registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
|
||||
(const uint16_t *)&message[2],
|
||||
(length - 2) / 2);
|
||||
|
||||
/* we got a new request while we were still sending the last reply - not supported */
|
||||
if (sending)
|
||||
return;
|
||||
|
||||
/* reads are page / offset / length */
|
||||
if (length == 3) {
|
||||
uint16_t *registers;
|
||||
unsigned count;
|
||||
|
||||
/* get registers for response, send an empty reply on error */
|
||||
if (registers_get(message[0], message[1], ®isters, &count) < 0)
|
||||
count = 0;
|
||||
|
||||
/* constrain count to requested size or message limit */
|
||||
if (count > message[2])
|
||||
count = message[2];
|
||||
if (count > HX_STREAM_MAX_FRAME)
|
||||
count = HX_STREAM_MAX_FRAME;
|
||||
|
||||
/* start sending the reply */
|
||||
sending = true;
|
||||
hx_stream_reset(if_stream);
|
||||
hx_stream_start(if_stream, registers, count * 2 + 2);
|
||||
|
||||
/* enable the TX-ready interrupt */
|
||||
rCR1 |= USART_CR1_TXEIE;
|
||||
return;
|
||||
|
||||
} else {
|
||||
|
||||
/* it's a write operation, pass it to the register API */
|
||||
registers_set(message[0],
|
||||
message[1],
|
||||
(const uint16_t *)&message[2],
|
||||
(length - 2) / 2);
|
||||
}
|
||||
|
||||
/* it's a read - must contain length byte */
|
||||
if (length != 3)
|
||||
return;
|
||||
uint16_t *registers;
|
||||
unsigned count;
|
||||
|
||||
tx_buf[0] = message[0];
|
||||
tx_buf[1] = message[1];
|
||||
|
||||
/* get registers for response, send an empty reply on error */
|
||||
if (registers_get(message[0], message[1], ®isters, &count) < 0)
|
||||
count = 0;
|
||||
|
||||
/* fill buffer with message, limited by length */
|
||||
#define TX_MAX ((sizeof(tx_buf) - 2) / 2)
|
||||
if (count > TX_MAX)
|
||||
count = TX_MAX;
|
||||
if (count > message[2])
|
||||
count = message[2];
|
||||
memcpy(&tx_buf[2], registers, count * 2);
|
||||
|
||||
/* try to send the message */
|
||||
hx_stream_send(if_stream, tx_buf, count * 2 + 2);
|
||||
}
|
||||
@ -96,20 +96,6 @@ hx_tx_raw(hx_stream_t stream, uint8_t c)
|
||||
stream->tx_error = true;
|
||||
}
|
||||
|
||||
static void
|
||||
hx_tx_byte(hx_stream_t stream, uint8_t c)
|
||||
{
|
||||
switch (c) {
|
||||
case FBO:
|
||||
case CEO:
|
||||
hx_tx_raw(stream, CEO);
|
||||
c ^= 0x20;
|
||||
break;
|
||||
}
|
||||
|
||||
hx_tx_raw(stream, c);
|
||||
}
|
||||
|
||||
static int
|
||||
hx_rx_frame(hx_stream_t stream)
|
||||
{
|
||||
@ -281,12 +267,12 @@ hx_stream_send(hx_stream_t stream,
|
||||
{
|
||||
int result;
|
||||
|
||||
result = hx_start(stream, data, count);
|
||||
result = hx_stream_start(stream, data, count);
|
||||
if (result != OK)
|
||||
return result;
|
||||
|
||||
int c;
|
||||
while ((c = hx_send_next(stream)) >= 0)
|
||||
while ((c = hx_stream_send_next(stream)) >= 0)
|
||||
hx_tx_raw(stream, c);
|
||||
|
||||
/* check for transmit error */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user