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.
}