Added initial libusb driver. This needs to be modified to build under GYP and with AOS.
diff --git a/gyro_board/src/libusb-driver/get.cc b/gyro_board/src/libusb-driver/get.cc
new file mode 100644
index 0000000..5922958
--- /dev/null
+++ b/gyro_board/src/libusb-driver/get.cc
@@ -0,0 +1,235 @@
+#include "get.h"
+
+#include <google/gflags.h>
+#include <glog/logging.h>
+#include <iostream>
+#include <signal.h>
+#include <stdio.h>
+#include <queue>
+#include <unistd.h>
+#include <map>
+
+#include "signal_handler.h"
+
+DEFINE_int32(vid, 0x1424, "Vendor ID of the device.");
+DEFINE_int32(pid, 0xd243, "Product ID of the device.");
+DEFINE_bool(send, true,
+ "True if the code should send packets. (default: true)");
+
+// USB descriptors in use:
+// 0x81 Interrupt IN endpoint with the received CAN packets.
+// These packets are 64 bytes maximum, and have the same format as
+// SocketCAN.
+// 0x04 Interrupt OUT endpoint with the CAN packets to send.
+// These packets are 64 bytes maximum, and have the same format as
+// SocketCAN.
+// 0x82 Bulk IN endpoint with printf output.
+// 0x05 Bulk OUT endpoint for stdin.
+
+bool can_frame_priority_comparator::operator() (
+ const struct can_frame& x,
+ const struct can_frame& y) const {
+ // Error frames win first.
+ if (x.can_id & CAN_EFF_FLAG) {
+ return false;
+ }
+ if (y.can_id & CAN_EFF_FLAG) {
+ return true;
+ }
+ // Extended frames send the first 11 bits out just like a standard frame.
+ int x_standard_bits = (x.can_id & CAN_SFF_MASK);
+ int y_standard_bits = (y.can_id & CAN_SFF_MASK);
+ if (x_standard_bits == y_standard_bits) {
+ // RTR wins next.
+ bool x_rtr = (x.can_id & CAN_RTR_FLAG);
+ bool y_rtr = (y.can_id & CAN_RTR_FLAG);
+ if (x_rtr == y_rtr) {
+ // Now check if it is an EFF packet.
+ bool x_eff = (x.can_id & CAN_EFF_FLAG);
+ bool y_eff = (y.can_id & CAN_EFF_FLAG);
+ if (x_eff == y_eff) {
+ // Now compare the bits in the extended frame.
+ return (x.can_id & CAN_EFF_MASK) < (y.can_id & CAN_EFF_MASK);
+ } else if (x_eff < y_eff) {
+ return false;
+ } else {
+ return true;
+ }
+ } else if (x_rtr < y_rtr) {
+ return true;
+ } else {
+ return false;
+ }
+ } else {
+ return x_standard_bits < y_standard_bits;
+ }
+}
+
+class GyroDriver::PacketReceiver : public Thread {
+ public:
+ // refusing to send any more frames.
+ static const int kMaxRXFrames = 128;
+
+ explicit PacketReceiver(LibUSBDeviceHandle *dev_handle)
+ : Thread(), dev_handle_(dev_handle), rx_queue_(new can_priority_queue) {}
+
+ // Serve.
+ void operator()();
+
+ bool GetCanFrame(struct can_frame *msg);
+
+ private:
+ typedef std::priority_queue<struct can_frame,
+ std::vector<struct can_frame>,
+ can_frame_priority_comparator> can_priority_queue;
+
+ LibUSBDeviceHandle *dev_handle_;
+ boost::mutex rx_mutex_;
+ std::unique_ptr<can_priority_queue> rx_queue_;
+ //boost::condition_variable rx_cond_;
+};
+
+GyroDriver::GyroDriver(LibUSBDeviceHandle *dev_handle)
+ : dev_handle_(dev_handle), dbg_(new DbgReader(this)),
+ debug_thread_(new boost::thread(boost::ref(*dbg_))),
+ rx_(new GyroDriver::PacketReceiver(dev_handle)),
+ rx_thread_(new boost::thread(boost::ref(*rx_))) { }
+
+
+std::string GyroDriver::GetDebugData() {
+ char data[64];
+ int r;
+ int transferred;
+ r = dev_handle_->bulk_transfer(0x82,
+ (unsigned char *)data, sizeof(data),
+ &transferred, 0);
+ return std::string(data, transferred);
+}
+
+void GyroDriver::Terminate() {
+ rx_->Terminate();
+}
+
+GyroDriver::~GyroDriver() {
+ rx_->Terminate();
+ rx_thread_->join();
+ dbg_->Terminate();
+ debug_thread_->join();
+}
+
+//bool GyroDriver::GetCanFrame(struct can_frame *msg) {
+// return rx_->GetCanFrame(msg);
+//}
+
+struct DataStruct{
+ int64_t gyro_angle;
+
+ int32_t right_drive;
+ int32_t left_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+} __attribute__((__packed__));
+
+void GyroDriver::PacketReceiver::operator()() {
+ int r;
+ int actual;
+ uint8_t data[64];
+ DataStruct *real_data;
+ real_data = (DataStruct *)data;
+
+ while (should_run()) {
+ r = dev_handle_->interrupt_transfer(
+ 0x81, data, sizeof(data), &actual, 1000);
+ printf("size: %d\n",sizeof(DataStruct));
+ CHECK_GT(actual, 0);
+
+ if (r != 0) {
+ LOG(ERROR) << "Read Error. Read " << actual;
+ }
+
+ printf("angle: %d\n", real_data->gyro_angle);
+ printf("drivel: %d\n", real_data->left_drive);
+ printf("driver: %d\n", real_data->right_drive);
+ printf("shooter: %d\n", real_data->shooter);
+ printf("shooter_angle: %d\n", real_data->shooter_angle);
+ printf("indexer: %d\n", real_data->indexer);
+ printf("wrist: %d\n", real_data->wrist);
+ printf("capture:\n");
+ printf(" wrist_rise{%d}: %d\n", real_data->wrist_rise_count,
+ real_data->capture_wrist_rise);
+ printf(" shooter_angle_rise{%d}: %d\n", real_data->shooter_angle_rise_count,
+ real_data->capture_shooter_angle_rise);
+ printf(" bottom_rise_delay{%d}; \n", real_data->bottom_rise_count);
+ printf(" bottom_fall_delay{%d,%d}: %d\n", real_data->bottom_fall_count,
+ real_data->bottom_fall_delay_count,
+ real_data->capture_bottom_fall_delay);
+ printf(" top_rise{%d}: %d\n", real_data->top_rise_count,
+ real_data->capture_top_rise);
+ printf(" top_fall{%d}: %d\n", real_data->top_fall_count,
+ real_data->capture_top_fall);
+
+
+
+
+ //if (actual > 1) {
+ // rx_cond_.notify_all();
+ //}
+ }
+ //rx_cond_.notify_all();
+ LOG(INFO) << "Receive thread down.";
+}
+
+DbgReader::DbgReader(GyroDriver *gyro)
+ : Thread(), gyro_(gyro) {}
+
+void DbgReader::operator()() {
+ unsigned char data[64];
+ LOG(INFO) << "Running debug dump thread.";
+ while (should_run()) {
+ printf("%s", gyro_->GetDebugData().c_str());
+ }
+ LOG(INFO) << "Exiting debug dump thread.";
+}
+
+
+int main(int argc, char ** argv) {
+ int r;
+ google::InitGoogleLogging(argv[0]);
+ google::ParseCommandLineFlags(&argc, &argv, true);
+ google::InstallFailureSignalHandler();
+
+ LibUSB libusb;
+
+ {
+ std::unique_ptr<LibUSBDeviceHandle> dev_handle(
+ CHECK_NOTNULL(libusb.FindDeviceWithVIDPID(FLAGS_vid, FLAGS_pid)));
+
+ GyroDriver gyro(dev_handle.release());
+
+ while(true){
+ sleep(50);
+ }
+ }
+ return 0;
+}