├── ArmController.cpp ├── ArmController.h ├── BoM.md ├── CMakeLists.txt ├── CMakePreBuild.sh ├── Config.h ├── Matrix4.h ├── Matrix4.hpp ├── MotorController.cpp ├── MotorController.h ├── README.md ├── Rover.cpp ├── Rover.h ├── ServoController.cpp ├── ServoController.h ├── UsbManager.cpp ├── UsbManager.h ├── consoleLinux.h ├── docs └── images │ ├── turbo1-tk1.jpg │ ├── turbo2+.jpg │ ├── turbo2_front.jpg │ ├── turbo2_internals.jpg │ └── turbo2_rear.jpg ├── evdevController.cpp ├── evdevController.h ├── main.cpp ├── panTilt.cpp ├── panTilt.h ├── phidgetIMU.cpp ├── phidgetIMU.h ├── rpLIDAR.cpp ├── rpLIDAR.h ├── rplidar-sdk ├── include │ ├── rplidar.h │ ├── rplidar_cmd.h │ ├── rplidar_driver.h │ ├── rplidar_protocol.h │ └── rptypes.h ├── rplidar_sdk_1.4.5 └── src │ ├── arch │ ├── linux │ │ ├── arch_linux.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── thread.hpp │ │ ├── timer.cpp │ │ └── timer.h │ ├── macOS │ │ ├── arch_macOS.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── thread.hpp │ │ ├── timer.cpp │ │ └── timer.h │ └── win32 │ │ ├── arch_win32.h │ │ ├── net_serial.cpp │ │ ├── net_serial.h │ │ ├── timer.cpp │ │ ├── timer.h │ │ └── winthread.hpp │ ├── hal │ ├── abs_rxtx.h │ ├── event.h │ ├── locker.h │ ├── thread.cpp │ ├── thread.h │ └── util.h │ ├── rplidar_driver.cpp │ ├── rplidar_driver_serial.h │ └── sdkcommon.h ├── scripts ├── config-0-all.sh ├── config-1-ksrc.sh ├── config-2-kconfig.sh ├── config-3-kmod.sh ├── config-4-hostapd.sh ├── config-5-network.sh ├── config-6-phidgets.sh ├── config-7-sixaxis.sh ├── init-0-all.sh ├── init-1-bluetooth.sh ├── init-2-maxclocks.sh ├── init-3-webserver.sh ├── init-4-gstreamer.sh └── init-5-turbo.sh ├── sixad ├── Makefile ├── bins │ ├── sixad-3in1 │ ├── sixad-bin │ ├── sixad-raw │ ├── sixad-remote │ └── sixad-sixaxis ├── bluetooth.cpp ├── bluetooth.h ├── manual.pdf ├── qtcreator │ ├── sixad-bin.pro │ ├── sixad-raw.pro │ ├── sixad-remote.pro │ └── sixad-sixaxis.pro ├── remote.cpp ├── remote.h ├── shared.cpp ├── shared.h ├── sixad ├── sixad-3in1.cpp ├── sixad-bin.cpp ├── sixad-dbus-blocker ├── sixad-raw.cpp ├── sixad-remote.cpp ├── sixad-sixaxis.cpp ├── sixad.default ├── sixad.init ├── sixad.log ├── sixaxis.cpp ├── sixaxis.h ├── textfile.cpp ├── textfile.h ├── uinput.cpp └── uinput.h ├── sixpair ├── sixpair └── sixpair.c ├── v4l2Camera.cpp └── v4l2Camera.h /ArmController.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #include "ArmController.h" 6 | 7 | 8 | // constructor 9 | ArmController::ArmController( ServoController* servoController ) : mServo(servoController) 10 | { 11 | //mServo->SetPosition(PAN_CHANNEL, PAN_CENTER); 12 | //mServo->SetPosition(TILT_CHANNEL, TILT_CENTER); 13 | } 14 | 15 | 16 | ArmController::~ArmController() 17 | { 18 | // ServoController is deleted by UsbManager 19 | } 20 | 21 | 22 | void ArmController::Update( evdevController* user ) 23 | { 24 | if( !user ) 25 | return; 26 | 27 | printf("ARM controller active!\n"); 28 | 29 | /** 30 | * pan 31 | */ 32 | const float x = float(user->GetState(evdevController::AXIS_LX)) / 128.0f; 33 | 34 | float pan = PAN_CENTER; 35 | 36 | if( x < 0.0f ) 37 | pan = ((PAN_CENTER - PAN_LEFT) * x) + PAN_CENTER; 38 | if( x > 0.0f ) 39 | pan = PAN_CENTER - ((PAN_CENTER - PAN_RIGHT) * x); 40 | 41 | mServo->SetPosition(PAN_CHANNEL, pan); 42 | 43 | arm1(user); 44 | arm2(user); 45 | arm3(user); 46 | } 47 | 48 | void ArmController::arm1( evdevController* user ) 49 | { 50 | const float y = float(user->GetState(evdevController::AXIS_LY)) / 128.0f; 51 | 52 | float tilt = TILT_CENTER_1; 53 | 54 | if( y < 0.0f ) 55 | tilt = ((TILT_CENTER_1 - TILT_DOWN_1) * y) + TILT_CENTER_1; 56 | if( y > 0.0f ) 57 | tilt = TILT_CENTER_1 - ((TILT_CENTER_1 - TILT_UP_1) * y); 58 | 59 | mServo->SetPosition(TILT_CHANNEL_1, tilt); 60 | } 61 | 62 | void ArmController::arm2( evdevController* user ) 63 | { 64 | const float y = float(user->GetState(evdevController::AXIS_RY)) / 128.0f; 65 | 66 | float tilt = TILT_CENTER_2; 67 | 68 | if( y < 0.0f ) 69 | tilt = ((TILT_CENTER_2 - TILT_DOWN_2) * y) + TILT_CENTER_2; 70 | if( y > 0.0f ) 71 | tilt = TILT_CENTER_2 - ((TILT_CENTER_2 - TILT_UP_2) * y); 72 | 73 | mServo->SetPosition(TILT_CHANNEL_2, tilt); 74 | } 75 | 76 | void ArmController::arm3( evdevController* user ) 77 | { 78 | const float y = float(user->GetState(evdevController::AXIS_RX)) / 128.0f; 79 | 80 | float tilt = TILT_CENTER_3; 81 | 82 | if( y < 0.0f ) 83 | tilt = ((TILT_CENTER_3 - TILT_DOWN_3) * y) + TILT_CENTER_3; 84 | if( y > 0.0f ) 85 | tilt = TILT_CENTER_3 - ((TILT_CENTER_3 - TILT_UP_3) * y); 86 | 87 | mServo->SetPosition(TILT_CHANNEL_3, tilt); 88 | } 89 | 90 | -------------------------------------------------------------------------------- /ArmController.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_ARM_CONTROLLER_H 6 | #define __ROVER_ARM_CONTROLLER_H 7 | 8 | 9 | #include "ServoController.h" 10 | #include "evdevController.h" 11 | 12 | 13 | /** 14 | * ArmController 15 | */ 16 | class ArmController 17 | { 18 | public: 19 | /** 20 | * constructor 21 | */ 22 | ArmController( ServoController* servoController ); 23 | 24 | /** 25 | * destructor 26 | */ 27 | ~ArmController(); 28 | 29 | /** 30 | * Update 31 | */ 32 | void Update( evdevController* userController ); 33 | 34 | static const uint8_t PAN_CHANNEL = 7; 35 | static const uint8_t TILT_CHANNEL_1 = 8; 36 | static const uint8_t TILT_CHANNEL_2 = 10; 37 | static const uint8_t TILT_CHANNEL_3 = 9; 38 | 39 | static const uint16_t PAN_CENTER = 1500; // arm L/R base 40 | static const uint16_t PAN_LEFT = 992; 41 | static const uint16_t PAN_RIGHT = 2000; 42 | 43 | static const uint16_t TILT_CENTER_1 = 1200; // 1st arm segment 44 | static const uint16_t TILT_DOWN_1 = 1392; 45 | static const uint16_t TILT_UP_1 = 992; 46 | 47 | static const uint16_t TILT_CENTER_2 = 1500; // 2nd arm segment 48 | static const uint16_t TILT_DOWN_2 = 2000; 49 | static const uint16_t TILT_UP_2 = 992; 50 | 51 | static const uint16_t TILT_CENTER_3 = 1500; // final DoF 52 | static const uint16_t TILT_DOWN_3 = 2000; 53 | static const uint16_t TILT_UP_3 = 992; 54 | 55 | protected: 56 | void arm1( evdevController* user ); 57 | void arm2( evdevController* user ); 58 | void arm3( evdevController* user ); 59 | 60 | ServoController* mServo; 61 | }; 62 | 63 | 64 | #endif 65 | 66 | -------------------------------------------------------------------------------- /BoM.md: -------------------------------------------------------------------------------- 1 | ## Bill of Materials 2 | 3 | ### turbo2 Chassis 4 | 5 | | Vendor | Part | Qty. | 6 | | ------------ | ------------- | ----- | 7 | | SuperDroids | [Welded Aluminum chassis](http://www.superdroidrobots.com/shop/item.aspx/welded-aluminum-enclosed-chassis-ig42-sb/1493/) | 1 | 8 | | SuperDroids | [Heavy-duty Pan/Tilt](http://www.superdroidrobots.com/shop/item.aspx/camera-360-pan-and-tilt-system-heavyduty/1145/) | 1 | 9 | | SuperDroids | [Traction Lug Tire Set](http://www.superdroidrobots.com/shop/item.aspx?itemid=1995) | 2 | 10 | | SuperDroids | [24VDC 78RPM geared motor](http://www.superdroidrobots.com/shop/item.aspx?itemid=1036) | 4 | 11 | | Pololu | [24V 12A motor controllers](https://www.pololu.com/product/1378) | 2 | 12 | | Pololu | [Micro Maestro](https://www.pololu.com/product/1350) | 1 | 13 | | HobbyKing | [6S 8000mAh LiPo battery](http://www.hobbyking.com/hobbyking/store/__66479__Multistar_High_Capacity_6S_8000mAh_Multi_Rotor_Lipo_Pack_AR_Warehouse_.html) | 1 or 2 | 14 | 15 | ### turbo1 Chassis 16 | 17 | | Vendor | Part | Qty. | 18 | | ------------ | ------------- | ----- | 19 | | Nexus Robot | [Tracked Robot Kit](http://www.robotshop.com/en/arduino-tracked-mobile-tank-robot-kit.html) | 1 | 20 | | Polulu | [12V 15A motor controllers](https://www.pololu.com/product/1376) | 2 | 21 | | Lynxmotion | [12V 2800mAh NiMH battery](http://www.robotshop.com/en/120v-2800mah-rechargeable-nimh-battery-pack.html) | 1 | 22 | | Auvidea | [Auvidea J120](http://igg.me/at/cJ7dDQkO6lw/x/13380589) | 1* | 23 | | ConnectTech | [ConnectTech Orbitty](http://www.wdlsystems.com/Computer-on-Module/Carrier-Boards/CTI-Orbitty-Carrier-for-NVIDIA-Jetson-TX1.html) | 1* | 24 | 25 | * Recommended either [Auvidea J120](http://igg.me/at/cJ7dDQkO6lw/x/13380589) or [ConnectTech Orbitty](http://www.wdlsystems.com/Computer-on-Module/Carrier-Boards/CTI-Orbitty-Carrier-for-NVIDIA-Jetson-TX1.html) mini-carriers for Jetson TX1, or similar can be used. 26 | 27 | ### Shared Electronics/Sensors 28 | 29 | | Vendor | Part | Qty. | 30 | | ------------ | ------------- | ----- | 31 | | NVIDIA | [Jetson TX1 Developer Kit](https://developer.nvidia.com/embedded/buy/jetson-tx1-devkit) | 1 | 32 | | Stereolabs | [ZED depth camera](https://www.stereolabs.com/zed/specs/) | 1 | 33 | | RoboPeak | [rpLIDAR rangefinder](http://www.slamtec.com/en/lidar) | 1 | 34 | | Phidgets | [Phidget 9-DoF IMU](http://www.phidgets.com/products.php?product_id=1044) | 1 | 35 | | GlobalSat | [BU-353 GPS Reciever](http://www.amazon.com/GlobalSat-BU-353-Receiver-Discontinued-Manufacturer/dp/B000PKX2KA) | 1 | 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | project(turbo2) 5 | 6 | # if this is the first time running cmake, perform pre-build dependency install script (or if the user manually triggers re-building the dependencies) 7 | set(BUILD_DEPS "YES" CACHE BOOL "If YES, will install dependencies. Automatically reset to NO after dependencies are installed.") 8 | set(ROVERNET_PATH "${PROJECT_BINARY_DIR}/rovernet" CACHE FILEPATH "Path to rovernet repo") 9 | set(INFERENCE_PATH "${PROJECT_BINARY_DIR}/jetson-inference" CACHE FILEPATH "Path to jetson-inference repo") 10 | #set(ROVERNET_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../rovernet" CACHE FILEPATH "Path to rovernet repo") 11 | 12 | if( ${BUILD_DEPS} ) 13 | message("Launching pre-build dependency installer script...") 14 | 15 | execute_process(COMMAND sh ../CMakePreBuild.sh 16 | WORKING_DIRECTORY ${PROJECT_BINARY_DIR} 17 | RESULT_VARIABLE PREBUILD_SCRIPT_RESULT) 18 | 19 | set(BUILD_DEPS "NO" CACHE BOOL "If YES, will install dependencies. Automatically reset to NO after dependencies are installed." FORCE) 20 | message("Finished installing dependencies") 21 | endif() 22 | 23 | find_package(CUDA) 24 | 25 | # Pass options to NVCC 26 | set( 27 | CUDA_NVCC_FLAGS 28 | ${CUDA_NVCC_FLAGS}; 29 | -O3 -gencode arch=compute_53,code=sm_53 30 | ) 31 | 32 | 33 | # build turbo2 34 | file(GLOB turboSources *.cpp) 35 | file(GLOB turboIncludes *.h) 36 | 37 | # rpLIDAR sdk 38 | file(GLOB rpSrcA rplidar-sdk/src/arch/linux/*.cpp) 39 | file(GLOB rpSrcB rplidar-sdk/src/hal/*.cpp) 40 | file(GLOB rpSrcC rplidar-sdk/src/*.cpp) 41 | 42 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/rplidar-sdk/include ${CMAKE_CURRENT_SOURCE_DIR}/rplidar-sdk/src) 43 | include_directories(${ROVERNET_PATH}/c ${ROVERNET_PATH}/cuda) 44 | link_directories(${ROVERNET_PATH}/build) 45 | message("path to rovernet: ${ROVERNET_PATH}") 46 | 47 | 48 | # compile executable 49 | cuda_add_executable(turbo2 ${turboSources} ${rpSrcA} ${rpSrcB} ${rpSrcC}) 50 | 51 | target_link_libraries(turbo2 usb-1.0 pthread rovernet phidget21) 52 | 53 | 54 | # transfer all LUA scripts to bin directory 55 | file(GLOB rovernetScripts ${ROVERNET_PATH}/lua/*.lua) 56 | 57 | foreach(rovernetScript ${rovernetScripts}) 58 | message("-- Configuring ${rovernetScript}") 59 | configure_file(${rovernetScript} ${PROJECT_BINARY_DIR} COPYONLY) 60 | endforeach() 61 | 62 | 63 | 64 | # transfer all startup scripts to bin directory 65 | file(GLOB init_scripts scripts/init*.sh) 66 | 67 | foreach(init_script ${init_scripts}) 68 | message("-- Configuring ${init_script}") 69 | configure_file(${init_script} ${PROJECT_BINARY_DIR} COPYONLY) 70 | endforeach() 71 | 72 | -------------------------------------------------------------------------------- /CMakePreBuild.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # this script is automatically run from CMakeLists.txt 3 | 4 | sudo apt-get update 5 | sudo apt-get install -y screen libusb-dev libusb-1.0-0-dev libbluetooth-dev libgstreamer0.10-dev libgstreamer-plugins-base0.10-dev 6 | sudo apt-get update 7 | 8 | 9 | git clone http://github.com/dusty-nv/jetson-inference 10 | cd jetson-inference 11 | mkdir build 12 | cd build 13 | cmake ../ 14 | make 15 | 16 | 17 | cd ../../ 18 | git clone http://github.com/dusty-nv/rovernet 19 | cd rovernet 20 | mkdir build 21 | cd build 22 | cmake ../ 23 | make 24 | 25 | -------------------------------------------------------------------------------- /Config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_CONFIG_H 6 | #define __ROVER_CONFIG_H 7 | 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /Matrix4.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_MATRIX4_TEMPLATE_H 6 | #define __ROVER_MATRIX4_TEMPLATE_H 7 | 8 | 9 | 10 | inline double Sqrt(double x) { return sqrt(x); } 11 | 12 | 13 | 14 | // TODO get rid of these Cos/Sin definitions... 15 | inline double Cos( double angle ) 16 | { 17 | return cos(angle); 18 | } 19 | 20 | inline float Cos( float angle ) 21 | { 22 | return cosf(angle); 23 | } 24 | 25 | inline double Sin( double angle ) 26 | { 27 | return sin(angle); 28 | } 29 | 30 | inline float Sin( float angle ) 31 | { 32 | return sinf(angle); 33 | } 34 | 35 | 36 | 37 | /** 38 | * [00 01 02 03] // m16 offsets 39 | * [04 05 06 07] 40 | * [08 09 10 11] 41 | * [12 13 14 15] 42 | * 43 | * [00 01 02 03] // m44 offsets --> [row][column] 44 | * [10 11 12 13] 45 | * [20 21 22 23] 46 | * [30 31 32 33] 47 | * 48 | * [Xx Xy Xz 0] // right 49 | * [Yx Yy Yz 0] // up 50 | * [Zx Zy Zz 0] // forward 51 | * [Tx Ty Tz 1] // translation 52 | * 53 | */ 54 | template class TMatrix 55 | { 56 | public: 57 | /** 58 | * Default constructor. 59 | * This leaves the matrix uninitialized. 60 | */ 61 | inline TMatrix() {} 62 | 63 | /** 64 | * Initialize to identity. 65 | */ 66 | inline TMatrix(const bool identity) { if (identity) Identity(); } 67 | 68 | /** 69 | * Copy constructor. 70 | */ 71 | inline TMatrix(const TMatrix& m); 72 | 73 | /** 74 | * Sets the matrix to identity. 75 | */ 76 | void Identity(); 77 | 78 | /** 79 | * Transpose the matrix (swap rows with columns). 80 | */ 81 | void Transpose(); 82 | 83 | /** 84 | * Multiply this matrix with another matrix and stores the result in itself. 85 | */ 86 | void MultMatrix(const TMatrix& right); 87 | 88 | /** 89 | * Multiply this matrix with the 3x3 rotation part of the other matrix. 90 | */ 91 | void MultMatrix3x3(const TMatrix& right); 92 | 93 | /** 94 | * Multiply the left matrix with the right matrix and store the result in this matrix object. 95 | */ 96 | void MultMatrix(const TMatrix& left, const TMatrix& right); 97 | 98 | /** 99 | * Multiply a vector with the 3x3 rotation part of this matrix. 100 | * T* is a float[3] or double[3] 3D vector 101 | */ 102 | inline void Mul3x3(T* v, T* o) const; 103 | 104 | /** 105 | * Makes the matrix an rotation matrix along the x-axis (in radians). 106 | */ 107 | void SetRotationMatrixX(const T angle); 108 | 109 | /** 110 | * Makes the matrix a rotation matrix along the y-axis (in radians). 111 | */ 112 | void SetRotationMatrixY(const T angle); 113 | 114 | /** 115 | * Makes the matrix a rotation matrix along the z-axis (in radians). 116 | */ 117 | void SetRotationMatrixZ(const T angle); 118 | 119 | /** 120 | * Translate the matrix. 121 | */ 122 | inline void Translate(const T x, const T y, const T z) { m44[3][0]+=x; m44[3][1]+=y; m44[3][2]+=z; } 123 | 124 | /** 125 | * Print the matrix 126 | */ 127 | void Print() const; 128 | 129 | 130 | // operators 131 | TMatrix operator + (const TMatrix& right) const; 132 | TMatrix operator - (const TMatrix& right) const; 133 | TMatrix operator * (const TMatrix& right) const; 134 | TMatrix& operator += (const TMatrix& right); 135 | TMatrix& operator -= (const TMatrix& right); 136 | TMatrix& operator *= (const TMatrix& right); 137 | inline void operator = (const TMatrix& right); 138 | 139 | // attributes 140 | union 141 | { 142 | T m16[16]; 143 | T m44[4][4]; 144 | }; 145 | }; 146 | 147 | 148 | /** 149 | * Matrix typedef 150 | */ 151 | typedef /*__ALIGN(16)*/ TMatrix Matrix4d; 152 | typedef /*__ALIGN(16)*/ TMatrix Matrix4f; 153 | typedef /*__ALIGN(16)*/ TMatrix Matrix4; 154 | 155 | 156 | #include "Matrix4.hpp" 157 | 158 | #endif 159 | 160 | -------------------------------------------------------------------------------- /MotorController.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #include "MotorController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | 14 | // constructor 15 | MotorController::MotorController( libusb_device* device ) : mDevice(device) 16 | { 17 | mDeviceCtx = NULL; 18 | 19 | memset(&mVariables, 0, sizeof(Variables)); 20 | } 21 | 22 | 23 | // destructor 24 | MotorController::~MotorController() 25 | { 26 | Close(); 27 | } 28 | 29 | 30 | // Open 31 | bool MotorController::Open() 32 | { 33 | if( libusb_open(mDevice, &mDeviceCtx) != 0 ) 34 | { 35 | printf("MotorController - failed to open USB device\n"); 36 | return false; 37 | } 38 | 39 | // get serial number 40 | libusb_device_descriptor desc; 41 | 42 | if( libusb_get_device_descriptor(mDevice, &desc) != 0 ) 43 | return false; 44 | 45 | char serialStr[512]; 46 | memset(serialStr, 0, sizeof(serialStr)); 47 | 48 | if( libusb_get_string_descriptor_ascii(mDeviceCtx, desc.iSerialNumber, (uint8_t*)serialStr, sizeof(serialStr)) > 0 ) 49 | mSerial = serialStr; 50 | 51 | return true; 52 | } 53 | 54 | 55 | 56 | // Close 57 | void MotorController::Close() 58 | { 59 | if( mDeviceCtx != NULL ) 60 | { 61 | libusb_close(mDeviceCtx); 62 | mDeviceCtx = NULL; 63 | } 64 | } 65 | 66 | 67 | // SetSpeed 68 | bool MotorController::SetSpeed( MotorController::Direction dir, int speed ) 69 | { 70 | if( speed > 3200 ) 71 | return false; 72 | 73 | //printf("set speed %i direction %i\n", speed, (int)dir); 74 | 75 | return ControlTransfer(0x40, 0x90, speed, dir); 76 | } 77 | 78 | // SetSpeed 79 | bool MotorController::SetSpeed( int speed ) 80 | { 81 | //printf("set speed %i\n", speed); 82 | 83 | //if( speed < 3200 || speed > 3200 ) 84 | //return false; 85 | 86 | if( speed < 0 ) 87 | return SetSpeed(DIRECTION_REVERSE, -speed); 88 | 89 | return SetSpeed(DIRECTION_FORWARD, speed); 90 | } 91 | 92 | 93 | bool MotorController::ExitSafeStart() 94 | { 95 | if( !ControlTransfer(0x40, 0x91, (uint)0, 0) ) 96 | return false; 97 | 98 | return true; 99 | } 100 | 101 | 102 | // ReadVariables 103 | bool MotorController::ReadVariables( MotorController::Variables* var ) 104 | { 105 | if( !var ) 106 | return false; 107 | 108 | if( !ControlTransfer(0xC0, 0x83, var, sizeof(Variables)) ) 109 | return false; 110 | 111 | return true; 112 | } 113 | 114 | 115 | // ControlTransfer 116 | bool MotorController::ControlTransfer( uint8_t requestType, uint8_t request, void* data, uint size ) 117 | { 118 | return ControlTransfer(requestType, request, 0, 0, data, size); 119 | } 120 | 121 | 122 | // ControlTransfer 123 | bool MotorController::ControlTransfer( uint8_t requestType, uint8_t request, uint value, uint index, void* data, uint size ) 124 | { 125 | const int res = libusb_control_transfer(mDeviceCtx, requestType, request, value, index, (uint8_t*)data, size, 5000); 126 | 127 | if( res != size ) 128 | { 129 | printf("MotorController - usb control transfer failed (size=%i result=%i)\n", size, res); 130 | return false; 131 | } 132 | 133 | return true; 134 | } 135 | 136 | 137 | // ControlTransfer 138 | bool MotorController::ControlTransfer( uint8_t requestType, uint8_t request, uint value, uint index ) 139 | { 140 | return ControlTransfer(requestType, request, value, index, 0, 0); 141 | } 142 | 143 | 144 | void MotorController::PrintVariables() 145 | { 146 | MotorController::Variables var; 147 | memset(&var, 0, sizeof(MotorController::Variables)); 148 | 149 | if( !ReadVariables(&var) ) 150 | { 151 | printf("failed to read status of motor %s\n", mSerial.c_str()); 152 | return; 153 | } 154 | 155 | printf("status of motor %s\n", mSerial.c_str()); 156 | 157 | // print variables 158 | printf(" errors: "); 159 | 160 | if( var.errorStatus.safeStart ) printf("safe-start "); 161 | if( var.errorStatus.lowVIN ) printf("low-VIN "); 162 | if( var.errorStatus.overheat ) printf("overheat "); 163 | 164 | printf("\n"); 165 | 166 | printf(" targetSpeed: %i\n", var.targetSpeed); 167 | printf(" speed: %i\n", var.speed); 168 | printf(" brake: %u\n", var.brake); 169 | printf(" temperature: %f degrees C\n", ((float)var.temperature * 0.1f)); 170 | printf(" voltage: %f V\n", ((float)var.voltage * 0.001f)); 171 | } 172 | 173 | #if 0 174 | #define PRINT_ENUM(var, x) if(var.x) printf(#x " "); 175 | 176 | // GetErrorStatus 177 | MotorController::ErrorStatus MotorController::GetErrorStatus() 178 | { 179 | ErrorStatus status; 180 | 181 | GetVariable(VAR_ERROR_STATUS, &status, sizeof(status)); 182 | 183 | printf("ErrorStatus: "); 184 | //PRINT_ENUM(status, SafeStart) 185 | if(status.SafeStart) printf("SafeStart "); 186 | printf("\n"); 187 | 188 | return status; 189 | } 190 | 191 | 192 | // GetTemperature 193 | float MotorController::GetTemperature( bool fahrenheit ) 194 | { 195 | uint16_t_t raw = 0; 196 | 197 | if( !GetVariable(VAR_TEMPERATURE, &raw, sizeof(raw)) ) 198 | return 0.0f; 199 | 200 | float temp = (float)raw * 0.1f; 201 | 202 | if( fahrenheit ) 203 | temp = (temp * 1.8f) + 32.0f; 204 | 205 | return temp; 206 | } 207 | 208 | 209 | // GetVoltage 210 | float MotorController::GetVoltage() 211 | { 212 | uint16_t_t raw = 0; 213 | 214 | if( !GetVariable(VAR_VOLTAGE, &raw, sizeof(raw)) ) 215 | return 0.0f; 216 | 217 | return (float)raw * 0.001f; 218 | } 219 | 220 | // GetVariable 221 | bool MotorController::GetVariable( MotorController::Variable var, void* buffer, uint size ) 222 | { 223 | if( size != 2 ) 224 | { 225 | printf("MotorController::GetVariable - size of buffer must be 2 uint8_ts (%u uint8_ts)\n", size); 226 | return false; 227 | } 228 | 229 | // send variable query 230 | uint8_t query[] = { 0xA1, (uint8_t)var }; 231 | 232 | if( !Send(query, sizeof(query)) ) 233 | { 234 | printf("MotorController::GetVariable - failed to query variable %i\n", var); 235 | return false; 236 | } 237 | 238 | // wait for reception 239 | if( Recieve(buffer, size) != size ) 240 | { 241 | printf("MotorController::GetVariable - failed to recieve variable %i\n", var); 242 | return false; 243 | } 244 | 245 | return true; 246 | } 247 | 248 | 249 | // Receieve 250 | uint MotorController::Recieve( void* buffer, uint size ) 251 | { 252 | if( mFD == -1 ) 253 | return 0; 254 | 255 | // initialise the timeout structure 256 | struct timeval timeout; 257 | 258 | timeout.tv_sec = 2; // 2 second timeout 259 | timeout.tv_usec = 0; 260 | 261 | // do the select 262 | fd_set rdfs; 263 | FD_ZERO( &rdfs ); 264 | FD_SET( mFD, &rdfs ); 265 | 266 | const int sRes = select(mFD + 1, &rdfs, NULL, NULL, &timeout); 267 | 268 | // check if an error has occured 269 | if(sRes < 0) 270 | { 271 | printf("MotorController::Recieve - select failed (errno %i)\n", sRes); 272 | return 0; 273 | } 274 | else if( sRes == 0 ) 275 | { 276 | printf("MotorController::Recieve - timeout\n"); 277 | return 0; 278 | } 279 | 280 | // read 281 | const int rRes = read(mFD, buffer, size); 282 | 283 | if( rRes < 0 ) 284 | { 285 | printf("MotorController::Recieve - read error %i\n", rRes); 286 | return 0; 287 | } 288 | 289 | return rRes; 290 | } 291 | 292 | 293 | 294 | // Close 295 | void MotorController::Close() 296 | { 297 | if( mFD == -1 ) 298 | return; 299 | 300 | close(mFD); 301 | mFD = -1; 302 | } 303 | 304 | 305 | // Send 306 | bool MotorController::Send( void* data, uint size ) 307 | { 308 | if( mFD == -1 ) 309 | return false; 310 | 311 | if( write(mFD, data, size) != size ) 312 | return false; 313 | 314 | return true; 315 | } 316 | 317 | 318 | // Senduint8_t 319 | bool MotorController::Senduint8_t( uint8_t value ) 320 | { 321 | if( mFD == -1 ) 322 | return false; 323 | 324 | if( write(mFD, &value, 1) != 1 ) 325 | return false; 326 | 327 | return true; 328 | } 329 | 330 | #endif 331 | 332 | 333 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## turbo2 2 | 3 | ![Alt text](https://github.com/dusty-nv/turbo2/raw/master/docs/images/turbo2+.jpg) 4 | 5 | TURBO2 is a deep-learning outdoor 4WD rover using NVIDIA Jetson TX1/TX2 for autonomous navigation and motion control. It uses Stereolabs [ZED](http://stereolabs.com) and LIDAR processed by neural networks with [reinforcement learning](http://github.com/dusty-nv/jetson-reinforcement)) for pathfinding, obstacle avoidance, and performing vision-guided tasks with it's arm like manipulation and picking of objects. 6 | 7 | All the materials and code for building the robot are released here on GitHub. See the Bill of Materials in [BoM.md](./BoM.md) 8 | ## 9 | 10 | ![Alt text](https://github.com/dusty-nv/turbo2/raw/master/docs/images/turbo2_internals.jpg) 11 | ## 12 | 13 | ![Alt text](https://github.com/dusty-nv/turbo2/raw/master/docs/images/turbo2_front.jpg) 14 | 19 | ## turbo1 20 | 21 | ![Alt text](https://github.com/dusty-nv/turbo2/raw/master/docs/images/turbo1-tk1.jpg) 22 | -------------------------------------------------------------------------------- /Rover.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_OBJECT_H 6 | #define __ROVER_OBJECT_H 7 | 8 | 9 | #include "UsbManager.h" 10 | #include "ArmController.h" 11 | #include "v4l2Camera.h" 12 | #include "evdevController.h" 13 | #include "panTilt.h" 14 | #include "rpLIDAR.h" 15 | #include "phidgetIMU.h" 16 | #include "rovernet.h" 17 | 18 | 19 | /** 20 | * Rover primary object. 21 | */ 22 | class Rover 23 | { 24 | public: 25 | /** 26 | * Create 27 | */ 28 | static Rover* Create(); 29 | 30 | /** 31 | * destructor 32 | */ 33 | ~Rover(); 34 | 35 | /** 36 | * Run the next frame / main loop. 37 | */ 38 | bool NextEpoch(); 39 | 40 | /** 41 | * SetGoal 42 | */ 43 | inline void SetGoal( float goal ) { if(mRewardTensor) mRewardTensor->cpuPtr[0] = goal; } 44 | 45 | 46 | protected: 47 | 48 | Rover(); 49 | 50 | bool init(); 51 | bool initMotors(); 52 | bool initBtController(); 53 | 54 | static const uint32_t NumMotorCon = 2; /**< number of motor controllers */ 55 | 56 | MotorController* mMotorCon[NumMotorCon]; 57 | ServoController* mServoCon; 58 | UsbManager* mUsbManager; 59 | ArmController* mArmController; 60 | panTilt* mPanTilt; 61 | v4l2Camera* mCamera; 62 | evdevController* mBtController; /**< Bluetooth /dev/event controller */ 63 | rpLIDAR* mLIDAR; 64 | phidgetIMU* mIMU; 65 | roverNet* mRoverNet; 66 | 67 | roverNet::Tensor* mIMUTensor; 68 | roverNet::Tensor* mLIDARTensor; 69 | roverNet::Tensor* mRangeMap; 70 | roverNet::Tensor* mOutputTensor; 71 | roverNet::Tensor* mRewardTensor; 72 | 73 | static const uint32_t RangeMapSize = 256; 74 | static const uint32_t RangeMapMax = 5000; 75 | static const uint32_t OutputStates = 2; 76 | static const uint32_t DownsampleFactor = 2; 77 | 78 | void* mCameraInputCPU; 79 | void* mCameraInputGPU; 80 | float* mCameraResizeCPU; 81 | float* mCameraResizeGPU; 82 | roverNet::Tensor* mCameraTensor; /**< input video in grayscale floating-point */ 83 | 84 | uint8_t mVersion; // 1 for TURBO1, 2 for TURBO2 85 | uint64_t mEpoch; 86 | }; 87 | 88 | 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /ServoController.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #include "ServoController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | 14 | // constructor 15 | ServoController::ServoController( libusb_device* device ) : mDevice(device) 16 | { 17 | mDeviceCtx = NULL; 18 | } 19 | 20 | 21 | // destructor 22 | ServoController::~ServoController() 23 | { 24 | Close(); 25 | } 26 | 27 | 28 | // Open 29 | bool ServoController::Open() 30 | { 31 | if( libusb_open(mDevice, &mDeviceCtx) != 0 ) 32 | { 33 | printf("ServoController - failed to open USB device\n"); 34 | return false; 35 | } 36 | 37 | // get serial number 38 | libusb_device_descriptor desc; 39 | 40 | if( libusb_get_device_descriptor(mDevice, &desc) != 0 ) 41 | return false; 42 | 43 | char serialStr[512]; 44 | memset(serialStr, 0, sizeof(serialStr)); 45 | 46 | if( libusb_get_string_descriptor_ascii(mDeviceCtx, desc.iSerialNumber, (uint8_t*)serialStr, sizeof(serialStr)) > 0 ) 47 | mSerial = serialStr; 48 | 49 | return true; 50 | } 51 | 52 | 53 | 54 | // Close 55 | void ServoController::Close() 56 | { 57 | if( mDeviceCtx != NULL ) 58 | { 59 | libusb_close(mDeviceCtx); 60 | mDeviceCtx = NULL; 61 | } 62 | } 63 | 64 | 65 | // SetSpeed 66 | bool ServoController::SetPosition( uint8_t servo, uint16_t position ) 67 | { 68 | //if( speed > 3200 ) 69 | //return false; 70 | 71 | printf("[ServoController] set position %u servo %u\n", (uint32_t)position, (uint32_t)servo); 72 | 73 | // 0x85 REQUEST_SET_TARGET 74 | return ControlTransfer(0x40, 0x85, position * 4, servo); 75 | } 76 | 77 | 78 | // ClearErrors 79 | bool ServoController::ClearErrors() 80 | { 81 | // 0x86 REQUEST_CLEAR_ERRORS 82 | if( !ControlTransfer(0x40, 0x86, 0, 0) ) 83 | return false; 84 | 85 | return true; 86 | } 87 | 88 | 89 | 90 | // ControlTransfer 91 | /*bool ServoController::ControlTransfer( uint8_t requestType, uint8_t request, void* data, uint size ) 92 | { 93 | return ControlTransfer(requestType, request, 0, 0, data, size); 94 | } */ 95 | 96 | 97 | // ControlTransfer 98 | bool ServoController::ControlTransfer( uint8_t requestType, uint8_t request, uint16_t value, uint16_t index, void* data, uint size ) 99 | { 100 | const int res = libusb_control_transfer(mDeviceCtx, requestType, request, value, index, (uint8_t*)data, size, 5000); 101 | 102 | if( res != size ) 103 | { 104 | printf("ServoController - usb control transfer failed (size=%i result=%i)\n", size, res); 105 | return false; 106 | } 107 | 108 | return true; 109 | } 110 | 111 | 112 | // ControlTransfer 113 | bool ServoController::ControlTransfer( uint8_t requestType, uint8_t request, uint16_t value, uint16_t index ) 114 | { 115 | return ControlTransfer(requestType, request, value, index, 0, 0); 116 | } 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /ServoController.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_SERVO_CONTROLLER_H 6 | #define __ROVER_SERVO_CONTROLLER_H 7 | 8 | 9 | #include "Config.h" 10 | #include "stdio.h" 11 | 12 | #include 13 | #include 14 | 15 | 16 | /** 17 | * ServoController 18 | */ 19 | class ServoController 20 | { 21 | public: 22 | /** 23 | * constructor 24 | */ 25 | ServoController( libusb_device* device ); 26 | 27 | /** 28 | * destructor 29 | */ 30 | ~ServoController(); 31 | 32 | /** 33 | * Open 34 | */ 35 | bool Open(); 36 | 37 | /** 38 | * Close 39 | */ 40 | void Close(); 41 | 42 | /** 43 | * Return device serial string. 44 | */ 45 | inline const char* GetSerial() const { return mSerial.c_str(); } 46 | 47 | /** 48 | * Usb Vendor Id 49 | */ 50 | static const uint16_t VendorID = 0x1ffb; // Pololu Corporation 51 | 52 | /** 53 | * Usb Product Id 54 | */ 55 | static const uint16_t ProductID_6ch = 0x89; // Maestro 6-channel 56 | static const uint16_t ProductID_12ch = 0x8a; // Maestro 12-channel 57 | static const uint16_t ProductID_18ch = 0x8b; // Maestro 18-channel 58 | 59 | /** 60 | * Set servo position 61 | */ 62 | bool SetPosition( uint8_t servo, uint16_t position ); 63 | 64 | /** 65 | * Clear errors 66 | */ 67 | bool ClearErrors(); 68 | 69 | protected: 70 | 71 | //bool ControlTransfer( uint8_t requestType, uint8_t request, void* data, uint size ); 72 | bool ControlTransfer( uint8_t requestType, uint8_t request, uint16_t value, uint16_t index, void* data, uint size ); 73 | bool ControlTransfer( uint8_t requestType, uint8_t request, uint16_t value, uint16_t index ); 74 | 75 | libusb_device* mDevice; 76 | libusb_device_handle* mDeviceCtx; 77 | 78 | std::string mSerial; 79 | }; 80 | 81 | 82 | #endif 83 | 84 | -------------------------------------------------------------------------------- /UsbManager.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #include "UsbManager.h" 6 | 7 | 8 | 9 | 10 | // constructor 11 | UsbManager::UsbManager() 12 | { 13 | mUsbContext = NULL; 14 | 15 | } 16 | 17 | 18 | // destructor 19 | UsbManager::~UsbManager() 20 | { 21 | for( uint n=0; n < mControllers.size(); n++ ) 22 | { 23 | if( mControllers[n] ) 24 | delete mControllers[n]; 25 | } 26 | 27 | if( mUsbContext ) 28 | libusb_exit(mUsbContext); 29 | } 30 | 31 | 32 | MotorController* UsbManager::FindBySerial( const char* serial ) const 33 | { 34 | const uint num = mControllers.size(); 35 | 36 | for( uint n=0; n < num; ++n ) 37 | { 38 | if( strcmp(mControllers[n]->GetSerial(), serial) == 0 ) 39 | return mControllers[n]; 40 | } 41 | 42 | return NULL; 43 | } 44 | 45 | 46 | // Init 47 | bool UsbManager::Init() 48 | { 49 | // open USB context 50 | if( libusb_init(&mUsbContext) != 0 ) 51 | { 52 | printf("UsbManager - failed to create libusb context\n"); 53 | return false; 54 | } 55 | 56 | // get devices 57 | libusb_device** devList; 58 | const uint numDevices = libusb_get_device_list(mUsbContext, &devList); 59 | 60 | printf("UsbManager - %u devices\n\n", numDevices); 61 | 62 | 63 | for( uint n=0; n < numDevices; n++ ) 64 | { 65 | libusb_device_descriptor desc; 66 | 67 | if( libusb_get_device_descriptor(devList[n], &desc) != 0 ) 68 | continue; 69 | 70 | printf(" [%02u] vendor %#04x\t\tproduct %#04x\n", n, desc.idVendor, desc.idProduct); 71 | 72 | if( desc.idVendor == MotorController::VendorId && 73 | (desc.idProduct == MotorController::ProductId_24v12 || desc.idProduct == MotorController::ProductId_18v15)) 74 | { 75 | MotorController* ctrl = new MotorController(devList[n]); 76 | 77 | if( !ctrl->Open() ) 78 | { 79 | delete ctrl; 80 | continue; 81 | } 82 | 83 | mControllers.push_back(ctrl); 84 | } 85 | else if( desc.idVendor == ServoController::VendorID && 86 | (desc.idProduct == ServoController::ProductID_6ch || desc.idProduct == ServoController::ProductID_12ch || desc.idProduct == ServoController::ProductID_18ch) ) 87 | { 88 | printf("found MAESTRO USB servo controller device\n"); 89 | 90 | ServoController* ctrl = new ServoController(devList[n]); 91 | 92 | if( !ctrl->Open() ) 93 | { 94 | delete ctrl; 95 | continue; 96 | } 97 | 98 | mServos.push_back(ctrl); 99 | } 100 | } 101 | 102 | printf("\nUsbManager - opened %u motor controllers\n", GetNumMotorControllers()); 103 | printf("UsbManager - open %u servo controllers\n", GetNumServoControllers()); 104 | 105 | return true; 106 | } 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /UsbManager.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_USB_MANAGER_H 6 | #define __ROVER_USB_MANAGER_H 7 | 8 | 9 | #include "Config.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "MotorController.h" 16 | #include "ServoController.h" 17 | 18 | 19 | /** 20 | * UsbManager 21 | */ 22 | class UsbManager 23 | { 24 | public: 25 | /** 26 | * constructor 27 | */ 28 | UsbManager(); 29 | 30 | /** 31 | * destructor 32 | */ 33 | ~UsbManager(); 34 | 35 | /** 36 | * Init 37 | */ 38 | bool Init(); 39 | 40 | /** 41 | * GetNumMotorControllers 42 | */ 43 | inline uint32_t GetNumMotorControllers() const { return mControllers.size(); } 44 | 45 | /** 46 | * GetMotorController 47 | */ 48 | inline MotorController* GetMotorController( uint32_t index ) const { return mControllers[index]; } 49 | 50 | /** 51 | * GetNumServoControllers 52 | */ 53 | inline uint32_t GetNumServoControllers() const { return mServos.size(); } 54 | 55 | /** 56 | * GetServoController 57 | */ 58 | inline ServoController* GetServoController( uint32_t index ) { return mServos[index]; } 59 | 60 | /** 61 | * FindBySerial 62 | */ 63 | MotorController* FindBySerial( const char* serial ) const; 64 | 65 | protected: 66 | 67 | std::vector mControllers; 68 | std::vector mServos; 69 | libusb_context* mUsbContext; 70 | }; 71 | 72 | 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /consoleLinux.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __CONSOLE_LINUX_H__ 3 | #define __CONSOLE_LINUX_H__ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | /* 11 | * See http://ascii-table.com/ansi-escape-sequences.php for escape sequences 12 | */ 13 | 14 | #define ANSI_COLOR_NORMAL "\x1b[0m" 15 | #define ANSI_COLOR_RED "\x1b[31m" 16 | #define ANSI_COLOR_GREEN "\x1b[32m" 17 | #define ANSI_COLOR_YELLOW "\x1b[33m" 18 | #define ANSI_COLOR_BLUE "\x1b[34m" 19 | #define ANSI_COLOR_MAGENTA "\x1b[35m" 20 | #define ANSI_COLOR_CYAN "\x1b[36m" 21 | 22 | 23 | /** 24 | * ANSI/VT100 helper class 25 | */ 26 | class console 27 | { 28 | public: 29 | enum consoleColor { 30 | NORMAL=0, 31 | RED, 32 | GREEN, 33 | YELLOW, 34 | BLUE, 35 | MAGENTA, 36 | CYAN, 37 | }; 38 | 39 | static inline const char* codeToColor(consoleColor col) { 40 | switch(col) { 41 | default: 42 | case NORMAL: return ANSI_COLOR_NORMAL; 43 | case RED: return ANSI_COLOR_RED; 44 | case GREEN: return ANSI_COLOR_GREEN; 45 | case YELLOW: return ANSI_COLOR_YELLOW; 46 | case BLUE: return ANSI_COLOR_BLUE; 47 | case MAGENTA: return ANSI_COLOR_MAGENTA; 48 | case CYAN: return ANSI_COLOR_CYAN; 49 | } 50 | } 51 | 52 | static inline void setCanonical( bool enabled ) { 53 | struct termios ttystate; 54 | tcgetattr(STDIN_FILENO, &ttystate); 55 | 56 | if( !enabled ) 57 | { 58 | ttystate.c_lflag &= ~ICANON; // turn off canonical mode 59 | ttystate.c_cc[VMIN] = 1; // minimum of number input read. 60 | } 61 | else 62 | { 63 | ttystate.c_lflag |= ICANON; // turn on canonical mode 64 | } 65 | 66 | tcsetattr(STDIN_FILENO, TCSANOW, &ttystate); 67 | } 68 | 69 | static inline void setEcho( bool enabled ) { 70 | struct termios ttystate; 71 | tcgetattr(STDIN_FILENO, &ttystate); 72 | 73 | if( !enabled ) 74 | ttystate.c_lflag &= ~ECHO; 75 | else 76 | ttystate.c_lflag |= ECHO; 77 | 78 | tcsetattr(STDIN_FILENO, TCSANOW, &ttystate); 79 | } 80 | 81 | static inline void noEcho() { 82 | system("stty -echo"); 83 | } 84 | 85 | static inline void echo() { 86 | system("stty echo"); 87 | } 88 | 89 | static inline void setColor(consoleColor col) { 90 | printf("%s", codeToColor(col)); 91 | } 92 | 93 | static inline void flush() { 94 | fflush(stdout); 95 | } 96 | 97 | static inline void resetScrollingRegion() { 98 | printf("\033[r"); flush(); 99 | } 100 | 101 | static inline void setScrollingRegion(int top, int bottom) { 102 | printf("\033[%i;%ir", top, bottom); flush(); 103 | } 104 | 105 | static inline void setCursorPosition(int col, int row) { 106 | printf("\033[%d;%dH", row, col); 107 | } 108 | 109 | static inline void clearScreen() { 110 | printf("\033[2J"); 111 | fflush(stdout); 112 | } 113 | 114 | static inline void clearLine() { 115 | printf("\033[K"); 116 | fflush(stdout); 117 | } 118 | 119 | // move cursor to uppper left corner 120 | static inline void updateScreen(void) { 121 | printf("\033[H"); 122 | fflush(stdout); 123 | } 124 | }; 125 | 126 | #endif -------------------------------------------------------------------------------- /docs/images/turbo1-tk1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/docs/images/turbo1-tk1.jpg -------------------------------------------------------------------------------- /docs/images/turbo2+.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/docs/images/turbo2+.jpg -------------------------------------------------------------------------------- /docs/images/turbo2_front.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/docs/images/turbo2_front.jpg -------------------------------------------------------------------------------- /docs/images/turbo2_internals.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/docs/images/turbo2_internals.jpg -------------------------------------------------------------------------------- /docs/images/turbo2_rear.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/docs/images/turbo2_rear.jpg -------------------------------------------------------------------------------- /evdevController.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #include "evdevController.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | 20 | // constructor 21 | evdevController::evdevController() 22 | { 23 | mFD = -1; 24 | //mRingbufferCurrent = 0; 25 | 26 | mMaxAxisCount = ABS_CNT; 27 | mAxisState = (int*)malloc(sizeof(int) * mMaxAxisCount); 28 | memset(mAxisState, 0, sizeof(int) * mMaxAxisCount); 29 | 30 | Axis[0] = 0.0f; 31 | Axis[1] = 0.0f; 32 | } 33 | 34 | 35 | // destructor 36 | evdevController::~evdevController() 37 | { 38 | Close(); 39 | } 40 | 41 | 42 | // printAxis 43 | void evdevController::printAxis() 44 | { 45 | const int numAxis = 16; 46 | 47 | for( int i=0; i < numAxis; i++ ) 48 | printf(" %02i ", i); 49 | 50 | printf("\n"); 51 | 52 | for( int i=0; i < numAxis; i++ ) 53 | printf(" %+04i", mAxisState[i]); 54 | 55 | printf("\n"); 56 | } 57 | 58 | 59 | // ProcessEmit 60 | bool evdevController::Poll( uint32_t timeout ) 61 | { 62 | const uint32_t max_ev = 64; 63 | struct input_event ev[max_ev]; 64 | 65 | //printf("evDevcontroller::read()\n"); 66 | 67 | fd_set fds; 68 | FD_ZERO(&fds); 69 | FD_SET(mFD, &fds); 70 | 71 | struct timeval tv; 72 | 73 | tv.tv_sec = 0; 74 | tv.tv_usec = timeout*1000; 75 | 76 | const int result = select(mFD + 1, &fds, NULL, NULL, &tv); 77 | 78 | if( result == -1 ) 79 | { 80 | //if (EINTR == errno) 81 | printf("evdevController -- select() failed (errno=%i) (%s)\n", errno, strerror(errno)); 82 | return false; 83 | } 84 | else if( result == 0 ) 85 | { 86 | //printf("evdevController -- select() timed out...\n"); 87 | return false; // timeout, not necessarily an error (TRY_AGAIN) 88 | } 89 | 90 | const int bytesRead = read(mFD, ev, sizeof(struct input_event) * max_ev); 91 | 92 | //printf("evDevcontroller::read() ==> %i\n", bytesRead); 93 | 94 | if( bytesRead < (int)sizeof(struct input_event) ) 95 | { 96 | printf("evdev read() expected %d bytes, got %d\n", (int)sizeof(struct input_event), bytesRead); 97 | return false; 98 | } 99 | 100 | 101 | /*struct input_event { 102 | struct timeval time; 103 | __u16 type; 104 | __u16 code; 105 | __s32 value; 106 | };*/ 107 | 108 | #define DUAL_AXIS 109 | 110 | #ifdef DUAL_AXIS 111 | const int axisA = 1; 112 | const int axisB = 3; 113 | #else 114 | const int axisA = 0; 115 | const int axisB = 1; 116 | #endif 117 | 118 | const int num_ev = bytesRead / sizeof(struct input_event); 119 | bool triggerMsg = false; 120 | 121 | for( int i = 0; i < num_ev; i++ ) 122 | { 123 | /*if( ev[i].type == EV_REL || ev[i].type == EV_ABS ) 124 | { 125 | printf("evt type %hu code %hu value %+04i\n", 126 | ev[i].type, ev[i].code, ev[i].value); 127 | }*/ 128 | 129 | if( ev[i].type == EV_ABS ) 130 | { 131 | if( ev[i].code >= mMaxAxisCount ) 132 | continue; 133 | 134 | const int axisCurr = ev[i].value; 135 | //const int axisPrev = mAxisState[ev[i].code]; 136 | //const int axisDiff = axisCurr - axisPrev; 137 | 138 | /*if( ev[i].code > 4 ) 139 | { 140 | const int sw = 50; 141 | 142 | if( axisCurr >= sw && axisPrev < sw ) 143 | execBind(ev[i].code, 1); 144 | else if( axisCurr < sw && axisPrev >= sw ) 145 | execBind(ev[i].code, 0); 146 | }*/ 147 | 148 | /*if( axisDiff != 0 ) 149 | if( ev[i].code == axisA || ev[i].code == axisB ) // TEMP only use axis 1 and 3 (Y-axis of left & right joysticks) 150 | triggerMsg = true;*/ 151 | 152 | mAxisState[ev[i].code] = axisCurr; 153 | } 154 | } 155 | 156 | 157 | //if( GetFlag(NODE_VERBOSE) ) 158 | // printAxis(); 159 | 160 | 161 | Axis[0] = float(mAxisState[axisA]) / 128.0f; 162 | Axis[1] = float(mAxisState[axisB]) / 128.0f; 163 | 164 | /* 165 | // get the next slot from the ringbuffer 166 | Buffer* buffer = mRingbuffer[mRingbufferCurrent]; 167 | mRingbufferCurrent = (mRingbufferCurrent + 1) % mRingbuffer.size(); 168 | 169 | float* vec = (float*)buffer->GetCPU(); 170 | const float axisMax = 128.0f; 171 | 172 | vec[0] = float(mAxisState[axisA]) / axisMax; 173 | vec[1] = float(mAxisState[axisB]) / axisMax * -1.0f; 174 | 175 | #ifdef DUAL_AXIS 176 | vec[0] *= -1.0f; 177 | #endif 178 | 179 | //printf("%f %f\n", vec[0], vec[1]); 180 | Emit(buffer); */ 181 | return true; 182 | } 183 | 184 | 185 | /** 186 | * Filter for the AutoDevProbe scandir on /dev/input. 187 | * @param dir The current directory entry provided by scandir. 188 | * @return Non-zero if the given directory entry starts with "event", or zero 189 | * otherwise. 190 | */ 191 | static int is_event_device(const struct dirent *dir) 192 | { 193 | return strncmp("event", dir->d_name, 5) == 0; 194 | } 195 | 196 | 197 | static char* scan_devices( const char* searchName ) 198 | { 199 | if( !searchName ) 200 | return NULL; 201 | 202 | struct dirent **namelist; 203 | int ndev = scandir("/dev/event", &namelist, is_event_device, versionsort); 204 | 205 | if (ndev <= 0) 206 | { 207 | ndev = scandir("/dev/input", &namelist, is_event_device, versionsort); 208 | 209 | if (ndev <= 0) 210 | return NULL; 211 | } 212 | 213 | printf("Available devices:\n"); 214 | 215 | for(int i = 0; i < ndev; i++) 216 | { 217 | char fname[64]; 218 | char name[256] = "???"; 219 | 220 | snprintf(fname, sizeof(fname), "%s/%s", "/dev/input", namelist[i]->d_name); 221 | int fd = open(fname, O_RDONLY); 222 | if (fd < 0) 223 | continue; 224 | ioctl(fd, EVIOCGNAME(sizeof(name)), name); 225 | 226 | printf("%s: %s\n", fname, name); 227 | 228 | bool matched = false; 229 | 230 | if( strcmp(name, searchName) == 0 ) 231 | matched = true; 232 | 233 | close(fd); 234 | free(namelist[i]); 235 | 236 | if( matched ) 237 | { 238 | char *filename; 239 | asprintf(&filename, "/dev/input/event%d",i); 240 | return filename; 241 | } 242 | } 243 | 244 | printf("failed to find /dev/input/event* node matching evdev %s\n", searchName); 245 | return NULL; 246 | } 247 | 248 | 249 | // Open 250 | bool evdevController::Open() 251 | { 252 | printf("evdevController::Open()\n"); 253 | 254 | // locate the /dev/event* path for this device 255 | char* filename = scan_devices(BLUETOOTH_DEVICE); 256 | 257 | if( !filename ) 258 | filename = scan_devices(BLUETOOTH_DEVICE2); 259 | 260 | if( !filename ) 261 | { 262 | printf("evdevController - failed to find Bluetooth device\n"); 263 | return false; //filename = "/dev/input/event1"; 264 | } 265 | 266 | // open file 267 | mFD = open(filename, O_RDONLY); 268 | 269 | if( mFD < 0) 270 | { 271 | const int error = errno; 272 | 273 | if( error == EACCES && getuid() != 0 ) 274 | printf("You do not have access to %s. Try " 275 | "running as root instead.\n", filename); 276 | else 277 | printf("errno %i opening %s\n", error, filename); 278 | 279 | return false; 280 | } 281 | 282 | // 283 | /*if( !allocRingbuffer(8) ) 284 | { 285 | printf("evdevController - failed to allocate ringbuffer\n"); 286 | return false; 287 | }*/ 288 | 289 | return true; //Node::Open(); 290 | } 291 | 292 | 293 | // allocRingbuffer 294 | /*bool evdevController::allocRingbuffer( uint32_t count ) 295 | { 296 | if( mRingbuffer.size() == count ) 297 | return true; 298 | 299 | for( uint32_t n=0; n < count; n++ ) 300 | { 301 | Buffer* buf = NULL; 302 | 303 | buf = Buffer::Alloc(8, typeOf(2)); 304 | 305 | if( !buf ) 306 | continue; 307 | 308 | mRingbuffer.push_back(buf); 309 | } 310 | 311 | if( mRingbuffer.size() > 1 ) 312 | return true; 313 | 314 | return false; 315 | }*/ 316 | 317 | 318 | 319 | // Close 320 | bool evdevController::Close() 321 | { 322 | //if( !Node::Close() ) 323 | //d return false; 324 | 325 | if( mFD >= 0 ) 326 | { 327 | close(mFD); 328 | mFD = -1; 329 | } 330 | 331 | return true; 332 | } 333 | 334 | 335 | 336 | 337 | -------------------------------------------------------------------------------- /evdevController.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #ifndef __GASKET_EVDEV_CONTROLLER_H 6 | #define __GASKET_EVDEV_CONTROLLER_H 7 | 8 | 9 | #include "Config.h" 10 | #include 11 | 12 | 13 | #define BLUETOOTH_DEVICE "PLAYSTATION(R)3 Controller (64:D4:BD:0C:6D:0D)" 14 | #define BLUETOOTH_DEVICE2 "PLAYSTATION(R)3 Controller (AC:7A:4D:23:A3:6D)" 15 | 16 | 17 | /** 18 | * Event Human-Interface Device Controller (/dev/event) 19 | */ 20 | class evdevController 21 | { 22 | public: 23 | evdevController(); 24 | ~evdevController(); 25 | 26 | bool Poll( uint32_t timeout=0 ); 27 | 28 | bool Open(); 29 | bool Close(); 30 | 31 | float Axis[2]; 32 | 33 | static const int TRIGGER_LEVEL_ACTIVATE = 50; 34 | 35 | enum 36 | { 37 | AXIS_LX = 0, 38 | AXIS_LY = 1, 39 | AXIS_RX = 2, 40 | AXIS_RY = 3, 41 | AXIS_L_TRIGGER = 12, 42 | AXIS_L_BUMPER = 14, 43 | AXIS_R_TRIGGER = 13, 44 | AXIS_R_BUMPER = 15 45 | }; 46 | 47 | inline int GetState( uint32_t index ) const { return mAxisState[index]; } 48 | 49 | private: 50 | 51 | void printAxis(); 52 | 53 | uint32_t mMaxAxisCount; // normally equals 64 on linux 54 | int* mAxisState; 55 | 56 | /*bool allocRingbuffer( uint32_t buffers ); 57 | std::vector mRingbuffer; 58 | size_t mRingbufferCurrent;*/ 59 | 60 | int mFD; 61 | 62 | /*struct Bind 63 | { 64 | uint8_t button; 65 | uint8_t state; 66 | XMLElement* xml; 67 | }; 68 | 69 | std::vector mBinds; 70 | 71 | void execBind( uint8_t button, uint8_t state );*/ 72 | }; 73 | 74 | 75 | 76 | #endif 77 | 78 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #include "Rover.h" 6 | #include "consoleLinux.h" 7 | 8 | #include 9 | 10 | 11 | bool quit_flag = false; 12 | 13 | void sig_handler(int s) 14 | { 15 | printf("caught signal %i\n", s); 16 | quit_flag = true; 17 | printf("signalling quit flag...\n"); 18 | //exit(0); 19 | } 20 | 21 | 22 | int main( int argc, char *argv[] ) 23 | { 24 | signal(SIGINT, sig_handler); 25 | 26 | // print the CLI 27 | for( int n=0; n < argc; n++ ) 28 | printf("%s ", argv[n]); 29 | 30 | printf("\n"); 31 | 32 | 33 | // get the terminal size, if provided 34 | int termCols = 80; 35 | int termRows = 24; 36 | 37 | if( argc == 3 ) 38 | { 39 | termCols = atoi(argv[1]); 40 | termRows = atoi(argv[2]); 41 | 42 | printf("[rover] terminal size: %i x %i\n", termCols, termRows); 43 | } 44 | 45 | //const int consoleBottom = termRows/2; 46 | //console::setScrollingRegion(1, termRows/2); 47 | //console::clearScreen(); 48 | //console::setEcho(false); 49 | 50 | 51 | // create the rover object 52 | Rover* rover = Rover::Create(); 53 | 54 | if( !rover ) 55 | return 0; 56 | 57 | printf("[rover] starting rover main loop...\n"); 58 | 59 | 60 | // main event loop 61 | while(!quit_flag) 62 | { 63 | //printf("quit flag: %i\n", (int)quit_flag); 64 | /*if( kbhit() ) 65 | { 66 | printf("keyboard hit\n"); 67 | }*/ 68 | 69 | rover->NextEpoch(); 70 | } 71 | 72 | printf("closing Rover\n"); 73 | delete rover; 74 | printf("exiting %s process.\n", argv[0]); 75 | //console::setEcho(true); 76 | //console::resetScrollingRegion(); 77 | return 0; 78 | } 79 | 80 | 81 | /* 82 | * else 83 | { 84 | printf("failed to read variables\n"); 85 | } 86 | 87 | printf("\n\nenter desired speed (-3200, 3200)\n"); 88 | int speed = 0; 89 | scanf("%i", &speed); 90 | 91 | const bool res = mc->SetSpeed(speed); 92 | 93 | printf("%s\n\n", res ? "success" : "failed"); 94 | 95 | } while(true); 96 | */ 97 | 98 | /* 99 | float fwd = (evt.motion.rx / 350.0f) * -1.0f; 100 | float turn = evt.motion.ry / 350.0f; 101 | 102 | 103 | 104 | if( absf(turn) > 0.1f ) 105 | { 106 | float aturn = absf(turn); 107 | 108 | if( turn > 0.0f ) 109 | { 110 | speed[0] = -aturn; 111 | speed[1] = aturn; 112 | } 113 | else if( turn < 0.0f ) 114 | { 115 | speed[0] = aturn; 116 | speed[1] = -aturn; 117 | } 118 | 119 | speedChanged = true; 120 | } 121 | else 122 | { 123 | if( absf(fwd) < 0.1f ) 124 | fwd = 0.0f; 125 | 126 | speed[0] = fwd; 127 | speed[1] = fwd; 128 | 129 | speedChanged = true; 130 | } 131 | } 132 | */ 133 | 134 | -------------------------------------------------------------------------------- /panTilt.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #include "panTilt.h" 6 | 7 | 8 | // constructor 9 | panTilt::panTilt( ServoController* servoController ) : mServo(servoController) 10 | { 11 | mServo->SetPosition(PAN_CHANNEL, PAN_CENTER); 12 | mServo->SetPosition(TILT_CHANNEL, TILT_CENTER); 13 | } 14 | 15 | 16 | panTilt::~panTilt() 17 | { 18 | // ServoController is deleted by UsbManager 19 | } 20 | 21 | 22 | void panTilt::Update( evdevController* user ) 23 | { 24 | if( !user ) 25 | return; 26 | 27 | /** 28 | * pan 29 | */ 30 | const float x = float(user->GetState(evdevController::AXIS_RX)) / 128.0f; 31 | 32 | float pan = PAN_CENTER; 33 | 34 | if( x < 0.0f ) 35 | pan = ((PAN_CENTER - PAN_LEFT) * x) + PAN_CENTER; 36 | if( x > 0.0f ) 37 | pan = PAN_CENTER - ((PAN_CENTER - PAN_RIGHT) * x); 38 | 39 | mServo->SetPosition(PAN_CHANNEL, pan); 40 | 41 | /** 42 | * tilt 43 | */ 44 | const float y = float(user->GetState(evdevController::AXIS_RY)) / 128.0f; 45 | 46 | float tilt = TILT_CENTER; 47 | 48 | if( y < 0.0f ) 49 | tilt = ((TILT_CENTER - TILT_DOWN) * y) + TILT_CENTER; 50 | if( y > 0.0f ) 51 | tilt = TILT_CENTER - ((TILT_CENTER - TILT_UP) * y); 52 | 53 | mServo->SetPosition(TILT_CHANNEL, tilt); 54 | } 55 | 56 | -------------------------------------------------------------------------------- /panTilt.h: -------------------------------------------------------------------------------- 1 | /** 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_PAN_TILT_H 6 | #define __ROVER_PAN_TILT_H 7 | 8 | 9 | #include "ServoController.h" 10 | #include "evdevController.h" 11 | 12 | 13 | /** 14 | * panTilt 15 | */ 16 | class panTilt 17 | { 18 | public: 19 | /** 20 | * constructor 21 | */ 22 | panTilt( ServoController* servoController ); 23 | 24 | /** 25 | * destructor 26 | */ 27 | ~panTilt(); 28 | 29 | /** 30 | * Update 31 | */ 32 | void Update( evdevController* userController ); 33 | 34 | static const uint8_t PAN_CHANNEL = 0; 35 | static const uint8_t TILT_CHANNEL = 1; 36 | 37 | static const uint16_t PAN_CENTER = 1590; //1650; 38 | static const uint16_t PAN_LEFT = 2000; 39 | static const uint16_t PAN_RIGHT = 1280; 40 | 41 | static const uint16_t TILT_CENTER = 1200; //1440; 42 | static const uint16_t TILT_DOWN = 1950; 43 | static const uint16_t TILT_UP = 1000; 44 | 45 | protected: 46 | 47 | ServoController* mServo; 48 | }; 49 | 50 | 51 | #endif 52 | 53 | -------------------------------------------------------------------------------- /phidgetIMU.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_PHIDGET_IMU_H 6 | #define __ROVER_PHIDGET_IMU_H 7 | 8 | 9 | #include "Config.h" 10 | 11 | //typedef struct _CPhidgetSpatial *CPhidgetSpatialHandle; 12 | 13 | #ifndef PI 14 | #define PI 3.14159265358979323846264338327950288 15 | #endif 16 | 17 | #ifndef RAD_TO_DEG 18 | #define RAD_TO_DEG 57.295779513082320876798154814105f 19 | #endif 20 | 21 | 22 | /** 23 | * phidgetIMU 24 | */ 25 | class phidgetIMU 26 | { 27 | public: 28 | static phidgetIMU* Create(); 29 | ~phidgetIMU(); 30 | 31 | inline float GetBearing() const { return mBearing; } 32 | inline void SetBearing( float value ) { mBearing = value; mNewBearing = true; } 33 | 34 | 35 | inline bool GetNewBearing( float* output ) 36 | { 37 | if( mNewBearing ) 38 | { 39 | *output = mBearing; 40 | mNewBearing = false; 41 | return true; 42 | } 43 | 44 | return false; 45 | } 46 | 47 | 48 | private: 49 | phidgetIMU(); 50 | bool Init(); 51 | 52 | void* mHandle; 53 | float mBearing; 54 | bool mNewBearing; 55 | }; 56 | 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /rpLIDAR.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #include "rpLIDAR.h" 6 | #include "rplidar.h" 7 | 8 | 9 | 10 | using namespace rp::standalone::rplidar; 11 | 12 | 13 | // constructor 14 | rpLIDAR::rpLIDAR() 15 | { 16 | mDriver = NULL; 17 | mMinQuality = 0; 18 | 19 | /* 20 | 21 | 22 | */ 23 | 24 | AddZone(0.0f, 40.0f, 1.0f, 2000.0f, 3); 25 | AddZone(325.0f, 361.0f, 1.0f, 2000.0f, 3); 26 | //AddZone(150.0f, 210.0f, 1000.0f, 2000.0f, 6); 27 | } 28 | 29 | 30 | // destructor 31 | rpLIDAR::~rpLIDAR() 32 | { 33 | if( mDriver != NULL ) 34 | { 35 | mDriver->disconnect(); 36 | RPlidarDriver::DisposeDriver(mDriver); 37 | mDriver = NULL; 38 | } 39 | } 40 | 41 | 42 | // Create 43 | rpLIDAR* rpLIDAR::Create( const char* path ) 44 | { 45 | if( !path ) 46 | return NULL; 47 | 48 | rpLIDAR* l = new rpLIDAR(); 49 | 50 | if( !l->init(path) ) 51 | { 52 | delete l; 53 | return NULL; 54 | } 55 | 56 | return l; 57 | } 58 | 59 | 60 | // init 61 | bool rpLIDAR::init( const char* path ) 62 | { 63 | mDriver = RPlidarDriver::CreateDriver(); 64 | 65 | if( !mDriver ) 66 | { 67 | printf("rpLidar -- failed to create RPlidarDriver instance.\n"); 68 | return false; 69 | } 70 | 71 | // enable serial connection 72 | const u_result result = mDriver->connect(path, 115200); 73 | 74 | printf("rpLIDAR::connect(%s) %s %u %x\n", path, IS_FAIL(result) ? "FAIL" : "OK", result, result); 75 | 76 | if( IS_FAIL(result) ) 77 | return false; 78 | 79 | mPath = path; 80 | 81 | // check device stats 82 | rplidar_response_device_info_t info; 83 | memset(&info, 0, sizeof(rplidar_response_device_info_t)); 84 | 85 | if( IS_OK(mDriver->getDeviceInfo(info)) ) 86 | { 87 | printf("rpLIDAR device %s\n", path); 88 | printf(" + model %hhu\n", info.model); 89 | printf(" + firmware v%hu\n", info.firmware_version); 90 | printf(" + hardware v%hu\n", info.hardware_version); 91 | } 92 | else 93 | printf("failed to get rpLIDAR device data...\n"); 94 | 95 | 96 | rplidar_response_device_health_t health; 97 | memset(&health, 0, sizeof(health)); 98 | 99 | if( IS_OK(mDriver->getHealth(health)) ) 100 | printf(" + health %u %u\n", (uint32_t)health.status, (uint32_t)health.error_code); 101 | 102 | return true; 103 | } 104 | 105 | 106 | /*typedef struct _rplidar_response_measurement_node_t { 107 | _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; 108 | _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; 109 | _u16 distance_q2; 110 | } __attribute__((packed)) rplidar_response_measurement_node_t;*/ 111 | 112 | 113 | // Poll 114 | bool rpLIDAR::Poll( float* samples_out, uint32_t timeout ) 115 | { 116 | if( !rpConnected() ) 117 | return false; 118 | 119 | // grab scan 120 | const uint32_t scanEntriesMax = 360; 121 | rplidar_response_measurement_node_t scan[scanEntriesMax]; 122 | 123 | size_t scanEntries = scanEntriesMax; 124 | const u_result result = mDriver->grabScanData(scan, scanEntries, timeout); 125 | 126 | if( result == RESULT_OPERATION_TIMEOUT ) 127 | { 128 | //printf("rpLIDAR::grabScan(%s) %s %u 0x%x\n", mPath.c_str(), "TIMEOUT", result, result); 129 | return false; 130 | } 131 | else if( IS_FAIL(result) ) 132 | { 133 | //printf("rpLIDAR::grabScan(%s) %s %u 0x%x\n", mPath.c_str(), IS_FAIL(result) ? "FAIL" : "OK", result, result); 134 | return false; 135 | } 136 | 137 | printf("rpLIDAR -- recieved scan with %zu entries\n", scanEntries); 138 | 139 | // reset zones 140 | const uint32_t numZones = mZones.size(); 141 | 142 | for( size_t z=0; z < numZones; z++ ) 143 | mZones[z]->detections = 0; 144 | 145 | // reset sample histogram 146 | memset(samples_out, 0, sizeof(float) * 360); 147 | 148 | // mDriver->ascendScanData(scan, scanEntries); 149 | for( size_t n=0; n < scanEntries; n++ ) 150 | { 151 | const float angle = (scan[n].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f; 152 | const uint16_t dist = scan[n].distance_q2; 153 | const uint8_t quality = (scan[n].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); 154 | 155 | if( quality >= mMinQuality && dist > 0 && angle >= 0.0f && angle <= 360.0f ) 156 | { 157 | samples_out[(int)angle] = dist; 158 | 159 | for( size_t z=0; z < numZones; z++ ) 160 | { 161 | if( angle >= mZones[z]->angleMin && angle <= mZones[z]->angleMax && 162 | dist >= mZones[z]->distMin && dist <= mZones[z]->distMax ) 163 | mZones[z]->detections++; 164 | } 165 | } 166 | //printf(" [%03zu] angle %07.3f dist %05hu quality %hhu\n", n, angle, dist, quality); 167 | } 168 | 169 | return true; 170 | } 171 | 172 | 173 | // AddZone 174 | void rpLIDAR::AddZone( float angleMin, float angleMax, float distMin, float distMax, uint32_t detectionLimit ) 175 | { 176 | Zone* z = new Zone(); 177 | 178 | z->angleMin = angleMin; 179 | z->angleMax = angleMax; 180 | z->distMin = distMin; 181 | z->distMax = distMax; 182 | z->detections = 0; 183 | z->detectionLimit = detectionLimit; 184 | 185 | mZones.push_back(z); 186 | } 187 | 188 | 189 | // CheckZones 190 | bool rpLIDAR::CheckZones() 191 | { 192 | const uint32_t numZones = mZones.size(); 193 | bool detected = false; 194 | 195 | for( uint32_t z=0; z < numZones; z++ ) 196 | { 197 | if( ZoneActive(z) ) 198 | { 199 | detected = true; 200 | printf("rpLIDAR -- zone %u -- %u / %u detections\n", z, mZones[z]->detections, mZones[z]->detectionLimit); 201 | } 202 | } 203 | 204 | return detected; 205 | } 206 | 207 | 208 | // ZoneActive 209 | bool rpLIDAR::ZoneActive( uint32_t z ) 210 | { 211 | //if( z >= mZones.size() ) 212 | // return false; 213 | 214 | if( mZones[z]->detections >= mZones[z]->detectionLimit ) 215 | return true; 216 | 217 | return false; 218 | } 219 | 220 | 221 | 222 | // AvoidZones 223 | bool rpLIDAR::AvoidZones( float* controls ) 224 | { 225 | const uint32_t numZones = mZones.size(); 226 | bool detected = false; 227 | 228 | if( numZones < 2 ) 229 | { 230 | printf("rpLIDAR::AvoidZones() -- not enough LIDAR zones\n"); 231 | return false; 232 | } 233 | 234 | if( !controls ) 235 | return false; 236 | 237 | if( ZoneActive(0) || ZoneActive(1) ) 238 | { 239 | if( controls[0] > 0.0f || controls[1] > 0.0f ) 240 | { 241 | controls[0] = 0.0f; 242 | controls[1] = 0.0f; 243 | 244 | detected = true; 245 | } 246 | } 247 | 248 | if( numZones > 2 && ZoneActive(2) ) 249 | { 250 | if( controls[0] < 0.0f || controls[1] < 0.0f ) 251 | { 252 | controls[0] = 0.0f; 253 | controls[1] = 0.0f; 254 | 255 | detected = true; 256 | } 257 | } 258 | 259 | return detected; 260 | } 261 | 262 | 263 | // Open 264 | bool rpLIDAR::Open() 265 | { 266 | printf("rpLIDAR::Open()\n"); 267 | 268 | // make sure serial connection to lidar is functioning 269 | if( !rpConnected() ) 270 | return false; 271 | 272 | // start lidar scanning 273 | const u_result result = mDriver->startScan(); 274 | 275 | printf("rpLIDAR::startScan(%s) %s %u %x\n", mPath.c_str(), IS_FAIL(result) ? "FAIL" : "OK", result, result); 276 | 277 | if( IS_FAIL(result) ) 278 | return false; 279 | 280 | return true; 281 | } 282 | 283 | 284 | 285 | // rpConnected 286 | bool rpLIDAR::rpConnected() 287 | { 288 | if( !mDriver ) 289 | return false; 290 | 291 | if( !mDriver->isConnected() ) 292 | { 293 | printf("rpLIDAR is not connected...\n"); 294 | return false; 295 | } 296 | 297 | return true; 298 | } 299 | 300 | 301 | // Close 302 | bool rpLIDAR::Close() 303 | { 304 | if( mDriver != NULL ) 305 | { 306 | const u_result result = mDriver->stop(); 307 | printf("rpLIDAR::stopScan(%s) %s %u %x\n", mPath.c_str(), IS_FAIL(result) ? "FAIL" : "OK", result, result); 308 | } 309 | 310 | return true; 311 | } 312 | 313 | 314 | 315 | 316 | -------------------------------------------------------------------------------- /rpLIDAR.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #ifndef __ROVER_RPLIDAR_H 6 | #define __ROVER_RPLIDAR_H 7 | 8 | 9 | #include "Config.h" 10 | 11 | #include 12 | #include 13 | 14 | 15 | namespace rp { namespace standalone{ namespace rplidar { class RPlidarDriver; } } } 16 | 17 | 18 | 19 | /** 20 | * rpLIDAR 21 | */ 22 | class rpLIDAR 23 | { 24 | public: 25 | /** 26 | * Create 27 | */ 28 | static rpLIDAR* Create( const char* path ); 29 | 30 | /** 31 | * Destructor 32 | */ 33 | ~rpLIDAR(); 34 | 35 | /** 36 | * Attempt to retrieve data from the rpLIDAR 37 | */ 38 | bool Poll( float* samples_out, uint32_t timeout=0 ); 39 | 40 | /** 41 | * Start the rpLIDAR scanning. 42 | */ 43 | bool Open(); 44 | 45 | /** 46 | * Stop the rpLIDAR scanning. 47 | */ 48 | bool Close(); 49 | 50 | /** 51 | * Add a collision detection zone. 52 | */ 53 | void AddZone( float angleMin, float angleMax, float distMin, float distMax, uint32_t detectionLimit=5 ); 54 | 55 | /** 56 | * Report if any zones have detected a collision. 57 | */ 58 | bool CheckZones(); 59 | 60 | /** 61 | * Null out motor controls if they are headed for a collision. 62 | * Returns true if the collision avoidance had to kick in and cancel the controls. 63 | * Otherwise, returns false if the controls were passed through untouched. 64 | */ 65 | bool AvoidZones( float* controls ); 66 | 67 | /** 68 | * Check if a zone is actively detecting. 69 | */ 70 | bool ZoneActive( uint32_t index ); 71 | 72 | 73 | protected: 74 | 75 | rpLIDAR(); 76 | bool init( const char* path ); 77 | 78 | std::string mPath; 79 | uint32_t mMinQuality; // minimum quality level of LIDAR scan point (default=0) 80 | 81 | bool rpConnected(); 82 | rp::standalone::rplidar::RPlidarDriver* mDriver; 83 | 84 | struct Zone 85 | { 86 | float angleMin; 87 | float angleMax; 88 | float distMin; 89 | float distMax; 90 | uint32_t detections; 91 | uint32_t detectionLimit; 92 | }; 93 | 94 | std::vector mZones; 95 | }; 96 | 97 | 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /rplidar-sdk/include/rplidar.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * SDK All-in-one Header 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | #include "rplidar_protocol.h" 41 | #include "rplidar_cmd.h" 42 | 43 | #include "rplidar_driver.h" 44 | -------------------------------------------------------------------------------- /rplidar-sdk/include/rplidar_cmd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Data Packet IO packet definition for RP-LIDAR 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | 38 | #pragma once 39 | 40 | #include "rplidar_protocol.h" 41 | 42 | // Commands 43 | //----------------------------------------- 44 | 45 | // Commands without payload and response 46 | #define RPLIDAR_CMD_STOP 0x25 47 | #define RPLIDAR_CMD_SCAN 0x20 48 | #define RPLIDAR_CMD_FORCE_SCAN 0x21 49 | #define RPLIDAR_CMD_RESET 0x40 50 | 51 | 52 | // Commands without payload but have response 53 | #define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 54 | #define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 55 | 56 | #if defined(_WIN32) 57 | #pragma pack(1) 58 | #endif 59 | 60 | 61 | // Response 62 | // ------------------------------------------ 63 | #define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 64 | 65 | #define RPLIDAR_ANS_TYPE_DEVINFO 0x4 66 | #define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 67 | 68 | 69 | #define RPLIDAR_STATUS_OK 0x0 70 | #define RPLIDAR_STATUS_WARNING 0x1 71 | #define RPLIDAR_STATUS_ERROR 0x2 72 | 73 | #define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) 74 | #define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 75 | #define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) 76 | #define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 77 | 78 | typedef struct _rplidar_response_measurement_node_t { 79 | _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; 80 | _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; 81 | _u16 distance_q2; 82 | } __attribute__((packed)) rplidar_response_measurement_node_t; 83 | 84 | typedef struct _rplidar_response_device_info_t { 85 | _u8 model; 86 | _u16 firmware_version; 87 | _u8 hardware_version; 88 | _u8 serialnum[16]; 89 | } __attribute__((packed)) rplidar_response_device_info_t; 90 | 91 | typedef struct _rplidar_response_device_health_t { 92 | _u8 status; 93 | _u16 error_code; 94 | } __attribute__((packed)) rplidar_response_device_health_t; 95 | 96 | #if defined(_WIN32) 97 | #pragma pack() 98 | #endif 99 | -------------------------------------------------------------------------------- /rplidar-sdk/include/rplidar_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Driver Interface 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | 40 | #ifndef __cplusplus 41 | #error "The RPlidar SDK requires a C++ compiler to be built" 42 | #endif 43 | 44 | namespace rp { namespace standalone{ namespace rplidar { 45 | 46 | class RPlidarDriver { 47 | public: 48 | enum { 49 | DEFAULT_TIMEOUT = 2000, //2000 ms 50 | }; 51 | 52 | enum { 53 | DRIVER_TYPE_SERIALPORT = 0x0, 54 | }; 55 | public: 56 | /// Create an RPLIDAR Driver Instance 57 | /// This interface should be invoked first before any other operations 58 | /// 59 | /// \param drivertype the connection type used by the driver. 60 | static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT); 61 | 62 | /// Dispose the RPLIDAR Driver Instance specified by the drv parameter 63 | /// Applications should invoke this interface when the driver instance is no longer used in order to free memory 64 | static void DisposeDriver(RPlidarDriver * drv); 65 | 66 | 67 | public: 68 | /// Open the specified serial port and connect to a target RPLIDAR device 69 | /// 70 | /// \param port_path the device path of the serial port 71 | /// e.g. on Windows, it may be com3 or \\.\com10 72 | /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc 73 | /// 74 | /// \param baudrate the baudrate used 75 | /// For most RPLIDAR models, the baudrate should be set to 115200 76 | /// 77 | /// \param flag other flags 78 | /// Reserved for future use, always set to Zero 79 | virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0) = 0; 80 | 81 | 82 | /// Disconnect with the RPLIDAR and close the serial port 83 | virtual void disconnect() = 0; 84 | 85 | /// Returns TRUE when the connection has been established 86 | virtual bool isConnected() = 0; 87 | 88 | /// Ask the RPLIDAR core system to reset it self 89 | /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. 90 | /// 91 | // \param timeout The operation timeout value (in millisecond) for the serial port communication 92 | virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0; 93 | 94 | /// Retrieve the health status of the RPLIDAR 95 | /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. 96 | /// 97 | /// \param health The health status info returned from the RPLIDAR 98 | /// 99 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 100 | virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0; 101 | 102 | /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. 103 | /// 104 | /// \param info The device information returned from the RPLIDAR 105 | /// 106 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 107 | virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0; 108 | 109 | 110 | /// Calcuate RPLIDAR's current scanning frequency from the given scan data 111 | /// Please refer to the application note doc for details 112 | /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data 113 | /// 114 | /// \param nodebuffer The buffer belongs to a 360degress scan data 115 | /// 116 | /// \param count The number of sample nodes inside the given buffer 117 | /// 118 | // \param frequency The scanning frequency (in HZ) calcuated by the interface. 119 | virtual u_result getFrequency(rplidar_response_measurement_node_t * nodebuffer, size_t count, float & frequency) = 0; 120 | 121 | /// Ask the RPLIDAR core system to enter the scan mode 122 | /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously. 123 | /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread. 124 | /// 125 | /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. 126 | /// 127 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 128 | virtual u_result startScan(bool force = false, _u32 timeout = DEFAULT_TIMEOUT) = 0; 129 | 130 | 131 | /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated 132 | /// 133 | /// \param timeout The operation timeout value (in millisecond) for the serial port communication 134 | virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0; 135 | 136 | 137 | /// Wait and grab a complete 0-360 degree scan data previously received. 138 | /// The grabbed scan data returned by this interface always has the following charactistics: 139 | /// 140 | /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 141 | /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan 142 | /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. 143 | /// 144 | /// \param nodebuffer Buffer provided by the caller application to store the scan data 145 | /// 146 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 147 | /// Once the interface returns, this parameter will store the actual received data count. 148 | /// 149 | /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. 150 | /// 151 | /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 152 | /// 153 | /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. 154 | virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; 155 | 156 | /// Ascending the scan data according to the angle value in the scan. 157 | /// 158 | /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData 159 | /// 160 | /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). 161 | /// Once the interface returns, this parameter will store the actual received data count. 162 | /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 163 | virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) = 0; 164 | 165 | protected: 166 | RPlidarDriver() {} 167 | virtual ~RPlidarDriver() {} 168 | }; 169 | 170 | 171 | }}} 172 | -------------------------------------------------------------------------------- /rplidar-sdk/include/rplidar_protocol.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Data Packet IO protocol definition for RP-LIDAR 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | 38 | #pragma once 39 | 40 | // RP-Lidar Input Packets 41 | 42 | #define RPLIDAR_CMD_SYNC_BYTE 0xA5 43 | #define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80 44 | 45 | 46 | #define RPLIDAR_ANS_SYNC_BYTE1 0xA5 47 | #define RPLIDAR_ANS_SYNC_BYTE2 0x5A 48 | 49 | #define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 50 | 51 | #define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF 52 | #define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) 53 | 54 | #if defined(_WIN32) 55 | #pragma pack(1) 56 | #endif 57 | 58 | typedef struct _rplidar_cmd_packet_t { 59 | _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE 60 | _u8 cmd_flag; 61 | _u8 size; 62 | _u8 data[0]; 63 | } __attribute__((packed)) rplidar_cmd_packet_t; 64 | 65 | 66 | typedef struct _rplidar_ans_header_t { 67 | _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 68 | _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 69 | _u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; 70 | _u8 type; 71 | } __attribute__((packed)) rplidar_ans_header_t; 72 | 73 | #if defined(_WIN32) 74 | #pragma pack() 75 | #endif 76 | -------------------------------------------------------------------------------- /rplidar-sdk/include/rptypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without modification, 6 | * are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 16 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 18 | * SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 19 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 20 | * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR 22 | * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 23 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | * 25 | */ 26 | /* 27 | * RoboPeak LIDAR System 28 | * Common Types definition 29 | * 30 | * Copyright 2009 - 2014 RoboPeak Team 31 | * http://www.robopeak.com 32 | * 33 | */ 34 | 35 | #pragma once 36 | 37 | 38 | #ifdef _WIN32 39 | 40 | //fake stdint.h for VC only 41 | 42 | typedef signed char int8_t; 43 | typedef unsigned char uint8_t; 44 | 45 | typedef __int16 int16_t; 46 | typedef unsigned __int16 uint16_t; 47 | 48 | typedef __int32 int32_t; 49 | typedef unsigned __int32 uint32_t; 50 | 51 | typedef __int64 int64_t; 52 | typedef unsigned __int64 uint64_t; 53 | 54 | #else 55 | 56 | #include 57 | 58 | #endif 59 | 60 | 61 | //based on stdint.h 62 | typedef int8_t _s8; 63 | typedef uint8_t _u8; 64 | 65 | typedef int16_t _s16; 66 | typedef uint16_t _u16; 67 | 68 | typedef int32_t _s32; 69 | typedef uint32_t _u32; 70 | 71 | typedef int64_t _s64; 72 | typedef uint64_t _u64; 73 | 74 | #define __small_endian 75 | 76 | #ifndef __GNUC__ 77 | #define __attribute__(x) 78 | #endif 79 | 80 | 81 | // The _word_size_t uses actual data bus width of the current CPU 82 | #ifdef _AVR_ 83 | typedef _u8 _word_size_t; 84 | #define THREAD_PROC 85 | #elif defined (WIN64) 86 | typedef _u64 _word_size_t; 87 | #define THREAD_PROC __stdcall 88 | #elif defined (WIN32) 89 | typedef _u32 _word_size_t; 90 | #define THREAD_PROC __stdcall 91 | #elif defined (__GNUC__) 92 | typedef unsigned long _word_size_t; 93 | #define THREAD_PROC 94 | #elif defined (__ICCARM__) 95 | typedef _u32 _word_size_t; 96 | #define THREAD_PROC 97 | #endif 98 | 99 | 100 | typedef uint32_t u_result; 101 | 102 | #define RESULT_OK 0 103 | #define RESULT_FAIL_BIT 0x80000000 104 | #define RESULT_ALREADY_DONE 0x20 105 | #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) 106 | #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) 107 | #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) 108 | #define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) 109 | #define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) 110 | #define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) 111 | #define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) 112 | 113 | #define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) 114 | #define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) 115 | 116 | typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); -------------------------------------------------------------------------------- /rplidar-sdk/rplidar_sdk_1.4.5: -------------------------------------------------------------------------------- 1 | 2 | source http://www.slamtec.com/download/lidar/sdk/rplidar_sdk.1.4.5.7z 3 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/linux/arch_linux.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Linux definition and includes 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | // libc dep 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // libc++ dep 50 | #include 51 | #include 52 | 53 | // linux specific 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | #include "arch/linux/timer.h" 66 | 67 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/linux/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Serial Device Driver for Linux 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "hal/abs_rxtx.h" 40 | 41 | namespace rp{ namespace arch{ namespace net{ 42 | 43 | class raw_serial : public rp::hal::serial_rxtx 44 | { 45 | public: 46 | enum{ 47 | SERIAL_RX_BUFFER_SIZE = 512, 48 | SERIAL_TX_BUFFER_SIZE = 128, 49 | }; 50 | 51 | raw_serial(); 52 | virtual ~raw_serial(); 53 | virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = NULL); 54 | virtual bool open(); 55 | virtual void close(); 56 | virtual void flush( _u32 flags); 57 | 58 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); 59 | 60 | virtual int senddata(const unsigned char * data, size_t size); 61 | virtual int recvdata(unsigned char * data, size_t size); 62 | 63 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); 64 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); 65 | 66 | virtual size_t rxqueue_count(); 67 | 68 | _u32 getTermBaudBitmap(_u32 baud); 69 | protected: 70 | bool open(const char * portname, uint32_t baudrate, uint32_t flags = NULL); 71 | void _init(); 72 | 73 | char _portName[200]; 74 | uint32_t _baudrate; 75 | uint32_t _flags; 76 | 77 | int serial_fd; 78 | 79 | size_t required_tx_cnt; 80 | size_t required_rx_cnt; 81 | }; 82 | 83 | }}} 84 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/linux/thread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Thread abstract layer implementation 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "arch/linux/arch_linux.h" 38 | 39 | #include 40 | 41 | namespace rp{ namespace hal{ 42 | 43 | Thread Thread::create(thread_proc_t proc, void * data) 44 | { 45 | Thread newborn(proc, data); 46 | 47 | // tricky code, we assume pthread_t is not a structure but a word size value 48 | assert( sizeof(newborn._handle) >= sizeof(pthread_t)); 49 | 50 | pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data); 51 | 52 | return newborn; 53 | } 54 | 55 | u_result Thread::terminate() 56 | { 57 | if (!this->_handle) return RESULT_OK; 58 | 59 | return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; 60 | } 61 | 62 | u_result Thread::setPriority( priority_val_t p) 63 | { 64 | if (!this->_handle) return RESULT_OPERATION_FAIL; 65 | 66 | // check whether current schedule policy supports priority levels 67 | 68 | int current_policy; 69 | struct sched_param current_param; 70 | int ans; 71 | if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) 72 | { 73 | // cannot retreieve values 74 | return RESULT_OPERATION_FAIL; 75 | } 76 | 77 | //int pthread_priority = 0 ; 78 | 79 | switch(p) 80 | { 81 | case PRIORITY_REALTIME: 82 | //pthread_priority = pthread_priority_max; 83 | current_policy = SCHED_RR; 84 | break; 85 | case PRIORITY_HIGH: 86 | //pthread_priority = (pthread_priority_max + pthread_priority_min)/2; 87 | current_policy = SCHED_RR; 88 | break; 89 | case PRIORITY_NORMAL: 90 | case PRIORITY_LOW: 91 | case PRIORITY_IDLE: 92 | //pthread_priority = 0; 93 | current_policy = SCHED_OTHER; 94 | break; 95 | } 96 | 97 | current_param.__sched_priority = current_policy; 98 | if ( (ans = pthread_setschedparam( (pthread_t) this->_handle, current_policy, ¤t_param)) ) 99 | { 100 | return RESULT_OPERATION_FAIL; 101 | } 102 | return RESULT_OK; 103 | } 104 | 105 | Thread::priority_val_t Thread::getPriority() 106 | { 107 | if (!this->_handle) return PRIORITY_NORMAL; 108 | 109 | int current_policy; 110 | struct sched_param current_param; 111 | if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) 112 | { 113 | // cannot retreieve values 114 | return PRIORITY_NORMAL; 115 | } 116 | 117 | int pthread_priority_max = sched_get_priority_max(SCHED_RR); 118 | int pthread_priority_min = sched_get_priority_min(SCHED_RR); 119 | 120 | if (current_param.__sched_priority ==(pthread_priority_max )) 121 | { 122 | return PRIORITY_REALTIME; 123 | } 124 | if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2) 125 | { 126 | return PRIORITY_HIGH; 127 | } 128 | return PRIORITY_NORMAL; 129 | } 130 | 131 | u_result Thread::join(unsigned long timeout) 132 | { 133 | if (!this->_handle) return RESULT_OK; 134 | 135 | pthread_join((pthread_t)(this->_handle), NULL); 136 | return RESULT_OK; 137 | } 138 | 139 | }} 140 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/linux/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on Linux 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "arch/linux/arch_linux.h" 38 | 39 | namespace rp{ namespace arch{ 40 | _u64 rp_getus() 41 | { 42 | struct timespec t; 43 | t.tv_sec = t.tv_nsec = 0; 44 | clock_gettime(CLOCK_MONOTONIC, &t); 45 | return t.tv_sec*1000000LL + t.tv_nsec/1000; 46 | } 47 | _u32 rp_getms() 48 | { 49 | struct timespec t; 50 | t.tv_sec = t.tv_nsec = 0; 51 | clock_gettime(CLOCK_MONOTONIC, &t); 52 | return t.tv_sec*1000L + t.tv_nsec/1000000L; 53 | } 54 | }} 55 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/linux/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on Linux 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | 41 | // TODO: the highest timer interface should be clock_gettime 42 | namespace rp{ namespace arch{ 43 | 44 | _u64 rp_getus(); 45 | _u32 rp_getms(); 46 | 47 | }} 48 | 49 | #define getms() rp::arch::rp_getms() 50 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/macOS/arch_macOS.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Darwin definition and includes 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | // libc dep 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // libc++ dep 50 | #include 51 | #include 52 | 53 | 54 | // POSIX specific 55 | extern "C" { 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | } 66 | 67 | #include "arch/macOS/timer.h" 68 | 69 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/macOS/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Serial Device Driver for MacOS 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "hal/abs_rxtx.h" 40 | 41 | namespace rp{ namespace arch{ namespace net{ 42 | 43 | class raw_serial : public rp::hal::serial_rxtx 44 | { 45 | public: 46 | enum{ 47 | SERIAL_RX_BUFFER_SIZE = 512, 48 | SERIAL_TX_BUFFER_SIZE = 128, 49 | }; 50 | 51 | raw_serial(); 52 | virtual ~raw_serial(); 53 | virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0); 54 | virtual bool open(); 55 | virtual void close(); 56 | virtual void flush( _u32 flags); 57 | 58 | virtual int waitfordata(_word_size_t data_count,_u32 timeout = -1, _word_size_t * returned_size = NULL); 59 | 60 | virtual int senddata(const unsigned char * data, _word_size_t size); 61 | virtual int recvdata(unsigned char * data, _word_size_t size); 62 | 63 | virtual int waitforsent(_u32 timeout = -1, _word_size_t * returned_size = NULL); 64 | virtual int waitforrecv(_u32 timeout = -1, _word_size_t * returned_size = NULL); 65 | 66 | virtual size_t rxqueue_count(); 67 | 68 | _u32 getTermBaudBitmap(_u32 baud); 69 | protected: 70 | bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); 71 | void _init(); 72 | 73 | char _portName[200]; 74 | uint32_t _baudrate; 75 | uint32_t _flags; 76 | 77 | int serial_fd; 78 | 79 | size_t required_tx_cnt; 80 | size_t required_rx_cnt; 81 | }; 82 | 83 | }}} -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/macOS/thread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Thread abstract layer implementation 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "arch/macOS/arch_macOS.h" 38 | 39 | namespace rp{ namespace hal{ 40 | 41 | Thread Thread::create(thread_proc_t proc, void * data) 42 | { 43 | Thread newborn(proc, data); 44 | 45 | // tricky code, we assume pthread_t is not a structure but a word size value 46 | assert( sizeof(newborn._handle) >= sizeof(pthread_t)); 47 | 48 | pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); 49 | 50 | return newborn; 51 | } 52 | 53 | u_result Thread::terminate() 54 | { 55 | if (!this->_handle) return RESULT_OK; 56 | 57 | // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; 58 | return RESULT_OK; 59 | } 60 | 61 | u_result Thread::setPriority( priority_val_t p) 62 | { 63 | if (!this->_handle) return RESULT_OPERATION_FAIL; 64 | // simply ignore this request 65 | return RESULT_OK; 66 | } 67 | 68 | Thread::priority_val_t Thread::getPriority() 69 | { 70 | return PRIORITY_NORMAL; 71 | } 72 | 73 | u_result Thread::join(unsigned long timeout) 74 | { 75 | if (!this->_handle) return RESULT_OK; 76 | 77 | pthread_join((pthread_t)(this->_handle), NULL); 78 | return RESULT_OK; 79 | } 80 | 81 | }} 82 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/macOS/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on MacOS 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | 38 | #include "arch/macOS/arch_macOS.h" 39 | 40 | 41 | namespace rp{ namespace arch{ 42 | _u64 getus() 43 | { 44 | timeval now; 45 | gettimeofday(&now,NULL); 46 | return now.tv_sec*1000000 + now.tv_usec; 47 | } 48 | 49 | _u32 rp_getms() 50 | { 51 | timeval now; 52 | gettimeofday(&now,NULL); 53 | return now.tv_sec*1000L + now.tv_usec/1000L; 54 | } 55 | 56 | }} 57 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/macOS/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on MacOS 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | 41 | // TODO: the highest timer interface should be clock_gettime 42 | namespace rp{ namespace arch{ 43 | 44 | _u64 rp_getus(); 45 | _u32 rp_getms(); 46 | 47 | }} 48 | 49 | #define getms() rp::arch::rp_getms() -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/win32/arch_win32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Win32 definition and includes 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #pragma warning (disable: 4996) 40 | #define _CRT_SECURE_NO_WARNINGS 41 | 42 | #ifndef WINVER 43 | #define WINVER 0x0500 44 | #endif 45 | 46 | #ifndef _WIN32_WINNT 47 | #define _WIN32_WINNT 0x0501 48 | #endif 49 | 50 | 51 | #ifndef _WIN32_IE 52 | #define _WIN32_IE 0x0501 53 | #endif 54 | 55 | #ifndef _RICHEDIT_VER 56 | #define _RICHEDIT_VER 0x0200 57 | #endif 58 | 59 | 60 | #include 61 | #include 62 | #include 63 | #include //for memcpy etc.. 64 | #include 65 | #include 66 | 67 | 68 | #include "timer.h" 69 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/win32/net_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Serial Device Driver for Win32 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "hal/abs_rxtx.h" 40 | 41 | namespace rp{ namespace arch{ namespace net{ 42 | 43 | class raw_serial : public rp::hal::serial_rxtx 44 | { 45 | public: 46 | enum{ 47 | SERIAL_RX_BUFFER_SIZE = 512, 48 | SERIAL_TX_BUFFER_SIZE = 128, 49 | SERIAL_RX_TIMEOUT = 2000, 50 | SERIAL_TX_TIMEOUT = 2000, 51 | }; 52 | 53 | raw_serial(); 54 | virtual ~raw_serial(); 55 | virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0); 56 | virtual bool open(); 57 | virtual void close(); 58 | virtual void flush( _u32 flags); 59 | 60 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); 61 | 62 | virtual int senddata(const unsigned char * data, size_t size); 63 | virtual int recvdata(unsigned char * data, size_t size); 64 | 65 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); 66 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); 67 | 68 | virtual size_t rxqueue_count(); 69 | 70 | 71 | protected: 72 | bool open(const char * portname, _u32 baudrate, _u32 flags); 73 | void _init(); 74 | 75 | char _portName[20]; 76 | uint32_t _baudrate; 77 | uint32_t _flags; 78 | 79 | OVERLAPPED _ro, _wo; 80 | OVERLAPPED _wait_o; 81 | volatile HANDLE _serial_handle; 82 | DCB _dcb; 83 | COMMTIMEOUTS _co; 84 | }; 85 | 86 | }}} 87 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/win32/timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on Win32 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "sdkcommon.h" 38 | #include 39 | #pragma comment(lib, "Winmm.lib") 40 | 41 | namespace rp{ namespace arch{ 42 | 43 | static LARGE_INTEGER _current_freq; 44 | 45 | void HPtimer_reset() 46 | { 47 | BOOL ans=QueryPerformanceFrequency(&_current_freq); 48 | _current_freq.QuadPart/=1000; 49 | } 50 | 51 | _u32 getHDTimer() 52 | { 53 | LARGE_INTEGER current; 54 | QueryPerformanceCounter(¤t); 55 | 56 | return (_u32)(current.QuadPart/_current_freq.QuadPart); 57 | } 58 | 59 | BEGIN_STATIC_CODE(timer_cailb) 60 | { 61 | HPtimer_reset(); 62 | }END_STATIC_CODE(timer_cailb) 63 | 64 | }} 65 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/win32/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * High Resolution Timer Impl on Win32 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | 41 | #define delay(x) ::Sleep(x) 42 | 43 | namespace rp{ namespace arch{ 44 | void HPtimer_reset(); 45 | _u32 getHDTimer(); 46 | }} 47 | 48 | #define getms() rp::arch::getHDTimer() 49 | 50 | -------------------------------------------------------------------------------- /rplidar-sdk/src/arch/win32/winthread.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Thread abstract layer implementation 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "sdkcommon.h" 38 | #include 39 | 40 | namespace rp{ namespace hal{ 41 | 42 | Thread Thread::create(thread_proc_t proc, void * data) 43 | { 44 | Thread newborn(proc, data); 45 | 46 | newborn._handle = (_word_size_t)( 47 | _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc, 48 | data, 0, NULL)); 49 | return newborn; 50 | } 51 | 52 | u_result Thread::terminate() 53 | { 54 | if (!this->_handle) return RESULT_OK; 55 | if (TerminateThread( reinterpret_cast(this->_handle), -1)) 56 | { 57 | CloseHandle(reinterpret_cast(this->_handle)); 58 | this->_handle = NULL; 59 | return RESULT_OK; 60 | }else 61 | { 62 | return RESULT_OPERATION_FAIL; 63 | } 64 | } 65 | 66 | u_result Thread::setPriority( priority_val_t p) 67 | { 68 | if (!this->_handle) return RESULT_OPERATION_FAIL; 69 | 70 | int win_priority = THREAD_PRIORITY_NORMAL; 71 | switch(p) 72 | { 73 | case PRIORITY_REALTIME: 74 | win_priority = THREAD_PRIORITY_TIME_CRITICAL; 75 | break; 76 | case PRIORITY_HIGH: 77 | win_priority = THREAD_PRIORITY_HIGHEST; 78 | break; 79 | case PRIORITY_NORMAL: 80 | win_priority = THREAD_PRIORITY_NORMAL; 81 | break; 82 | case PRIORITY_LOW: 83 | win_priority = THREAD_PRIORITY_LOWEST; 84 | break; 85 | case PRIORITY_IDLE: 86 | win_priority = THREAD_PRIORITY_IDLE; 87 | break; 88 | } 89 | 90 | if (SetThreadPriority(reinterpret_cast(this->_handle), win_priority)) 91 | { 92 | return RESULT_OK; 93 | } 94 | return RESULT_OPERATION_FAIL; 95 | } 96 | 97 | Thread::priority_val_t Thread::getPriority() 98 | { 99 | if (!this->_handle) return PRIORITY_NORMAL; 100 | int win_priority = ::GetThreadPriority(reinterpret_cast(this->_handle)); 101 | 102 | if (win_priority == THREAD_PRIORITY_ERROR_RETURN) 103 | { 104 | return PRIORITY_NORMAL; 105 | } 106 | 107 | if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL ) 108 | { 109 | return PRIORITY_REALTIME; 110 | } 111 | else if (win_priority=THREAD_PRIORITY_ABOVE_NORMAL) 112 | { 113 | return PRIORITY_HIGH; 114 | } 115 | else if (win_priorityTHREAD_PRIORITY_BELOW_NORMAL) 116 | { 117 | return PRIORITY_NORMAL; 118 | }else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE) 119 | { 120 | return PRIORITY_LOW; 121 | }else if (win_priority<=THREAD_PRIORITY_IDLE) 122 | { 123 | return PRIORITY_IDLE; 124 | } 125 | return PRIORITY_NORMAL; 126 | } 127 | 128 | u_result Thread::join(unsigned long timeout) 129 | { 130 | if (!this->_handle) return RESULT_OK; 131 | switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) 132 | { 133 | case WAIT_OBJECT_0: 134 | CloseHandle(reinterpret_cast(this->_handle)); 135 | this->_handle = NULL; 136 | return RESULT_OK; 137 | case WAIT_ABANDONED: 138 | return RESULT_OPERATION_FAIL; 139 | case WAIT_TIMEOUT: 140 | return RESULT_OPERATION_TIMEOUT; 141 | } 142 | 143 | return RESULT_OK; 144 | } 145 | 146 | }} 147 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/abs_rxtx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Serial Driver Interface 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | 41 | namespace rp{ namespace hal{ 42 | 43 | class serial_rxtx 44 | { 45 | public: 46 | enum{ 47 | ANS_OK = 0, 48 | ANS_TIMEOUT = -1, 49 | ANS_DEV_ERR = -2, 50 | }; 51 | 52 | static serial_rxtx * CreateRxTx(); 53 | static void ReleaseRxTx( serial_rxtx * ); 54 | 55 | serial_rxtx():_is_serial_opened(false){} 56 | virtual ~serial_rxtx(){} 57 | 58 | virtual void flush( _u32 flags) = 0; 59 | 60 | virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0; 61 | virtual bool open() = 0; 62 | virtual void close() = 0; 63 | 64 | virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; 65 | 66 | virtual int senddata(const unsigned char * data, size_t size) = 0; 67 | virtual int recvdata(unsigned char * data, size_t size) = 0; 68 | 69 | virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0; 70 | virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0; 71 | 72 | virtual size_t rxqueue_count() = 0; 73 | 74 | virtual bool isOpened() 75 | { 76 | return _is_serial_opened; 77 | } 78 | 79 | protected: 80 | volatile bool _is_serial_opened; 81 | }; 82 | 83 | }} 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/event.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Lock abstract layer 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | 38 | #pragma once 39 | namespace rp{ namespace hal{ 40 | 41 | class Event 42 | { 43 | public: 44 | 45 | enum 46 | { 47 | EVENT_OK = 1, 48 | EVENT_TIMEOUT = -1, 49 | EVENT_FAILED = 0, 50 | }; 51 | 52 | Event(bool isAutoReset = true, bool isSignal = false) 53 | #ifdef _WIN32 54 | : _event(NULL) 55 | #else 56 | : _is_signalled(isSignal) 57 | , _isAutoReset(isAutoReset) 58 | #endif 59 | { 60 | #ifdef _WIN32 61 | _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 62 | #else 63 | pthread_mutex_init(&_cond_locker, NULL); 64 | pthread_cond_init(&_cond_var, NULL); 65 | #endif 66 | } 67 | 68 | ~ Event() 69 | { 70 | release(); 71 | } 72 | 73 | void set( bool isSignal = true ) 74 | { 75 | if (isSignal){ 76 | #ifdef _WIN32 77 | SetEvent(_event); 78 | #else 79 | pthread_mutex_lock(&_cond_locker); 80 | 81 | if ( _is_signalled == false ) 82 | { 83 | _is_signalled = true; 84 | pthread_cond_signal(&_cond_var); 85 | } 86 | pthread_mutex_unlock(&_cond_locker); 87 | #endif 88 | } 89 | else 90 | { 91 | #ifdef _WIN32 92 | ResetEvent(_event); 93 | #else 94 | pthread_mutex_lock(&_cond_locker); 95 | _is_signalled = false; 96 | pthread_mutex_unlock(&_cond_locker); 97 | #endif 98 | } 99 | } 100 | 101 | unsigned long wait( unsigned long timeout = 0xFFFFFFFF ) 102 | { 103 | #ifdef _WIN32 104 | switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 105 | { 106 | case WAIT_FAILED: 107 | return EVENT_FAILED; 108 | case WAIT_OBJECT_0: 109 | return EVENT_OK; 110 | case WAIT_TIMEOUT: 111 | return EVENT_TIMEOUT; 112 | } 113 | return EVENT_OK; 114 | #else 115 | unsigned long ans = EVENT_OK; 116 | pthread_mutex_lock( &_cond_locker ); 117 | 118 | if ( !_is_signalled ) 119 | { 120 | 121 | if (timeout == 0xFFFFFFFF){ 122 | pthread_cond_wait(&_cond_var,&_cond_locker); 123 | }else 124 | { 125 | timespec wait_time; 126 | timeval now; 127 | gettimeofday(&now,NULL); 128 | 129 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 130 | wait_time.tv_nsec = (timeout%1000)*1000000ULL + now.tv_usec*1000; 131 | 132 | if (wait_time.tv_nsec >= 1000000000) 133 | { 134 | ++wait_time.tv_sec; 135 | wait_time.tv_nsec -= 1000000000; 136 | } 137 | switch (pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time)) 138 | { 139 | case 0: 140 | // signalled 141 | break; 142 | case ETIMEDOUT: 143 | // time up 144 | ans = EVENT_TIMEOUT; 145 | goto _final; 146 | break; 147 | default: 148 | ans = EVENT_FAILED; 149 | goto _final; 150 | } 151 | 152 | } 153 | } 154 | 155 | assert(_is_signalled); 156 | 157 | if ( _isAutoReset ) 158 | { 159 | _is_signalled = false; 160 | } 161 | _final: 162 | pthread_mutex_unlock( &_cond_locker ); 163 | 164 | return ans; 165 | #endif 166 | 167 | } 168 | protected: 169 | 170 | void release() 171 | { 172 | #ifdef _WIN32 173 | CloseHandle(_event); 174 | #else 175 | pthread_mutex_destroy(&_cond_locker); 176 | pthread_cond_destroy(&_cond_var); 177 | #endif 178 | } 179 | 180 | #ifdef _WIN32 181 | HANDLE _event; 182 | #else 183 | pthread_cond_t _cond_var; 184 | pthread_mutex_t _cond_locker; 185 | bool _is_signalled; 186 | bool _isAutoReset; 187 | #endif 188 | }; 189 | }} 190 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/locker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Lock abstract layer 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | namespace rp{ namespace hal{ 39 | 40 | class Locker 41 | { 42 | public: 43 | enum LOCK_STATUS 44 | { 45 | LOCK_OK = 1, 46 | LOCK_TIMEOUT = -1, 47 | LOCK_FAILED = 0 48 | }; 49 | 50 | Locker(){ 51 | #ifdef _WIN32 52 | _lock = NULL; 53 | #endif 54 | init(); 55 | } 56 | 57 | ~Locker() 58 | { 59 | release(); 60 | } 61 | 62 | Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF) 63 | { 64 | #ifdef _WIN32 65 | switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout)) 66 | { 67 | case WAIT_ABANDONED: 68 | return LOCK_FAILED; 69 | case WAIT_OBJECT_0: 70 | return LOCK_OK; 71 | case WAIT_TIMEOUT: 72 | return LOCK_TIMEOUT; 73 | } 74 | 75 | #else 76 | #ifdef _MACOS 77 | if (timeout !=0 ) { 78 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 79 | } 80 | #else 81 | if (timeout == 0xFFFFFFFF){ 82 | if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK; 83 | } 84 | #endif 85 | else if (timeout == 0) 86 | { 87 | if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK; 88 | } 89 | #ifndef _MACOS 90 | else 91 | { 92 | timespec wait_time; 93 | timeval now; 94 | gettimeofday(&now,NULL); 95 | 96 | wait_time.tv_sec = timeout/1000 + now.tv_sec; 97 | wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000; 98 | 99 | if (wait_time.tv_nsec >= 1000000000) 100 | { 101 | ++wait_time.tv_sec; 102 | wait_time.tv_nsec -= 1000000000; 103 | } 104 | switch (pthread_mutex_timedlock(&_lock,&wait_time)) 105 | { 106 | case 0: 107 | return LOCK_OK; 108 | case ETIMEDOUT: 109 | return LOCK_TIMEOUT; 110 | } 111 | } 112 | #endif 113 | #endif 114 | 115 | return LOCK_FAILED; 116 | } 117 | 118 | 119 | void unlock() 120 | { 121 | #ifdef _WIN32 122 | ReleaseMutex(_lock); 123 | #else 124 | pthread_mutex_unlock(&_lock); 125 | #endif 126 | } 127 | 128 | #ifdef _WIN32 129 | HANDLE getLockHandle() 130 | { 131 | return _lock; 132 | } 133 | #else 134 | pthread_mutex_t *getLockHandle() 135 | { 136 | return &_lock; 137 | } 138 | #endif 139 | 140 | 141 | 142 | protected: 143 | void init() 144 | { 145 | #ifdef _WIN32 146 | _lock = CreateMutex(NULL,FALSE,NULL); 147 | #else 148 | pthread_mutex_init(&_lock, NULL); 149 | #endif 150 | } 151 | 152 | void release() 153 | { 154 | unlock(); //force unlock before release 155 | #ifdef _WIN32 156 | 157 | if (_lock) CloseHandle(_lock); 158 | _lock = NULL; 159 | #else 160 | pthread_mutex_destroy(&_lock); 161 | #endif 162 | } 163 | 164 | #ifdef _WIN32 165 | HANDLE _lock; 166 | #else 167 | pthread_mutex_t _lock; 168 | #endif 169 | }; 170 | 171 | class AutoLocker 172 | { 173 | public : 174 | AutoLocker(Locker &l): _binded(l) 175 | { 176 | _binded.lock(); 177 | } 178 | 179 | void forceUnlock() { 180 | _binded.unlock(); 181 | } 182 | ~AutoLocker() {_binded.unlock();} 183 | Locker & _binded; 184 | }; 185 | 186 | 187 | }} 188 | 189 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/thread.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Thread abstract layer 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #include "sdkcommon.h" 38 | #include "hal/thread.h" 39 | 40 | #if defined(_WIN32) 41 | #include "arch/win32/winthread.hpp" 42 | #elif defined(_MACOS) 43 | #include "arch/macOS/thread.hpp" 44 | #elif defined(__GNUC__) 45 | #include "arch/linux/thread.hpp" 46 | #else 47 | #error no threading implemention found for this platform. 48 | #endif 49 | 50 | 51 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/thread.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Thread abstract layer 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | #include "rptypes.h" 40 | #define CLASS_THREAD(c , x ) \ 41 | rp::hal::Thread::create_member(this ) 42 | 43 | namespace rp{ namespace hal{ 44 | 45 | class Thread 46 | { 47 | public: 48 | enum priority_val_t 49 | { 50 | PRIORITY_REALTIME = 0, 51 | PRIORITY_HIGH = 1, 52 | PRIORITY_NORMAL = 2, 53 | PRIORITY_LOW = 3, 54 | PRIORITY_IDLE = 4, 55 | }; 56 | 57 | template 58 | static Thread create_member(T * pthis) 59 | { 60 | return create(_thread_thunk, pthis); 61 | } 62 | 63 | template 64 | static _word_size_t THREAD_PROC _thread_thunk(void * data) 65 | { 66 | return (static_cast(data)->*PROC)(); 67 | } 68 | static Thread create(thread_proc_t proc, void * data = NULL ); 69 | 70 | public: 71 | ~Thread() { } 72 | Thread(): _data(NULL),_func(NULL),_handle(0) {} 73 | _word_size_t getHandle(){ return _handle;} 74 | u_result terminate(); 75 | void *getData() { return _data;} 76 | u_result join(unsigned long timeout = -1); 77 | u_result setPriority( priority_val_t p); 78 | priority_val_t getPriority(); 79 | 80 | bool operator== ( const Thread & right) { return this->_handle == right._handle; } 81 | protected: 82 | Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0) {} 83 | void * _data; 84 | thread_proc_t _func; 85 | _word_size_t _handle; 86 | }; 87 | 88 | }} 89 | 90 | -------------------------------------------------------------------------------- /rplidar-sdk/src/hal/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Utility Functions 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | 40 | //------ 41 | /* _countof helper */ 42 | #if !defined(_countof) 43 | #if !defined(__cplusplus) 44 | #define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0])) 45 | #else 46 | extern "C++" 47 | { 48 | template 49 | char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray]; 50 | #define _countof(_Array) sizeof(*__countof_helper(_Array)) 51 | } 52 | #endif 53 | #endif 54 | 55 | /* _offsetof helper */ 56 | #if !defined(offsetof) 57 | #define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field)) 58 | #endif 59 | 60 | 61 | #define BEGIN_STATIC_CODE( _blockname_ ) \ 62 | static class _static_code_##_blockname_ { \ 63 | public: \ 64 | _static_code_##_blockname_ () 65 | 66 | 67 | #define END_STATIC_CODE( _blockname_ ) \ 68 | } _instance_##_blockname_; 69 | 70 | -------------------------------------------------------------------------------- /rplidar-sdk/src/rplidar_driver_serial.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * Serial based RPlidar Driver 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #pragma once 38 | 39 | namespace rp { namespace standalone{ namespace rplidar { 40 | 41 | class RPlidarDriverSerialImpl : public RPlidarDriver 42 | { 43 | public: 44 | 45 | enum { 46 | MAX_SCAN_NODES = 2048, 47 | }; 48 | 49 | RPlidarDriverSerialImpl(); 50 | virtual ~RPlidarDriverSerialImpl(); 51 | 52 | public: 53 | virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag); 54 | virtual void disconnect(); 55 | virtual bool isConnected(); 56 | 57 | virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); 58 | 59 | virtual u_result getHealth(rplidar_response_device_health_t &, _u32 timeout = DEFAULT_TIMEOUT); 60 | virtual u_result getDeviceInfo(rplidar_response_device_info_t &, _u32 timeout = DEFAULT_TIMEOUT); 61 | virtual u_result getFrequency(rplidar_response_measurement_node_t * nodebuffer, size_t count, float & frequency); 62 | 63 | virtual u_result startScan(bool force, _u32 timeout = DEFAULT_TIMEOUT); 64 | virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT); 65 | virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); 66 | virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); 67 | 68 | protected: 69 | u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout); 70 | u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); 71 | u_result _cacheScanData(); 72 | u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0); 73 | u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout); 74 | 75 | void _disableDataGrabbing(); 76 | 77 | bool _isConnected; 78 | bool _isScanning; 79 | 80 | rp::hal::Locker _lock; 81 | rp::hal::Event _dataEvt; 82 | rp::hal::serial_rxtx * _rxtx; 83 | rplidar_response_measurement_node_t _cached_scan_node_buf[2048]; 84 | size_t _cached_scan_node_count; 85 | rp::hal::Thread _cachethread; 86 | 87 | 88 | }; 89 | 90 | 91 | }}} 92 | -------------------------------------------------------------------------------- /rplidar-sdk/src/sdkcommon.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, RoboPeak 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 17 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 19 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 20 | * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 21 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 22 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 23 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 24 | * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 25 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | /* 29 | * RoboPeak LIDAR System 30 | * SDK Internal Common Header 31 | * 32 | * Copyright 2009 - 2014 RoboPeak Team 33 | * http://www.robopeak.com 34 | * 35 | */ 36 | 37 | #if defined(_WIN32) 38 | #include "arch\win32\arch_win32.h" 39 | #elif defined(_MACOS) 40 | #include "arch/macOS/arch_macOS.h" 41 | #elif defined(__GNUC__) 42 | #include "arch/linux/arch_linux.h" 43 | #else 44 | #error "unsupported target" 45 | #endif 46 | 47 | #include "rplidar.h" 48 | 49 | #include "hal/util.h" 50 | -------------------------------------------------------------------------------- /scripts/config-0-all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | -------------------------------------------------------------------------------- /scripts/config-1-ksrc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $(id -u) != 0 ]; then 4 | echo "This script requires root permissions" 5 | echo "$ sudo "$0"" 6 | exit 7 | fi 8 | 9 | 10 | # Get the kernel source for LT4 21.4 11 | cd /usr/src/ 12 | wget --no-check-certificate 'https://developer.nvidia.com/embedded/dlc/l4t-sources-24-2-1' -O sources_r24.2.1.tbz2 13 | 14 | # Decompress 15 | tar -xvf sources_r24.2.1.tbz2 16 | cd sources 17 | 18 | tar -xvf kernel_src.tbz2 19 | cd kernel 20 | 21 | 22 | -------------------------------------------------------------------------------- /scripts/config-2-kconfig.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $(id -u) != 0 ]; then 4 | echo "This script requires root permissions" 5 | echo "$ sudo "$0"" 6 | exit 7 | fi 8 | 9 | 10 | cd /usr/src/sources/kernel 11 | 12 | # Get the kernel configuration file 13 | zcat /proc/config.gz > .config 14 | 15 | 16 | #sudo sed -i 's/# CONFIG_USB_SERIAL_FTDI_SIO is not set/CONFIG_USB_SERIAL_FTDI_SIO=m/' .config 17 | sudo sed -i 's/# CONFIG_USB_SERIAL_CP210X is not set/CONFIG_USB_SERIAL_CP210X=m/' .config 18 | sudo sed -i 's/CONFIG_LOCALVERSION=""/CONFIG_LOCALVERSION="-tegra"/' .config 19 | 20 | -------------------------------------------------------------------------------- /scripts/config-3-kmod.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $(id -u) != 0 ]; then 4 | echo "This script requires root permissions" 5 | echo "$ sudo "$0"" 6 | exit 7 | fi 8 | 9 | 10 | cd /usr/src/sources/kernel 11 | 12 | 13 | # Prepare the module for compilation 14 | # note - if it asks for confirmation in step below, enter 'm' for build as module 15 | make prepare 16 | make modules_prepare 17 | 18 | # Compile the module 19 | make M=drivers/usb/serial/ 20 | 21 | cp drivers/usb/serial/cp210x.ko /lib/modules/$(uname -r)/kernel/drivers/usb/serial 22 | depmod -a 23 | /bin/echo -e "\e[1;32mCP210x Driver Module Installed.\e[0m" 24 | 25 | -------------------------------------------------------------------------------- /scripts/config-4-hostapd.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # install hostapd (access point) package 4 | sudo apt-get install hostapd 5 | 6 | # configure hostapd daemon 7 | zcat /usr/share/doc/hostapd/examples/hostapd.conf.gz | sudo tee -a /etc/hostapd/hostapd.conf 8 | sudo sed -i 's/ssid=test/ssid=ROVERNET/' /etc/hostapd/hostapd.conf 9 | sudo sed -i 's/# driver=hostap/driver=nl80211/' /etc/hostapd/hostapd.conf 10 | 11 | 12 | # enable hostapd on starup 13 | sudo sed -i 's/#DAEMON_CONF=""/DAEMON_CONF="\/etc\/hostapd\/hostapd.conf"/' /etc/default/hostapd 14 | 15 | # enable access point mode in Jetson wifi driver 16 | echo "options bcmdhd op_mode=2" | sudo tee -a /etc/modprobe.d/bcmdhd.conf 17 | 18 | # (reboot here) 19 | # check that the brcm change has taken hold 20 | cat /sys/module/bcmdhd/parameters/op-mode 21 | -------------------------------------------------------------------------------- /scripts/config-5-network.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | echo " " | sudo tee -a /etc/network/interfaces 5 | echo "auto wlan0" | sudo tee -a /etc/network/interfaces 6 | echo "iface wlan0 inet static" | sudo tee -a /etc/network/interfaces 7 | echo "address 192.168.2.1" | sudo tee -a /etc/network/interfaces 8 | echo "netmask 255.255.255.0" | sudo tee -a /etc/network/interfaces 9 | -------------------------------------------------------------------------------- /scripts/config-6-phidgets.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo apt-get install -y libusb-1.0-0-dev 4 | 5 | wget http://www.phidgets.com/downloads/libraries/libphidget.tar.gz 6 | tar -xvzf libphidget.tar.gz 7 | 8 | cd libphidget-2.1.8.20151217 9 | ./configure 10 | make 11 | sudo make install 12 | -------------------------------------------------------------------------------- /scripts/config-7-sixaxis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ../sixpair 4 | gcc -o sixpair sixpair.c -lusb 5 | 6 | sudo rfkill unblock bluetooth 7 | sleep 3 8 | hcitool dev 9 | sleep 2 10 | 11 | sudo ./sixpair 12 | 13 | cd ../sixad 14 | make clean 15 | make 16 | sudo make install 17 | 18 | 19 | -------------------------------------------------------------------------------- /scripts/init-0-all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | -------------------------------------------------------------------------------- /scripts/init-1-bluetooth.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "starting Dualshock bluetooth controller" 4 | 5 | sudo rfkill unblock bluetooth 6 | sleep 3 7 | hcitool dev 8 | sleep 2 9 | #sudo service bluetooth stop 10 | sleep 1 11 | sixad --start 12 | 13 | -------------------------------------------------------------------------------- /scripts/init-2-maxclocks.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | sudo ~/jetson_clocks.sh 4 | 5 | -------------------------------------------------------------------------------- /scripts/init-3-webserver.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "starting Torch display webserver" 4 | 5 | cd /home/ubuntu/workspace/rovernet/build/torch/bin/ 6 | sudo ./th -ldisplay.start 80 0.0.0.0 7 | #sleep 3 8 | #cd ../../ 9 | #./rovernet-console 10 | 11 | -------------------------------------------------------------------------------- /scripts/init-4-gstreamer.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "starting gstreamer" 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /scripts/init-5-turbo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "starting TURBO2" 4 | 5 | 6 | # start with sudo (under SSH, evdev needs root priveledge) 7 | sudo ./turbo2 $(tput cols) $(tput lines) 8 | -------------------------------------------------------------------------------- /sixad/Makefile: -------------------------------------------------------------------------------- 1 | # You know, there are pre-compile DEBs of this... 2 | 3 | CXX ?= g++ 4 | CXXFLAGS ?= -O2 -Wall 5 | LDFLAGS += -Wl,-Bsymbolic-functions 6 | 7 | all: sixad_bins 8 | 9 | sixad_bins: 10 | mkdir -p bins 11 | $(CXX) $(CXXFLAGS) $(LDFLAGS) sixad-bin.cpp bluetooth.cpp shared.cpp textfile.cpp -o bins/sixad-bin `pkg-config --cflags --libs bluez` -lpthread -fpermissive 12 | $(CXX) $(CXXFLAGS) $(LDFLAGS) sixad-sixaxis.cpp sixaxis.cpp shared.cpp uinput.cpp textfile.cpp -o bins/sixad-sixaxis -lpthread -lrt 13 | $(CXX) $(CXXFLAGS) $(LDFLAGS) sixad-remote.cpp remote.cpp shared.cpp uinput.cpp textfile.cpp -o bins/sixad-remote -lrt 14 | $(CXX) $(CXXFLAGS) $(LDFLAGS) sixad-raw.cpp sixaxis.cpp shared.cpp uinput.cpp textfile.cpp -o bins/sixad-raw 15 | $(CXX) $(CXXFLAGS) $(LDFLAGS) sixad-3in1.cpp sixaxis.cpp shared.cpp uinput.cpp textfile.cpp -o bins/sixad-3in1 16 | 17 | clean: 18 | rm -f *~ bins/* 19 | 20 | install: 21 | install -d $(DESTDIR)/etc/default/ 22 | install -d $(DESTDIR)/etc/init.d/ 23 | install -d $(DESTDIR)/etc/logrotate.d/ 24 | install -d $(DESTDIR)/usr/bin/ 25 | install -d $(DESTDIR)/usr/sbin/ 26 | install -d $(DESTDIR)/var/lib/sixad/ 27 | install -d $(DESTDIR)/var/lib/sixad/profiles/ 28 | install -m 644 sixad.default $(DESTDIR)/etc/default/sixad 29 | install -m 755 sixad.init $(DESTDIR)/etc/init.d/sixad 30 | install -m 644 sixad.log $(DESTDIR)/etc/logrotate.d/sixad 31 | install -m 755 sixad $(DESTDIR)/usr/bin/ 32 | install -m 755 bins/sixad-bin $(DESTDIR)/usr/sbin/ 33 | install -m 755 bins/sixad-sixaxis $(DESTDIR)/usr/sbin/ 34 | install -m 755 bins/sixad-remote $(DESTDIR)/usr/sbin/ 35 | install -m 755 bins/sixad-3in1 $(DESTDIR)/usr/sbin/ 36 | install -m 755 bins/sixad-raw $(DESTDIR)/usr/sbin/ 37 | install -m 755 sixad-dbus-blocker $(DESTDIR)/usr/sbin/ 38 | @chmod 777 -R $(DESTDIR)/var/lib/sixad/ 39 | @echo "Installation is Complete!" 40 | 41 | uninstall: 42 | rm -f $(DESTDIR)/etc/default/sixad 43 | rm -f $(DESTDIR)/etc/init.d/sixad 44 | rm -f $(DESTDIR)/etc/logrotate.d/sixad 45 | rm -f $(DESTDIR)/usr/bin/sixad 46 | rm -f $(DESTDIR)/usr/sbin/sixad-bin 47 | rm -f $(DESTDIR)/usr/sbin/sixad-sixaxis 48 | rm -f $(DESTDIR)/usr/sbin/sixad-remote 49 | rm -f $(DESTDIR)/usr/sbin/sixad-raw 50 | rm -f $(DESTDIR)/usr/sbin/sixad-dbus-blocker 51 | rm -rf $(DESTDIR)/var/lib/sixad/ 52 | -------------------------------------------------------------------------------- /sixad/bins/sixad-3in1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/bins/sixad-3in1 -------------------------------------------------------------------------------- /sixad/bins/sixad-bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/bins/sixad-bin -------------------------------------------------------------------------------- /sixad/bins/sixad-raw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/bins/sixad-raw -------------------------------------------------------------------------------- /sixad/bins/sixad-remote: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/bins/sixad-remote -------------------------------------------------------------------------------- /sixad/bins/sixad-sixaxis: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/bins/sixad-sixaxis -------------------------------------------------------------------------------- /sixad/bluetooth.h: -------------------------------------------------------------------------------- 1 | /* 2 | * bluetooth.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-10 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef BLUETOOTH_H 19 | #define BLUETOOTH_H 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #define L2CAP_PSM_HIDP_CTRL 0x11 26 | #define L2CAP_PSM_HIDP_INTR 0x13 27 | 28 | void do_search(int ctl, bdaddr_t *bdaddr, int debug); 29 | void do_connect(int ctl, bdaddr_t *src, bdaddr_t *dst, int debug); 30 | 31 | int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm, int lm, int backlog); 32 | void l2cap_accept(int ctl, int csk, int isk, int debug, int legacy); 33 | int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm); 34 | 35 | void hid_server(int ctl, int csk, int isk, int debug, int legacy); 36 | int create_device(int ctl, int csk, int isk); 37 | 38 | int get_sdp_device_info(const bdaddr_t *src, const bdaddr_t *dst, struct hidp_connadd_req *req); 39 | void epox_endian_quirk(unsigned char *data, int size); 40 | 41 | #endif // BLUETOOTH_H 42 | -------------------------------------------------------------------------------- /sixad/manual.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixad/manual.pdf -------------------------------------------------------------------------------- /sixad/qtcreator/sixad-bin.pro: -------------------------------------------------------------------------------- 1 | # QtCreator project file 2 | 3 | CONFIG = qt 4 | QT -= gui 5 | 6 | TEMPLATE = app 7 | 8 | SOURCES = ../sixad-bin.cpp ../bluetooth.cpp ../shared.cpp ../textfile.cpp 9 | 10 | HEADERS = ../bluetooth.h ../shared.h ../textfile.h 11 | 12 | TARGET = sixad-bin 13 | 14 | LIBS += -lbluetooth -lpthread 15 | -------------------------------------------------------------------------------- /sixad/qtcreator/sixad-raw.pro: -------------------------------------------------------------------------------- 1 | # QtCreator project file 2 | 3 | CONFIG = qt 4 | QT -= gui 5 | 6 | TEMPLATE = app 7 | 8 | SOURCES = ../sixad-raw.cpp ../sixaxis.cpp ../shared.cpp ../uinput.cpp ../textfile.cpp 9 | 10 | HEADERS = ../sixaxis.h ../shared.h ../uinput.h ../textfile.h 11 | 12 | TARGET = sixad-raw 13 | 14 | -------------------------------------------------------------------------------- /sixad/qtcreator/sixad-remote.pro: -------------------------------------------------------------------------------- 1 | # QtCreator project file 2 | 3 | CONFIG = qt 4 | QT -= gui 5 | 6 | TEMPLATE = app 7 | 8 | SOURCES = ../sixad-remote.cpp ../remote.cpp ../shared.cpp ../uinput.cpp ../textfile.cpp 9 | 10 | HEADERS = ../remote.h ../shared.h ../uinput.h ../textfile.h 11 | 12 | TARGET = sixad-remote 13 | 14 | LIBS += -lrt 15 | -------------------------------------------------------------------------------- /sixad/qtcreator/sixad-sixaxis.pro: -------------------------------------------------------------------------------- 1 | # QtCreator project file 2 | 3 | CONFIG = qt 4 | QT -= gui 5 | 6 | TEMPLATE = app 7 | 8 | SOURCES = ../sixad-sixaxis.cpp ../sixaxis.cpp ../shared.cpp ../uinput.cpp ../textfile.cpp 9 | 10 | HEADERS = ../sixaxis.h ../shared.h ../uinput.h ../textfile.h 11 | 12 | TARGET = sixad-sixaxis 13 | 14 | LIBS += -lpthread -lrt 15 | -------------------------------------------------------------------------------- /sixad/remote.h: -------------------------------------------------------------------------------- 1 | /* 2 | * remote.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2011 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef REMOTE_H 19 | #define REMOTE_H 20 | 21 | #define REMOTE_KEY_1 0x00 22 | #define REMOTE_KEY_2 0x01 23 | #define REMOTE_KEY_3 0x02 24 | #define REMOTE_KEY_4 0x03 25 | #define REMOTE_KEY_5 0x04 26 | #define REMOTE_KEY_6 0x05 27 | #define REMOTE_KEY_7 0x06 28 | #define REMOTE_KEY_8 0x07 29 | #define REMOTE_KEY_9 0x08 30 | #define REMOTE_KEY_0 0x09 31 | #define REMOTE_KEY_ENTER 0x0B 32 | #define REMOTE_KEY_RETURN 0x0E 33 | #define REMOTE_KEY_CLEAR 0x0F 34 | #define REMOTE_KEY_EJECT 0x16 35 | #define REMOTE_KEY_TIME 0x28 36 | #define REMOTE_KEY_PREV 0x30 37 | #define REMOTE_KEY_NEXT 0x31 38 | #define REMOTE_KEY_PLAY 0x32 39 | #define REMOTE_KEY_SCAN_PREV 0x33 40 | #define REMOTE_KEY_SCAN_FORW 0x34 41 | #define REMOTE_KEY_STOP 0x38 42 | #define REMOTE_KEY_PAUSE 0x39 43 | #define REMOTE_KEY_POP_UP_MENU 0x40 44 | #define REMOTE_KEY_SLOW_PREV 0x60 45 | #define REMOTE_KEY_SLOW_FORW 0x61 46 | #define REMOTE_KEY_SUBTITLE 0x63 47 | #define REMOTE_KEY_AUDIO 0x64 48 | #define REMOTE_KEY_ANGLE 0x65 49 | #define REMOTE_KEY_DISPLAY 0x70 50 | #define REMOTE_KEY_BLUE 0x80 51 | #define REMOTE_KEY_RED 0x81 52 | #define REMOTE_KEY_GREEN 0x82 53 | #define REMOTE_KEY_YELLOW 0x83 54 | #define REMOTE_KEY_TOP_MENU 0x1A 55 | 56 | #define REMOTE_KEY_JS_SELECT 0x50 57 | #define REMOTE_KEY_JS_L3 0x51 58 | #define REMOTE_KEY_JS_R3 0x52 59 | #define REMOTE_KEY_JS_START 0x53 60 | #define REMOTE_KEY_JS_UP 0x54 61 | #define REMOTE_KEY_JS_RIGHT 0x55 62 | #define REMOTE_KEY_JS_DOWN 0x56 63 | #define REMOTE_KEY_JS_LEFT 0x57 64 | #define REMOTE_KEY_JS_L2 0x58 65 | #define REMOTE_KEY_JS_R2 0x59 66 | #define REMOTE_KEY_JS_L1 0x5A 67 | #define REMOTE_KEY_JS_R1 0x5B 68 | #define REMOTE_KEY_JS_TRIANGLE 0x5C 69 | #define REMOTE_KEY_JS_CIRCLE 0x5D 70 | #define REMOTE_KEY_JS_CROSS 0x5E 71 | #define REMOTE_KEY_JS_SQUARE 0x5F 72 | 73 | #define REMOTE_KEY_OPTIONS REMOTE_KEY_JS_TRIANGLE 74 | #define REMOTE_KEY_BACK REMOTE_KEY_JS_CIRCLE 75 | #define REMOTE_KEY_VIEW REMOTE_KEY_JS_SQUARE 76 | 77 | #define REMOTE_KEYMODE_NUMBERIC 0x01 78 | #define REMOTE_KEYMODE_DVD 0x02 79 | #define REMOTE_KEYMODE_DIRECTIONAL 0x04 80 | #define REMOTE_KEYMODE_MULTIMEDIA 0x08 81 | 82 | void do_joystick(int fd, unsigned char* buf, struct dev_joystick joystick); 83 | void do_remote(int fd, unsigned char* buf, int modes); 84 | void do_input(int fd, unsigned char* buf, struct dev_input input); 85 | 86 | #endif // REMOTE_H 87 | -------------------------------------------------------------------------------- /sixad/shared.h: -------------------------------------------------------------------------------- 1 | /* 2 | * shared.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-2011 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef SHARED_H 19 | #define SHARED_H 20 | 21 | #include 22 | 23 | struct dev_led { 24 | bool enabled; 25 | bool anim; 26 | bool auto_sel; 27 | int number; 28 | }; 29 | 30 | struct dev_joystick { 31 | bool enabled; 32 | bool buttons; 33 | bool axis; 34 | bool sbuttons; 35 | bool accel; 36 | bool accon; 37 | bool speed; 38 | bool pos; 39 | }; 40 | 41 | struct dev_remote { 42 | bool enabled; 43 | bool numeric; 44 | bool dvd; 45 | bool directional; 46 | bool multimedia; 47 | }; 48 | 49 | struct dev_input { 50 | bool enabled; 51 | int key_select, key_l3, key_r3, key_start, key_up, key_right, key_down, key_left; 52 | int key_l2, key_r2, key_l1, key_r1, key_tri, key_cir, key_squ, key_cro, key_ps; 53 | int axis_l_type, axis_r_type, axis_speed; 54 | int axis_l_up, axis_l_right, axis_l_down, axis_l_left; 55 | int axis_r_up, axis_r_right, axis_r_down, axis_r_left; 56 | bool use_lr3; 57 | }; 58 | 59 | struct dev_rumble { 60 | bool enabled; 61 | bool old_mode; 62 | }; 63 | 64 | struct dev_timeout { 65 | bool enabled; 66 | int timeout; 67 | }; 68 | 69 | struct device_settings { 70 | bool auto_disconnect; 71 | struct dev_led led; 72 | struct dev_joystick joystick; 73 | struct dev_remote remote; 74 | struct dev_input input; 75 | struct dev_rumble rumble; 76 | struct dev_timeout timeout; 77 | }; 78 | 79 | bool was_active(); 80 | void set_active(int active); 81 | 82 | bool io_canceled(); 83 | void sig_term(int sig); 84 | void open_log(const char *app_name); 85 | 86 | struct device_settings init_values(const char *mac); 87 | 88 | int get_joystick_number(); 89 | void enable_sixaxis(int csk); 90 | 91 | #endif // SHARED_H 92 | -------------------------------------------------------------------------------- /sixad/sixad: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # sixad-bin wrapper 4 | # written by falkTX 5 | 6 | DEBUG=0 7 | LEGACY=0 8 | 9 | . /etc/default/sixad 10 | 11 | if [ -f /usr/bin/sudo ]; then 12 | SUDO="/usr/bin/sudo" 13 | else 14 | SUDO="" 15 | fi 16 | 17 | bt_device_check () { 18 | if (which hciconfig > /dev/null); then 19 | if (hciconfig dev > /dev/null); then 20 | VER=`hciconfig default version | grep "HCI Ver" | awk '{print$3}'` 21 | if [ "$VER" == "1.1" ]; then 22 | echo "***** NOTICE *****" 23 | echo "You're using a very old bluetooth dongle," 24 | echo "the Sixaxis will not work properly!" 25 | elif [ "$VER" == "1.0" ]; then 26 | echo "***** WARNING *****" 27 | echo "You're using a _really_ old bluetooth dongle," 28 | echo "the Sixaxis will just not work!" 29 | fi 30 | else 31 | echo "No bluetooth adapters found on the system!" 32 | echo "sixad will now quit" 33 | exit 34 | fi 35 | fi 36 | } 37 | 38 | sixad_running_check () { 39 | ps -e | grep sixad-bin > /dev/null 40 | } 41 | 42 | bluetoothd_running_check () { 43 | ps -e | grep bluetoothd > /dev/null 44 | } 45 | 46 | modprobe_check () { 47 | $SUDO /sbin/modprobe uinput 48 | } 49 | 50 | bt_start () { 51 | $SUDO rm -f /tmp/.sixad-dbus-blocker.pid 52 | env sleep 1 53 | 54 | if [ -f /lib/udev/rules.d/97-bluetooth.rules ]; then 55 | $SUDO /usr/sbin/bluetoothd --udev 56 | elif [ -f /etc/rc.d/bluetooth ]; then 57 | $SUDO /etc/rc.d/bluetooth start 58 | else 59 | $SUDO /etc/init.d/bluetooth start 60 | fi 61 | } 62 | 63 | bt_stop() { 64 | if (bluetoothd_running_check); then 65 | $SUDO pkill -KILL bluetoothd 66 | fi 67 | 68 | $SUDO /usr/sbin/sixad-dbus-blocker & 69 | } 70 | 71 | case $1 in 72 | 73 | --start|-start|start|-s) 74 | REMOTE=0 75 | bt_device_check 76 | if (sixad_running_check); then 77 | echo "sixad is already running." 78 | echo "run '$0 --stop' to stop it" 79 | else 80 | if (modprobe_check); then #Check for root access before running, If NO access, quit 81 | bt_stop 82 | $SUDO /usr/sbin/sixad-bin $DEBUG $LEGACY $REMOTE 83 | else 84 | echo "You need admin/root access to run this application" 85 | fi 86 | fi 87 | ;; 88 | 89 | --stop|-stop|stop) 90 | $SUDO pkill -KILL sixad-sixaxis 91 | $SUDO pkill -KILL sixad-remote 92 | $SUDO pkill -TERM sixad-bin 93 | bt_start 94 | ;; 95 | 96 | --remote|-remote|remote) 97 | REMOTE=1 98 | bt_device_check 99 | if (modprobe_check); then #Check for root access before running, If NO access, quit 100 | bt_stop 101 | $SUDO /usr/sbin/sixad-bin $DEBUG $LEGACY $REMOTE 102 | else 103 | echo "You need admin/root access to run this application" 104 | fi 105 | ;; 106 | 107 | --restore|-restore|restore|-r) 108 | bt_start 109 | ;; 110 | 111 | --boot-yes) 112 | # ArchLinux 113 | if [ -f /etc/arch-release ]; then 114 | $SUDO sed '/DAEMONS=/ s/)/ sixad)/g' -i /etc/rc.conf 115 | # Gentoo 116 | elif [ -f /etc/gentoo-release ]; then 117 | $SUDO rc-update add sixad 118 | # Debian (default) 119 | else 120 | if [ -f /etc/rc2.d/S90sixad ]; then true; else $SUDO ln -s /etc/init.d/sixad /etc/rc2.d/S90sixad; fi 121 | if [ -f /etc/rc3.d/S90sixad ]; then true; else $SUDO ln -s /etc/init.d/sixad /etc/rc3.d/S90sixad; fi 122 | if [ -f /etc/rc4.d/S90sixad ]; then true; else $SUDO ln -s /etc/init.d/sixad /etc/rc4.d/S90sixad; fi 123 | if [ -f /etc/rc5.d/S90sixad ]; then true; else $SUDO ln -s /etc/init.d/sixad /etc/rc5.d/S90sixad; fi 124 | fi 125 | ;; 126 | 127 | --boot-no) 128 | # ArchLinux 129 | if [ -f /etc/arch-release ]; then 130 | $SUDO sed "s/ sixad//" -i /etc/rc.conf 131 | # Gentoo 132 | elif [ -f /etc/gentoo-release ]; then 133 | $SUDO rc-update delete sixad 134 | # Debian (default) 135 | else 136 | if [ -f /etc/rc2.d/S90sixad ]; then $SUDO rm /etc/rc2.d/S90sixad; fi 137 | if [ -f /etc/rc3.d/S90sixad ]; then $SUDO rm /etc/rc3.d/S90sixad; fi 138 | if [ -f /etc/rc4.d/S90sixad ]; then $SUDO rm /etc/rc4.d/S90sixad; fi 139 | if [ -f /etc/rc5.d/S90sixad ]; then $SUDO rm /etc/rc5.d/S90sixad; fi 140 | fi 141 | ;; 142 | 143 | --help|-help|help|-h) 144 | echo "[Qt]SixA Daemon" 145 | $0 146 | ;; 147 | 148 | --version|-version|version|-v) 149 | echo "[Qt]SixA Daemon - version 1.5.1" 150 | ;; 151 | 152 | *) 153 | echo "usage: $0 154 | 155 | command can be: 156 | -h, --help Show help (this message) 157 | -v, --version Show sixad version 158 | 159 | -s, --start Start sixad 160 | --stop Stop sixad 161 | --remote BD Remote mode 162 | 163 | -r, --restore Restore regular bluetooth 164 | 165 | --boot-yes Auto-starts sixad at boot time 166 | --boot-no Does not auto-start sixad at boot time 167 | 168 | You can also check: sixad-raw, sixad-notify" 169 | ;; 170 | 171 | esac 172 | -------------------------------------------------------------------------------- /sixad/sixad-3in1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sixad-raw.cpp 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-10 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #include "shared.h" 19 | #include "sixaxis.h" 20 | #include "uinput.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define KEYMOTE_KEY_SELECT 1 << 0 30 | #define KEYMOTE_KEY_START 1 << 1 31 | #define KEYMOTE_KEY_L3 1 << 2 32 | #define KEYMOTE_KEY_R3 1 << 3 33 | #define KEYMOTE_KEY_PS 1 << 4 34 | 35 | #define KEYMOTE_KEY_SQUARE 1 << 0 36 | #define KEYMOTE_KEY_CROSS 1 << 1 37 | #define KEYMOTE_KEY_CIRCLE 1 << 2 38 | #define KEYMOTE_KEY_TRIANGLE 1 << 3 39 | #define KEYMOTE_KEY_L1 1 << 4 40 | #define KEYMOTE_KEY_R1 1 << 5 41 | #define KEYMOTE_KEY_L2 1 << 6 42 | #define KEYMOTE_KEY_R2 1 << 7 43 | 44 | int main(int argc, char **argv) 45 | { 46 | int fd, nr; 47 | unsigned char buf[128]; 48 | struct uinput_fd *ufd; 49 | struct device_settings settings; 50 | 51 | if (argc < 2) { 52 | std::cout << "Usage: " << argv[0] << " /dev/hidrawX" << std::endl; 53 | return 1; 54 | } 55 | 56 | if ((fd = open(argv[1], O_RDONLY|O_NONBLOCK)) < 0) { 57 | std::cerr << "sixad-3in1::open(hidrawX) - failed to open hidraw device" << std::endl; 58 | return 1; 59 | } 60 | 61 | nr=read(fd, buf, sizeof(buf)); 62 | if (nr < 0 && errno != EAGAIN) { 63 | std::cerr << "sixad-3in1::read(fd) - failed to read from device" << std::endl; 64 | return 1; 65 | } 66 | 67 | if (nr != -1 && nr != 19) { 68 | std::cerr << "sixad-3in1::read(fd) - not a 3in1 keymote (nr = " << nr << ")" << std::endl; 69 | return 1; 70 | } 71 | 72 | open_log("sixad-3in1"); 73 | 74 | memset(&settings, 0, sizeof(device_settings)); 75 | 76 | settings.led.enabled = 0; 77 | settings.joystick.enabled = 0; 78 | settings.remote.enabled = 0; 79 | settings.input.enabled = 1; 80 | settings.input.key_select = 29; 81 | settings.input.key_l3 = 0; 82 | settings.input.key_r3 = 0; 83 | settings.input.key_start = 56; 84 | settings.input.key_up = 103; 85 | settings.input.key_right = 106; 86 | settings.input.key_down = 108; 87 | settings.input.key_left = 105; 88 | settings.input.key_l2 = 102; 89 | settings.input.key_r2 = 107; 90 | settings.input.key_l1 = 67; 91 | settings.input.key_r1 = 87; 92 | settings.input.key_tri = 14; 93 | settings.input.key_cir = 273; 94 | settings.input.key_squ = 28; 95 | settings.input.key_cro = 272; 96 | settings.input.key_ps = 42; 97 | settings.input.axis_l_type = 3; 98 | settings.input.axis_l_up = 1; 99 | settings.input.axis_l_right = 0; 100 | settings.input.axis_l_down = 0; 101 | settings.input.axis_l_left = 0; 102 | settings.input.axis_r_type = 3; 103 | settings.input.axis_r_up = 8; 104 | settings.input.axis_r_right = 6; 105 | settings.input.axis_r_down = 0; 106 | settings.input.axis_r_left = 0; 107 | settings.input.axis_speed = 9; 108 | settings.input.use_lr3 = 1; 109 | settings.rumble.enabled = 0; 110 | 111 | ufd = uinput_open(DEV_TYPE_3IN1, "3in1", settings); 112 | 113 | if (ufd->js != 0 || ufd->mk == 0) { 114 | syslog(LOG_ERR, "Error! something is not right..."); 115 | return 1; 116 | } 117 | 118 | syslog(LOG_INFO, "Connected 'Brooklyn 3in1 KeyMote'"); 119 | 120 | int b0, b1, lx, ly, rx, ry; 121 | int kUp, kDown, kLeft, kRight; 122 | int last_b1 = 0; 123 | int last_ib0 = 0; 124 | int last_ib1 = 0; 125 | int last_lx = 0; 126 | int last_ly = 0; 127 | int last_rx = 0; 128 | int last_ry = 0; 129 | int last_kUp = 0; 130 | int last_kDown = 0; 131 | int last_kLeft = 0; 132 | int last_kRight = 0; 133 | bool lr3_axis = true; 134 | bool lr3_buttons = true; 135 | int rw_timer = 0; 136 | 137 | while (true) { 138 | nr=read(fd, buf, sizeof(buf)); 139 | 140 | if (nr == 19) { 141 | // read successful 142 | b0 = buf[0]; 143 | b1 = buf[1]; 144 | lx = buf[3] - 128; 145 | ly = buf[4] - 128; 146 | rx = buf[5] - 128; 147 | ry = buf[6] - 128; 148 | kRight = buf[7]; 149 | kLeft = buf[8]; 150 | kUp = buf[9]; 151 | kDown = buf[10]; 152 | 153 | } else { 154 | if (errno != EAGAIN) { 155 | std::cerr << "sixad-3in1::read(fd, buf) - failed to read from device" << std::endl; 156 | break; 157 | } 158 | 159 | b0 = last_ib0; 160 | b1 = last_ib1; 161 | lx = last_lx; 162 | ly = last_ly; 163 | rx = last_rx; 164 | ry = last_ry; 165 | kRight = last_kRight; 166 | kLeft = last_kLeft; 167 | kUp = last_kUp; 168 | kDown = last_kDown; 169 | } 170 | 171 | //lr3 enable/disable 172 | if ((b1 & KEYMOTE_KEY_L3) && b1 != last_b1) 173 | lr3_axis = !lr3_axis; 174 | 175 | if ((b1 & KEYMOTE_KEY_R3) && b1 != last_b1) 176 | lr3_buttons = !lr3_buttons; 177 | 178 | last_b1 = b1; 179 | 180 | //buttons 181 | if (lr3_buttons) { 182 | //part1 183 | if (last_ib0 != b0) { 184 | uinput_send(ufd->mk, EV_KEY, settings.input.key_l2, b0 & KEYMOTE_KEY_L2 ? 1 : 0); 185 | uinput_send(ufd->mk, EV_KEY, settings.input.key_r2, b0 & KEYMOTE_KEY_R2 ? 1 : 0); 186 | uinput_send(ufd->mk, EV_KEY, settings.input.key_l1, b0 & KEYMOTE_KEY_L1 ? 1 : 0); 187 | uinput_send(ufd->mk, EV_KEY, settings.input.key_r1, b0 & KEYMOTE_KEY_R1 ? 1 : 0); 188 | uinput_send(ufd->mk, EV_KEY, settings.input.key_tri, b0 & KEYMOTE_KEY_TRIANGLE ? 1 : 0); 189 | uinput_send(ufd->mk, EV_KEY, settings.input.key_cir, b0 & KEYMOTE_KEY_CIRCLE ? 1 : 0); 190 | uinput_send(ufd->mk, EV_KEY, settings.input.key_cro, b0 & KEYMOTE_KEY_CROSS ? 1 : 0); 191 | uinput_send(ufd->mk, EV_KEY, settings.input.key_squ, b0 & KEYMOTE_KEY_SQUARE ? 1 : 0); 192 | } 193 | //part2 194 | if (last_ib1 != b1) { 195 | uinput_send(ufd->mk, EV_KEY, settings.input.key_select, b1 & KEYMOTE_KEY_SELECT ? 1 : 0); 196 | uinput_send(ufd->mk, EV_KEY, settings.input.key_start, b1 & KEYMOTE_KEY_START ? 1 : 0); 197 | } 198 | uinput_send(ufd->mk, EV_KEY, settings.input.key_up, kUp ? 1 : 0); 199 | uinput_send(ufd->mk, EV_KEY, settings.input.key_right, kRight ? 1 : 0); 200 | uinput_send(ufd->mk, EV_KEY, settings.input.key_down, kDown ? 1 : 0); 201 | uinput_send(ufd->mk, EV_KEY, settings.input.key_left, kLeft ? 1 : 0); 202 | } 203 | 204 | //axis 205 | if (lr3_axis) { 206 | bool rw_do; 207 | 208 | if (rw_timer%(settings.input.axis_speed*2) == 0) 209 | rw_do = true; 210 | else 211 | rw_do = false; 212 | 213 | uinput_send(ufd->mk, EV_REL, REL_X, lx/4/settings.input.axis_speed); 214 | uinput_send(ufd->mk, EV_REL, REL_Y, ly/4/settings.input.axis_speed); 215 | 216 | if (rw_do) { 217 | rx = rx/20; 218 | ry = ry/20; 219 | ry = -ry; //Inverted 220 | 221 | uinput_send(ufd->mk, EV_REL, REL_HWHEEL, rx); 222 | uinput_send(ufd->mk, EV_REL, REL_WHEEL, ry); 223 | } 224 | } 225 | 226 | last_ib0 = b0; 227 | last_ib1 = b1; 228 | last_lx = lx; 229 | last_ly = ly; 230 | last_rx = rx; 231 | last_ry = ry; 232 | last_kUp = kUp; 233 | last_kDown = kDown; 234 | last_kLeft = kLeft; 235 | last_kRight = kRight; 236 | 237 | uinput_send(ufd->mk, EV_SYN, SYN_REPORT, 0); 238 | 239 | if (rw_timer > 0xff) 240 | rw_timer = 0; 241 | else 242 | rw_timer += 1; 243 | 244 | if (nr != 19) 245 | usleep(10000); 246 | } 247 | 248 | uinput_close(ufd->mk, 0); 249 | 250 | std::cerr << "sixad-3in1::read(buf) - connection has been broken" << std::endl; 251 | 252 | delete ufd; 253 | 254 | return 0; 255 | } 256 | -------------------------------------------------------------------------------- /sixad/sixad-bin.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sixad-bin.cpp 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-10 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #include "bluetooth.h" 19 | #include "shared.h" 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #if 0 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | static struct hci_dev_info di; 34 | 35 | static void cmd_reset(int ctl, int hdev) 36 | { 37 | /* Reset HCI device */ 38 | ioctl(ctl, HCIDEVUP, hdev); 39 | ioctl(ctl, HCIDEVDOWN, hdev); 40 | } 41 | #endif 42 | 43 | static void sig_hup(int sig) 44 | { 45 | } 46 | 47 | int main(int argc, char *argv[]) 48 | { 49 | struct sigaction sa; 50 | bdaddr_t bdaddr; 51 | int ctl, csk, isk, debug, legacy, remote; 52 | 53 | if (argc > 3) { 54 | debug = atoi(argv[1]); 55 | legacy = atoi(argv[2]); 56 | remote = atoi(argv[3]); 57 | } else { 58 | std::cerr << argv[0] << " requires 'sixad'. Please run sixad instead" << std::endl; 59 | return 1; 60 | } 61 | 62 | 63 | #if 0 64 | // Enable all bluetooth adapters 65 | int hci_ctl; 66 | if ((hci_ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) >= 0) { 67 | for (int i=0; i < 4; i++) 68 | { 69 | di.dev_id = i; 70 | if (ioctl(hci_ctl, HCIGETDEVINFO, (void *) &di) == 0) 71 | { 72 | if (hci_test_bit(HCI_RAW, &di.flags) && !bacmp(&di.bdaddr, BDADDR_ANY)) { 73 | int dd = hci_open_dev(di.dev_id); 74 | hci_read_bd_addr(dd, &di.bdaddr, 1000); 75 | hci_close_dev(dd); 76 | } 77 | } 78 | cmd_reset(hci_ctl, di.dev_id); 79 | } 80 | } 81 | #endif 82 | 83 | open_log("sixad-bin"); 84 | syslog(LOG_INFO, "started"); 85 | bacpy(&bdaddr, BDADDR_ANY); 86 | 87 | ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP); 88 | if (ctl < 0) { 89 | syslog(LOG_ERR, "Can't open HIDP control socket"); 90 | close(ctl); 91 | return 1; 92 | } 93 | 94 | if (remote) { 95 | // BD Remote only 96 | 97 | syslog(LOG_INFO, "BD Remote mode active, hold Enter+Start on your remote now"); 98 | 99 | while (!io_canceled()) { 100 | do_search(ctl, &bdaddr, debug); 101 | sleep(2); 102 | } 103 | 104 | } else { 105 | // Normal behaviour 106 | 107 | csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, L2CAP_LM_MASTER, 10); 108 | if (csk < 0) { 109 | syslog(LOG_ERR, "Can't listen on HID control channel"); 110 | close(csk); 111 | close(ctl); 112 | return 1; 113 | } 114 | 115 | isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, L2CAP_LM_MASTER, 10); 116 | if (isk < 0) { 117 | syslog(LOG_ERR, "Can't listen on HID interrupt channel"); 118 | close(isk); 119 | close(csk); 120 | close(ctl); 121 | return 1; 122 | } 123 | 124 | memset(&sa, 0, sizeof(sa)); 125 | sa.sa_flags = SA_NOCLDSTOP; 126 | 127 | sa.sa_handler = sig_term; 128 | sigaction(SIGTERM, &sa, NULL); 129 | sigaction(SIGINT, &sa, NULL); 130 | sa.sa_handler = sig_hup; 131 | sigaction(SIGHUP, &sa, NULL); 132 | 133 | sa.sa_handler = SIG_IGN; 134 | sigaction(SIGCHLD, &sa, NULL); 135 | sigaction(SIGPIPE, &sa, NULL); 136 | 137 | syslog(LOG_INFO, "sixad started, press the PS button now"); 138 | 139 | hid_server(ctl, csk, isk, debug, legacy); 140 | 141 | close(isk); 142 | close(csk); 143 | } 144 | 145 | close(ctl); 146 | syslog(LOG_INFO, "Done"); 147 | 148 | return 0; 149 | } 150 | -------------------------------------------------------------------------------- /sixad/sixad-dbus-blocker: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import os 5 | import dbus 6 | import dbus.service 7 | from dbus.mainloop.glib import DBusGMainLoop 8 | from time import sleep 9 | 10 | class BluezBlockService(dbus.service.Object): 11 | def __init__(self): 12 | bus_name = dbus.service.BusName('org.bluez', bus=dbus.SystemBus()) 13 | dbus.service.Object.__init__(self, bus_name, '/org/bluez') 14 | 15 | DBusGMainLoop(set_as_default=True) 16 | dbus_blocker = BluezBlockService() 17 | 18 | if os.path.exists("/tmp/.sixad-dbus-blocker.pid"): 19 | os.remove("/tmp/.sixad-dbus-blocker.pid") 20 | 21 | os.mknod("/tmp/.sixad-dbus-blocker.pid") 22 | 23 | while True: 24 | if os.path.exists("/tmp/.sixad-dbus-blocker.pid"): 25 | sleep(1) 26 | else: 27 | break 28 | 29 | -------------------------------------------------------------------------------- /sixad/sixad-raw.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sixad-raw.cpp 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-10 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #include "shared.h" 19 | #include "sixaxis.h" 20 | #include "uinput.h" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | int main(int argc, char **argv) 27 | { 28 | int i, fd, nr; 29 | unsigned char buf[128]; 30 | struct uinput_fd *ufd; 31 | struct device_settings settings; 32 | 33 | if (argc < 2) { 34 | std::cout << "Usage: " << argv[0] << " /dev/hidrawX" << std::endl; 35 | return 1; 36 | } 37 | 38 | if ((fd = open(argv[1], O_RDONLY)) < 0) { 39 | std::cerr << "sixad-raw::open(hidrawX) - failed to open hidraw device" << std::endl; 40 | return 1; 41 | } 42 | 43 | if ((nr=read(fd, buf, sizeof(buf))) < 0) { 44 | std::cerr << "sixad-raw::read(fd) - failed to read from device" << std::endl; 45 | return 1; 46 | } 47 | 48 | if (nr < 49 || nr > 50) { 49 | std::cerr << "sixad-raw::read(fd) - not a sixaxis (nr = " << nr << ")" << std::endl; 50 | return 1; 51 | } 52 | 53 | open_log("sixad-raw"); 54 | settings = init_values("hidraw"); 55 | 56 | // hidraw has no rumble/led support 57 | settings.remote.enabled = false; 58 | settings.led.enabled = false; 59 | settings.rumble.enabled = false; 60 | 61 | ufd = uinput_open(DEV_TYPE_SIXAXIS, "hidraw", settings); 62 | 63 | if (ufd->js < 0 || ufd->mk < 0) { 64 | return 1; 65 | } else if (ufd->js == 0 && ufd->mk == 0) { 66 | syslog(LOG_ERR, "sixaxis config has no joystick or input mode selected - please choose one!"); 67 | return 1; 68 | } 69 | 70 | bool msg = true; 71 | while (true) { 72 | 73 | nr=read(fd, buf, sizeof(buf)); 74 | 75 | if (nr < 49 || nr > 50) { 76 | std::cerr << "sixad-raw::read(fd, buf) - failed to read from device" << std::endl; 77 | break; 78 | } else if (nr == 49) { 79 | for (i=50; i>0; i--) { 80 | buf[i] = buf[i-1]; 81 | } 82 | } 83 | 84 | if (msg) { 85 | syslog(LOG_INFO, "Connected 'PLAYSTATION(R)3 Controller (hidraw)' [Battery %02X]", buf[31]); 86 | if (nr == 49) syslog(LOG_INFO, "Notice: non-standard Sixaxis buffer size (49)"); 87 | msg = false; 88 | } 89 | 90 | if (settings.joystick.enabled) do_joystick(ufd->js, buf, settings.joystick); 91 | if (settings.input.enabled) do_input(ufd->mk, buf, settings.input); 92 | } 93 | 94 | if (settings.joystick.enabled) { 95 | uinput_close(ufd->js, 0); 96 | } 97 | if (settings.input.enabled) { 98 | uinput_close(ufd->mk, 0); 99 | } 100 | 101 | std::cerr << "sixad-raw::read(buf) - connection has been broken" << std::endl; 102 | 103 | delete ufd; 104 | 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /sixad/sixad-remote.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * sixad-sixaxis.cpp 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2011 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #include "shared.h" 19 | #include "remote.h" 20 | #include "uinput.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | int csk = 0; 32 | int isk = 1; 33 | int debug; 34 | int timeout = 30; 35 | 36 | volatile bool active = false; 37 | 38 | struct uinput_fd *ufd; 39 | 40 | static int get_time() 41 | { 42 | timespec tp; 43 | if (!clock_gettime(CLOCK_MONOTONIC, &tp)) { 44 | return tp.tv_sec/60; 45 | } else { 46 | return -1; 47 | } 48 | } 49 | 50 | static void process_remote(struct device_settings settings, const char *mac, int modes) 51 | { 52 | int br; 53 | bool msg = true; 54 | unsigned char buf[128]; 55 | 56 | int last_time_action = get_time(); 57 | 58 | while (!io_canceled()) { 59 | br = read(isk, buf, sizeof(buf)); 60 | if (msg) { 61 | syslog(LOG_INFO, "Connected 'PLAYSTATION(R)3 Remote (%s)'", mac); 62 | msg = false; 63 | } 64 | 65 | if (settings.timeout.enabled) { 66 | int current_time = get_time(); 67 | if (was_active()) { 68 | last_time_action = current_time; 69 | set_active(false); 70 | } else if (current_time-last_time_action >= settings.timeout.timeout) { 71 | syslog(LOG_INFO, "Remote was not in use, and timeout reached, disconneting..."); 72 | sig_term(0); 73 | break; 74 | } 75 | } 76 | 77 | if (br < 0) { 78 | break; 79 | } else if (br==13 && buf[0]==0xa1 && buf[1]==0x01) { //only continue if we've got a Remote 80 | if (settings.joystick.enabled) do_joystick(ufd->js, buf, settings.joystick); 81 | if (settings.remote.enabled) do_remote(ufd->mk, buf, modes); 82 | if (settings.input.enabled) do_input(ufd->mk, buf, settings.input); 83 | } else { 84 | if (debug) syslog(LOG_ERR, "Non-Remote packet received and ignored (0x%02x|0x%02x|0x%02x)", buf[0], buf[1], buf[2]); 85 | } 86 | } 87 | 88 | if (debug) syslog(LOG_ERR, "Read loop was broken on the Remote process"); 89 | } 90 | 91 | int main(int argc, char *argv[]) 92 | { 93 | struct pollfd p[3]; 94 | struct timespec timeout; 95 | struct device_settings settings; 96 | struct sigaction sa; 97 | sigset_t sigs; 98 | short events; 99 | 100 | if (argc < 3) { 101 | std::cout << "Running " << argv[0] << " requires 'sixad'. Please run sixad instead" << std::endl; 102 | return 1; 103 | } 104 | 105 | const char *mac = argv[1]; 106 | debug = atoi(argv[2]); 107 | 108 | open_log("sixad-remote"); 109 | settings = init_values(mac); 110 | settings.joystick.axis = false; 111 | settings.joystick.sbuttons = false; 112 | settings.joystick.accel = false; 113 | settings.joystick.speed = false; 114 | settings.joystick.pos = false;; 115 | settings.led.enabled = false; 116 | settings.rumble.enabled = false; 117 | 118 | ufd = uinput_open(DEV_TYPE_REMOTE, mac, settings); 119 | 120 | if (ufd->js < 0 || ufd->mk < 0) { 121 | return 1; 122 | } else if (ufd->js == 0 && ufd->mk == 0) { 123 | syslog(LOG_ERR, "remote config has no joystick or input mode selected - please choose one!"); 124 | return 1; 125 | } 126 | 127 | int modes = 0; 128 | if (settings.remote.numeric) modes |= REMOTE_KEYMODE_NUMBERIC; 129 | if (settings.remote.dvd) modes |= REMOTE_KEYMODE_DVD; 130 | if (settings.remote.directional) modes |= REMOTE_KEYMODE_DIRECTIONAL; 131 | if (settings.remote.multimedia) modes |= REMOTE_KEYMODE_MULTIMEDIA; 132 | 133 | sigfillset(&sigs); 134 | // sigdelset(&sigs, SIGCHLD); 135 | // sigdelset(&sigs, SIGPIPE); 136 | // sigdelset(&sigs, SIGTERM); 137 | // sigdelset(&sigs, SIGINT); 138 | // sigdelset(&sigs, SIGHUP); 139 | 140 | memset(&sa, 0, sizeof(sa)); 141 | sa.sa_flags = SA_NOCLDSTOP; 142 | 143 | sa.sa_handler = sig_term; 144 | sigaction(SIGTERM, &sa, NULL); 145 | sigaction(SIGINT, &sa, NULL); 146 | 147 | sa.sa_handler = SIG_IGN; 148 | sigaction(SIGCHLD, &sa, NULL); 149 | sigaction(SIGPIPE, &sa, NULL); 150 | 151 | if (debug) syslog(LOG_INFO, "Press any to activate"); 152 | 153 | p[0].fd = 0; 154 | p[0].events = POLLIN | POLLERR | POLLHUP; 155 | 156 | p[1].fd = 1; 157 | p[1].events = POLLIN | POLLERR | POLLHUP; 158 | 159 | p[2].fd = ufd->mk ? ufd->mk : ufd->js; 160 | p[2].events = POLLIN | POLLERR | POLLHUP; 161 | 162 | while (!io_canceled()) { 163 | int i, idx = 3; 164 | for (i = 0; i < idx; i++) 165 | p[i].revents = 0; 166 | 167 | timeout.tv_sec = 1; 168 | timeout.tv_nsec = 0; 169 | 170 | if (ppoll(p, idx, &timeout, &sigs) < 1) 171 | continue; 172 | 173 | if (p[1].revents & POLLIN) { 174 | process_remote(settings, mac, modes); 175 | } 176 | 177 | events = p[0].revents | p[1].revents | p[2].revents; 178 | 179 | if (events & (POLLERR | POLLHUP)) { 180 | break; 181 | } 182 | } 183 | 184 | if (debug) syslog(LOG_INFO, "Closing uinput..."); 185 | 186 | if (settings.joystick.enabled) { 187 | uinput_close(ufd->js, debug); 188 | } 189 | if (settings.remote.enabled || settings.input.enabled) { 190 | uinput_close(ufd->mk, debug); 191 | } 192 | 193 | delete ufd; 194 | 195 | shutdown(isk, SHUT_RDWR); 196 | shutdown(csk, SHUT_RDWR); 197 | 198 | if (debug) syslog(LOG_INFO, "Done"); 199 | 200 | return 0; 201 | } 202 | -------------------------------------------------------------------------------- /sixad/sixad.default: -------------------------------------------------------------------------------- 1 | # System-wide options for sixad 2 | DEBUG=0 3 | LEGACY=0 4 | -------------------------------------------------------------------------------- /sixad/sixad.init: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ### BEGIN INIT INFO 3 | # Provides: sixad 4 | # Required-Start: $local_fs $syslog $remote_fs bluetooth 5 | # Required-Stop: $local_fs $syslog $remote_fs 6 | # Default-Start: 2 3 4 5 7 | # Default-Stop: 0 1 6 8 | # Short-Description: Start sixad 9 | ### END INIT INFO 10 | # 11 | # Author: falkTX 12 | # 13 | # set -e 14 | 15 | PATH=/bin:/usr/bin:/sbin:/usr/sbin 16 | DAEMON=/usr/bin/sixad 17 | 18 | sixad_already_running_check () { 19 | ps -e | grep sixad-bin > /dev/null 20 | } 21 | 22 | . /lib/lsb/init-functions 23 | 24 | case "$1" in 25 | start) 26 | if (sixad_already_running_check "$1"); then 27 | log_warning_msg "sixad is already running" 28 | else 29 | { 30 | log_daemon_msg "Starting sixad" 31 | $DAEMON --start &>>/var/log/sixad & 32 | log_end_msg 0 33 | } 34 | fi 35 | ;; 36 | stop) 37 | if (sixad_already_running_check "$1"); then 38 | { 39 | log_daemon_msg "Stopping sixad" 40 | $DAEMON --stop || true 41 | log_end_msg 0 42 | } 43 | else 44 | log_warning_msg "sixad is not running" 45 | fi 46 | ;; 47 | restart) 48 | $0 stop 49 | $0 start 50 | ;; 51 | status) 52 | status_of_proc "sixad-bin" "sixad" && exit 0 || exit $? 53 | ;; 54 | *) 55 | echo "Usage: /etc/init.d/sixad {start|stop|restart|status}" >&2 56 | exit 1 57 | ;; 58 | esac 59 | 60 | exit 0 61 | -------------------------------------------------------------------------------- /sixad/sixad.log: -------------------------------------------------------------------------------- 1 | /var/log/sixad.log { 2 | rotate 10 3 | size 50M 4 | missingok 5 | compress 6 | } 7 | -------------------------------------------------------------------------------- /sixad/sixaxis.h: -------------------------------------------------------------------------------- 1 | /* 2 | * sixaxis.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-10 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef SIXAXIS_H 19 | #define SIXAXIS_H 20 | 21 | #define INPUT_TYPE_KEYS 2 22 | #define INPUT_TYPE_MOUSE 3 23 | 24 | #define SIXAXIS_KEY_SELECT 0x01 25 | #define SIXAXIS_KEY_L3 0x02 26 | #define SIXAXIS_KEY_R3 0x04 27 | #define SIXAXIS_KEY_START 0x08 28 | #define SIXAXIS_KEY_UP 0x10 29 | #define SIXAXIS_KEY_RIGHT 0x20 30 | #define SIXAXIS_KEY_DOWN 0x40 31 | #define SIXAXIS_KEY_LEFT 0x80 32 | 33 | #define SIXAXIS_KEY_L2 0x01 34 | #define SIXAXIS_KEY_R2 0x02 35 | #define SIXAXIS_KEY_L1 0x04 36 | #define SIXAXIS_KEY_R1 0x08 37 | #define SIXAXIS_KEY_TRIANGLE 0x10 38 | #define SIXAXIS_KEY_CIRCLE 0x20 39 | #define SIXAXIS_KEY_CROSS 0x40 40 | #define SIXAXIS_KEY_SQUARE 0x80 41 | 42 | #define SIXAXIS_KEY_PS 0x01 43 | 44 | struct rumble_effect { 45 | int id; 46 | int weak; 47 | int strong; 48 | int timeout; 49 | }; 50 | 51 | void do_joystick(int fd, unsigned char* buf, struct dev_joystick joystick); 52 | void do_input(int fd, unsigned char* buf, struct dev_input input); 53 | void do_rumble(int csk, int led_n, int weak, int strong, int timeout); 54 | 55 | int set_sixaxis_led(int csk, struct dev_led led, int rumble); 56 | 57 | #endif // SIXAXIS_H 58 | -------------------------------------------------------------------------------- /sixad/textfile.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * textfile.cpp 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-2010 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #include "textfile.h" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | inline char *find_key(char *map, size_t size, const char *key, size_t len, int icase) 30 | { 31 | char *ptr = map; 32 | size_t ptrlen = size; 33 | 34 | while (ptrlen > len + 1) { 35 | int cmp = (icase) ? strncasecmp(ptr, key, len) : strncmp(ptr, key, len); 36 | if (cmp == 0) { 37 | if (ptr == map) 38 | return ptr; 39 | 40 | if ((*(ptr - 1) == '\r' || *(ptr - 1) == '\n') && 41 | *(ptr + len) == ' ') 42 | return ptr; 43 | } 44 | 45 | if (icase) { 46 | char *p1 = (char*)memchr(ptr + 1, tolower(*key), ptrlen - 1); 47 | char *p2 = (char*)memchr(ptr + 1, toupper(*key), ptrlen - 1); 48 | 49 | if (!p1) 50 | ptr = p2; 51 | else if (!p2) 52 | ptr = p1; 53 | else 54 | ptr = (p1 < p2) ? p1 : p2; 55 | } else 56 | ptr = (char*)memchr(ptr + 1, *key, ptrlen - 1); 57 | 58 | if (!ptr) 59 | return NULL; 60 | 61 | ptrlen = size - (ptr - map); 62 | } 63 | 64 | return NULL; 65 | } 66 | 67 | char *read_key(const char *pathname, const char *key, int icase) 68 | { 69 | struct stat st; 70 | char *map, *off, *end, *str = NULL; 71 | off_t size; size_t len; 72 | int fd, err = 0; 73 | 74 | fd = open(pathname, O_RDONLY); 75 | if (fd < 0) 76 | return NULL; 77 | 78 | if (flock(fd, LOCK_SH) < 0) { 79 | err = errno; 80 | goto close; 81 | } 82 | 83 | if (fstat(fd, &st) < 0) { 84 | err = errno; 85 | goto unlock; 86 | } 87 | 88 | size = st.st_size; 89 | 90 | map = (char*)mmap(NULL, size, PROT_READ, MAP_SHARED, fd, 0); 91 | if (!map || map == MAP_FAILED) { 92 | err = errno; 93 | goto unlock; 94 | } 95 | 96 | len = strlen(key); 97 | off = find_key(map, size, key, len, icase); 98 | if (!off) { 99 | err = EILSEQ; 100 | goto unmap; 101 | } 102 | 103 | end = strpbrk(off, "\r\n"); 104 | if (!end) { 105 | err = EILSEQ; 106 | goto unmap; 107 | } 108 | 109 | str = (char*)malloc(end - off - len); 110 | if (!str) { 111 | err = EILSEQ; 112 | goto unmap; 113 | } 114 | 115 | memset(str, 0, end - off - len); 116 | strncpy(str, off + len + 1, end - off - len - 1); 117 | 118 | unmap: 119 | munmap(map, size); 120 | 121 | unlock: 122 | flock(fd, LOCK_UN); 123 | 124 | close: 125 | close(fd); 126 | errno = err; 127 | 128 | return str; 129 | } 130 | 131 | int textfile_get_int(const char *pathname, const char *key, int default_key) 132 | { 133 | char* ret_key = read_key(pathname, key, 0); 134 | if (ret_key != NULL) 135 | return atoi(ret_key); 136 | else 137 | return default_key; 138 | } 139 | -------------------------------------------------------------------------------- /sixad/textfile.h: -------------------------------------------------------------------------------- 1 | /* 2 | * textfile.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-2010 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef TEXTFILE_H 19 | #define TEXTFILE_H 20 | 21 | #include 22 | 23 | inline char *find_key(char *map, size_t size, const char *key, size_t len, int icase); 24 | char *read_key(const char *pathname, const char *key, int icase); 25 | int textfile_get_int(const char *pathname, const char *key, int default_key); 26 | 27 | #endif // TEXTFILE_H 28 | -------------------------------------------------------------------------------- /sixad/uinput.h: -------------------------------------------------------------------------------- 1 | /* 2 | * uinput.h 3 | * 4 | * This file is part of the QtSixA, the Sixaxis Joystick Manager 5 | * Copyright 2008-2011 Filipe Coelho 6 | * 7 | * QtSixA can be redistributed and/or modified under the terms of the GNU General 8 | * Public License (Version 2), as published by the Free Software Foundation. 9 | * A copy of the license is included in the QtSixA source code, or can be found 10 | * online at www.gnu.org/licenses. 11 | * 12 | * QtSixA is distributed in the hope that it will be useful, but WITHOUT ANY 13 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 14 | * A PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | * 16 | */ 17 | 18 | #ifndef UINPUT_H 19 | #define UINPUT_H 20 | 21 | #include 22 | #include 23 | 24 | #define MAX_RUMBLE_EFFECTS 16 25 | 26 | #define DEV_TYPE_SIXAXIS 1 27 | #define DEV_TYPE_REMOTE 2 28 | #define DEV_TYPE_3IN1 3 29 | 30 | #define AXIS_PADDING 10 31 | 32 | struct uinput_fd { 33 | int js; 34 | int mk; 35 | }; 36 | 37 | struct uinput_fd *uinput_open(int DEV_TYPE, const char *mac, struct device_settings settings); 38 | int uinput_close(int fd, int debug=0); 39 | int uinput_send(int fd, unsigned short type, unsigned short code, int value); 40 | 41 | #endif // UINPUT_H 42 | -------------------------------------------------------------------------------- /sixpair/sixpair: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dusty-nv/turbo2/c56e8518f2c1eb65b0ae6c3c128a0749ced6199c/sixpair/sixpair -------------------------------------------------------------------------------- /sixpair/sixpair.c: -------------------------------------------------------------------------------- 1 | /* 2 | * sixpair.c version 2007-04-18 3 | * Compile with: gcc -o sixpair sixpair.c -lusb 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; version 2. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program; if not, write to the Free Software 16 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 17 | * 18 | */ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #define VENDOR 0x054c 26 | #define PRODUCT 0x0268 27 | 28 | #define USB_DIR_IN 0x80 29 | #define USB_DIR_OUT 0 30 | 31 | void fatal(char *msg) { perror(msg); exit(1); } 32 | 33 | void show_master(usb_dev_handle *devh, int itfnum) { 34 | printf("Current Bluetooth master: "); 35 | unsigned char msg[8]; 36 | int res = usb_control_msg 37 | (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 38 | 0x01, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000); 39 | if ( res < 0 ) { perror("USB_REQ_GET_CONFIGURATION"); return; } 40 | printf("%02x:%02x:%02x:%02x:%02x:%02x\n", 41 | msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]); 42 | } 43 | 44 | void set_master(usb_dev_handle *devh, int itfnum, int mac[6]) { 45 | printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n", 46 | mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); 47 | char msg[8]= { 0x01, 0x00, mac[0],mac[1],mac[2],mac[3],mac[4],mac[5] }; 48 | int res = usb_control_msg 49 | (devh, 50 | USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 51 | 0x09, 52 | 0x03f5, itfnum, msg, sizeof(msg), 53 | 5000); 54 | if ( res < 0 ) fatal("USB_REQ_SET_CONFIGURATION"); 55 | } 56 | 57 | void process_device(int argc, char **argv, struct usb_device *dev, 58 | struct usb_config_descriptor *cfg, int itfnum) { 59 | int mac[6], have_mac=0; 60 | 61 | usb_dev_handle *devh = usb_open(dev); 62 | if ( ! devh ) fatal("usb_open"); 63 | 64 | usb_detach_kernel_driver_np(devh, itfnum); 65 | 66 | int res = usb_claim_interface(devh, itfnum); 67 | if ( res < 0 ) fatal("usb_claim_interface"); 68 | 69 | show_master(devh, itfnum); 70 | 71 | if ( argc >= 2 ) { 72 | if ( sscanf(argv[1], "%x:%x:%x:%x:%x:%x", 73 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 74 | 75 | printf("usage: %s []\n", argv[0]); 76 | exit(1); 77 | } 78 | } else { 79 | FILE *f = popen("hcitool dev", "r"); 80 | if ( !f || 81 | fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x", 82 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 83 | printf("Unable to retrieve local bd_addr from `hcitool dev`.\n"); 84 | printf("Please enable Bluetooth or specify an address manually.\n"); 85 | exit(1); 86 | } 87 | pclose(f); 88 | } 89 | 90 | set_master(devh, itfnum, mac); 91 | 92 | usb_close(devh); 93 | } 94 | 95 | int main(int argc, char *argv[]) { 96 | 97 | usb_init(); 98 | if ( usb_find_busses() < 0 ) fatal("usb_find_busses"); 99 | if ( usb_find_devices() < 0 ) fatal("usb_find_devices"); 100 | struct usb_bus *busses = usb_get_busses(); 101 | if ( ! busses ) fatal("usb_get_busses"); 102 | 103 | int found = 0; 104 | 105 | struct usb_bus *bus; 106 | for ( bus=busses; bus; bus=bus->next ) { 107 | struct usb_device *dev; 108 | for ( dev=bus->devices; dev; dev=dev->next) { 109 | struct usb_config_descriptor *cfg; 110 | for ( cfg = dev->config; 111 | cfg < dev->config + dev->descriptor.bNumConfigurations; 112 | ++cfg ) { 113 | int itfnum; 114 | for ( itfnum=0; itfnumbNumInterfaces; ++itfnum ) { 115 | struct usb_interface *itf = &cfg->interface[itfnum]; 116 | struct usb_interface_descriptor *alt; 117 | for ( alt = itf->altsetting; 118 | alt < itf->altsetting + itf->num_altsetting; 119 | ++alt ) { 120 | if ( dev->descriptor.idVendor == VENDOR && 121 | dev->descriptor.idProduct == PRODUCT && 122 | alt->bInterfaceClass == 3 ) { 123 | process_device(argc, argv, dev, cfg, itfnum); 124 | ++found; 125 | } 126 | } 127 | } 128 | } 129 | } 130 | } 131 | 132 | if ( ! found ) printf("No controller found on USB busses.\n"); 133 | return 0; 134 | 135 | } 136 | 137 | -------------------------------------------------------------------------------- /v4l2Camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rover 3 | */ 4 | 5 | #ifndef __V4L2_CAPTURE_H 6 | #define __V4L2_CAPTURE_H 7 | 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | 17 | struct v4l2_mmap 18 | { 19 | struct v4l2_buffer buf; 20 | void* ptr; 21 | }; 22 | 23 | 24 | /** 25 | * Video4Linux2 camera capture streaming. 26 | */ 27 | class v4l2Camera 28 | { 29 | public: 30 | /** 31 | * Create V4L2 interface 32 | * @param path Filename of the video device (e.g. /dev/video0) 33 | */ 34 | static v4l2Camera* Create( const char* device_path ); 35 | 36 | /** 37 | * Destructor 38 | */ 39 | ~v4l2Camera(); 40 | 41 | /** 42 | * Start streaming 43 | */ 44 | bool Open(); 45 | 46 | /** 47 | * Stop streaming 48 | */ 49 | bool Close(); 50 | 51 | /** 52 | * Return the next image. 53 | */ 54 | void* Capture(); 55 | 56 | /** 57 | * Get width, in pixels, of camera image. 58 | */ 59 | inline uint32_t GetWidth() const { return mWidth; } 60 | 61 | /** 62 | * Retrieve height, in pixels, of camera image. 63 | */ 64 | inline uint32_t GetHeight() const { return mHeight; } 65 | 66 | /** 67 | * Return the size in bytes of one line of the image. 68 | */ 69 | inline uint32_t GetPitch() const { return mPitch; } 70 | 71 | /** 72 | * Return the bit depth per pixel. 73 | */ 74 | inline uint32_t GetPixelDepth() const { return mPixelDepth; } 75 | 76 | private: 77 | 78 | v4l2Camera( const char* device_path ); 79 | 80 | bool init(); 81 | bool initCaps(); 82 | bool initFormats(); 83 | bool initStream(); 84 | 85 | bool initUserPtr(); 86 | bool initMMap(); 87 | 88 | int mFD; 89 | int mRequestFormat; 90 | uint32_t mRequestWidth; 91 | uint32_t mRequestHeight; 92 | uint32_t mWidth; 93 | uint32_t mHeight; 94 | uint32_t mPitch; 95 | uint32_t mPixelDepth; 96 | 97 | v4l2_mmap* mBuffersMMap; 98 | size_t mBufferCountMMap; 99 | 100 | std::vector mFormats; 101 | std::string mDevicePath; 102 | }; 103 | 104 | 105 | #endif 106 | 107 | 108 | --------------------------------------------------------------------------------