got byte-oriented UART communication working
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index ef90635..0a3f3c6 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -26,7 +26,8 @@
namespace {
// TODO(brians): Determine this in some way that allows easy switching for
// testing with /dev/ttyUSB0 for example.
-const char *device = "/dev/ttyO1";
+// TODO(brians): Change back to /dev/ttyO1.
+const char *device = "/dev/ttyUSB0";
} // namespace
UartReader::UartReader(int32_t baud_rate)
@@ -41,62 +42,77 @@
" Did you read my note in bbb/uart_reader.cc?\n",
device, errno, strerror(errno));
}
+
+ {
+ termios options;
+ if (tcgetattr(fd_, &options) != 0) {
+ LOG(FATAL, "tcgetattr(%d, %p) failed with %d: %s\n",
+ fd_, &options, errno, strerror(errno));
+ }
+ if (cfsetispeed(&options, B38400) != 0) {
+ LOG(FATAL, "cfsetispeed(%p, B38400) failed with %d: %s\n",
+ &options, errno, strerror(errno));
+ }
+ if (cfsetospeed(&options, B38400) != 0) {
+ LOG(FATAL, "cfsetospeed(%p, B38400) failed with %d: %s\n",
+ &options, errno, strerror(errno));
+ }
+ cfmakeraw(&options);
+ options.c_cflag |= CLOCAL; // ignore modem flow control
+ options.c_cflag |= CREAD; // enable receiver
+ options.c_cflag &= ~CRTSCTS; // disable flow control
+ //options.c_cflag |= PARENB; // enable parity; defaults to even
+ options.c_cflag = (options.c_cflag & ~CSIZE) | CS8; // 8 bits
+ options.c_cflag &= ~CSTOPB; // 1 stop bit
+ options.c_iflag = 0;
+ // Replace any received characters with parity or framing errors with 0s.
+ options.c_iflag = (options.c_iflag & ~(IGNPAR | PARMRK)) | INPCK;
+ //options.c_iflag |= IGNCR | PARMRK;
+ options.c_oflag = 0;
+ options.c_lflag = 0;
+ options.c_cc[VMIN] = 0;
+ options.c_cc[VTIME] = 1;
+ if (tcsetattr(fd_, TCSANOW, &options) != 0) {
+ LOG(FATAL, "tcsetattr(%d, TCSANOW, %p) failed with %d: %s\n",
+ fd_, &options, errno, strerror(errno));
+ }
+ }
- serial_struct serinfo;
- // Must implement an ugly custom divisor.
- serinfo.reserved_char[0] = 0;
- if (ioctl(fd_, TIOCGSERIAL, &serinfo) < 0) {
- LOG(FATAL, "ioctl(%d, TIOCGSERIAL, %p) failed with %d: %s\n",
- fd_, &serinfo, errno, strerror(errno));
- }
- serinfo.flags &= ~ASYNC_SPD_MASK;
- serinfo.flags |= ASYNC_SPD_CUST;
- serinfo.custom_divisor = (serinfo.baud_base + (baud_rate_ / 2)) / baud_rate_;
- if (serinfo.custom_divisor < 1) serinfo.custom_divisor = 1;
- if (ioctl(fd_, TIOCSSERIAL, &serinfo) < 0) {
- LOG(FATAL, "ioctl(%d, TIOCSSERIAL, %p) failed with %d: %s\n",
- fd_, &serinfo, errno, strerror(errno));
- }
- if (ioctl(fd_, TIOCGSERIAL, &serinfo) < 0) {
- LOG(FATAL, "ioctl(%d, TIOCGSERIAL, %p) failed with %d: %s\n",
- fd_, &serinfo, errno, strerror(errno));
- }
- if (serinfo.custom_divisor * baud_rate_ != serinfo.baud_base) {
- LOG(WARNING, "actual baudrate is %d / %d = %f\n",
- serinfo.baud_base, serinfo.custom_divisor,
- static_cast<double>(serinfo.baud_base) / serinfo.custom_divisor);
+ {
+ serial_struct serinfo;
+ if (ioctl(fd_, TIOCGSERIAL, &serinfo) != 0) {
+ LOG(FATAL, "ioctl(%d, TIOCGSERIAL, %p) failed with %d: %s\n",
+ fd_, &serinfo, errno, strerror(errno));
+ }
+ serinfo.reserved_char[0] = 0;
+ serinfo.flags &= ~ASYNC_SPD_MASK;
+ serinfo.flags |= ASYNC_SPD_CUST;
+ //serinfo.flags |= ASYNC_LOW_LATENCY;
+ serinfo.custom_divisor = static_cast<int>(
+ static_cast<double>(serinfo.baud_base) / baud_rate_ + 0.5);
+ if (serinfo.custom_divisor < 1) serinfo.custom_divisor = 1;
+ if (ioctl(fd_, TIOCSSERIAL, &serinfo) < 0) {
+ LOG(FATAL, "ioctl(%d, TIOCSSERIAL, %p) failed with %d: %s\n",
+ fd_, &serinfo, errno, strerror(errno));
+ }
+ if (ioctl(fd_, TIOCGSERIAL, &serinfo) < 0) {
+ LOG(FATAL, "ioctl(%d, TIOCGSERIAL, %p) failed with %d: %s\n",
+ fd_, &serinfo, errno, strerror(errno));
+ }
+ if ((serinfo.flags & ASYNC_SPD_CUST) == 0) {
+ LOG(FATAL, "can not set custom baud rate\n");
+ }
+ if (serinfo.custom_divisor * baud_rate_ != serinfo.baud_base) {
+ LOG(WARNING, "actual baudrate is %d / %d = %f\n",
+ serinfo.baud_base, serinfo.custom_divisor,
+ static_cast<double>(serinfo.baud_base) / serinfo.custom_divisor);
+ }
}
if (fcntl(fd_, F_SETFL, 0) != 0) {
LOG(FATAL, "fcntl(%d, F_SETFL, 0) failed with %d: %s\n",
fd_, errno, strerror(errno));
}
-
- termios options;
- if (tcgetattr(fd_, &options) != 0) {
- LOG(FATAL, "tcgetattr(%d, %p) failed with %d: %s\n",
- fd_, &options, errno, strerror(errno));
- }
- if (cfsetispeed(&options, B38400) != 0) {
- LOG(FATAL, "cfsetispeed(%p, B38400) failed with %d: %s\n",
- &options, errno, strerror(errno));
- }
- if (cfsetospeed(&options, B38400) != 0) {
- LOG(FATAL, "cfsetospeed(%p, B38400) failed with %d: %s\n",
- &options, errno, strerror(errno));
- }
- cfmakeraw(&options);
- options.c_cflag |= (CLOCAL | CREAD);
- options.c_cflag &= ~CRTSCTS;
- options.c_iflag = 0;
- options.c_oflag = 0;
- options.c_lflag = 0;
- options.c_cc[VMIN] = 0;
- options.c_cc[VTIME] = 1;
- if (tcsetattr(fd_, TCSANOW, &options) != 0) {
- LOG(FATAL, "tcsetattr(%d, TCSANOW, %p) failed with %d: %s\n",
- fd_, &options, errno, strerror(errno));
- }
}
UartReader::~UartReader() {
@@ -114,6 +130,11 @@
size_t already_read = ::std::max(packet_bytes, 0);
ssize_t new_bytes =
read(fd_, buf_ + already_read, DATA_STRUCT_SEND_SIZE - already_read);
+ LOG(DEBUG, "read %zd, wanted %d\n", new_bytes,
+ DATA_STRUCT_SEND_SIZE - already_read);
+ for (int i = 0; i < new_bytes; ++i) {
+ LOG(DEBUG, "%x\n", buf_[i]);
+ }
if (new_bytes < 0) {
if (errno == EINTR) continue;
LOG(WARNING, "read(%d, %p, %zd) failed with %d: %s\n",
diff --git a/bbb_cape/src/bbb/uart_reader_main.cc b/bbb_cape/src/bbb/uart_reader_main.cc
index 3d78039..d0fd028 100644
--- a/bbb_cape/src/bbb/uart_reader_main.cc
+++ b/bbb_cape/src/bbb/uart_reader_main.cc
@@ -8,23 +8,28 @@
using ::aos::time::Time;
-int main() {
- aos::Init();
+#define DO_RESET 0
+int main() {
+ ::aos::Init();
+
+#if DO_RESET
// time since last good packet before we reset
// the board.
- Time kPacketTimeout = Time::InSeconds(1);
+ static const Time kPacketTimeout = Time::InSeconds(1);
- bbb::UartReader receiver(3000000);
- bbb::Pin reset_pin = bbb::Pin(1, 6);
+ ::bbb::Pin reset_pin = bbb::Pin(1, 6);
reset_pin.MakeOutput();
+#endif
- DataStruct packet;
+ //::bbb::UartReader receiver(3000000);
+ ::bbb::UartReader receiver(300000);
+ //::bbb::UartReader receiver(19200);
+
Time last_packet_time = Time::Now();
while (true) {
- if (!last_packet_time.IsWithin(Time::Now(),
- kPacketTimeout.ToNSec())) {
-
+#if DO_RESET
+ if (!last_packet_time.IsWithin(Time::Now(), kPacketTimeout.ToNSec())) {
LOG(ERROR, "No good packets for too long. Resetting cape.\n");
reset_pin.Write(1);
::aos::time::SleepFor(Time::InSeconds(1));
@@ -32,16 +37,18 @@
last_packet_time = Time::Now();
}
+#endif
- if (receiver.GetPacket(&packet)) {
- LOG(INFO, "Could not get a packet.\n");
+ DataStruct packet;
+ if (!receiver.GetPacket(&packet)) {
+ LOG(WARNING, "Could not get a packet.\n");
continue;
}
last_packet_time = Time::Now();
+ LOG(DEBUG, "got one!\n");
//TODO (danielp): Do stuff here with the data we got.
}
- aos::Cleanup();
- return 0;
+ ::aos::Cleanup();
}
diff --git a/bbb_cape/src/cape/Makefile b/bbb_cape/src/cape/Makefile
index 2439b89..39aa3aa 100644
--- a/bbb_cape/src/cape/Makefile
+++ b/bbb_cape/src/cape/Makefile
@@ -49,6 +49,7 @@
OBJECTS_main_test := $(OBJECTS_main_common) \
robot_test \
+ uart_byte \
OUTPUTS := main_test bootloader
diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 870541b..eb7d565 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -16,6 +16,8 @@
#include "cape/digital.h"
#include "cape/led.h"
+#include "cape/uart_byte.h"
+
#define TIMESTAMP_TIM TIM6
#define RCC_APB1ENR_TIMESTAMP_TIMEN RCC_APB1ENR_TIM6EN
@@ -84,6 +86,20 @@
led_set(LED_ERR, 0);
//gyro_init();
- uart_common_configure(3000000);
+ //uart_common_configure(3000000);
+ uart_common_configure(300000);
+ //uart_common_configure(19200);
+#if 0
+ //for (int i = 0; i < 5; ++i) uart_byte_send(255);
+ for (int i = 0; i < 10; ++i) uart_byte_send(i + 20);
+ //uart_byte_send('a');
+ //uart_byte_send('b');
+ //uart_byte_send('c');
+ //uart_byte_send('d');
+ led_set(LED_DB, 1);
+ (void)buffer1;
+ (void)buffer2;
+#else
uart_dma_configure(DATA_STRUCT_SEND_SIZE, buffer1, buffer2);
+#endif
}
diff --git a/bbb_cape/src/cape/uart_common.c b/bbb_cape/src/cape/uart_common.c
index 783c154..0cb7507 100644
--- a/bbb_cape/src/cape/uart_common.c
+++ b/bbb_cape/src/cape/uart_common.c
@@ -3,20 +3,30 @@
#include "cape/util.h"
+#define RCC_APB2ENR_UARTEN RCC_APB2ENR_USART1EN
+
#define FPCLK 60000000
// The UART is on PA9 and PA10.
void uart_common_configure(int baud) {
gpio_setup_alt(GPIOA, 9, 7);
gpio_setup_alt(GPIOA, 10, 7);
- GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR9; // we want to go FAST!
- RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
+ //GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR9; // we want to go FAST!
+ RCC->APB2ENR |= RCC_APB2ENR_UARTEN;
- // baud = 60MHz / (8 * (2 - OVER8) * (mantissa / fraction))
- int fraction = 8; // the biggest it can be with OVER8=0
- int mantissa = FPCLK * (16 /* 8 * (2 - OVER8) */ / fraction) / baud;
- UART->BRR = (mantissa << 4) | fraction;
- UART->CR1 = USART_CR1_UE /* enable it */ |
- USART_CR1_M /* 9th bit for the parity */ |
- USART_CR1_PCE /* enable parity (even by default) */;
+ // baud = 60MHz / kMultiplier * (whole_part + fraction / kMultiplier))
+ static const int kMultiplier = 16 /* 8 * (2 - OVER8) */;
+ // The divisor of FPCLK that we want (*2).
+ int divisor = FPCLK * 2 / baud;
+ // The whole-number part of the divisor.
+ int mantissa = divisor / kMultiplier / 2;
+ // The fractional part of the divisor (*2).
+ int fraction = divisor % (kMultiplier * 2);
+ UART->BRR = (mantissa << 4) | ((fraction + 1) / 2);
+ UART->CR1 =
+ //USART_CR1_M /* 9th bit for the parity */ |
+ //USART_CR1_PCE /* enable parity (even by default) */ |
+ USART_CR1_TE /* enable transmitter */ |
+ USART_CR1_RE /* enable receiver */;
+ UART->CR1 |= USART_CR1_UE; // enable it
}
diff --git a/bbb_cape/src/cape/uart_dma.c b/bbb_cape/src/cape/uart_dma.c
index f5c1967..8493632 100644
--- a/bbb_cape/src/cape/uart_dma.c
+++ b/bbb_cape/src/cape/uart_dma.c
@@ -3,16 +3,17 @@
#include "cape/util.h"
#include "cape/uart_common.h"
+#include "cape/led.h"
-#define DMA DMA1
-#define DMA_STREAM_NUMBER 7
-#define DMA_Stream DMA1_Stream7
-#define DMA_SR DMA1->HISR
-#define DMA_FCR DMA1->HIFCR
+#define DMA DMA2
+#define DMA_Stream DMA2_Stream7
+#define DMA_SR DMA2->HISR
+#define DMA_FCR DMA2->HIFCR
#define DMA_SR_SHIFT 3
-#define DMA_Stream_IRQHandler DMA1_Stream7_IRQHandler
-#define DMA_Stream_IRQn DMA1_Stream7_IRQn
-#define RCC_AHB1ENR_DMAEN RCC_AHB1ENR_DMA1EN
+#define DMA_Stream_IRQHandler DMA2_Stream7_IRQHandler
+#define DMA_Stream_IRQn DMA2_Stream7_IRQn
+#define DMA_CHANNEL_NUMBER 4
+#define RCC_AHB1ENR_DMAEN RCC_AHB1ENR_DMA2EN
#define DMA_SR_BIT(bit) (1 << (bit + 6 * DMA_SR_SHIFT))
@@ -22,6 +23,7 @@
static uint8_t *volatile buffer1, *volatile buffer2;
void DMA_Stream_IRQHandler(void) {
+ led_set(LED_DB, 1);
uint32_t status = DMA_SR;
if (status & DMA_SR_BIT(5)) { // transfer completed
DMA_FCR = DMA_SR_BIT(5);
@@ -34,6 +36,7 @@
// If we're fighting somebody else writing stuff, we'll do this a bunch of
// times, but oh well.
DMA_Stream->CR |= DMA_SxCR_EN;
+ led_set(LED_ERR, 1);
}
}
@@ -42,27 +45,33 @@
buffer2 = buffer2_in;
uart_dma_callback(buffer1);
+ UART->CR3 = USART_CR3_DMAT;
+
RCC->AHB1ENR |= RCC_AHB1ENR_DMAEN;
+ DMA_Stream->CR = DMA_CHANNEL_NUMBER << 25 |
+ DMA_SxCR_DBM /* enable double buffer mode */ |
+ 2 << 16 /* priority */ |
+ //2 << 13 /* memory data size = 32 bits */ |
+ 0 << 13 /* memory data size = 8 bits */ |
+ 0 << 11 /* peripherial data size = 8 bits */ |
+ DMA_SxCR_MINC /* increment memory address */ |
+ 1 << 6 /* memory to peripherial */ |
+ DMA_SxCR_HTIE | DMA_SxCR_DMEIE |
+ DMA_SxCR_TCIE | DMA_SxCR_TEIE;
DMA_Stream->PAR = (uint32_t)&UART->DR;
DMA_Stream->M0AR = (uint32_t)buffer1;
DMA_Stream->M1AR = (uint32_t)buffer2;
// This is measured in chunks of PSIZE bytes, not MSIZE.
DMA_Stream->NDTR = bytes;
DMA_FCR = 0xF << DMA_SR_SHIFT;
- DMA_Stream->CR = DMA_STREAM_NUMBER << 25 |
- DMA_SxCR_DBM /* enable double buffer mode */ |
- 2 << 16 /* priority */ |
- 2 << 13 /* memory data size = 32 bits */ |
- 0 << 11 /* peripherial data size = 8 bits */ |
- DMA_SxCR_MINC /* increment memory address */ |
- 1 << 6 /* memory to peripherial */ |
- DMA_SxCR_TCIE | DMA_SxCR_TEIE;
DMA_Stream->FCR =
DMA_SxFCR_DMDIS /* disable direct mode (enable the FIFO) */ |
- 1 /* 1/2 full threshold */;
+ //1 /* 1/2 full threshold */;
+ 3 /* 100% full threshold */;
DMA_Stream->CR |= DMA_SxCR_EN; // enable it
NVIC_SetPriority(DMA_Stream_IRQn, 8);
NVIC_EnableIRQ(DMA_Stream_IRQn);
uart_dma_callback(buffer2);
+ led_set(LED_Z, 1);
}
diff --git a/bbb_cape/src/flasher/stm32_flasher.cc b/bbb_cape/src/flasher/stm32_flasher.cc
index 73f778c..35d95a7 100644
--- a/bbb_cape/src/flasher/stm32_flasher.cc
+++ b/bbb_cape/src/flasher/stm32_flasher.cc
@@ -37,8 +37,14 @@
int file = open(filename.c_str(), O_RDONLY);
if (file == -1) {
- LOG(FATAL, "open(%s, O_RDONLY) failed with %d: %s\n",
- filename.c_str(), errno, strerror(errno));
+ filename = target;
+ file = open(filename.c_str(), O_RDONLY);
+ if (file == -1) {
+ LOG(FATAL, "open(%s, O_RDONLY) failed with %d: %s\n",
+ filename.c_str(), errno, strerror(errno));
+ } else {
+ LOG(INFO, "using filename %s from the command line\n", filename.c_str());
+ }
}
uint16_t start_address = 0;