fixed style issues + got encoders and timestamp actually working
diff --git a/bbb_cape/src/bbb/packet_finder.cc b/bbb_cape/src/bbb/packet_finder.cc
index b6c1dc7..f0ea66d 100644
--- a/bbb_cape/src/bbb/packet_finder.cc
+++ b/bbb_cape/src/bbb/packet_finder.cc
@@ -15,17 +15,17 @@
PacketFinder::PacketFinder()
: buf_(new AlignedChar[PACKET_SIZE]),
- unstuffed_data_(new AlignedChar[PACKET_SIZE - 4]) {
+ unstuffed_data_(new AlignedChar[PACKET_SIZE - 4]) {
static_assert((PACKET_SIZE % 4) == 0,
"We can't do checksums of lengths that aren't multiples of 4.");
- }
+}
PacketFinder::~PacketFinder() {
delete buf_;
delete unstuffed_data_;
}
-// TODO(brians): Figure out why this (sometimes?) gets confused right after
+// TODO(brians): Figure out why this (sometimes) gets confused right after
// flashing the cape.
bool PacketFinder::FindPacket() {
// How many 0 bytes we've found at the front so far.
@@ -93,10 +93,7 @@
return true;
}
-bool PacketFinder::GetPacket(DataStruct *packet) {
- static_assert(sizeof(*packet) <= PACKET_SIZE - 8,
- "output data type is too big");
-
+bool PacketFinder::ReadPacket() {
if (!FindPacket()) return false;
if (!ProcessPacket()) {
@@ -119,7 +116,6 @@
} else {
packet_bytes_ = -1;
}
- memcpy(packet, unstuffed_data_, sizeof(*packet));
return true;
}
diff --git a/bbb_cape/src/bbb/packet_finder.h b/bbb_cape/src/bbb/packet_finder.h
index 3dd249e..ee2e133 100644
--- a/bbb_cape/src/bbb/packet_finder.h
+++ b/bbb_cape/src/bbb/packet_finder.h
@@ -11,21 +11,34 @@
namespace bbb {
class PacketFinder {
- protected:
- typedef char __attribute__((aligned(4))) AlignedChar;
-
public:
PacketFinder();
virtual ~PacketFinder();
- // Returns true if it finds one or false if it gets an I/O error first.
- // packet must be aligned to 4 bytes.
- bool GetPacket(DataStruct *packet);
+ // Returns true if it succeeds or false if it gets an I/O error first.
+ bool ReadPacket();
+
+ // Gets a reference to the received packet.
+ // The most recent call to ReadPacket() must have returned true or the data
+ // pointed to is undefined.
+ template <typename T>
+ const T *get_packet() {
+ static_assert(alignof(T) <= alignof(*unstuffed_data_),
+ "We need to align our data better.");
+ /*static_assert(sizeof(T) <= PACKET_SIZE - 8,
+ "We aren't getting that much data.");*/
+ return reinterpret_cast<const T *>(unstuffed_data_);
+ }
+
+ protected:
+ typedef char __attribute__((aligned(8))) AlignedChar;
+
+ private:
// Implemented by subclasses to provide a data source
// for these algorithms.
+ // Returns the number of bytes read or -1 if there is an error in errno.
virtual int ReadBytes(AlignedChar *dest, size_t max_bytes) = 0;
-
- private:
+
// Reads bytes until there are 4 zeros and then fills up buf_.
// Returns true if it finds one or false if it gets an I/O error first or the
// packet is invalid in some way.
@@ -37,7 +50,6 @@
// data.
bool ProcessPacket();
-
AlignedChar *const buf_;
AlignedChar *const unstuffed_data_;
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index a29944d..ceea78a 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -113,4 +113,4 @@
return read(fd_, dest, max_bytes);
}
-} // namespace bbb
+} // namespace bbb
diff --git a/bbb_cape/src/bbb/uart_reader.h b/bbb_cape/src/bbb/uart_reader.h
index a717c33..bd022b0 100644
--- a/bbb_cape/src/bbb/uart_reader.h
+++ b/bbb_cape/src/bbb/uart_reader.h
@@ -9,14 +9,16 @@
namespace bbb {
class UartReader : public PacketFinder {
- int fd_;
-
-public:
+ public:
UartReader(int32_t baud_rate);
- ~UartReader();
+ virtual ~UartReader();
+
int ReadBytes(AlignedChar *dest, size_t max_bytes);
+
+ private:
+ const int fd_;
};
-} // namespace bbb
+} // namespace bbb
#endif
diff --git a/bbb_cape/src/bbb/uart_reader_main.cc b/bbb_cape/src/bbb/uart_reader_main.cc
index 8815f66..475d354 100644
--- a/bbb_cape/src/bbb/uart_reader_main.cc
+++ b/bbb_cape/src/bbb/uart_reader_main.cc
@@ -39,16 +39,16 @@
}
#endif
- DataStruct packet;
- if (!receiver.GetPacket(&packet)) {
- LOG(WARNING, "Could not get a packet.\n");
+ if (!receiver.ReadPacket()) {
+ LOG(WARNING, "Could not read a packet.\n");
continue;
}
last_packet_time = Time::Now();
+ const DataStruct *packet = receiver.get_packet<DataStruct>();
LOG(DEBUG, "got one!\n");
- LOG(DEBUG, "timestamp %" PRIu64 "\n", packet.timestamp);
- LOG(DEBUG, "0=%d\n", packet.main.encoders[0]);
+ LOG(DEBUG, "timestamp %" PRIu64 "\n", packet->timestamp);
+ LOG(DEBUG, "0=%d\n", packet->main.encoders[0]);
//TODO (danielp): Do stuff here with the data we got.
}
diff --git a/bbb_cape/src/cape/analog.c b/bbb_cape/src/cape/analog.c
index 14c45a1..7345916 100644
--- a/bbb_cape/src/cape/analog.c
+++ b/bbb_cape/src/cape/analog.c
@@ -46,7 +46,7 @@
analog_readings[current_channel] = value & 0x3FF;
CSEL_GPIO->BSRRH = 1 << CSEL_NUM;
- TIM->CR1 = TIM_CR1_UDIS | TIM_CR1_OPM;
+ TIM->CR1 = TIM_CR1_OPM;
TIM->EGR = TIM_EGR_UG;
TIM->CR1 |= TIM_CR1_CEN;
}
@@ -76,7 +76,7 @@
NVIC_SetPriority(TIM_IRQn, 6);
NVIC_EnableIRQ(TIM_IRQn);
- TIM->CR1 = TIM_CR1_UDIS | TIM_CR1_OPM;
+ TIM->CR1 = TIM_CR1_OPM;
TIM->DIER = TIM_DIER_CC1IE;
TIM->CCMR1 = 0;
// Make each tick take 500ns.
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 797648f..7cce3be 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -15,7 +15,7 @@
union {
struct {
- // In 1/3us since the cape last reset.
+ // In 10us since the cape last reset.
uint64_t timestamp;
// The CRC32 (same algorithm as the checksum for the packet) of the whole
diff --git a/bbb_cape/src/cape/encoder.c b/bbb_cape/src/cape/encoder.c
index 5199bd4..09c4918 100644
--- a/bbb_cape/src/cape/encoder.c
+++ b/bbb_cape/src/cape/encoder.c
@@ -64,9 +64,9 @@
}
static void encoder_setup(TIM_TypeDef *timer) {
- timer->CR1 = TIM_CR1_UDIS /* wait until we tell it to do anything */ |
- TIM_CR1_URS /* don't generate spurious update interrupts that
- might be shared with other timers */;
+ timer->CR1 =
+ TIM_CR1_URS /* don't generate spurious update interrupts that
+ might be shared with other timers */;
timer->SMCR = 3; // 4x quadrature encoder mode
timer->CCMR1 =
TIM_CCMR1_CC2S_0 | /* input pin 2 -> timer input 2 */
@@ -90,7 +90,7 @@
// Set up the A2 software encoder input through TIM9.
gpio_setup_alt(GPIOA, 2, 3);
RCC->APB2ENR |= RCC_APB2ENR_TIM9EN;
- TIM9->CR1 = TIM_CR1_UDIS;
+ TIM9->CR1 = 0;
TIM9->DIER = TIM_DIER_CC1IE;
TIM9->CCMR1 = TIM_CCMR1_CC1S_0; /* input pin 1 -> timer input 1 */
TIM9->CCER = TIM_CCER_CC1NP | TIM_CCER_CC1P | TIM_CCER_CC1E;
diff --git a/bbb_cape/src/cape/fill_packet.c b/bbb_cape/src/cape/fill_packet.c
index 2c95ef1..4ff5b38 100644
--- a/bbb_cape/src/cape/fill_packet.c
+++ b/bbb_cape/src/cape/fill_packet.c
@@ -45,6 +45,8 @@
packet->bad_gyro = gyro_output.gyro_bad;
robot_fill_packet(packet);
+ //counter_update_u64_u16(×tamp, TIMESTAMP_TIM->CNT);
+ //packet->main.encoders[0] = timestamp;
}
// Fills the new packet with data.
@@ -70,7 +72,8 @@
void fill_packet_start(void) {
RCC->APB1ENR |= RCC_APB1ENR_TIMESTAMP_TIMEN;
- TIMESTAMP_TIM->CR1 = TIM_CR1_UDIS;
+ TIMESTAMP_TIM->CR1 = 0;
+ TIMESTAMP_TIM->PSC = 600 - 1;
TIMESTAMP_TIM->EGR = TIM_EGR_UG;
TIMESTAMP_TIM->CR1 |= TIM_CR1_CEN;
diff --git a/bbb_cape/src/cape/gyro.c b/bbb_cape/src/cape/gyro.c
index 98c2b42..b565ed8 100644
--- a/bbb_cape/src/cape/gyro.c
+++ b/bbb_cape/src/cape/gyro.c
@@ -77,7 +77,7 @@
// Switches to new_state in time TIM milliseconds (aka it shows in the TIM ISR).
static void switch_state(enum State new_state, int time) {
- TIM->CR1 = TIM_CR1_UDIS;
+ TIM->CR1 = 0;
state = new_state;
TIM->CCR1 = time;
TIM->EGR = TIM_EGR_UG;
@@ -385,7 +385,7 @@
NVIC_SetPriority(TIM_IRQn, 5);
NVIC_EnableIRQ(TIM_IRQn);
- TIM->CR1 = TIM_CR1_UDIS;
+ TIM->CR1 = 0;
TIM->DIER = TIM_DIER_CC1IE;
TIM->CCMR1 = 0;
// Make it generate 1 tick every ms.
diff --git a/bbb_cape/src/cape/uart_byte.c b/bbb_cape/src/cape/uart_byte.c
index 6bafdb6..cfcf1bf 100644
--- a/bbb_cape/src/cape/uart_byte.c
+++ b/bbb_cape/src/cape/uart_byte.c
@@ -9,7 +9,7 @@
void uart_byte_configure(void) {
RCC->APB1ENR |= RCC_APB1ENR_TIMEOUT_TIMEN;
- TIMEOUT_TIM->CR1 = TIM_CR1_UDIS;
+ TIMEOUT_TIM->CR1 = 0;
}
int uart_byte_receive(uint16_t timeout_count, uint16_t timeout_divider) {