got gyro_sensor_receiver so it compiles with glibusb
diff --git a/aos/common/glibusb/gbuffer.cc b/aos/common/glibusb/gbuffer.cc
index b675953..c19b2af 100644
--- a/aos/common/glibusb/gbuffer.cc
+++ b/aos/common/glibusb/gbuffer.cc
@@ -15,20 +15,23 @@
namespace glibusb {
-Buffer::Buffer() {}
+Buffer::Buffer() {
+ buffer_ = NULL;
+ length_ = allocated_length_ = 0;
+}
Buffer::~Buffer() {}
-Buffer::Buffer(const void *src, Buffer::size_type length) {
- buffer_.resize(length);
+Buffer::Buffer(const void *src, Buffer::size_type length) : Buffer() {
+ Resize(length);
if (length > 0) {
- uint8_t *dst = &(buffer_[0]);
- memcpy(dst, src, length);
+ memcpy(buffer_, src, length);
}
}
bool Buffer::operator==(const Buffer &other) const {
- return buffer_ == other.buffer_;
+ return length_ == other.length_ &&
+ memcmp(buffer_, other.buffer_, length_) == 0;
}
bool Buffer::operator!=(const Buffer &other) const {
@@ -37,7 +40,7 @@
Buffer *Buffer::MakeSlice(Buffer::size_type offset,
Buffer::size_type length) const {
- CHECK_LE(offset + length, buffer_.size());
+ CHECK_LE(offset + length, length_);
if (length == 0) {
return new Buffer();
} else {
@@ -47,11 +50,24 @@
}
void Buffer::Clear() {
- buffer_.clear();
+ length_ = 0;
}
void Buffer::Resize(Buffer::size_type length) {
- buffer_.resize(length);
+ if (length > allocated_length_) {
+ if (length_ > 0) {
+ uint8_t *old = buffer_;
+ buffer_ = new uint8_t[length];
+ memcpy(buffer_, old, length_);
+ delete[] old;
+ } else {
+ delete[] buffer_;
+ buffer_ = new uint8_t[length];
+ }
+ } else {
+ memset(&buffer_[length_], 0, length_ - length);
+ }
+ length_ = length;
}
void *Buffer::GetBufferPointer(Buffer::size_type length) {
@@ -67,7 +83,7 @@
if (length == 0) {
return NULL;
} else {
- CHECK_LE(offset + length, buffer_.size());
+ CHECK_LE(offset + length, length_);
uint8_t *p = &(buffer_[offset]);
return static_cast<void *>(p);
}
@@ -78,7 +94,7 @@
if (length == 0) {
return NULL;
} else {
- CHECK_LE(offset + length, buffer_.size());
+ CHECK_LE(offset + length, length_);
const uint8_t *p = &(buffer_[offset]);
return static_cast<const void *>(p);
}
@@ -88,7 +104,7 @@
template <>
Buffer::size_type Buffer::Get(Buffer::size_type byte_offset,
uint8_t *value_out) const {
- CHECK_LT(byte_offset, buffer_.size());
+ CHECK_LT(byte_offset, length_);
*CHECK_NOTNULL(value_out) = buffer_[byte_offset];
return sizeof(uint8_t);
}
@@ -107,7 +123,7 @@
template <>
Buffer::size_type Buffer::Get(Buffer::size_type byte_offset,
uint16_t *value_out) const {
- CHECK_LT(byte_offset + 1, buffer_.size());
+ CHECK_LT(byte_offset + 1, length_);
uint16_t byte0 = static_cast<uint16_t>(buffer_[byte_offset]);
uint16_t byte1 = static_cast<uint16_t>(buffer_[byte_offset + 1]);
uint16_t value = byte0 | (byte1 << 8);
@@ -129,7 +145,7 @@
template <>
Buffer::size_type Buffer::Get(Buffer::size_type byte_offset,
uint32_t *value_out) const {
- CHECK_LT(byte_offset + 3, buffer_.size());
+ CHECK_LT(byte_offset + 3, length_);
uint32_t byte0 = static_cast<uint32_t>(buffer_[byte_offset]);
uint32_t byte1 = static_cast<uint32_t>(buffer_[byte_offset + 1]);
uint32_t byte2 = static_cast<uint32_t>(buffer_[byte_offset + 2]);
@@ -153,7 +169,7 @@
template <>
Buffer::size_type Buffer::Get(Buffer::size_type byte_offset,
uint64_t *value_out) const {
- CHECK_LT(byte_offset + 7, buffer_.size());
+ CHECK_LT(byte_offset + 7, length_);
uint64_t byte0 = static_cast<uint64_t>(buffer_[byte_offset]);
uint64_t byte1 = static_cast<uint64_t>(buffer_[byte_offset + 1]);
uint64_t byte2 = static_cast<uint64_t>(buffer_[byte_offset + 2]);
@@ -201,7 +217,7 @@
// Specialized template for Put
template <>
Buffer::size_type Buffer::Put(Buffer::size_type byte_offset, uint8_t value) {
- CHECK_LT(byte_offset, buffer_.size());
+ CHECK_LT(byte_offset, length_);
buffer_[byte_offset] = value;
return sizeof(uint8_t);
}
@@ -215,7 +231,7 @@
// Specialized template for Put
template <>
Buffer::size_type Buffer::Put(Buffer::size_type byte_offset, uint16_t value) {
- CHECK_LT(byte_offset + 1, buffer_.size());
+ CHECK_LT(byte_offset + 1, length_);
uint8_t byte_0 = static_cast<uint8_t>(value & 0xff);
uint8_t byte_1 = static_cast<uint8_t>((value >> 8) & 0xff);
buffer_[byte_offset] = byte_0;
@@ -232,7 +248,7 @@
// Specialized template for Put
template <>
Buffer::size_type Buffer::Put(Buffer::size_type byte_offset, uint32_t value) {
- CHECK_LT(byte_offset + 3, buffer_.size());
+ CHECK_LT(byte_offset + 3, length_);
uint8_t byte_0 = static_cast<uint8_t>(value & 0xff);
uint8_t byte_1 = static_cast<uint8_t>((value >> 8) & 0xff);
uint8_t byte_2 = static_cast<uint8_t>((value >> 16) & 0xff);
@@ -253,7 +269,7 @@
// Specialized template for Put
template <>
Buffer::size_type Buffer::Put(Buffer::size_type byte_offset, uint64_t value) {
- CHECK_LT(byte_offset + 7, buffer_.size());
+ CHECK_LT(byte_offset + 7, length_);
uint8_t byte_0 = static_cast<uint8_t>(value & 0xff);
uint8_t byte_1 = static_cast<uint8_t>((value >> 8) & 0xff);
uint8_t byte_2 = static_cast<uint8_t>((value >> 16) & 0xff);
@@ -280,22 +296,31 @@
}
void Buffer::Append(const Buffer &source) {
- buffer_.insert(buffer_.end(), source.buffer_.begin(), source.buffer_.end());
+ if (source.length_ == 0) return;
+ size_type start_length = length_;
+ Resize(length_ + source.length_);
+ memcpy(&buffer_[start_length], source.buffer_, source.length_);
}
void Buffer::AddHeader(Buffer::size_type length) {
- buffer_.insert(buffer_.begin(), length, 0);
+ size_type start_length = length_;
+ Resize(length_ + length);
+ memmove(&buffer_[length], buffer_, start_length);
+ memset(buffer_, 0, length);
}
void Buffer::RemoveHeader(Buffer::size_type length) {
if (length > 0) {
- CHECK_LE(length, buffer_.size());
- buffer_.erase(buffer_.begin(), buffer_.begin() + length);
+ CHECK_LE(length, length_);
+ size_type end_length = length_ - length;
+ memmove(buffer_, &buffer_[length], end_length);
+ Resize(end_length);
}
}
void Buffer::Copy(const Buffer &source) {
- buffer_.assign(source.buffer_.begin(), source.buffer_.end());
+ Resize(source.length_);
+ memcpy(buffer_, source.buffer_, length_);
}
#if 0
diff --git a/aos/common/glibusb/gbuffer.h b/aos/common/glibusb/gbuffer.h
index 5c1d8c6..504d6e5 100644
--- a/aos/common/glibusb/gbuffer.h
+++ b/aos/common/glibusb/gbuffer.h
@@ -1,5 +1,7 @@
// Copyright 2012 Google Inc. All Rights Reserved.
//
+// Modified by FRC Team 971.
+//
// A buffer for dealing with data. Some special support for PTP is
// available.
@@ -8,16 +10,15 @@
#include <stdint.h>
#include <string>
-#include <vector>
namespace glibusb {
// Buffer of bytes.
+// Only allocates memory when increasing the length beyond what the maximum
+// length for the lifetime of an instance.
class Buffer {
public:
- // Underlying byte store type.
- typedef std::vector<uint8_t> ByteBuffer;
- typedef ByteBuffer::size_type size_type;
+ typedef size_t size_type;
// Destructor.
~Buffer();
@@ -40,7 +41,7 @@
Buffer *MakeSlice(size_type offset, size_type length) const;
// Returns the length.
- size_type Length() const { return buffer_.size(); }
+ size_type Length() const { return length_; }
// Clears (as in std::vector) the buffer.
void Clear();
@@ -110,7 +111,12 @@
private:
// The underlying byte store.
- ByteBuffer buffer_;
+ // An operator new[]-allocated array.
+ uint8_t *buffer_;
+ // How many bytes of buffer_ are valid. If this is 0, buffer_ might be NULL.
+ size_type length_;
+ // How many bytes long buffer_ is currently allocated as.
+ size_type allocated_length_;
Buffer(const Buffer &) = delete;
void operator=(const Buffer &) = delete;
diff --git a/aos/common/glibusb/glibusb.h b/aos/common/glibusb/glibusb.h
index e2afb0f..1e893de 100644
--- a/aos/common/glibusb/glibusb.h
+++ b/aos/common/glibusb/glibusb.h
@@ -1,5 +1,7 @@
// Copyright 2012 Google Inc. All Rights Reserved.
//
+// Modified by FRC Team 971.
+//
// Wrapper for libusb.
#ifndef _GLIBUSB_GLIBUSB_H_
@@ -43,8 +45,8 @@
// Structure to hold the USB vendor and product ids for a device.
struct VendorProductId {
- VendorProductId() : vendor_id(0), product_id(0) {}
- VendorProductId(uint16_t new_vendor_id, uint16_t new_product_id)
+ constexpr VendorProductId() : vendor_id(0), product_id(0) {}
+ constexpr VendorProductId(uint16_t new_vendor_id, uint16_t new_product_id)
: vendor_id(new_vendor_id), product_id(new_product_id) {}
bool operator==(const struct VendorProductId &rhs) const {
diff --git a/aos/common/glibusb/glibusb_endpoint.cc b/aos/common/glibusb/glibusb_endpoint.cc
index 1bfd583..ee69ec9 100644
--- a/aos/common/glibusb/glibusb_endpoint.cc
+++ b/aos/common/glibusb/glibusb_endpoint.cc
@@ -209,9 +209,8 @@
LOG(DEBUG, "read on 0x%x, size 0x%x, timeout %" PRId32 " [ms]\n",
endpoint_address_and_direction(), length, timeout_milliseconds);
- ::std::unique_ptr<Buffer> whole_buffer(new Buffer());
- whole_buffer->Resize(length);
- void *p = whole_buffer->GetBufferPointer(length);
+ out->Resize(length);
+ void *p = out->GetBufferPointer(length);
int transferred;
const unsigned int timeout = static_cast<unsigned int>(timeout_milliseconds);
int r;
@@ -227,12 +226,11 @@
case LIBUSB_SUCCESS:
{
size_t size_transferred = static_cast<size_t>(transferred);
- whole_buffer->Resize(size_transferred);
+ out->Resize(size_transferred);
// TODO(brians): Conditionally enable this.
LOG(DEBUG, "read on 0x%x, size_transferred=%zx\n",
endpoint_address_and_direction(), size_transferred);
- out->Copy(*whole_buffer);
return kSuccess;
}
case LIBUSB_ERROR_TIMEOUT:
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
index 8fb6cb4..b260de8 100644
--- a/aos/common/sensors/sensor_receiver-tmpl.h
+++ b/aos/common/sensors/sensor_receiver-tmpl.h
@@ -198,7 +198,7 @@
template<class Values>
bool SensorReceiver<Values>::ReceiveData() {
int old_count = data_.count;
- DoReceiveData();
+ if (DoReceiveData()) return true;
if (data_.count < 0) {
LOG(FATAL, "data count overflowed. currently %" PRId32 "\n", data_.count);
@@ -243,7 +243,7 @@
}
template<class Values>
-void NetworkSensorReceiver<Values>::DoReceiveData() {
+bool NetworkSensorReceiver<Values>::DoReceiveData() {
while (true) {
if (socket_.Receive(this->data(), sizeof(*this->data())) ==
sizeof(*this->data())) {
@@ -254,7 +254,7 @@
}
this->data()->NetworkToHost();
- return;
+ return false;
}
LOG(WARNING, "received incorrect amount of data\n");
}
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
index 335ff08..16b72ac 100644
--- a/aos/common/sensors/sensor_receiver.h
+++ b/aos/common/sensors/sensor_receiver.h
@@ -55,7 +55,8 @@
// is available) into data().
// It needs to have the correct byte order etc and not be corrupted
// (subclasses can check the checksum if they want).
- virtual void DoReceiveData() = 0;
+ // Returns whether or not to Reset() and try synchronizing again.
+ virtual bool DoReceiveData() = 0;
// Optional: if subclasses can do anything to reinitialize after there are
// problems, they should do it here.
@@ -104,7 +105,7 @@
// How long to read data as fast as possible for (to clear out buffers etc).
static const time::Time kWarmupTime;
- virtual void DoReceiveData();
+ virtual bool DoReceiveData();
virtual void Reset();
ReceiveSocket socket_;
diff --git a/aos/common/sensors/sensor_receiver_test.cc b/aos/common/sensors/sensor_receiver_test.cc
index 803ec47..5032dfd 100644
--- a/aos/common/sensors/sensor_receiver_test.cc
+++ b/aos/common/sensors/sensor_receiver_test.cc
@@ -29,12 +29,13 @@
void Reset() {
LOG(DEBUG, "reset for the %dth time\n", ++resets_);
}
- void DoReceiveData() {
+ bool DoReceiveData() {
last_received_count_ = ++data()->count;
data()->values.count = last_received_count_;
Time::IncrementMockTime(kSensorSendFrequency);
data()->FillinChecksum();
data()->HostToNetwork();
+ return false;
}
int resets() { return resets_; }
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 4dec23e..db8ce0a 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -1,4 +1,3 @@
-#include <libusb-1.0/libusb.h>
#include <memory>
#include "aos/common/inttypes.h"
@@ -7,6 +6,8 @@
#include "aos/common/time.h"
#include "aos/common/sensors/sensor_unpacker.h"
#include "aos/common/sensors/sensor_receiver.h"
+#include "aos/common/glibusb/glibusb.h"
+#include "aos/common/glibusb/gbuffer.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
@@ -14,7 +15,6 @@
#include "frc971/control_loops/index/index_motor.q.h"
#include "frc971/control_loops/shooter/shooter_motor.q.h"
#include "frc971/input/gyro_board_data.h"
-#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
#ifndef M_PI
@@ -194,31 +194,43 @@
// 0 is unlimited
static const unsigned int kReadTimeout = 1000;
- // vendor ID
- static const int32_t kVid = 0x1424;
- // product ID
- static const int32_t kPid = 0xd243;
+ static constexpr glibusb::VendorProductId kDeviceId =
+ glibusb::VendorProductId(0x1424 /* vendor ID */,
+ 0xd243 /* product ID */);
// How big of a buffer to give the function.
static const size_t kDataLength = 64;
- virtual void DoReceiveData() {
+ virtual bool DoReceiveData() {
// Loop and then return once we get a good one.
while (true) {
- completed_transfer_ = NULL;
- while (completed_transfer_ == NULL) {
- libusb_.HandleEvents();
+ using glibusb::UsbEndpoint;
+ UsbEndpoint::IoStatus result =
+ endpoint_->ReadAtMostWithTimeout(sizeof(GyroBoardData),
+ kReadTimeout,
+ &buffer_);
+ switch (result) {
+ case UsbEndpoint::kSuccess:
+ break;
+ case UsbEndpoint::kTimeout:
+ LOG(WARNING, "read timed out\n");
+ continue;
+ case UsbEndpoint::kNoDevice:
+ LOG(ERROR, "no device\n");
+ return true;
+ case UsbEndpoint::kUnknown:
+ case UsbEndpoint::kFail:
+ case UsbEndpoint::kAbort:
+ LOG(ERROR, "read failed\n");
+ continue;
}
- LOG(DEBUG, "processing transfer %p\n", completed_transfer_);
-
- if (completed_transfer_->read_bytes() <
- static_cast<ssize_t>(sizeof(GyroBoardData))) {
- LOG(ERROR, "read %d bytes instead of at least %zd\n",
- completed_transfer_->read_bytes(), sizeof(GyroBoardData));
+ if (buffer_.Length() < sizeof(GyroBoardData)) {
+ LOG(ERROR, "read %zd bytes instead of at least %zd\n",
+ buffer_.Length(), sizeof(GyroBoardData));
continue;
}
- memcpy(&data()->values, completed_transfer_->data(),
+ memcpy(&data()->values, buffer_.GetBufferPointer(sizeof(GyroBoardData)),
sizeof(GyroBoardData));
if (data()->count == 0) {
start_time_ = ::aos::time::Time::Now();
@@ -228,64 +240,35 @@
data()->count = static_cast<int32_t>(
(delta_time / ::aos::sensors::kSensorSendFrequency) + 0.5);
}
- return;
+ return false;
}
}
virtual void Reset() {
- transfer1_.reset();
- transfer2_.reset();
- dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
- libusb_.FindDeviceWithVIDPID(kVid, kPid));
- if (!dev_handle_) {
- LOG(ERROR, "couldn't find device. exiting\n");
- exit(1);
- }
- transfer1_ = ::std::unique_ptr<libusb::Transfer>(
- new libusb::Transfer(kDataLength, StaticTransferCallback, this));
- transfer2_ = ::std::unique_ptr<libusb::Transfer>(
- new libusb::Transfer(kDataLength, StaticTransferCallback, this));
- transfer1_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
- transfer2_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
- transfer1_->Submit();
- transfer2_->Submit();
+ device_ = ::std::unique_ptr<glibusb::UsbDevice>(
+ libusb_.FindSingleMatchingDeviceOrLose(kDeviceId));
+ CHECK(device_);
+ endpoint_ = ::std::unique_ptr<glibusb::UsbInEndpoint>(
+ device_->InEndpoint(kEndpoint));
data()->count = 0;
}
- static void StaticTransferCallback(libusb::Transfer *transfer, void *self) {
- static_cast<GyroSensorReceiver *>(self)->TransferCallback(transfer);
- }
- void TransferCallback(libusb::Transfer *transfer) {
- if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
- LOG(DEBUG, "transfer %p completed\n", transfer);
- completed_transfer_ = transfer;
- } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
- LOG(WARNING, "transfer %p timed out\n", transfer);
- } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
- LOG(DEBUG, "transfer %p cancelled\n", transfer);
- } else {
- LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
- }
- transfer->Submit();
- }
-
virtual void Synchronized(::aos::time::Time start_time) {
// Subtract off how many packets it read while synchronizing from the time.
start_time_ = start_time -
::aos::sensors::kSensorSendFrequency * data()->count;
}
- ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
- ::std::unique_ptr<libusb::Transfer> transfer1_, transfer2_;
- // Temporary variable for holding a completed transfer to communicate that
- // information from the callback to the code that wants it.
- libusb::Transfer *completed_transfer_;
+ ::std::unique_ptr<glibusb::UsbDevice> device_;
+ ::std::unique_ptr<glibusb::UsbInEndpoint> endpoint_;
+ glibusb::Buffer buffer_;
::aos::time::Time start_time_;
- LibUSB libusb_;
+ glibusb::Libusb libusb_;
};
+constexpr glibusb::VendorProductId GyroSensorReceiver::kDeviceId;
} // namespace frc971
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 5e08a20..a0e6559 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -35,11 +35,11 @@
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/atom_code/atom_code.gyp:init',
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(EXTERNALS):libusb',
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ '<(AOS)/common/glibusb/glibusb.gyp:glibusb',
+ '<(AOS)/common/glibusb/glibusb.gyp:gbuffer',
],
},
{