├── .github └── workflows │ └── build_test.yml ├── .gitignore ├── .gitmodules ├── CHANGELOG.rst ├── LICENSE ├── README.md ├── firmware ├── 98-versa-vis.rules ├── libraries │ └── versavis │ │ ├── keywords.txt │ │ ├── library.properties │ │ └── src │ │ ├── ADIS16445.cpp │ │ ├── ADIS16445.h │ │ ├── ADIS16448AMLZ.cpp │ │ ├── ADIS16448AMLZ.h │ │ ├── ADIS16448BMLZ.cpp │ │ ├── ADIS16448BMLZ.h │ │ ├── ADIS16460.cpp │ │ ├── ADIS16460.h │ │ ├── Camera.cpp │ │ ├── Camera.h │ │ ├── Imu.cpp │ │ ├── Imu.h │ │ ├── Sensor.cpp │ │ ├── Sensor.h │ │ ├── Timer.cpp │ │ ├── Timer.h │ │ ├── VN100.cpp │ │ ├── VN100.h │ │ ├── helper.cpp │ │ ├── helper.h │ │ └── versavis_configuration.h ├── setup.sh └── versavis │ └── versavis.ino ├── image_numbered_msgs ├── CMakeLists.txt ├── msg │ └── ImageNumbered.msg └── package.xml ├── scripts ├── calculate_img_delay.py ├── calculate_trigger_rates.m ├── evaluation │ └── cam_imu.m └── show_timings.py └── versavis ├── CMakeLists.txt ├── cfg ├── basler_acA1920-155uc_master.yaml ├── basler_acA1920-155uc_slave.yaml └── chameleon3.yaml ├── include └── versavis │ └── versavis_synchronizer.h ├── launch ├── run_versavis.launch └── run_versavis_bravo.launch ├── msg ├── ImuMicro.msg └── TimeNumbered.msg ├── nodelet_plugin.xml ├── package.xml └── src ├── versavis_imu_receiver.cpp ├── versavis_synchronizer.cpp ├── versavis_synchronizer_node.cpp └── versavis_synchronizer_nodelet.cpp /.github/workflows/build_test.yml: -------------------------------------------------------------------------------- 1 | name: Build Test 2 | on: 3 | schedule: 4 | - cron: '30 18 * * *' 5 | push: 6 | branches: 7 | - 'master' 8 | pull_request: 9 | branches: 10 | - '*' 11 | jobs: 12 | build: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | fail-fast: false 16 | matrix: 17 | config: 18 | - {rosdistro: 'melodic', container: 'ros:melodic-ros-base-bionic'} 19 | - {rosdistro: 'noetic', container: 'ros:noetic-ros-base-focal'} 20 | container: ${{ matrix.config.container }} 21 | steps: 22 | - uses: actions/checkout@v1 23 | with: 24 | token: ${{ secrets.ACCESS_TOKEN }} 25 | github-token: ${{ secrets.GITHUB_TOKEN }} 26 | - name: Install catkin-tools on melodic 27 | if: ${{ matrix.config.container == 'ros:melodic-ros-base-bionic' }} 28 | run: | 29 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 30 | apt update && apt install -y python3-wstool python-catkin-tools 31 | - name: Install catkin-tools on Noetic 32 | if: ${{ matrix.config.container == 'ros:noetic-ros-base-focal' }} 33 | run: | 34 | apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 35 | apt update && apt install -y python3-pip 36 | pip3 install osrf-pycommon 37 | apt update && apt install -y python3-wstool python3-catkin-tools 38 | - name: release_build_test 39 | working-directory: 40 | run: | 41 | 42 | # apt update && apt install -y python3-wstool python3-catkin-tools 43 | mkdir -p $HOME/catkin_ws/src; 44 | cd $HOME/catkin_ws 45 | catkin init 46 | catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}" 47 | catkin config --merge-devel 48 | cd $HOME/catkin_ws/src 49 | ln -s $GITHUB_WORKSPACE 50 | cd $HOME/catkin_ws 51 | rosdep update 52 | rosdep install --from-paths src --ignore-src -y --rosdistro ${{matrix.config.rosdistro}} 53 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False 54 | catkin build -j$(nproc) -l$(nproc) versavis 55 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ros_lib 2 | firmware/libraries/ros_lib 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "dependencies/rosserial"] 2 | path = dependencies/rosserial 3 | url = https://github.com/ethz-asl/rosserial.git 4 | [submodule "dependencies/versavis_hw"] 5 | path = dependencies/versavis_hw 6 | url = https://github.com/ethz-asl/versavis_hw.git 7 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 1.0.1 (2020-03-12) 2 | ------------------ 3 | * Added support for Vectornav VN-100 4 | * Moved from recursive update of IMU message to iterative. 5 | Details in PR7_ 6 | 7 | .. _PR7: https://github.com/ethz-asl/versavis/pull/7 8 | 9 | 1.0.0 (2019-12-09) 10 | ------------------ 11 | 12 | First public *stable* release 13 | 14 | Accompanies our paper Tschopp, F., Riner, M., Fehr, M., Bernreiter, L., Furrer, F., Novkovic, T., … Nieto, J. (2020). VersaVIS—An Open Versatile Multi-Camera Visual-Inertial Sensor Suite. Sensors, 20(5), 1439. Available at: https://doi.org/10.3390/s20051439https://www.mdpi.com/1424-8220/20/5/1439. 15 | 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | VersaVIS 2 | Copyright (c) 2019, Autonomous Systems Lab / ETH Zurich 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions, the list of authors and contributors and the following 9 | disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions, the list of authors and contributors and the following 12 | disclaimer in the documentation and/or other materials provided with the 13 | distribution. 14 | * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 15 | its contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | Authors and contributors: 31 | - Florian Tschopp 32 | - Michael Riner-Kuhn 33 | - Andreas Pfrunder 34 | - Timo Hinzmann 35 | - Fadri Furrer 36 | - Tonci Novkovic 37 | - Marius Fehr 38 | - Lukas Bernreiter 39 | - Rik Bähnemann 40 | - Gabriel Waibel 41 | - Pascal Enderli 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VersaVIS -- An Open Versatile Multi-Camera Visual-Inertial Sensor Suite 2 | [![Build Test](https://github.com/ethz-asl/versavis/actions/workflows/build_test.yml/badge.svg)](https://github.com/ethz-asl/versavis/actions/workflows/build_test.yml) 3 | 4 | https://youtu.be/bsvSZJq76Mc 5 | 6 | VersaVIS provides a complete, open-source hardware, firmware and software bundle to perform time synchronization of multiple cameras with an IMU featuring exposure compensation, host clock translation and independent and stereo camera triggering. 7 | 8 | ## News 9 | * Enjoy our new [trailer video](https://youtu.be/bsvSZJq76Mc) explaining the main conecpt behind VersaVIS. 10 | * VersaVIS runs on Ubuntu 20.04 focal fossa / ROS Noetic now, look at this [issue](https://github.com/ethz-asl/versavis/issues/21#issuecomment-853007617). 11 | 12 | ## Supported camera drivers 13 | * [Basler (not open-source)](https://github.com/ethz-asl/ros_basler_camera/tree/devel/versavis) tested with acA1920-155uc 14 | * [MatrixVision](https://github.com/ethz-asl/bluefox2/tree/devel/versavis) tested with Bluefox 2 MLC200WG, needs adaption for new format 15 | * [PointGrey/Flir](https://github.com/ethz-asl/flir_camera_driver/tree/devel/versavis) tested with Chameleon 3, Blackfly S 16 | * [CamBoard](https://github.com/ethz-asl/pico_flexx_driver/tree/devel/versavis) tested with CamBoard pico monstar 17 | 18 | ## Citing 19 | 20 | Please cite the [following paper](https://www.mdpi.com/1424-8220/20/5/1439) when using VersaVIS for your research: 21 | 22 | ```bibtex 23 | @article{Tschopp2020, 24 | author = {Tschopp, Florian and Riner, Michael and Fehr, Marius and Bernreiter, Lukas and Furrer, Fadri and Novkovic, Tonci and Pfrunder, Andreas and Cadena, Cesar and Siegwart, Roland and Nieto, Juan}, 25 | doi = {10.3390/s20051439}, 26 | journal = {Sensors}, 27 | number = {5}, 28 | pages = {1439}, 29 | publisher = {Multidisciplinary Digital Publishing Institute}, 30 | title = {{VersaVIS—An Open Versatile Multi-Camera Visual-Inertial Sensor Suite}}, 31 | url = {https://www.mdpi.com/1424-8220/20/5/1439}, 32 | volume = {20}, 33 | year = {2020} 34 | } 35 | ``` 36 | 37 | Additional information can be found [here](https://docs.google.com/presentation/d/1Yi71cYtIBGUP5bFDKDFcUF2MjNS_CV3LM7W1_3jNLEs/edit?usp=sharing). 38 | ## Install 39 | 40 | ### Clone and build 41 | 42 | ``` 43 | cd ~/catkin_ws/src/ 44 | git clone git@github.com:ethz-asl/versavis.git --recursive 45 | catkin build versavis 46 | cd versavis/firmware 47 | ./setup.sh 48 | ``` 49 | 50 | ### Setup udev rule 51 | Add yourself to `dialout` group 52 | ``` 53 | sudo adduser dialout 54 | ``` 55 | 56 | Copy udev rule file to your system: 57 | ``` 58 | sudo cp firmware/98-versa-vis.rules /etc/udev/rules.d/98-versa-vis.rules 59 | ``` 60 | Afterwards, use the following commands to reload the rules 61 | ``` 62 | sudo udevadm control --reload-rules 63 | sudo udevadm trigger 64 | sudo ldconfig 65 | ``` 66 | Note: You might have to reboot your computer for this to take effect. You can check by see whether a `/dev/versavis` is available and pointing to the correct device. 67 | 68 | ### Configure 69 | Adapt the [configuration file](https://github.com/ethz-asl/versavis/blob/master/firmware/libraries/versavis/src/versavis_configuration.h) to your setup needs. Also check the [datasheet](https://drive.google.com/file/d/11QCjc5PVuMU9bAr8Kjvqz2pqVIhoMbHA/view?ts=5dc98776) for how to configure the hardware switches. 70 | 71 | ### Flash firmware on the VersaVIS board 72 | * Install the arduino IDE from [here](https://www.arduino.cc/en/Main/OldSoftwareReleases#previous). Use version 1.8.2! 73 | - Note that a small modification of the install script (`install.sh`) might be required. In particular you may need to change the line `RESOURCE_NAME=cc.arduino.arduinoide` to `RESOURCE_NAME=arduino-arduinoide` as per the issue [here](https://github.com/arduino/Arduino/issues/6116#issuecomment-290012812). 74 | * Open `firmware/versavis/versavis.ino` in the IDE 75 | * Go to `File -> Preferences` 76 | * Change Sketchbook location to `versavis/firmware/` 77 | * Install board support: 78 | - For AVI 2.1 (the green one): Tools -> Boards -> Boards Manager -> Arduino SAMD Boards (32-bits ARM Cortex-M0+) -> Use version 1.6.20! 79 | - For VersaVIS 1.0 (the black one): [Check here](https://github.com/ethz-asl/versavis_hw/) 80 | * Set `Tools -> Port -> tty/ACM0 (Arduino Zero)`, and `Tools -> Board -> VersaVIS`. 81 | * Compile using the *Verify* menu option 82 | * Flash using the *Upload* menu option 83 | 84 | ## Calibration 85 | Typically, a VI Setup needs to be carefully calibrated for camera intrinsics and camera-camera extrinsics and camera-imu extrinsics. 86 | Refer to [Kalibr](https://github.com/ethz-asl/kalibr) for a good calibration framework. Note: To enable a good calibration, a high-quality calibration target needs to be available. Furthermore, a good and uniform light source is needed in order to reduce motion blur, especially during camera-imu calibration. 87 | 88 | ## Usage 89 | * Adapt `versavis/launch/run_versavis.launch` to your needs. 90 | * Run with 91 | ``` 92 | roslaunch versavis run_versavis.launch 93 | ``` 94 | * Wait for successfull initialization. 95 | 96 | 97 | ## Troubleshooting 98 | ### My sensor is stuck at initialization 99 | Main symptoms are: 100 | * No IMU message published. 101 | * Cameras are not triggered or only very slowly (e.g. 1 Hz). 102 | * 103 | Troubleshooting steps: 104 | * Check that your camera receives a triggering signal by checking the trigger LED (Note: As the trigger pulse is very short, look for a dim flicker.). 105 | * Check that all topics are correctly set up and connected. 106 | * If the USB3 blackfly is powered over the Hirose plug, there seems to be a longer delay (0.2s+) until the image arrives on the host computer. Initialization does not succeed because it only allows successful synchronization if the image message is not older than 0.1s compared to the time message coming from the triggering board. With power over USB things seem to work fine. One can increase the threshold [kMaxImageDelayThreshold](https://github.com/ethz-asl/versavis/blob/af83f34d4471a7886a197f305dbe76603b92747a/versavis/src/versavis_synchronizer.cpp#L20) but keep in mind that `kMaxImageDelayThreshold >> 1/f_init`. Decrease initialization frequency [`f_init`](https://github.com/ethz-asl/versavis/blob/af83f34d4471a7886a197f305dbe76603b92747a/firmware/versavis/versavis.ino#L87) if necessairy. 107 | 108 | ### The board is not doing what I expect / How can I enter debug mode 109 | Easiest way to debug is to enable `DEBUG` mode in `firmware/libraries/versavis/versavis_configuration.h` and check the debug output using the Arduino Serial Monitor or `screen /dev/versavis`. 110 | Note: In debug mode, ROS/rosserial communication is deactivated! 111 | ### I don't get any IMU messages on `/versavis/imu` 112 | This is normal during initialization as no IMU messages are published. Check [Inintialization issues](https://github.com/ethz-asl/versavis#my-sensor-is-stuck-at-initialization) for further info. 113 | ### After uploading a new firmware, I am unable to communicate with the VersaVIS board 114 | This is most likely due to an infinite loop in the code in an error case. Reset the board by double clicking the reset button and upload your code in `DEBUG` mode. Then check your debug output. 115 | ### IMU shows strange data or spikes 116 | To decrease the bandwidth between VersaVIS and host but keep all information, only the IMu raw data is transferred and later scaled. If there is a scale offset, adapt the [scale/sensitivity parameters](https://github.com/ethz-asl/versavis/blob/af83f34d4471a7886a197f305dbe76603b92747a/versavis/launch/run_versavis.launch#L140) in your launch file. 117 | 118 | Depending on the IMU, recursive data grabbing is implemented with a CRC or temperature check. If this fails multiple time, the latest message is used. Check your IMU if this persists. 119 | ### It looks like my exposure time is not correctly compensated 120 | Check whether your board can correctly detect your exposure time in the [debug output](https://github.com/ethz-asl/versavis#my-sensor-is-stuck-at-initialization). 121 | Troubleshooting steps: 122 | * Enable exposure/strobe output on your camera. 123 | * Check that all dip switches are set according to the [datasheet](https://drive.google.com/file/d/11QCjc5PVuMU9bAr8Kjvqz2pqVIhoMbHA/view?ts=5dc98776). 124 | * Check that the exposure LED on the board flashes with exposure time. 125 | ### I receive errors on the host computer 126 | #### Time candidate overflow 127 | ```bash 128 | [ WARN] [1619791310.834852014]: /versavis/camO/tmage_raw: Time candidates buffer overflow at 1025. 129 | ... 130 | ``` 131 | Means that the synchronizer receives more timestamps than images. Double check if the camera is actually triggering with every pulse it receives. A typical problem is when the exposure time is higher than the measurement period. 132 | 133 | #### Image candidate overflow 134 | ```bash 135 | [ WARN] [1619791310.834852014]: /versavis/camO/tmage_raw: Image candidates buffer overflow at 1025. 136 | ... 137 | ``` 138 | Means that the synchronizer receives more iamges than timestamps. Double check if the camera is actually triggering and not free running. 139 | -------------------------------------------------------------------------------- /firmware/98-versa-vis.rules: -------------------------------------------------------------------------------- 1 | SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="824d", SYMLINK+="versavis", GROUP="dialout", MODE="0666" 2 | SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="804d", SYMLINK+="versavis", GROUP="dialout", MODE="0666" 3 | SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="004d", SYMLINK+="versavis", GROUP="dialout", MODE="0666" 4 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/keywords.txt: -------------------------------------------------------------------------------- 1 | ADIS16445 KEYWORD1 2 | ADIS16460 KEYWORD1 3 | ADIS16448AMLZ KEYWORD1 4 | ADIS16448BMLZ KEYWORD1 5 | VN100 KEYWORD1 6 | Camera KEYWORD1 7 | Imu KEYWORD1 8 | Sensor KEYWORD1 9 | Timer KEYWORD1 10 | error KEYWORD2 11 | DEBUG_INIT KEYWORD2 12 | DEBUG_PRINT KEYWORD2 13 | DEBUG_PRINTDEC KEYWORD2 14 | DEBUG_PRINTLN KEYWORD2 15 | DEBUG_PRINTDECLN KEYWORD2 16 | setup KEYWORD2 17 | begin KEYWORD2 18 | initialize KEYWORD2 19 | isInitialized KEYWORD2 20 | exposurePin KEYWORD2 21 | publish KEYWORD2 22 | triggerMeasurement KEYWORD2 23 | exposureEnd KEYWORD2 24 | isExposing KEYWORD2 25 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/library.properties: -------------------------------------------------------------------------------- 1 | name=versavis 2 | version=1.0.0 3 | author=Florian Tschopp 4 | maintainer=Florian Tschopp 5 | sentence=Enables triggering cameras and IMUs for VI operation. 6 | paragraph= 7 | category=Sensors 8 | url=https://github.com/ethz-asl/versavis 9 | architectures=samd 10 | includes=versavis_configuration.h,helper.h,Camera.h,Imu.h,Timer.h,ADIS16445.h,ADIS16448AMLZ.h,ADIS16448BMLZ.h,ADIS16460.h,VN100.h 11 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16445.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // July 2015 3 | // Author: Juan Jose Chong 4 | // Adapted for ADIS16445 by Florian Tschopp 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16445.cpp 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the ADIS16445 11 | // IMU with an 8-Bit Atmel-based Arduino development board. Functions for SPI 12 | // configuration, reads and writes, and scaling are included. This library may 13 | // be used for the entire ADIS164XX family of devices with some modification. 14 | // 15 | // This example is free software. You can redistribute it and/or modify it 16 | // under the terms of the GNU Lesser Public License as published by the Free 17 | // Software Foundation, either version 3 of the License, or any later version. 18 | // 19 | // This example is distributed in the hope that it will be useful, but WITHOUT 20 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser Public License for 22 | // more details. 23 | // 24 | // You should have received a copy of the GNU Lesser Public License along with 25 | // this example. If not, see . 26 | // 27 | //////////////////////////////////////////////////////////////////////////// 28 | 29 | #include "ADIS16445.h" 30 | #include "helper.h" 31 | #include "versavis_configuration.h" 32 | 33 | //////////////////////////////////////////////////////////////////////////// 34 | // Constructor with configurable CS, DR, and RST 35 | //////////////////////////////////////////////////////////////////////////// 36 | // CS - Chip select pin 37 | // DR - DR output pin for data ready 38 | // RST - Hardware reset pin 39 | //////////////////////////////////////////////////////////////////////////// 40 | ADIS16445::ADIS16445(ros::NodeHandle *nh, const String &topic, 41 | const int rate_hz, Timer &timer, int CS, int DR, int RST) 42 | : Imu(nh, topic, rate_hz, timer), CS_(CS), RST_(RST) { 43 | if (DR >= 0) { 44 | pinMode(DR, INPUT); // Set DR pin to be an input 45 | } 46 | 47 | if (RST_ >= 0) { 48 | pinMode(RST_, OUTPUT); // Set RST pin to be an output 49 | digitalWrite(RST_, HIGH); // Initialize RST pin to be high 50 | } 51 | } 52 | 53 | void ADIS16445::setup() { 54 | DEBUG_PRINTLN((topic_ + " (ADIS16445.cpp): Setup.").c_str()); 55 | 56 | SPI.begin(); // Initialize SPI bus 57 | configSPI(); // Configure SPI 58 | 59 | // Reset IMU. 60 | regWrite(GLOB_CMD, 0xBE80); // Perform an IMU reset. 61 | delay(300); 62 | regWrite(GLOB_CMD, 0xBE00); // Take IMU out of reset state. 63 | delay(100); 64 | configSPI(); // Configure SPI 65 | delay(100); 66 | 67 | // Set default pin states 68 | pinMode(CS_, OUTPUT); // Set CS pin to be an output 69 | pinMode(RST_, OUTPUT); // Set RST pin to be an output 70 | digitalWrite(CS_, HIGH); // Initialize CS pin to be high 71 | digitalWrite(RST_, HIGH); // Initialize RST pin to be high 72 | 73 | /* ---------------- Perform IMU configuration --------------------------- */ 74 | regWrite(MSC_CTRL, 0x6); // Enable Data Ready on IMU. 75 | delay(20); 76 | regWrite(SENS_AVG, 0x2); // Set Digital Filter on IMU. 77 | delay(20); 78 | regWrite(SMPL_PRD, 0x1); // Set Decimation on IMU. 79 | delay(20); 80 | 81 | Imu::setupPublisher(); 82 | } 83 | 84 | //////////////////////////////////////////////////////////////////////////// 85 | // Destructor 86 | //////////////////////////////////////////////////////////////////////////// 87 | ADIS16445::~ADIS16445() { 88 | // Close SPI bus 89 | SPI.end(); 90 | } 91 | 92 | //////////////////////////////////////////////////////////////////////////// 93 | // Performs a hardware reset by setting RST_ pin low for delay (in ms). 94 | //////////////////////////////////////////////////////////////////////////// 95 | int ADIS16445::resetDUT(uint8_t ms) { 96 | digitalWrite(RST_, LOW); 97 | delay(100); 98 | digitalWrite(RST_, HIGH); 99 | delay(ms); 100 | return (1); 101 | } 102 | 103 | //////////////////////////////////////////////////////////////////////////// 104 | // Sets SPI bit order, clock divider, and data mode. This function is useful 105 | // when there are multiple SPI devices using different settings. 106 | // Returns 1 when complete. 107 | //////////////////////////////////////////////////////////////////////////// 108 | int ADIS16445::configSPI() { 109 | SPI.setBitOrder(MSBFIRST); // Per the datasheet 110 | SPI.setClockDivider(SPI_CLOCK_DIV16); // Config for 1MHz (ADIS16445 max 2MHz, 111 | // burst read max 1MHz) 112 | SPI.setDataMode(SPI_MODE3); // Clock base at one, sampled on falling edge 113 | return (1); 114 | } 115 | 116 | //////////////////////////////////////////////////////////////////////////////////////////// 117 | // Reads two bytes (one word) in two sequential registers over SPI 118 | //////////////////////////////////////////////////////////////////////////////////////////// 119 | // regAddr - address of register to be read 120 | // return - (int) signed 16 bit 2's complement number 121 | //////////////////////////////////////////////////////////////////////////////////////////// 122 | int16_t ADIS16445::regRead(uint8_t regAddr) { 123 | // Read registers using SPI 124 | 125 | // Write register address to be read 126 | digitalWrite(CS_, LOW); // Set CS low to enable device 127 | SPI.transfer(regAddr); // Write address over SPI bus 128 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 129 | // requirement 130 | digitalWrite(CS_, HIGH); // Set CS high to disable device 131 | 132 | // delayMicroseconds(25); // Delay to not violate read rate (40us) 133 | 134 | // Read data from requested register 135 | digitalWrite(CS_, LOW); // Set CS low to enable device 136 | uint8_t _msbData = 137 | SPI.transfer(0x00); // Send (0x00) and place upper byte into variable 138 | uint8_t _lsbData = 139 | SPI.transfer(0x00); // Send (0x00) and place lower byte into variable 140 | digitalWrite(CS_, HIGH); // Set CS high to disable device 141 | 142 | delayMicroseconds(25); // Delay to not violate read rate (40us) 143 | 144 | int16_t _dataOut = 145 | (_msbData << 8) | (_lsbData & 0xFF); // Concatenate upper and lower bytes 146 | // Shift MSB data left by 8 bits, mask LSB data with 0xFF, and OR both bits. 147 | 148 | return (_dataOut); 149 | } 150 | 151 | //////////////////////////////////////////////////////////////////////////////////////////// 152 | // Reads all gyro and accel registers 153 | //////////////////////////////////////////////////////////////////////////////////////////// 154 | // regAddr - address of register to be read 155 | // return - (pointer) array of signed 16 bit 2's complement numbers 156 | //////////////////////////////////////////////////////////////////////////////////////////// 157 | int16_t *ADIS16445::sensorRead() { 158 | // Read registers using SPI 159 | // Initialize sensor data arrays and stall time variable 160 | uint8_t sensorData[12]; 161 | int16_t joinedData[6]; 162 | int stall = 25; 163 | 164 | // Write each requested register address and read back it's data 165 | digitalWrite(CS_, LOW); // Set CS low to enable communication with the device 166 | SPI.transfer(XGYRO_OUT); // Initial SPI read. Returned data for this transfer 167 | // is invalid 168 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 169 | // requirement 170 | digitalWrite(CS_, 171 | HIGH); // Set CS high to disable communication with the device 172 | delayMicroseconds(stall); // Delay to not violate read rate (40us) 173 | digitalWrite(CS_, LOW); 174 | sensorData[0] = SPI.transfer( 175 | YGYRO_OUT); // Write next address to device and read upper byte 176 | sensorData[1] = SPI.transfer(0x00); // Read lower byte 177 | joinedData[0] = (sensorData[0] << 8) | 178 | (sensorData[1] & 0xFF); // Concatenate two bytes into word 179 | digitalWrite(CS_, HIGH); 180 | delayMicroseconds(stall); 181 | digitalWrite(CS_, LOW); 182 | sensorData[2] = SPI.transfer(ZGYRO_OUT); 183 | sensorData[3] = SPI.transfer(0x00); 184 | joinedData[1] = (sensorData[2] << 8) | (sensorData[3] & 0xFF); 185 | digitalWrite(CS_, HIGH); 186 | delayMicroseconds(stall); 187 | digitalWrite(CS_, LOW); 188 | sensorData[4] = SPI.transfer(XACCL_OUT); 189 | sensorData[5] = SPI.transfer(0x00); 190 | joinedData[2] = (sensorData[4] << 8) | (sensorData[5] & 0xFF); 191 | digitalWrite(CS_, HIGH); 192 | delayMicroseconds(stall); 193 | digitalWrite(CS_, LOW); 194 | sensorData[6] = SPI.transfer(YACCL_OUT); 195 | sensorData[7] = SPI.transfer(0x00); 196 | joinedData[3] = (sensorData[6] << 8) | (sensorData[7] & 0xFF); 197 | digitalWrite(CS_, HIGH); 198 | delayMicroseconds(stall); 199 | digitalWrite(CS_, LOW); 200 | sensorData[8] = SPI.transfer(ZACCL_OUT); 201 | sensorData[9] = SPI.transfer(0x00); 202 | joinedData[4] = (sensorData[8] << 8) | (sensorData[9] & 0xFF); 203 | digitalWrite(CS_, HIGH); 204 | delayMicroseconds(stall); 205 | digitalWrite(CS_, LOW); 206 | sensorData[10] = 207 | SPI.transfer(FLASH_CNT); // Final transfer. Data after this invalid 208 | sensorData[11] = SPI.transfer(0x00); 209 | joinedData[5] = (sensorData[10] << 8) | (sensorData[11] & 0xFF); 210 | digitalWrite(CS_, HIGH); 211 | 212 | return (joinedData); // Return pointer with data 213 | } 214 | 215 | //////////////////////////////////////////////////////////////////////////////////////////// 216 | // Reads all gyro and accel registers in one instance (faster) 217 | //////////////////////////////////////////////////////////////////////////////////////////// 218 | // regAddr - address of register to be read 219 | // return - (pointer) array of signed 16 bit 2's complement numbers 220 | //////////////////////////////////////////////////////////////////////////////////////////// 221 | int16_t *ADIS16445::sensorReadAll() { 222 | // Read registers using SPI 223 | // Initialize sensor data arrays and stall time variable 224 | uint8_t sensorData[16]; 225 | int16_t joinedData[8]; 226 | // Write each requested register address and read back it's data 227 | digitalWrite(CS_, LOW); // Set CS low to enable communication with the device 228 | SPI.transfer(GLOB_CMD); // Initial SPI read. Returned data for this transfer 229 | // is invalid 230 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 231 | // requirement 232 | 233 | sensorData[0] = SPI.transfer( 234 | FLASH_CNT); // Write next address to device and read upper byte 235 | sensorData[1] = SPI.transfer(0x00); // Read lower byte 236 | joinedData[0] = (sensorData[0] << 8) | 237 | (sensorData[1] & 0xFF); // Concatenate two bytes into word 238 | 239 | sensorData[2] = SPI.transfer(FLASH_CNT); 240 | sensorData[3] = SPI.transfer(0x00); 241 | joinedData[1] = (sensorData[2] << 8) | (sensorData[3] & 0xFF); 242 | 243 | sensorData[4] = SPI.transfer(FLASH_CNT); 244 | sensorData[5] = SPI.transfer(0x00); 245 | joinedData[2] = (sensorData[4] << 8) | (sensorData[5] & 0xFF); 246 | 247 | sensorData[6] = SPI.transfer(FLASH_CNT); 248 | sensorData[7] = SPI.transfer(0x00); 249 | joinedData[3] = (sensorData[6] << 8) | (sensorData[7] & 0xFF); 250 | 251 | sensorData[8] = SPI.transfer(FLASH_CNT); 252 | sensorData[9] = SPI.transfer(0x00); 253 | joinedData[4] = (sensorData[8] << 8) | (sensorData[9] & 0xFF); 254 | 255 | sensorData[10] = SPI.transfer(FLASH_CNT); 256 | sensorData[11] = SPI.transfer(0x00); 257 | joinedData[5] = (sensorData[10] << 8) | (sensorData[11] & 0xFF); 258 | 259 | sensorData[12] = SPI.transfer(FLASH_CNT); 260 | sensorData[13] = SPI.transfer(0x00); 261 | joinedData[6] = (sensorData[12] << 8) | (sensorData[13] & 0xFF); 262 | 263 | sensorData[14] = 264 | SPI.transfer(FLASH_CNT); // Final transfer. Data after this invalid 265 | sensorData[15] = SPI.transfer(0x00); 266 | joinedData[7] = (sensorData[14] << 8) | (sensorData[15] & 0xFF); 267 | digitalWrite(CS_, HIGH); 268 | 269 | return (joinedData); // Return pointer with data 270 | } 271 | 272 | //////////////////////////////////////////////////////////////////////////// 273 | // Writes one byte of data to the specified register over SPI. 274 | // Returns 1 when complete. 275 | //////////////////////////////////////////////////////////////////////////// 276 | // regAddr - address of register to be written 277 | // regData - data to be written to the register 278 | //////////////////////////////////////////////////////////////////////////// 279 | int ADIS16445::regWrite(uint8_t regAddr, int16_t regData) { 280 | // Write register address and data 281 | uint16_t addr = 282 | (((regAddr & 0x7F) | 0x80) 283 | << 8); // Toggle sign bit, and check that the address is 8 bits 284 | uint16_t lowWord = 285 | (addr | (regData & 0xFF)); // OR Register address (A) with data(D) (AADD) 286 | uint16_t highWord = 287 | ((addr | 0x100) | 288 | ((regData >> 8) & 289 | 0xFF)); // OR Register address with data and increment address 290 | 291 | // Split words into chars 292 | uint8_t highBytehighWord = (highWord >> 8); 293 | uint8_t lowBytehighWord = (highWord & 0xFF); 294 | uint8_t highBytelowWord = (lowWord >> 8); 295 | uint8_t lowBytelowWord = (lowWord & 0xFF); 296 | 297 | // Write highWord to SPI bus 298 | digitalWrite(CS_, LOW); // Set CS low to enable device 299 | SPI.transfer(highBytehighWord); // Write high byte from high word to SPI bus 300 | SPI.transfer(lowBytehighWord); // Write low byte from high word to SPI bus 301 | digitalWrite(CS_, HIGH); // Set CS high to disable device 302 | 303 | delayMicroseconds(40); // Delay to not violate read rate (40us) 304 | 305 | // Write lowWord to SPI bus 306 | digitalWrite(CS_, LOW); // Set CS low to enable device 307 | SPI.transfer(highBytelowWord); // Write high byte from low word to SPI bus 308 | SPI.transfer(lowBytelowWord); // Write low byte from low word to SPI bus 309 | digitalWrite(CS_, HIGH); // Set CS high to disable device 310 | 311 | return (1); 312 | } 313 | 314 | ///////////////////////////////////////////////////////////////////////////////////////// 315 | // Converts accelerometer data output from the regRead() function and returns 316 | // acceleration in g's 317 | ///////////////////////////////////////////////////////////////////////////////////////// 318 | // sensorData - data output from regRead() 319 | // return - (float) signed/scaled accelerometer in g's 320 | ///////////////////////////////////////////////////////////////////////////////////////// 321 | float ADIS16445::accelScale(int16_t sensorData) { 322 | int signedData = 0; 323 | int isNeg = sensorData & 0x8000; 324 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 325 | signedData = sensorData - 0xFFFF; 326 | else 327 | signedData = sensorData; // Else return the raw number 328 | float finalData = 329 | signedData * 0.00025; // Multiply by accel sensitivity (4000 LSB/g) 330 | return finalData; 331 | } 332 | 333 | ///////////////////////////////////////////////////////////////////////////////////////////// 334 | // Converts gyro data output from the regRead() function and returns gyro rate 335 | // in deg/sec 336 | ///////////////////////////////////////////////////////////////////////////////////////////// 337 | // sensorData - data output from regRead() 338 | // return - (float) signed/scaled gyro in degrees/sec 339 | ///////////////////////////////////////////////////////////////////////////////////////// 340 | float ADIS16445::gyroScale(int16_t sensorData) { 341 | int signedData = 0; 342 | int isNeg = sensorData & 0x8000; 343 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 344 | signedData = sensorData - 0xFFFF; 345 | else 346 | signedData = sensorData; 347 | float finalData = 348 | signedData * 0.01; // Multiply by gyro sensitivity (100 LSB/dps) 349 | return finalData; 350 | } 351 | 352 | ///////////////////////////////////////////////////////////////////////////////////////////// 353 | // Converts temperature data output from the regRead() function and returns 354 | // temperature in degrees Celcius 355 | ///////////////////////////////////////////////////////////////////////////////////////////// 356 | // sensorData - data output from regRead() 357 | // return - (float) signed/scaled temperature in degrees Celcius 358 | ///////////////////////////////////////////////////////////////////////////////////////// 359 | float ADIS16445::tempScale(int16_t sensorData) { 360 | int signedData = 0; 361 | int isNeg = sensorData & 0x8000; 362 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 363 | signedData = sensorData - 0xFFFF; 364 | else 365 | signedData = sensorData; 366 | float finalData = 367 | (signedData * 0.07386) + 368 | 31; // Multiply by temperature scale and add 31 to equal 0x0000 369 | return finalData; 370 | } 371 | 372 | ///////////////////////////////////////////////////////////////////////////////////////////// 373 | // Method to update the sensor data without any validity checks (may result in 374 | // spikes). 375 | /////////////////////////////////////////////////////////////////////////////////////////////// 376 | bool ADIS16445::updateData() { 377 | sensor_data_ = sensorReadAll(); 378 | return sensor_data_ != nullptr; 379 | } 380 | 381 | ///////////////////////////////////////////////////////////////////////////////////////////// 382 | // Method to update the internally stored sensor data recusivelly by checking 383 | // the validity. 384 | /////////////////////////////////////////////////////////////////////////////////////////////// 385 | bool ADIS16445::updateDataIterative() { 386 | uint64_t tic = micros(); 387 | bool success = false; 388 | for (size_t depth = 0; depth < kMaxRecursiveUpdateDepth; ++depth) { 389 | Sensor::setTimestampNow(); 390 | sensor_data_ = sensorReadAll(); 391 | // Assuming that 1) temperature changes much slower than IMU sampling rate, 392 | // 2) all other measurements are correct if TEMP_OUT is correct. It may be 393 | // necessary to filter outliers out by using moving average filter before 394 | // publishing IMU topic (i.e., versavis_imu_receiver.cpp) 395 | if (sensor_data_ == nullptr || sensor_data_[7] != regRead(TEMP_OUT)) { 396 | if (micros() - tic > kImuSyncTimeoutUs) { 397 | return false; 398 | } 399 | DEBUG_PRINTLN( 400 | topic_ + 401 | " (ADIS16445.cpp): Failed IMU update detected, trying again " + 402 | (String)(kMaxRecursiveUpdateDepth - depth) + " times."); 403 | } else { 404 | return true; 405 | } 406 | } 407 | return false; 408 | } 409 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16445.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // May 2015 3 | // Author: Juan Jose Chong 4 | // Adapted for ADIS16445 by Florian Tschopp 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16445.h 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the ADIS16445 11 | // IMU with an 8-Bit Atmel-based Arduino development board. Functions for SPI 12 | // configuration, reads and writes, and scaling are included. This library may 13 | // be used for the entire ADIS164XX family of devices with some modification. 14 | // 15 | // This example is free software. You can redistribute it and/or modify it 16 | // under the terms of the GNU Lesser Public License as published by the Free 17 | // Software Foundation, either version 3 of the License, or any later version. 18 | // 19 | // This example is distributed in the hope that it will be useful, but WITHOUT 20 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 21 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser Public License for 22 | // more details. 23 | // 24 | // You should have received a copy of the GNU Lesser Public License along with 25 | // this example. If not, see . 26 | // 27 | //////////////////////////////////////////////////////////////////////////////// 28 | 29 | #ifndef ADIS16445_h 30 | #define ADIS16445_h 31 | #include "Arduino.h" 32 | #include "Imu.h" 33 | #include 34 | #include 35 | 36 | // User Register Memory Map from Table 6 37 | #define FLASH_CNT 0x00 // Flash memory write count 38 | #define XGYRO_OUT 0x04 // X-axis gyroscope output 39 | #define YGYRO_OUT 0x06 // Y-axis gyroscope output 40 | #define ZGYRO_OUT 0x08 // Z-axis gyroscope output 41 | #define XACCL_OUT 0x0A // X-axis accelerometer output 42 | #define YACCL_OUT 0x0C // Y-axis accelerometer output 43 | #define ZACCL_OUT 0x0E // Z-axis accelerometer output 44 | #define TEMP_OUT 0x18 // Temperature output 45 | #define XGYRO_OFF 0x1A // X-axis gyroscope bias offset factor 46 | #define YGYRO_OFF 0x1C // Y-axis gyroscope bias offset factor 47 | #define ZGYRO_OFF 0x1E // Z-axis gyroscope bias offset factor 48 | #define XACCL_OFF 0x20 // X-axis acceleration bias offset factor 49 | #define YACCL_OFF 0x22 // Y-axis acceleration bias offset factor 50 | #define GPIO_CTRL 0x32 // GPIO control 51 | #define MSC_CTRL 0x34 // MISC control 52 | #define SMPL_PRD 0x36 // Sample clock/Decimation filter control 53 | #define SENS_AVG 0x38 // Digital filter control 54 | #define DIAG_STAT 0x3C // System status 55 | #define GLOB_CMD 0x3E // System command 56 | #define ALM_MAG1 0x40 // Alarm 1 amplitude threshold 57 | #define ALM_MAG2 0x42 // Alarm 2 amplitude threshold 58 | #define ALM_SMPL1 0x44 // Alarm 1 sample size 59 | #define ALM_SMPL2 0x46 // Alarm 2 sample size 60 | #define ALM_CTRL 0x48 // Alarm control 61 | #define LOT_ID1 0x52 // Lot identification number 62 | #define LOT_ID2 0x54 // Lot identification number 63 | #define PROD_ID 0x56 // Product identifier 64 | #define SERIAL_NUM 0x58 // Lot-specific serial number 65 | 66 | class ADIS16445 : public Imu { 67 | public: 68 | // ADIS16445 Constructor (ChipSelect, DataReady, Reset Pins) 69 | ADIS16445(ros::NodeHandle *nh, const String &topic, const int rate_hz, 70 | Timer &timer, int CS, int DR, int RST); 71 | 72 | // Destructor 73 | ~ADIS16445(); 74 | 75 | // Initialize 76 | void setup(); 77 | 78 | // Performs hardware reset. Delay in miliseconds. Returns 1 when complete. 79 | int resetDUT(uint8_t ms); 80 | 81 | // Sets SPI bit order, clock divider, and data mode. Returns 1 when complete. 82 | int configSPI(); 83 | 84 | // Read register (two bytes) Returns signed 16 bit data. 85 | int16_t regRead(uint8_t regAddr); 86 | 87 | int16_t *sensorRead(); 88 | 89 | int16_t *sensorReadAll(); 90 | // Write register (two bytes). Returns 1 when complete. 91 | int regWrite(uint8_t regAddr, int16_t regData); 92 | 93 | // Scale accelerometer data. Returns scaled data as float. 94 | float accelScale(int16_t sensorData); 95 | 96 | // Scale gyro data. Returns scaled data as float. 97 | float gyroScale(int16_t sensorData); 98 | 99 | // Scale temperature data. Returns scaled data as float. 100 | float tempScale(int16_t sensorData); 101 | 102 | // Update data internally with validity checks. 103 | bool updateDataIterative(); 104 | 105 | // Update data withoput recursion. 106 | bool updateData(); 107 | 108 | private: 109 | // Variables to store hardware pin assignments. 110 | int CS_; 111 | int RST_; 112 | }; 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16448AMLZ.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // July 2015 3 | // Author: Juan Jose Chong 4 | // Updated by Inkyu Sa for ADIS16448AMLZ 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16448AMLZ.cpp 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the 11 | // ADIS16448AMLZ IMU with an 8-Bit Atmel-based Arduino development board. 12 | // Functions for SPI configuration, reads and writes, and scaling are included. 13 | // This library may be used for the entire ADIS164XX family of devices with 14 | // some modification. 15 | // 16 | // This example is free software. You can redistribute it and/or modify it 17 | // under the terms of the GNU Lesser Public License as published by the Free 18 | // Software Foundation, either version 3 of the License, or any later version. 19 | // 20 | // This example is distributed in the hope that it will be useful, but WITHOUT 21 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 22 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser Public License for 23 | // more details. 24 | // 25 | // You should have received a copy of the GNU Lesser Public License along with 26 | // this example. If not, see . 27 | // 28 | //////////////////////////////////////////////////////////////////////////////// 29 | 30 | #include "ADIS16448AMLZ.h" 31 | #include "helper.h" 32 | #include "versavis_configuration.h" 33 | 34 | //////////////////////////////////////////////////////////////////////////// 35 | // Constructor with configurable CS, DR, RST 36 | //////////////////////////////////////////////////////////////////////////// 37 | // CS - Chip select pin 38 | // DR - DR output pin for data ready 39 | // RST - Hardware reset pin 40 | //////////////////////////////////////////////////////////////////////////// 41 | ADIS16448AMLZ::ADIS16448AMLZ(ros::NodeHandle *nh, const String &topic, 42 | const int rate_hz, Timer &timer, int CS, int DR, 43 | int RST) 44 | : Imu(nh, topic, rate_hz, timer), CS_(CS), RST_(RST) { 45 | if (DR >= 0) { 46 | pinMode(DR, INPUT); // Set DR pin to be an input 47 | } 48 | 49 | if (RST_ >= 0) { 50 | pinMode(RST_, OUTPUT); // Set RST pin to be an output 51 | digitalWrite(RST_, HIGH); // Initialize RST pin to be high 52 | } 53 | } 54 | 55 | void ADIS16448AMLZ::setup() { 56 | DEBUG_PRINTLN((topic_ + " (ADIS16448AMLZ.cpp): Setup.").c_str()); 57 | 58 | SPI.begin(); // Initialize SPI bus 59 | configSPI(); // Configure SPI 60 | 61 | // Reset IMU. 62 | regWrite(GLOB_CMD, 0xBE80); // Perform an IMU reset. 63 | delay(300); 64 | regWrite(GLOB_CMD, 0xBE00); // Take IMU out of reset state. 65 | delay(100); 66 | configSPI(); // Configure SPI 67 | delay(100); 68 | 69 | // Set default pin states 70 | pinMode(CS_, OUTPUT); // Set CS pin to be an output 71 | digitalWrite(CS_, HIGH); // Initialize CS pin to be high 72 | 73 | /* ---------------- Perform IMU configuration --------------------------- */ 74 | 75 | // See also configuration of ADIS16448BMLZ. 76 | // Enable CRC doesn't work for USE_ADIS16448AMLZ. The last 16 bits filled with 77 | // zeros. Disable data ready. Disable CRC (CRC is only valid for 16448BMLZ nor 78 | // AMLZ..) 79 | int16_t kMscCtrlRegister = 0x0000; // B0000 0000 0001 0000 80 | regWrite(MSC_CTRL, kMscCtrlRegister); 81 | delay(20); 82 | 83 | // See above same as ADIS16448BMLZ, 84 | // 0x0100: ±250°/sec, note that the lower dynamic range settings limit the 85 | // minimum filter tap sizes to maintain resolution. 86 | int16_t kSensAvgRegister = 0x0400; // B0000 0100 0000 0000; 87 | regWrite(SENS_AVG, kSensAvgRegister); 88 | delay(20); 89 | 90 | // See above same as ADIS16448BMLZ, 91 | // internal clock, 92 | int16_t kSmplPrdRegister = 0x0001; // B0000 0000 0000 0001; 93 | regWrite(SMPL_PRD, kSmplPrdRegister); 94 | delay(20); 95 | 96 | Imu::setupPublisher(); 97 | } 98 | 99 | //////////////////////////////////////////////////////////////////////////// 100 | // Destructor 101 | //////////////////////////////////////////////////////////////////////////// 102 | ADIS16448AMLZ::~ADIS16448AMLZ() { 103 | // Close SPI bus 104 | SPI.end(); 105 | } 106 | 107 | //////////////////////////////////////////////////////////////////////////// 108 | // Performs a hardware reset by setting RST_ pin low for delay (in ms). 109 | //////////////////////////////////////////////////////////////////////////// 110 | int ADIS16448AMLZ::resetDUT(uint8_t ms) { 111 | if (RST_ == -1) { 112 | return -1; 113 | } else { 114 | digitalWrite(RST_, LOW); 115 | delay(100); 116 | digitalWrite(RST_, HIGH); 117 | delay(ms); 118 | return (1); 119 | } 120 | } 121 | 122 | //////////////////////////////////////////////////////////////////////////// 123 | // Sets SPI bit order, clock divider, and data mode. This function is useful 124 | // when there are multiple SPI devices using different settings. 125 | // Returns 1 when complete. 126 | //////////////////////////////////////////////////////////////////////////// 127 | int ADIS16448AMLZ::configSPI() { 128 | SPI.setBitOrder(MSBFIRST); // Per the datasheet 129 | SPI.setClockDivider(SPI_CLOCK_DIV16); // Config for 1MHz (ADIS16448AMLZ max 130 | // 2MHz, burst read max 1MHz) 131 | SPI.setDataMode(SPI_MODE3); // Clock base at one, sampled on falling edge 132 | return (1); 133 | } 134 | 135 | //////////////////////////////////////////////////////////////////////////////////////////// 136 | // Reads two bytes (one word) in two sequential registers over SPI 137 | //////////////////////////////////////////////////////////////////////////////////////////// 138 | // regAddr - address of register to be read 139 | // return - (int) signed 16 bit 2's complement number 140 | //////////////////////////////////////////////////////////////////////////////////////////// 141 | int16_t ADIS16448AMLZ::regRead(uint8_t regAddr) { 142 | // Read registers using SPI 143 | 144 | // Write register address to be read 145 | digitalWrite(CS_, LOW); // Set CS low to enable device 146 | SPI.transfer(regAddr); // Write address over SPI bus 147 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 148 | // requirement 149 | digitalWrite(CS_, HIGH); // Set CS high to disable device 150 | 151 | delayMicroseconds(25); // Delay to not violate read rate (40us) 152 | 153 | // Read data from requested register 154 | digitalWrite(CS_, LOW); // Set CS low to enable device 155 | uint8_t _msbData = 156 | SPI.transfer(0x00); // Send (0x00) and place upper byte into variable 157 | uint8_t _lsbData = 158 | SPI.transfer(0x00); // Send (0x00) and place lower byte into variable 159 | digitalWrite(CS_, HIGH); // Set CS high to disable device 160 | 161 | delayMicroseconds(25); // Delay to not violate read rate (40us) 162 | 163 | int16_t _dataOut = 164 | (_msbData << 8) | (_lsbData & 0xFF); // Concatenate upper and lower bytes 165 | // Shift MSB data left by 8 bits, mask LSB data with 0xFF, and OR both bits. 166 | 167 | return (_dataOut); 168 | } 169 | 170 | //////////////////////////////////////////////////////////////////////////////////////////// 171 | // Reads all gyro, accel, magn, baro, and tmp registers in one instance (faster) 172 | // excluding CRC 173 | //////////////////////////////////////////////////////////////////////////////////////////// 174 | // regAddr - address of register to be read 175 | // return - (pointer) array of signed 16 bit 2's complement numbers 176 | //////////////////////////////////////////////////////////////////////////////////////////// 177 | int16_t *ADIS16448AMLZ::sensorReadWoCRC() { 178 | // Read registers using SPI 179 | // Initialize sensor data arrays. 180 | uint8_t sensorData[24]; 181 | // Write each requested register address and read back it's data 182 | digitalWrite(CS_, LOW); // Set CS low to enable communication with the device 183 | SPI.transfer(GLOB_CMD); // Initial SPI read. Returned data for this transfer 184 | // is invalid 185 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 186 | // requirement 187 | 188 | // DIAG_STAT 189 | sensorData[0] = 190 | SPI.transfer(0x00); // Write next address to device and read upper byte 191 | sensorData[1] = SPI.transfer(0x00); // Read lower byte 192 | // XGYRO_OUT 193 | sensorData[2] = SPI.transfer(0x00); 194 | sensorData[3] = SPI.transfer(0x00); 195 | // YGYRO_OUT 196 | sensorData[4] = SPI.transfer(0x00); 197 | sensorData[5] = SPI.transfer(0x00); 198 | // ZGYRO_OUT 199 | sensorData[6] = SPI.transfer(0x00); 200 | sensorData[7] = SPI.transfer(0x00); 201 | // XACCL_OUT 202 | sensorData[8] = SPI.transfer(0x00); 203 | sensorData[9] = SPI.transfer(0x00); 204 | // YACCL_OUT 205 | sensorData[10] = SPI.transfer(0x00); 206 | sensorData[11] = SPI.transfer(0x00); 207 | // ZACCL_OUT 208 | sensorData[12] = SPI.transfer(0x00); 209 | sensorData[13] = SPI.transfer(0x00); 210 | // XMAGN_OUT 211 | sensorData[14] = SPI.transfer(0x00); 212 | sensorData[15] = SPI.transfer(0x00); 213 | // YMAGN_OUT 214 | sensorData[16] = SPI.transfer(0x00); 215 | sensorData[17] = SPI.transfer(0x00); 216 | // ZMAGN_OUT 217 | sensorData[18] = SPI.transfer(0x00); 218 | sensorData[19] = SPI.transfer(0x00); 219 | // BARO_OUT 220 | sensorData[20] = SPI.transfer(0x00); 221 | sensorData[21] = SPI.transfer(0x00); 222 | // TEMP_OUT 223 | sensorData[22] = SPI.transfer(0x00); 224 | sensorData[23] = SPI.transfer(0x00); 225 | 226 | digitalWrite(CS_, HIGH); // Disable communication with device. 227 | 228 | // Concatenate two bytes into word 229 | static int16_t joinedData[12]; 230 | joinedData[0] = (sensorData[0] << 8) | (sensorData[1] & 0xFF); // DIAG_STAT 231 | joinedData[1] = (sensorData[2] << 8) | (sensorData[3] & 0xFF); // XGYRO_OUT 232 | joinedData[2] = (sensorData[4] << 8) | (sensorData[5] & 0xFF); // YGYRO_OUT 233 | joinedData[3] = (sensorData[6] << 8) | (sensorData[7] & 0xFF); // ZGYRO_OUT 234 | joinedData[4] = (sensorData[8] << 8) | (sensorData[9] & 0xFF); // XACCL_OUT 235 | joinedData[5] = (sensorData[10] << 8) | (sensorData[11] & 0xFF); // YACCL_OUT 236 | joinedData[6] = (sensorData[12] << 8) | (sensorData[13] & 0xFF); // ZACCL_OUT 237 | joinedData[7] = (sensorData[14] << 8) | (sensorData[15] & 0xFF); // XMAGN_OUT 238 | joinedData[8] = (sensorData[16] << 8) | (sensorData[17] & 0xFF); // YMAGN_OUT 239 | joinedData[9] = (sensorData[18] << 8) | (sensorData[19] & 0xFF); // ZMAGN_OUT 240 | joinedData[10] = (sensorData[20] << 8) | (sensorData[21] & 0xFF); // BARO_OUT 241 | joinedData[11] = (sensorData[22] << 8) | (sensorData[23] & 0xFF); // TEMP_OUT 242 | return (joinedData); // Return pointer with data 243 | } 244 | 245 | //////////////////////////////////////////////////////////////////////////// 246 | // Writes one byte of data to the specified register over SPI. 247 | // Returns 1 when complete. 248 | //////////////////////////////////////////////////////////////////////////// 249 | // regAddr - address of register to be written 250 | // regData - data to be written to the register 251 | //////////////////////////////////////////////////////////////////////////// 252 | int ADIS16448AMLZ::regWrite(uint8_t regAddr, int16_t regData) { 253 | // Write register address and data 254 | uint16_t addr = 255 | (((regAddr & 0x7F) | 0x80) 256 | << 8); // Toggle sign bit, and check that the address is 8 bits 257 | uint16_t lowWord = 258 | (addr | (regData & 0xFF)); // OR Register address (A) with data(D) (AADD) 259 | uint16_t highWord = 260 | ((addr | 0x100) | 261 | ((regData >> 8) & 262 | 0xFF)); // OR Register address with data and increment address 263 | 264 | // Split words into chars 265 | uint8_t highBytehighWord = (highWord >> 8); 266 | uint8_t lowBytehighWord = (highWord & 0xFF); 267 | uint8_t highBytelowWord = (lowWord >> 8); 268 | uint8_t lowBytelowWord = (lowWord & 0xFF); 269 | 270 | // Write highWord to SPI bus 271 | digitalWrite(CS_, LOW); // Set CS low to enable device 272 | SPI.transfer(highBytehighWord); // Write high byte from high word to SPI bus 273 | SPI.transfer(lowBytehighWord); // Write low byte from high word to SPI bus 274 | digitalWrite(CS_, HIGH); // Set CS high to disable device 275 | 276 | delayMicroseconds(40); // Delay to not violate read rate (40us) 277 | 278 | // Write lowWord to SPI bus 279 | digitalWrite(CS_, LOW); // Set CS low to enable device 280 | SPI.transfer(highBytelowWord); // Write high byte from low word to SPI bus 281 | SPI.transfer(lowBytelowWord); // Write low byte from low word to SPI bus 282 | digitalWrite(CS_, HIGH); // Set CS high to disable device 283 | 284 | return (1); 285 | } 286 | 287 | ///////////////////////////////////////////////////////////////////////////////////////// 288 | // Converts accelerometer data output from the regRead() function and returns 289 | // acceleration in g's 290 | ///////////////////////////////////////////////////////////////////////////////////////// 291 | // sensorData - data output from regRead() 292 | // return - (float) signed/scaled accelerometer in g's 293 | ///////////////////////////////////////////////////////////////////////////////////////// 294 | float ADIS16448AMLZ::accelScale(int16_t sensorData) { 295 | int signedData = 0; 296 | int isNeg = sensorData & 0x8000; 297 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 298 | signedData = sensorData - 0xFFFF; 299 | else 300 | signedData = sensorData; // Else return the raw number 301 | float finalData = 302 | signedData * 0.000833; // Multiply by accel sensitivity (0.833 mg/LSB) 303 | return finalData; 304 | } 305 | 306 | ///////////////////////////////////////////////////////////////////////////////////////////// 307 | // Converts gyro data output from the regRead() function and returns gyro rate 308 | // in deg/sec 309 | ///////////////////////////////////////////////////////////////////////////////////////////// 310 | // sensorData - data output from regRead() 311 | // return - (float) signed/scaled gyro in degrees/sec 312 | ///////////////////////////////////////////////////////////////////////////////////////// 313 | float ADIS16448AMLZ::gyroScale(int16_t sensorData) { 314 | int signedData = 0; 315 | int isNeg = sensorData & 0x8000; 316 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 317 | signedData = sensorData - 0xFFFF; 318 | else 319 | signedData = sensorData; 320 | float finalData = 321 | signedData * 0.04; // Multiply by gyro sensitivity (0.04 dps/LSB) 322 | return finalData; 323 | } 324 | 325 | ///////////////////////////////////////////////////////////////////////////////////////////// 326 | // Converts temperature data output from the regRead() function and returns 327 | // temperature 328 | // in degrees Celcius 329 | ///////////////////////////////////////////////////////////////////////////////////////////// 330 | // sensorData - data output from regRead() 331 | // return - (float) signed/scaled temperature in degrees Celcius 332 | ///////////////////////////////////////////////////////////////////////////////////////// 333 | float ADIS16448AMLZ::tempScale(int16_t sensorData) { 334 | int signedData = 0; 335 | int isNeg = sensorData & 0x8000; 336 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 337 | signedData = sensorData - 0xFFFF; 338 | else 339 | signedData = sensorData; 340 | float finalData = 341 | (signedData * 0.07386) + 342 | 31; // Multiply by temperature scale and add 31 to equal 0x0000 343 | return finalData; 344 | } 345 | 346 | ///////////////////////////////////////////////////////////////////////////////////////////// 347 | // Converts barometer data output from regRead() function and returns pressure 348 | // in bar 349 | ///////////////////////////////////////////////////////////////////////////////////////////// 350 | // sensorData - data output from regRead() 351 | // return - (float) signed/scaled pressure in mBar 352 | ///////////////////////////////////////////////////////////////////////////////////////// 353 | float ADIS16448AMLZ::pressureScale(int16_t sensorData) { 354 | int signedData = 0; 355 | int isNeg = sensorData & 0x8000; 356 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 357 | signedData = sensorData - 0xFFFF; 358 | else 359 | signedData = sensorData; 360 | float finalData = 361 | (signedData * 0.02); // Multiply by barometer sensitivity (0.02 mBar/LSB) 362 | return finalData; 363 | } 364 | 365 | ///////////////////////////////////////////////////////////////////////////////////////////// 366 | // Converts magnetometer output from regRead() function and returns magnetic 367 | // field 368 | // reading in Gauss 369 | ///////////////////////////////////////////////////////////////////////////////////////////// 370 | // sensorData - data output from regRead() 371 | // return - (float) signed/scaled magnetometer data in mgauss 372 | ///////////////////////////////////////////////////////////////////////////////////////// 373 | float ADIS16448AMLZ::magnetometerScale(int16_t sensorData) { 374 | int signedData = 0; 375 | int isNeg = sensorData & 0x8000; 376 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 377 | signedData = sensorData - 0xFFFF; 378 | else 379 | signedData = sensorData; 380 | float finalData = 381 | (signedData * 0.0001429); // Multiply by sensor resolution (142.9 uGa/LSB) 382 | return finalData; 383 | } 384 | 385 | ///////////////////////////////////////////////////////////////////////////////////////////// 386 | // Method to update the sensor data without any validity checks (may result in 387 | // spikes). 388 | /////////////////////////////////////////////////////////////////////////////////////////////// 389 | bool ADIS16448AMLZ::updateData() { 390 | sensor_data_ = sensorReadWoCRC(); 391 | return sensor_data_ != nullptr; 392 | } 393 | 394 | ///////////////////////////////////////////////////////////////////////////////////////////// 395 | // Method to update the internally stored sensor data recusivelly by checking 396 | // the validity. 397 | /////////////////////////////////////////////////////////////////////////////////////////////// 398 | bool ADIS16448AMLZ::updateDataIterative() { 399 | uint64_t tic = micros(); 400 | bool success = false; 401 | for (size_t depth = 0; depth < kMaxRecursiveUpdateDepth; ++depth) { 402 | Sensor::setTimestampNow(); 403 | sensor_data_ = sensorReadWoCRC(); 404 | // Assuming that 1) temperature changes much slower than IMU sampling rate, 405 | // 2) all other measurements are correct if TEMP_OUT is correct. It may be 406 | // necessary to filter outliers out by using moving average filter before 407 | // publishing IMU topic (i.e., versavis_imu_receiver.cpp) 408 | if (sensor_data_ == nullptr || sensor_data_[11] != regRead(TEMP_OUT)) { 409 | if (micros() - tic > kImuSyncTimeoutUs) { 410 | return false; 411 | } 412 | DEBUG_PRINTLN( 413 | topic_ + 414 | " (ADIS16448AMLZ.cpp): Failed IMU update detected, trying again " + 415 | (String)(kMaxRecursiveUpdateDepth - depth) + " times."); 416 | } else { 417 | return true; 418 | } 419 | } 420 | return false; 421 | } 422 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16448AMLZ.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // May 2015 3 | // Author: Juan Jose Chong 4 | // Updated by Inkyu Sa for ADIS16448AMLZ 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16448AMLZ.h 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the 11 | // ADIS16448AMLZ IMU with an 8-Bit Atmel-based Arduino development board. 12 | // Functions for SPI configuration, reads and writes, and scaling are included. 13 | // This library may be used for the entire ADIS164XX family of devices with 14 | // some modification. 15 | // 16 | // This example is free software. You can redistribute it and/or modify it 17 | // under the terms of the GNU Lesser Public License as published by the Free 18 | // Software Foundation, either version 3 of the License, or any later version. 19 | // 20 | // This example is distributed in the hope that it will be useful, but WITHOUT 21 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 22 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser Public License for 23 | // more details. 24 | // 25 | // You should have received a copy of the GNU Lesser Public License along with 26 | // this example. If not, see . 27 | // 28 | //////////////////////////////////////////////////////////////////////////////// 29 | 30 | #ifndef ADIS16448AMLZ_h 31 | #define ADIS16448AMLZ_h 32 | #include "Arduino.h" 33 | #include "Imu.h" 34 | #include 35 | #include 36 | 37 | // User Register Memory Map from Table 6 38 | #define FLASH_CNT 0x00 // Flash memory write count 39 | #define XGYRO_OUT 0x04 // X-axis gyroscope output 40 | #define YGYRO_OUT 0x06 // Y-axis gyroscope output 41 | #define ZGYRO_OUT 0x08 // Z-axis gyroscope output 42 | #define XACCL_OUT 0x0A // X-axis accelerometer output 43 | #define YACCL_OUT 0x0C // Y-axis accelerometer output 44 | #define ZACCL_OUT 0x0E // Z-axis accelerometer output 45 | #define XMAGN_OUT 0X10 // X-axis magnetometer output 46 | #define YMAGN_OUT 0x12 // Y-axis magnetometer output 47 | #define ZMAGN_OUT 0x14 // Z-axis magnetometer output 48 | #define BARO_OUT 0x16 // Barometer pressure measurement, high word 49 | #define TEMP_OUT 0x18 // Temperature output 50 | #define XGYRO_OFF 0x1A // X-axis gyroscope bias offset factor 51 | #define YGYRO_OFF 0x1C // Y-axis gyroscope bias offset factor 52 | #define ZGYRO_OFF 0x1E // Z-axis gyroscope bias offset factor 53 | #define XACCL_OFF 0x20 // X-axis acceleration bias offset factor 54 | #define YACCL_OFF 0x22 // Y-axis acceleration bias offset factor 55 | #define ZACCL_OFF 0x24 // Z-axis acceleration bias offset factor 56 | #define XMAGN_HIC 0x26 // X-axis magnetometer, hard iron factor 57 | #define YMAGN_HIC 0x28 // Y-axis magnetometer, hard iron factor 58 | #define ZMAGN_HIC 0x2A // Z-axis magnetometer, hard iron factor 59 | #define XMAGN_SIC 0x2C // X-axis magnetometer, soft iron factor 60 | #define YMAGN_SIC 0x2E // Y-axis magnetometer, soft iron factor 61 | #define ZMAGN_SIC 0x30 // Z-axis magnetometer, soft iron factor 62 | #define GPIO_CTRL 0x32 // GPIO control 63 | #define MSC_CTRL 0x34 // MISC control 64 | #define SMPL_PRD 0x36 // Sample clock/Decimation filter control 65 | #define SENS_AVG 0x38 // Digital filter control 66 | #define SEQ_CNT 0x3A // MAGN_OUT and BARO_OUT counter 67 | #define DIAG_STAT 0x3C // System status 68 | #define GLOB_CMD 0x3E // System command 69 | #define ALM_MAG1 0x40 // Alarm 1 amplitude threshold 70 | #define ALM_MAG2 0x42 // Alarm 2 amplitude threshold 71 | #define ALM_SMPL1 0x44 // Alarm 1 sample size 72 | #define ALM_SMPL2 0x46 // Alarm 2 sample size 73 | #define ALM_CTRL 0x48 // Alarm control 74 | #define LOT_ID1 0x52 // Lot identification number 75 | #define LOT_ID2 0x54 // Lot identification number 76 | #define PROD_ID 0x56 // Product identifier 77 | #define SERIAL_NUM 0x58 // Lot-specific serial number 78 | 79 | // ADIS16448AMLZ Class Definition 80 | class ADIS16448AMLZ : public Imu { 81 | public: 82 | // ADIS16448AMLZ Constructor (ChipSelect, DataReady output pin, HardwareReset) 83 | ADIS16448AMLZ(ros::NodeHandle *nh, const String &topic, const int rate_hz, 84 | Timer &timer, int CS, int DR, int RST); 85 | 86 | // Destructor 87 | ~ADIS16448AMLZ(); 88 | 89 | void setup(); 90 | 91 | // Hardware reset sent to pin. 92 | int resetDUT(uint8_t ms); 93 | 94 | // Sets SPI bit order, clock divider, and data mode. Returns 1 when complete. 95 | int configSPI(); 96 | 97 | // Read register (two bytes) Returns signed 16 bit data. 98 | int16_t regRead(uint8_t regAddr); 99 | 100 | // Burst read imu data excluding checksum. 101 | int16_t *sensorReadWoCRC(); 102 | 103 | // Write register (two bytes). Returns 1 when complete. 104 | int regWrite(uint8_t regAddr, int16_t regData); 105 | 106 | // Scale accelerometer data. Returns scaled data as float. 107 | float accelScale(int16_t sensorData); 108 | 109 | // Scale gyro data. Returns scaled data as float. 110 | float gyroScale(int16_t sensorData); 111 | 112 | // Scale temperature data. Returns scaled data as float. 113 | float tempScale(int16_t sensorData); 114 | 115 | // Scale barometer data. Returns scaled data as float. 116 | float pressureScale(int16_t sensorData); 117 | 118 | // Scale magnetometer data. Returns scaled data as float. 119 | float magnetometerScale(int16_t sensorData); 120 | 121 | // Calculate CRC-16 Checksum. 122 | int checksum(int16_t *sensorData); 123 | void updateCRC(unsigned int *crc, unsigned int *data, 124 | const unsigned int &POLY); 125 | 126 | // Update data internally with validity checks. 127 | bool updateDataIterative(); 128 | 129 | // Update data withoput recursion. 130 | bool updateData(); 131 | 132 | private: 133 | // Variables to store hardware pin assignments. 134 | int CS_; 135 | int RST_; 136 | }; 137 | 138 | #endif 139 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16448BMLZ.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // May 2015 3 | // Author: Juan Jose Chong 4 | // Updated by Inkyu Sa for ADIS16448BMLZ 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16448BMLZ.h 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the 11 | // ADIS16448BMLZ IMU with an 8-Bit Atmel-based Arduino development board. 12 | // Functions for SPI configuration, reads and writes, and scaling are included. 13 | // This library may be used for the entire ADIS164XX family of devices with 14 | // some modification. 15 | // 16 | // This example is free software. You can redistribute it and/or modify it 17 | // under the terms of the GNU Lesser Public License as published by the Free 18 | // Software Foundation, either version 3 of the License, or any later version. 19 | // 20 | // This example is distributed in the hope that it will be useful, but WITHOUT 21 | // ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 22 | // FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser Public License for 23 | // more details. 24 | // 25 | // You should have received a copy of the GNU Lesser Public License along with 26 | // this example. If not, see . 27 | // 28 | //////////////////////////////////////////////////////////////////////////////// 29 | 30 | #ifndef ADIS16448BMLZ_h 31 | #define ADIS16448BMLZ_h 32 | #include "Arduino.h" 33 | #include "Imu.h" 34 | #include 35 | #include 36 | 37 | // User Register Memory Map from Table 6 38 | #define FLASH_CNT 0x00 // Flash memory write count 39 | #define XGYRO_OUT 0x04 // X-axis gyroscope output 40 | #define YGYRO_OUT 0x06 // Y-axis gyroscope output 41 | #define ZGYRO_OUT 0x08 // Z-axis gyroscope output 42 | #define XACCL_OUT 0x0A // X-axis accelerometer output 43 | #define YACCL_OUT 0x0C // Y-axis accelerometer output 44 | #define ZACCL_OUT 0x0E // Z-axis accelerometer output 45 | #define XMAGN_OUT 0X10 // X-axis magnetometer output 46 | #define YMAGN_OUT 0x12 // Y-axis magnetometer output 47 | #define ZMAGN_OUT 0x14 // Z-axis magnetometer output 48 | #define BARO_OUT 0x16 // Barometer pressure measurement, high word 49 | #define TEMP_OUT 0x18 // Temperature output 50 | #define XGYRO_OFF 0x1A // X-axis gyroscope bias offset factor 51 | #define YGYRO_OFF 0x1C // Y-axis gyroscope bias offset factor 52 | #define ZGYRO_OFF 0x1E // Z-axis gyroscope bias offset factor 53 | #define XACCL_OFF 0x20 // X-axis acceleration bias offset factor 54 | #define YACCL_OFF 0x22 // Y-axis acceleration bias offset factor 55 | #define ZACCL_OFF 0x24 // Z-axis acceleration bias offset factor 56 | #define XMAGN_HIC 0x26 // X-axis magnetometer, hard iron factor 57 | #define YMAGN_HIC 0x28 // Y-axis magnetometer, hard iron factor 58 | #define ZMAGN_HIC 0x2A // Z-axis magnetometer, hard iron factor 59 | #define XMAGN_SIC 0x2C // X-axis magnetometer, soft iron factor 60 | #define YMAGN_SIC 0x2E // Y-axis magnetometer, soft iron factor 61 | #define ZMAGN_SIC 0x30 // Z-axis magnetometer, soft iron factor 62 | #define GPIO_CTRL 0x32 // GPIO control 63 | #define MSC_CTRL 0x34 // MISC control 64 | #define SMPL_PRD 0x36 // Sample clock/Decimation filter control 65 | #define SENS_AVG 0x38 // Digital filter control 66 | #define SEQ_CNT 0x3A // MAGN_OUT and BARO_OUT counter 67 | #define DIAG_STAT 0x3C // System status 68 | #define GLOB_CMD 0x3E // System command 69 | #define ALM_MAG1 0x40 // Alarm 1 amplitude threshold 70 | #define ALM_MAG2 0x42 // Alarm 2 amplitude threshold 71 | #define ALM_SMPL1 0x44 // Alarm 1 sample size 72 | #define ALM_SMPL2 0x46 // Alarm 2 sample size 73 | #define ALM_CTRL 0x48 // Alarm control 74 | #define LOT_ID1 0x52 // Lot identification number 75 | #define LOT_ID2 0x54 // Lot identification number 76 | #define PROD_ID 0x56 // Product identifier 77 | #define SERIAL_NUM 0x58 // Lot-specific serial number 78 | 79 | // ADIS16448BMLZ Class Definition 80 | class ADIS16448BMLZ : public Imu { 81 | public: 82 | // ADIS16448BMLZ Constructor (ChipSelect, DataReady output pin, HardwareReset) 83 | ADIS16448BMLZ(ros::NodeHandle *nh, const String &topic, const int rate_hz, 84 | Timer &timer, int CS, int DR, int RST); 85 | 86 | // Destructor 87 | ~ADIS16448BMLZ(); 88 | 89 | void setup(); 90 | 91 | // Hardware reset sent to pin. 92 | int resetDUT(uint8_t ms); 93 | 94 | // Sets SPI bit order, clock divider, and data mode. Returns 1 when complete. 95 | int configSPI(); 96 | 97 | // Read register (two bytes) Returns signed 16 bit data. 98 | int16_t regRead(uint8_t regAddr); 99 | 100 | // Burst read imu data including checksum. 101 | int16_t *sensorReadAllCRC(); 102 | 103 | // Write register (two bytes). Returns 1 when complete. 104 | int regWrite(uint8_t regAddr, int16_t regData); 105 | 106 | // Scale accelerometer data. Returns scaled data as float. 107 | float accelScale(int16_t sensorData); 108 | 109 | // Scale gyro data. Returns scaled data as float. 110 | float gyroScale(int16_t sensorData); 111 | 112 | // Scale temperature data. Returns scaled data as float. 113 | float tempScale(int16_t sensorData); 114 | 115 | // Scale barometer data. Returns scaled data as float. 116 | float pressureScale(int16_t sensorData); 117 | 118 | // Scale magnetometer data. Returns scaled data as float. 119 | float magnetometerScale(int16_t sensorData); 120 | 121 | // Calculate CRC-16 Checksum. 122 | int16_t checksum(int16_t *sensorData); 123 | void updateCRC(unsigned int *crc, unsigned int *data, 124 | const unsigned int &POLY); 125 | 126 | // Update data internally with recursion. 127 | bool updateDataIterative(); 128 | 129 | // Update data withoput recursion. 130 | bool updateData(); 131 | 132 | private: 133 | // Variables to store hardware pin assignments. 134 | int CS_; 135 | int RST_; 136 | }; 137 | 138 | #endif 139 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16460.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // September 2016 3 | // Author: Juan Jose Chong 4 | // Updated by Andreas Pfrunder for ADIS16460 5 | // Adapted by Florian Tschopp for use in versavis 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // ADIS16460.cpp 8 | //////////////////////////////////////////////////////////////////////////////// 9 | // 10 | // This library provides all the functions necessary to interface the ADIS16460 11 | // IMU with a PJRC 32-Bit Teensy 3.2 Development Board. Functions for SPI 12 | // configuration, reads and writes, and scaling are included. This library may 13 | // be used for the entire ADIS1646X family of devices with some modification. 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining 16 | // a copy of this software and associated documentation files (the 17 | // "Software"), to deal in the Software without restriction, including 18 | // without limitation the rights to use, copy, modify, merge, publish, 19 | // distribute, sublicense, and/or sell copies of the Software, and to 20 | // permit persons to whom the Software is furnished to do so, subject to 21 | // the following conditions: 22 | // 23 | // The above copyright notice and this permission notice shall be 24 | // included in all copies or substantial portions of the Software. 25 | // 26 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 27 | // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 28 | // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 29 | // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 30 | // LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 31 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 32 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 33 | // 34 | //////////////////////////////////////////////////////////////////////////////// 35 | 36 | #include "ADIS16460.h" 37 | #include "helper.h" 38 | #include "versavis_configuration.h" 39 | 40 | //////////////////////////////////////////////////////////////////////////// 41 | // Constructor with configurable CS, DR, and RST 42 | //////////////////////////////////////////////////////////////////////////// 43 | // CS - Chip select pin 44 | // DR - DR output pin for data ready 45 | // RST - Hardware reset pin 46 | //////////////////////////////////////////////////////////////////////////// 47 | ADIS16460::ADIS16460(ros::NodeHandle *nh, const String &topic, 48 | const int rate_hz, Timer &timer, int CS, int DR, int RST) 49 | : Imu(nh, topic, rate_hz, timer), CS_(CS), RST_(RST) { 50 | if (DR >= 0) { 51 | pinMode(DR, INPUT); // Set DR pin to be an input 52 | } 53 | 54 | if (RST_ >= 0) { 55 | pinMode(RST_, OUTPUT); // Set RST pin to be an output 56 | digitalWrite(RST_, HIGH); // Initialize RST pin to be high 57 | } 58 | } 59 | 60 | void ADIS16460::setup() { 61 | DEBUG_PRINTLN((topic_ + " (ADIS16460.cpp): Setup.")); 62 | 63 | SPI.begin(); // Initialize SPI bus 64 | configSPI(); // Configure SPI 65 | 66 | // Reset IMU. 67 | regWrite(GLOB_CMD, 0xBE80); // Perform an IMU reset. 68 | delay(300); 69 | regWrite(GLOB_CMD, 0xBE00); // Take IMU out of reset state. 70 | delay(100); 71 | configSPI(); // Configure SPI controller 72 | delay(100); 73 | 74 | // Set default pin states 75 | pinMode(CS_, OUTPUT); // Set CS pin to be an output 76 | pinMode(RST_, OUTPUT); // Set RST pin to be an output 77 | digitalWrite(CS_, HIGH); // Initialize CS pin to be high 78 | digitalWrite(RST_, HIGH); // Initialize RST pin to be high 79 | 80 | /* ---------------- Perform IMU configuration --------------------------- */ 81 | regWrite(MSC_CTRL, 0x00C1); // Enable Data Ready on IMU. 82 | delay(20); 83 | regWrite(FLTR_CTRL, 0x0500); // Set Digital Filter on IMU. 84 | delay(20); 85 | regWrite(DEC_RATE, 0x0000); // Set Decimation on IMU. 86 | delay(20); 87 | 88 | Imu::setupPublisher(); 89 | } 90 | 91 | //////////////////////////////////////////////////////////////////////////// 92 | // Destructor 93 | //////////////////////////////////////////////////////////////////////////// 94 | ADIS16460::~ADIS16460() { 95 | // Close SPI bus 96 | SPI.end(); 97 | } 98 | 99 | //////////////////////////////////////////////////////////////////////////// 100 | // Performs a hardware reset by setting RST_ pin low for delay (in ms). 101 | //////////////////////////////////////////////////////////////////////////// 102 | int ADIS16460::resetDUT(uint8_t ms) { 103 | digitalWrite(RST_, LOW); 104 | delay(100); 105 | digitalWrite(RST_, HIGH); 106 | delay(ms); 107 | return (1); 108 | } 109 | 110 | //////////////////////////////////////////////////////////////////////////// 111 | // Sets SPI bit order, clock divider, and data mode. This function is useful 112 | // when there are multiple SPI devices using different settings. 113 | // Returns 1 when complete. 114 | //////////////////////////////////////////////////////////////////////////// 115 | int ADIS16460::configSPI() { 116 | SPISettings IMUSettings(1000000, MSBFIRST, SPI_MODE3); 117 | SPI.beginTransaction(IMUSettings); 118 | return (1); 119 | } 120 | 121 | //////////////////////////////////////////////////////////////////////////////////////////// 122 | // Reads two bytes (one word) in two sequential registers over SPI 123 | //////////////////////////////////////////////////////////////////////////////////////////// 124 | // regAddr - address of register to be read 125 | // return - (int) signed 16 bit 2's complement number 126 | //////////////////////////////////////////////////////////////////////////////////////////// 127 | int16_t ADIS16460::regRead(uint8_t regAddr) { 128 | // Read registers using SPI 129 | 130 | // Write register address to be read 131 | digitalWrite(CS_, LOW); // Set CS low to enable device 132 | SPI.transfer(regAddr); // Write address over SPI bus 133 | SPI.transfer(0x00); // Write 0x00 to the SPI bus fill the 16 bit transaction 134 | // requirement 135 | digitalWrite(CS_, HIGH); // Set CS high to disable device 136 | 137 | delayMicroseconds(40); // Delay to not violate read rate (16 us) 138 | 139 | // Read data from requested register 140 | digitalWrite(CS_, LOW); // Set CS low to enable device 141 | uint8_t _msbData = 142 | SPI.transfer(0x00); // Send (0x00) and place upper byte into variable 143 | uint8_t _lsbData = 144 | SPI.transfer(0x00); // Send (0x00) and place lower byte into variable 145 | digitalWrite(CS_, HIGH); // Set CS high to disable device 146 | 147 | delayMicroseconds(40); // Delay to not violate read rate (16 us) 148 | 149 | int16_t _dataOut = 150 | (_msbData << 8) | (_lsbData & 0xFF); // Concatenate upper and lower bytes 151 | // Shift MSB data left by 8 bits, mask LSB data with 0xFF, and OR both bits. 152 | 153 | return (_dataOut); 154 | } 155 | 156 | //////////////////////////////////////////////////////////////////////////// 157 | // Writes one byte of data to the specified register over SPI. 158 | // Returns 1 when complete. 159 | //////////////////////////////////////////////////////////////////////////// 160 | // regAddr - address of register to be written 161 | // regData - data to be written to the register 162 | //////////////////////////////////////////////////////////////////////////// 163 | int ADIS16460::regWrite(uint8_t regAddr, int16_t regData) { 164 | 165 | // Write register address and data 166 | uint16_t addr = 167 | (((regAddr & 0x7F) | 0x80) 168 | << 8); // Toggle sign bit, and check that the address is 8 bits 169 | uint16_t lowWord = 170 | (addr | (regData & 0xFF)); // OR Register address (A) with data(D) (AADD) 171 | uint16_t highWord = 172 | ((addr | 0x100) | 173 | ((regData >> 8) & 174 | 0xFF)); // OR Register address with data and increment address 175 | 176 | // Split words into chars 177 | uint8_t highBytehighWord = (highWord >> 8); 178 | uint8_t lowBytehighWord = (highWord & 0xFF); 179 | uint8_t highBytelowWord = (lowWord >> 8); 180 | uint8_t lowBytelowWord = (lowWord & 0xFF); 181 | 182 | // Write highWord to SPI bus 183 | digitalWrite(CS_, LOW); // Set CS low to enable device 184 | SPI.transfer(highBytelowWord); // Write high byte from low word to SPI bus 185 | SPI.transfer(lowBytelowWord); // Write low byte from low word to SPI bus 186 | digitalWrite(CS_, HIGH); // Set CS high to disable device 187 | 188 | delayMicroseconds(40); // Delay to not violate read rate (16 us) 189 | 190 | // Write lowWord to SPI bus 191 | digitalWrite(CS_, LOW); // Set CS low to enable device 192 | SPI.transfer(highBytehighWord); // Write high byte from high word to SPI bus 193 | SPI.transfer(lowBytehighWord); // Write low byte from high word to SPI bus 194 | digitalWrite(CS_, HIGH); // Set CS high to disable device 195 | 196 | return (1); 197 | } 198 | 199 | //////////////////////////////////////////////////////////////////////////// 200 | // Intiates a burst read from the sensor. 201 | // Returns a pointer to an array of sensor data. 202 | //////////////////////////////////////////////////////////////////////////// 203 | // No inputs required. 204 | //////////////////////////////////////////////////////////////////////////// 205 | int16_t *ADIS16460::burstRead(void) { 206 | uint8_t burstdata[20]; 207 | static int16_t burstwords[10]; 208 | // Trigger Burst Read 209 | digitalWrite(CS_, LOW); 210 | SPI.transfer(0x3E); 211 | SPI.transfer(0x00); 212 | // Read Burst Data 213 | burstdata[0] = SPI.transfer(0x00); // DIAG_STAT 214 | burstdata[1] = SPI.transfer(0x00); 215 | burstdata[2] = SPI.transfer(0x00); // XGYRO_OUT 216 | burstdata[3] = SPI.transfer(0x00); 217 | burstdata[4] = SPI.transfer(0x00); // YGYRO_OUT 218 | burstdata[5] = SPI.transfer(0x00); 219 | burstdata[6] = SPI.transfer(0x00); // ZGYRO_OUT 220 | burstdata[7] = SPI.transfer(0x00); 221 | burstdata[8] = SPI.transfer(0x00); // XACCEL_OUT 222 | burstdata[9] = SPI.transfer(0x00); 223 | burstdata[10] = SPI.transfer(0x00); // YACCEL_OUT 224 | burstdata[11] = SPI.transfer(0x00); 225 | burstdata[12] = SPI.transfer(0x00); // ZACCEL_OUT 226 | burstdata[13] = SPI.transfer(0x00); 227 | burstdata[14] = SPI.transfer(0x00); // TEMP_OUT 228 | burstdata[15] = SPI.transfer(0x00); 229 | burstdata[16] = SPI.transfer(0x00); // SMPL_CNTR 230 | burstdata[17] = SPI.transfer(0x00); 231 | burstdata[18] = SPI.transfer(0x00); // CHECKSUM 232 | burstdata[19] = SPI.transfer(0x00); 233 | digitalWrite(CS_, HIGH); 234 | 235 | // Join bytes into words 236 | burstwords[0] = ((burstdata[0] << 8) | (burstdata[1] & 0xFF)); // DIAG_STAT 237 | burstwords[1] = ((burstdata[2] << 8) | (burstdata[3] & 0xFF)); // XGYRO 238 | burstwords[2] = ((burstdata[4] << 8) | (burstdata[5] & 0xFF)); // YGYRO 239 | burstwords[3] = ((burstdata[6] << 8) | (burstdata[7] & 0xFF)); // ZGYRO 240 | burstwords[4] = ((burstdata[8] << 8) | (burstdata[9] & 0xFF)); // XACCEL 241 | burstwords[5] = ((burstdata[10] << 8) | (burstdata[11] & 0xFF)); // YACCEL 242 | burstwords[6] = ((burstdata[12] << 8) | (burstdata[13] & 0xFF)); // ZACCEL 243 | burstwords[7] = ((burstdata[14] << 8) | (burstdata[15] & 0xFF)); // TEMP_OUT 244 | burstwords[8] = ((burstdata[16] << 8) | (burstdata[17] & 0xFF)); // SMPL_CNTR 245 | burstwords[9] = ((burstdata[18] << 8) | (burstdata[19] & 0xFF)); // CHECKSUM 246 | 247 | return burstwords; 248 | } 249 | 250 | //////////////////////////////////////////////////////////////////////////// 251 | // Calculates checksum based on burst data. 252 | // Returns the calculated checksum. 253 | //////////////////////////////////////////////////////////////////////////// 254 | // *burstArray - array of burst data 255 | // return - (int16_t) signed calculated checksum 256 | //////////////////////////////////////////////////////////////////////////// 257 | int16_t ADIS16460::checksum(int16_t *burstArray) { 258 | int16_t s = 0; 259 | for (int i = 0; i < 9; i++) // Checksum value is not part of the sum!! 260 | { 261 | s += (burstArray[i] & 0xFF); // Count lower byte 262 | s += ((burstArray[i] >> 8) & 0xFF); // Count upper byte 263 | } 264 | 265 | return s; 266 | } 267 | 268 | ///////////////////////////////////////////////////////////////////////////////////////// 269 | // Converts accelerometer data output from the regRead() function and returns 270 | // acceleration in mg's 271 | ///////////////////////////////////////////////////////////////////////////////////////// 272 | // sensorData - data output from regRead() 273 | // return - (float) signed/scaled accelerometer in g's 274 | ///////////////////////////////////////////////////////////////////////////////////////// 275 | float ADIS16460::accelScale(int16_t sensorData) { 276 | float finalData = 277 | sensorData * 0.00025 * 9.81; // Multiply by accel sensitivity (25 mg/LSB) 278 | return finalData; 279 | } 280 | 281 | ///////////////////////////////////////////////////////////////////////////////////////////// 282 | // Converts gyro data output from the regRead() function and returns gyro rate 283 | // in deg/sec 284 | ///////////////////////////////////////////////////////////////////////////////////////////// 285 | // sensorData - data output from regRead() 286 | // return - (float) signed/scaled gyro in degrees/sec 287 | ///////////////////////////////////////////////////////////////////////////////////////// 288 | float ADIS16460::gyroScale(int16_t sensorData) { 289 | float finalData = sensorData * 0.005 * M_PI / 180; 290 | return finalData; 291 | } 292 | 293 | ///////////////////////////////////////////////////////////////////////////////////////////// 294 | // Converts temperature data output from the regRead() function and returns 295 | // temperature in degrees Celcius 296 | ///////////////////////////////////////////////////////////////////////////////////////////// 297 | // sensorData - data output from regRead() 298 | // return - (float) signed/scaled temperature in degrees Celcius 299 | ///////////////////////////////////////////////////////////////////////////////////////// 300 | float ADIS16460::tempScale(int16_t sensorData) { 301 | int signedData = 0; 302 | int isNeg = sensorData & 0x8000; 303 | if (isNeg == 0x8000) // If the number is negative, scale and sign the output 304 | signedData = sensorData - 0xFFFF; 305 | else 306 | signedData = sensorData; 307 | float finalData = 308 | (signedData * 0.05) + 309 | 25; // Multiply by temperature scale and add 25 to equal 0x0000 310 | return finalData; 311 | } 312 | 313 | ///////////////////////////////////////////////////////////////////////////////////////////// 314 | // Converts integrated angle data output from the regRead() function and returns 315 | // delta angle in degrees 316 | ///////////////////////////////////////////////////////////////////////////////////////////// 317 | // sensorData - data output from regRead() 318 | // return - (float) signed/scaled delta angle in degrees 319 | ///////////////////////////////////////////////////////////////////////////////////////// 320 | float ADIS16460::deltaAngleScale(int16_t sensorData) { 321 | float finalData = 322 | sensorData * 0.005; // Multiply by delta angle scale (0.005 degrees/LSB) 323 | return finalData; 324 | } 325 | 326 | ///////////////////////////////////////////////////////////////////////////////////////////// 327 | // Converts integrated velocity data output from the regRead() function and 328 | // returns delta velocity in mm/sec 329 | ///////////////////////////////////////////////////////////////////////////////////////////// 330 | // sensorData - data output from regRead() 331 | // return - (float) signed/scaled delta velocity in mm/sec 332 | ///////////////////////////////////////////////////////////////////////////////////////// 333 | float ADIS16460::deltaVelocityScale(int16_t sensorData) { 334 | float finalData = 335 | sensorData * 2.5; // Multiply by velocity scale (2.5 mm/sec/LSB) 336 | return finalData; 337 | } 338 | 339 | ///////////////////////////////////////////////////////////////////////////////////////////// 340 | // Method to update the sensor data without any validity checks (may result in 341 | // spikes). 342 | /////////////////////////////////////////////////////////////////////////////////////////////// 343 | bool ADIS16460::updateData() { 344 | sensor_data_ = burstRead(); 345 | return sensor_data_ != nullptr; 346 | } 347 | 348 | ///////////////////////////////////////////////////////////////////////////////////////////// 349 | // Method to update the internally stored sensor data recusivelly by checking 350 | // the validity. 351 | /////////////////////////////////////////////////////////////////////////////////////////////// 352 | bool ADIS16460::updateDataIterative() { 353 | uint64_t tic = micros(); 354 | bool success = false; 355 | for (size_t depth = 0; depth < kMaxRecursiveUpdateDepth; ++depth) { 356 | Sensor::setTimestampNow(); 357 | sensor_data_ = burstRead(); 358 | if (sensor_data_ == nullptr || sensor_data_[9] != checksum(sensor_data_)) { 359 | if (micros() - tic > kImuSyncTimeoutUs) { 360 | return false; 361 | } 362 | DEBUG_PRINTLN( 363 | topic_ + 364 | " (ADIS16460.cpp): Failed IMU update detected, trying again " + 365 | (String)(kMaxRecursiveUpdateDepth - depth) + " times."); 366 | } else { 367 | return true; 368 | } 369 | } 370 | return false; 371 | } 372 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/ADIS16460.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // September 2016 3 | // Author: Juan Jose Chong 4 | // Adapted by Florian Tschopp for use in versavis 5 | //////////////////////////////////////////////////////////////////////////////// 6 | // ADIS16460.h 7 | //////////////////////////////////////////////////////////////////////////////// 8 | // 9 | // This library provides all the functions necessary to interface the ADIS16460 10 | // IMU with a PJRC 32-Bit Teensy 3.2 Development Board. Functions for SPI 11 | // configuration, reads and writes, and scaling are included. This library may 12 | // be used for the entire ADIS1646X family of devices with some modification. 13 | // 14 | // Permission is hereby granted, free of charge, to any person obtaining 15 | // a copy of this software and associated documentation files (the 16 | // "Software"), to deal in the Software without restriction, including 17 | // without limitation the rights to use, copy, modify, merge, publish, 18 | // distribute, sublicense, and/or sell copies of the Software, and to 19 | // permit persons to whom the Software is furnished to do so, subject to 20 | // the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be 23 | // included in all copies or substantial portions of the Software. 24 | // 25 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 26 | // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 27 | // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 28 | // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 29 | // LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 30 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 31 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 32 | // 33 | //////////////////////////////////////////////////////////////////////////////// 34 | 35 | #define ADIS16460_h 36 | #include "Arduino.h" 37 | #include "Imu.h" 38 | #include 39 | #include 40 | 41 | // User Register Memory Map from Table 6 42 | #define FLASH_CNT 0x00 // Flash memory write count 43 | #define DIAG_STAT 0x02 // Diagnostic and operational status 44 | #define X_GYRO_LOW 0x04 // X-axis gyroscope output, lower word 45 | #define X_GYRO_OUT 0x06 // X-axis gyroscope output, upper word 46 | #define Y_GYRO_LOW 0x08 // Y-axis gyroscope output, lower word 47 | #define Y_GYRO_OUT 0x0A // Y-axis gyroscope output, upper word 48 | #define Z_GYRO_LOW 0x0C // Z-axis gyroscope output, lower word 49 | #define Z_GYRO_OUT 0x0E // Z-axis gyroscope output, upper word 50 | #define X_ACCL_LOW 0x10 // X-axis accelerometer output, lower word 51 | #define X_ACCL_OUT 0x12 // X-axis accelerometer output, upper word 52 | #define Y_ACCL_LOW 0x14 // Y-axis accelerometer output, lower word 53 | #define Y_ACCL_OUT 0x16 // Y-axis accelerometer output, upper word 54 | #define Z_ACCL_LOW 0x18 // Z-axis accelerometer output, lower word 55 | #define Z_ACCL_OUT 0x1A // Z-axis accelerometer output, upper word 56 | #define SMPL_CNTR 0x1C // Sample Counter, MSC_CTRL[3:2]=11 57 | #define TEMP_OUT 0x1E // Temperature output (internal, not calibrated) 58 | #define X_DELT_ANG 0x24 // X-axis delta angle output 59 | #define Y_DELT_ANG 0x26 // Y-axis delta angle output 60 | #define Z_DELT_ANG 0x28 // Z-axis delta angle output 61 | #define X_DELT_VEL 0x2A // X-axis delta velocity output 62 | #define Y_DELT_VEL 0x2C // Y-axis delta velocity output 63 | #define Z_DELT_VEL 0x2E // Z-axis delta velocity output 64 | #define MSC_CTRL 0x32 // Miscellaneous control 65 | #define SYNC_SCAL 0x34 // Sync input scale control 66 | #define DEC_RATE 0x36 // Decimation rate control 67 | #define FLTR_CTRL 0x38 // Filter control, auto-null record time 68 | #define GLOB_CMD 0x3E // Global commands 69 | #define XGYRO_OFF 0x40 // X-axis gyroscope bias offset error 70 | #define YGYRO_OFF 0x42 // Y-axis gyroscope bias offset error 71 | #define ZGYRO_OFF 0x44 // Z-axis gyroscope bias offset factor 72 | #define XACCL_OFF 0x46 // X-axis acceleration bias offset factor 73 | #define YACCL_OFF 0x48 // Y-axis acceleration bias offset factor 74 | #define ZACCL_OFF 0x4A // Z-axis acceleration bias offset factor 75 | #define LOT_ID1 0x52 // Lot identification number 76 | #define LOT_ID2 0x54 // Lot identification number 77 | #define PROD_ID 0x56 // Product identifier 78 | #define SERIAL_NUM 0x58 // Lot-specific serial number 79 | #define CAL_SGNTR 0x60 // Calibration memory signature value 80 | #define CAL_CRC 0x62 // Calibration memory CRC values 81 | #define CODE_SGNTR 0x64 // Code memory signature value 82 | #define CODE_CRC 0x66 // Code memory CRC values 83 | 84 | // ADIS16460 class definition 85 | class ADIS16460 : public Imu { 86 | 87 | public: 88 | // Constructor with configurable CS, data ready, and HW reset pins 89 | ADIS16460(ros::NodeHandle *nh, const String &topic, const int rate_hz, 90 | Timer &timer, int CS, int DR, int RST); 91 | 92 | // Destructor 93 | ~ADIS16460(); 94 | 95 | // Configure IMU 96 | void setup(); 97 | 98 | // Performs hardware reset by sending pin 8 low on the DUT for 2 seconds 99 | int resetDUT(uint8_t ms); 100 | 101 | // Sets SPI bit order, clock divider, and data mode 102 | int configSPI(); 103 | 104 | // Read single register from sensor 105 | int16_t regRead(uint8_t regAddr); 106 | 107 | // Write register 108 | int regWrite(uint8_t regAddr, int16_t regData); 109 | 110 | // Read sensor data using a burst read 111 | int16_t *burstRead(void); 112 | 113 | // Verify checksum 114 | int16_t checksum(int16_t *burstArray); 115 | 116 | // Scale accelerator data 117 | float accelScale(int16_t sensorData); 118 | 119 | // Scale gyro data 120 | float gyroScale(int16_t sensorData); 121 | 122 | // Scale temperature data 123 | float tempScale(int16_t sensorData); 124 | 125 | // Scale delta angle data 126 | float deltaAngleScale(int16_t sensorData); 127 | 128 | // Scale delta velocity 129 | float deltaVelocityScale(int16_t sensorData); 130 | 131 | // Update data internally with recursion. 132 | bool updateDataIterative(); 133 | 134 | // Update data withoput recursion. 135 | bool updateData(); 136 | 137 | private: 138 | // Variables to store hardware pin assignments 139 | int CS_; 140 | int RST_; 141 | }; 142 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Camera.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // Camera.cpp 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // This library provides functions regarding cameras in the versavis 9 | // package. Refer to the parent package versavis for license 10 | // information. 11 | // 12 | //////////////////////////////////////////////////////////////////////////////// 13 | 14 | #include "Camera.h" 15 | #include "helper.h" 16 | #include "versavis_configuration.h" 17 | 18 | Camera::Camera(ros::NodeHandle *nh, const String &topic, const int rate_hz, 19 | Timer &timer, const trigger_type &type, 20 | const uint8_t trigger_pin /*= 0 */, 21 | const uint8_t exposure_pin /*= 0 */, 22 | const bool exposure_compensation /*= true*/) 23 | : Sensor(nh, topic, rate_hz, timer, image_time_msg_, type), 24 | trigger_pin_(trigger_pin), exposure_pin_(exposure_pin), 25 | exposure_compensation_(exposure_compensation), is_configured_(true), 26 | compensating_(false), exposing_(false), image_number_(0), 27 | init_subscriber_((topic + "init").c_str(), &Camera::initCallback, this), 28 | initialized_(false) { 29 | // Check the input. 30 | if (trigger_pin == 0) { 31 | error((topic_ + " (Camera.cpp): Trigger pin is set to 0.").c_str(), 10); 32 | } 33 | if (exposure_pin == 0) { 34 | error((topic_ + " (Camera.cpp): Exposure pin is set to 0.").c_str(), 10); 35 | } 36 | Sensor::newMeasurementIsNotAvailable(); 37 | } 38 | 39 | void Camera::setup() { 40 | if (topic_.length() == 0) { 41 | // Cameras without a topic are considered as disconnected. 42 | DEBUG_PRINTLN( 43 | F("NO_TOPIC (Camera.cpp): Skip camera setup for disconnected camera.")); 44 | is_configured_ = false; 45 | initialized_ = true; 46 | return; 47 | } 48 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Setup.").c_str()); 49 | 50 | setupInitSubscriber(); 51 | setupPublisher(); 52 | 53 | pinMode(trigger_pin_, OUTPUT); 54 | digitalWrite(trigger_pin_, LOW); 55 | pinMode(exposure_pin_, INPUT); 56 | } 57 | 58 | void Camera::initialize() { 59 | if (!is_configured_ || initialized_) { 60 | return; 61 | } 62 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Initialize.").c_str()); 63 | Sensor::trigger(trigger_pin_, TRIGGER_PULSE_US * 10, type_); 64 | Sensor::setTimestampNow(); 65 | ++image_number_; 66 | image_time_msg_.number = image_number_; 67 | Sensor::newMeasurementIsAvailable(); 68 | publish(); 69 | #ifdef DEBUG 70 | initialized_ = true; 71 | #endif 72 | } 73 | 74 | void Camera::begin() { 75 | if (!is_configured_) { 76 | return; 77 | } 78 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Begin.").c_str()); 79 | // Maximal exposure time to still be able to keep up with the frequency 80 | // considering a security factor of 0.99, in us. 81 | max_exposure_time_us_ = 0.99 * 1e6 / rate_hz_; 82 | 83 | // Setup timer to periodically trigger the camera. 84 | Sensor::setupTimer(); 85 | } 86 | 87 | void Camera::setupPublisher() { 88 | pub_topic_ = topic_ + "image_time"; 89 | publisher_ = ros::Publisher(pub_topic_.c_str(), &image_time_msg_); 90 | DEBUG_PRINT((topic_ + " (Camera.cpp): Setup publisher with topic ").c_str()); 91 | DEBUG_PRINTLN(publisher_.topic_); 92 | #ifndef DEBUG 93 | nh_->advertise(publisher_); 94 | #endif 95 | } 96 | 97 | void Camera::setupInitSubscriber() { 98 | init_sub_topic_ = topic_ + "init"; 99 | init_subscriber_ = ros::Subscriber( 100 | init_sub_topic_.c_str(), &Camera::initCallback, this); 101 | DEBUG_PRINT( 102 | (topic_ + " (Camera.cpp): Setup init subscriber with topic ").c_str()); 103 | DEBUG_PRINTLN(init_subscriber_.topic_); 104 | #ifndef DEBUG 105 | nh_->subscribe(init_subscriber_); 106 | #endif 107 | } 108 | 109 | void Camera::initCallback(const std_msgs::Bool &msg) { 110 | initialized_ = msg.data; 111 | } 112 | 113 | void Camera::triggerMeasurement() { 114 | // Check whether an overflow caused the interrupt. 115 | if (!timer_.checkOverflow()) { 116 | DEBUG_PRINTLN( 117 | (topic_ + " (Camera.cpp): Timer interrupt but not overflown.").c_str()); 118 | return; 119 | } 120 | if (!is_configured_ || !initialized_) { 121 | return; 122 | } 123 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Timer overflow.").c_str()); 124 | 125 | if (exposure_compensation_ && compensating_) { 126 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Compensating.").c_str()); 127 | // Exposure-time compensating mode (Nikolic 2014). During exposure, the 128 | // timer will interrupt in the middle of the exposure time. At the end of 129 | // the exposure, the external interrupt will trigger exposureEnd() and 130 | // reset the timer to trigger the camera at the appropriate time. 131 | if (!exposing_) { 132 | DEBUG_PRINTLN( 133 | (topic_ + " (Camera.cpp): Not exposing. Trigger camera.").c_str()); 134 | // The camera is currently not exposing meaning that the interrupt 135 | // triggers at the beginning of the next image. 136 | exposing_ = true; 137 | 138 | #ifdef ILLUMINATION_MODULE 139 | // If the illumination module is active, the LEDs should turn on just 140 | // before the camera is exposing. 141 | digitalWrite(ILLUMINATION_PIN, HIGH); 142 | // Here, a warm-up delay for the LEDs can be added (needs checking). 143 | // delayMicroseconds(10); 144 | #endif 145 | // Trigger the actual pulse. 146 | Sensor::trigger(trigger_pin_, TRIGGER_PULSE_US, type_); 147 | 148 | // Save the current time to estimate the exposure time in the pin 149 | // interrupt. 150 | exposure_start_us_ = micros(); 151 | 152 | // Increament the image number as the camera is triggered now. 153 | ++image_number_; 154 | 155 | // Set the timer to the mid exposure point, e.g. half the exposure time. 156 | timer_.setCompare(exposure_delay_ticks_ > 0 ? exposure_delay_ticks_ - 1 157 | : compare_); 158 | } else { 159 | DEBUG_PRINTLN( 160 | (topic_ + " (Camera.cpp): Exposing right now, get timestamp.") 161 | .c_str()); 162 | // The camera is currently exposing. In this case, the interrupt is 163 | // triggered in the middle of the exposure time, where the timestamp 164 | // should be taken. 165 | Sensor::setTimestampNow(); 166 | Sensor::newMeasurementIsAvailable(); 167 | #ifdef ADD_TRIGGERS 168 | trigger(ADDITIONAL_TEST_PIN, TRIGGER_PULSE_US, 169 | Sensor::trigger_type::NON_INVERTED); 170 | #endif 171 | 172 | // Even though we are still in the compensating mode, deactivating here 173 | // ensures that we detect if a exposure signal is dropped and we switch 174 | // to non-compensating mode. 175 | compensating_ = false; 176 | 177 | // Set the timer to the standard period as we dont know the current 178 | // exposure time yet. 179 | timer_.setCompare(compare_); 180 | } 181 | } else { 182 | // "Standard" mode where the camera is triggered purely periodic. 183 | DEBUG_PRINTLN( 184 | (topic_ + 185 | " (Camera.cpp): Not compensating. Trigger camera and take timestamp.") 186 | .c_str()); 187 | exposing_ = true; 188 | 189 | #ifdef ILLUMINATION_MODULE 190 | // Deactivate the LEDs as we are not sure yet whether we get an exposure 191 | // signal. 192 | digitalWrite(ILLUMINATION_PIN, LOW); 193 | #endif 194 | // Trigger the actual pulse. 195 | Sensor::trigger(trigger_pin_, TRIGGER_PULSE_US, type_); 196 | 197 | Sensor::setTimestampNow(); 198 | Sensor::newMeasurementIsAvailable(); 199 | 200 | // Save the current time to estimate the exposure time in the pin 201 | // interrupt. 202 | exposure_start_us_ = micros(); 203 | 204 | // Increament the image number as the camera is triggered now. 205 | image_number_++; 206 | 207 | // Set the timer to make sure that the camera is triggered in a periodic 208 | // mode. 209 | timer_.setCompare(compare_); 210 | } 211 | // Reset the timer. 212 | timer_.resetOverflow(); 213 | } 214 | 215 | void Camera::exposureEnd() { 216 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Exposure end.").c_str()); 217 | if (exposure_compensation_) { 218 | unsigned long last_exposure_time_us = micros() - exposure_start_us_; 219 | DEBUG_PRINT((topic_ + " (Camera.cpp): exposure time [us] ").c_str()); 220 | DEBUG_PRINTDECLN(last_exposure_time_us); 221 | calculateDelayTicksAndCompensate(last_exposure_time_us); 222 | exposing_ = false; 223 | } 224 | } 225 | 226 | void Camera::publish() { 227 | if (Sensor::isNewMeasurementAvailable()) { 228 | DEBUG_PRINTLN((topic_ + " (Camera.cpp): Publish.").c_str()); 229 | image_time_msg_.time = Sensor::getTimestamp(); 230 | image_time_msg_.number = image_number_; 231 | #ifndef DEBUG 232 | publisher_.publish(&image_time_msg_); 233 | #endif 234 | Sensor::newMeasurementIsNotAvailable(); 235 | } 236 | } 237 | 238 | void Camera::calculateDelayTicksAndCompensate( 239 | const unsigned long &last_exposure_time_us) { 240 | // The goal here is to shift the time of the next camera trigger half the 241 | // exposure time before the mid-exposure time. 242 | 243 | // The next frame should be triggered by this time before the mid-exposure 244 | // time (in CPU ticks). 245 | if (last_exposure_time_us == 0 || 246 | last_exposure_time_us >= max_exposure_time_us_) { 247 | // In this case, something with the measurement went wrong or the camera 248 | // is dropping triggers due to a too high exposure time (constrain the 249 | // maximal exposure time of your camera to be within the period time of 250 | // your triggering). Switch to non-compensating mode. 251 | exposure_delay_ticks_ = 0; 252 | compensating_ = false; 253 | } else { 254 | exposure_delay_ticks_ = static_cast(last_exposure_time_us) / 2.0 / 255 | 1000000.0 * cpu_freq_prescaler_; 256 | compensating_ = true; 257 | } 258 | 259 | // Reset the compare register of the timer. 260 | timer_.setCompare(compare_ - exposure_delay_ticks_); 261 | } 262 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Camera.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // Camera.h 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // This library provides functions regarding cameras in the versavis 9 | // package. Refer to the parent package versavis for license 10 | // information. 11 | // 12 | //////////////////////////////////////////////////////////////////////////////// 13 | 14 | #ifndef Camera_h 15 | #define Camera_h 16 | 17 | #include "Arduino.h" 18 | #include "Sensor.h" 19 | #include 20 | #include 21 | #include 22 | 23 | // timers Class Definition 24 | class Camera : public Sensor { 25 | public: 26 | Camera(ros::NodeHandle *nh, const String &topic, const int rate_hz, 27 | Timer &timer, const trigger_type &type, const uint8_t trigger_pin = 0, 28 | const uint8_t exposure_pin = 0, 29 | const bool exposure_compensation = true); 30 | 31 | void begin(); 32 | int exposurePin() const { return exposure_pin_; }; 33 | bool isExposing() const { return exposing_; }; 34 | bool isInitialized() const { return initialized_; }; 35 | 36 | void triggerMeasurement(); 37 | void exposureEnd(); 38 | 39 | void publish(); 40 | void setup(); // Setup publishers and pins. 41 | void initialize(); // Perform initialization procedure. 42 | void setupPublisher(); 43 | void setupInitSubscriber(); 44 | void 45 | initCallback(const std_msgs::Bool &msg); // Gets called as soon as the host 46 | // reports the camera as initialized. 47 | 48 | void 49 | calculateDelayTicksAndCompensate(const unsigned long &last_exposure_time_us); 50 | 51 | private: 52 | // Message to save sequence number and timestamp. 53 | versavis::TimeNumbered image_time_msg_; 54 | 55 | // Hardware pins for trigger and exposure signal. 56 | const uint8_t trigger_pin_; 57 | const uint8_t exposure_pin_; 58 | 59 | // Flag whether the camera should perform exposure compensation and is 60 | // currently in compensating mode (see Nikolic 2014). 61 | const bool exposure_compensation_; 62 | bool is_configured_; 63 | bool compensating_; 64 | 65 | // Flag whether the camera is currently exposing or not. 66 | bool exposing_; 67 | 68 | // Maximum exposure time to check whether measurement is valid. 69 | uint32_t max_exposure_time_us_; 70 | 71 | // Time when exposure started. 72 | uint64_t exposure_start_us_; 73 | 74 | // The image number is a strictly incrasing number used for data association 75 | // of time stamp and image. 76 | uint32_t image_number_; 77 | 78 | // Time the next frame should be triggered before the mid-exposure time (in 79 | // CPU ticks). 80 | unsigned long exposure_delay_ticks_; 81 | 82 | // ROS related. 83 | String pub_topic_; 84 | String init_sub_topic_; 85 | ros::Subscriber init_subscriber_; 86 | bool initialized_; 87 | }; 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Imu.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "Imu.h" 3 | #include "helper.h" 4 | #include "versavis_configuration.h" 5 | 6 | Imu::Imu(ros::NodeHandle *nh, const String &topic, const int rate_hz, 7 | Timer &timer) 8 | : Sensor(nh, topic, rate_hz, timer, imu_msg_), kMaxRecursiveUpdateDepth(5u), 9 | kImuSyncTimeoutUs(4000) {} 10 | 11 | void Imu::triggerMeasurement() { 12 | // Check whether an overflow caused the interrupt. 13 | if (!timer_.checkOverflow()) { 14 | DEBUG_PRINTLN( 15 | (topic_ + " (Imu.cpp): Timer interrupt but not overflown.").c_str()); 16 | return; 17 | } 18 | 19 | DEBUG_PRINTLN((topic_ + " (Imu.cpp): Trigger.").c_str()); 20 | Sensor::setTimestampNow(); 21 | Sensor::newMeasurementIsAvailable(); 22 | 23 | #ifdef ADD_TRIGGERS 24 | trigger(ADDITIONAL_TEST_PIN, TRIGGER_PULSE_US, 25 | Sensor::trigger_type::NON_INVERTED); 26 | #endif 27 | // Reset the timer. 28 | timer_.resetOverflow(); 29 | } 30 | 31 | void Imu::publish() { 32 | if (Sensor::isNewMeasurementAvailable()) { 33 | DEBUG_PRINTLN((topic_ + " (Imu.cpp): Publish.")); 34 | bool success; 35 | #ifdef DEBUG 36 | success = updateDataIterative(); 37 | #else 38 | success = updateDataIterative(); 39 | #endif 40 | if (!success) { 41 | Sensor::newMeasurementIsNotAvailable(); 42 | DEBUG_PRINTLN((topic_ + " (Imu.cpp): IMU update failed.").c_str()); 43 | } else { 44 | imu_msg_.time.data = Sensor::getTimestamp(); 45 | if (sensor_data_ == nullptr) { 46 | error((topic_ + " (Imu.cpp): sensor_data == nullptr").c_str(), 10); 47 | } 48 | imu_msg_.gx = sensor_data_[ImuReading::GX]; 49 | imu_msg_.gy = sensor_data_[ImuReading::GY]; 50 | imu_msg_.gz = sensor_data_[ImuReading::GZ]; 51 | imu_msg_.ax = sensor_data_[ImuReading::AX]; 52 | imu_msg_.ay = sensor_data_[ImuReading::AY]; 53 | imu_msg_.az = sensor_data_[ImuReading::AZ]; 54 | #ifndef DEBUG 55 | publisher_.publish(&imu_msg_); 56 | #endif 57 | Sensor::newMeasurementIsNotAvailable(); 58 | } 59 | } 60 | } 61 | 62 | void Imu::setupPublisher() { 63 | publisher_ = ros::Publisher(topic_.c_str(), &imu_msg_); 64 | DEBUG_PRINT((topic_ + " (Imu.cpp): Setup publisher with topic ").c_str()); 65 | DEBUG_PRINTLN(publisher_.topic_); 66 | #ifndef DEBUG 67 | nh_->advertise(publisher_); 68 | #endif 69 | } 70 | 71 | void Imu::begin() { 72 | DEBUG_PRINTLN((topic_ + " (Imu.cpp): Begin.").c_str()); 73 | Sensor::setupTimer(); 74 | } 75 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Imu.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // Imu.h 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Basic implementation for IMUs in the versavis framework. Refer to 9 | // the parent package versavis for license information. 10 | // 11 | //////////////////////////////////////////////////////////////////////////////// 12 | 13 | #ifndef Imu_h 14 | #define Imu_h 15 | 16 | #include "Arduino.h" 17 | #include "Sensor.h" 18 | #include 19 | #include 20 | 21 | enum ImuReading { 22 | STAT = 0, 23 | GX = 1, 24 | GY = 2, 25 | GZ = 3, 26 | AX = 4, 27 | AY = 5, 28 | AZ = 6, 29 | // The reminder of the entries is not identical for the different ADIS IMUs. 30 | // Check burst read function of the specific IMU. 31 | }; 32 | 33 | class Imu : public Sensor { 34 | public: 35 | Imu(ros::NodeHandle *nh, const String &topic, const int rate_hz, 36 | Timer &timer); 37 | virtual void setup() = 0; 38 | void begin(); 39 | void triggerMeasurement(); 40 | void publish(); 41 | void setupPublisher(); 42 | 43 | // Update data internally with recursion. 44 | virtual bool updateDataIterative() = 0; 45 | 46 | // Update data internally without recursion. 47 | virtual bool updateData() = 0; 48 | 49 | protected: 50 | int16_t *sensor_data_; 51 | const unsigned int kMaxRecursiveUpdateDepth; 52 | const uint64_t kImuSyncTimeoutUs; 53 | 54 | private: 55 | versavis::ImuMicro imu_msg_; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "Arduino.h" 2 | #include "Sensor.h" 3 | #include "helper.h" 4 | #include "versavis_configuration.h" 5 | 6 | // Image time message version. 7 | Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz, 8 | Timer &timer, versavis::TimeNumbered &img_time_msg, 9 | const trigger_type type /* = trigger_type::NON_INVERTED */) 10 | : nh_(nh), topic_(topic), 11 | publisher_((topic + "img_time").c_str(), &img_time_msg), 12 | new_measurement_available_(false), rate_hz_(rate_hz), timer_(timer), 13 | type_(type), max_compare_(pow(2, 16)) { 14 | if (nh == nullptr) { 15 | error((topic_ + " (Sensor.cpp): The node handle is not available.").c_str(), 16 | 49); 17 | } 18 | if (rate_hz <= 0) { 19 | error((topic_ + " (Sensor.cpp): The rate of a sensor needs to be positive.") 20 | .c_str(), 21 | 50); 22 | } 23 | } 24 | 25 | // IMU message version. 26 | Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz, 27 | Timer &timer, versavis::ImuMicro &imu_msg, 28 | const trigger_type type /* = trigger_type::NON_INVERTED */) 29 | : nh_(nh), topic_(topic), publisher_(topic.c_str(), &imu_msg), 30 | new_measurement_available_(false), rate_hz_(rate_hz), timer_(timer), 31 | type_(type), max_compare_(pow(2, 16)) { 32 | if (nh == nullptr) { 33 | error((topic_ + " (Sensor.cpp): The node handle is not available.").c_str(), 34 | 49); 35 | } 36 | if (rate_hz <= 0) { 37 | error((topic_ + " (Sensor.cpp): The rate of a sensor needs to be positive.") 38 | .c_str(), 39 | 50); 40 | } 41 | if (topic.length() == 0) { 42 | error((topic_ + " (Sensor.cpp): IMU topic is empty.").c_str(), 51); 43 | } 44 | } 45 | 46 | void Sensor::setTimestampNow() { timestamp_ = nh_->now(); } 47 | 48 | ros::Time Sensor::getTimestamp() const { return timestamp_; } 49 | 50 | bool Sensor::isNewMeasurementAvailable() const { 51 | return new_measurement_available_; 52 | } 53 | 54 | void Sensor::newMeasurementIsAvailable() { new_measurement_available_ = true; } 55 | 56 | void Sensor::newMeasurementIsNotAvailable() { 57 | new_measurement_available_ = false; 58 | } 59 | 60 | uint16_t Sensor::calculatePrescaler(const uint16_t rate_hz) const { 61 | // All timers on Arduino Zero are assumed to be set to 16 bit. 62 | const uint32_t kMaxCount = 65536; 63 | 64 | // Amount of clock ticks required to count to the specified period time. 65 | const uint32_t kRequiredTicks = CPU_FREQ_HZ / static_cast(rate_hz); 66 | DEBUG_PRINT((topic_ + " (Sensor.cpp): required ticks ")); 67 | DEBUG_PRINTDECLN(kRequiredTicks); 68 | // Available prescalers on Arduino Zero are restricted to those values. 69 | const uint16_t kAvailablePrescalers[8] = {1, 2, 4, 8, 16, 64, 256, 1024}; 70 | 71 | for (uint8_t i = 0; i < 8; ++i) { 72 | if (kMaxCount > kRequiredTicks / kAvailablePrescalers[i] && 73 | kRequiredTicks % kAvailablePrescalers[i] == 0) { 74 | return kAvailablePrescalers[i]; 75 | } 76 | } 77 | // If this part is reached, no available prescaler fits with the goal 78 | // framerate on this MCU. 79 | error((topic_ + " (Sensor.cpp): No prescaler found.").c_str(), 50); 80 | return 0; 81 | } 82 | 83 | void Sensor::setupTimer() { 84 | prescaler_ = calculatePrescaler(rate_hz_); 85 | if (prescaler_ == 0) { 86 | error((topic_ + " (Sensor.cpp): prescaler_ is zero.").c_str(), 99); 87 | } 88 | cpu_freq_prescaler_ = CPU_FREQ_HZ / prescaler_; 89 | DEBUG_PRINT((topic_ + " (Sensor.cpp): Setup timer with prescaler ").c_str()); 90 | DEBUG_PRINTDECLN(prescaler_); 91 | // The compare value defines at which tick the timer is going to overflow 92 | // and triggering the corresponding interrupt. 93 | compare_ = 1.0 / rate_hz_ * cpu_freq_prescaler_ - 1; 94 | if (compare_ > max_compare_) { 95 | // The compare value can never be reached by a 16bit timer. Choose a 96 | // higher prescaler or another timer with 32bit. 97 | error((topic_ + " (Sensor.cpp): compare_ > pow(2,16) failed.").c_str(), 98 | 100); 99 | } 100 | 101 | // Actually setting up the timer registers. 102 | timer_.initialize(prescaler_, compare_, topic_); 103 | } 104 | 105 | void Sensor::trigger(const int pin, const int pulse_time_us, 106 | const trigger_type &type) { 107 | // Perform the trigger pulse. 108 | if (type == NON_INVERTED) { 109 | digitalWrite(pin, HIGH); 110 | delayMicroseconds(pulse_time_us); 111 | digitalWrite(pin, LOW); 112 | } else if (type == INVERTED) { 113 | digitalWrite(pin, LOW); 114 | delayMicroseconds(pulse_time_us); 115 | digitalWrite(pin, HIGH); 116 | } else { 117 | error((topic_ + " (Sensor.cpp): Trigger type does not exist.").c_str(), 118 | 1000); 119 | } 120 | } 121 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Sensor.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // Sensor.h 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Basic implementation for generic sensors in the versavis framework. Refer to 9 | // the parent package versavis for license information. 10 | // 11 | //////////////////////////////////////////////////////////////////////////////// 12 | 13 | #ifndef Sensor_h 14 | #define Sensor_h 15 | 16 | #include "Arduino.h" 17 | #include "Timer.h" 18 | #include 19 | #include 20 | #include 21 | 22 | enum trigger_type { INVERTED, NON_INVERTED }; 23 | 24 | class Sensor { 25 | public: 26 | Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz, 27 | Timer &timer, versavis::TimeNumbered &img_time_msg, 28 | const trigger_type type = trigger_type::NON_INVERTED); 29 | Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz, 30 | Timer &timer, versavis::ImuMicro &imu_msg, 31 | const trigger_type type = trigger_type::NON_INVERTED); 32 | inline virtual void setup(){/* do nothing */}; 33 | inline virtual void begin(){/* do nothing */}; 34 | inline virtual void triggerMeasurement() = 0; 35 | inline virtual void publish() = 0; 36 | inline virtual void setupPublisher() = 0; 37 | 38 | void setTimestampNow(); 39 | ros::Time getTimestamp() const; 40 | bool isNewMeasurementAvailable() const; 41 | void newMeasurementIsAvailable(); 42 | void newMeasurementIsNotAvailable(); 43 | 44 | void setupTimer(); 45 | 46 | void trigger(const int pin, const int pulse_time_us, 47 | const trigger_type &type); 48 | 49 | // ------------ROS members----------- 50 | protected: 51 | ros::NodeHandle *nh_; 52 | String topic_; 53 | ros::Publisher publisher_; 54 | 55 | private: 56 | ros::Time timestamp_; 57 | bool new_measurement_available_; 58 | 59 | //------------Timer members----------- 60 | protected: 61 | // Frequency of the timer in Hz. 62 | uint16_t rate_hz_; 63 | // The timer object (can be TCC or TcCount16). 64 | Timer timer_; 65 | // Trigger type. Non-inverted: The logic level is at 0 and goes to 1 for 66 | // triggering. Inverted: The logic level is at 1 and goes to 0 for triggering. 67 | const trigger_type type_; 68 | 69 | // Compare register to use for timer overlow / interrupt. 70 | unsigned long compare_; 71 | const unsigned long max_compare_; 72 | // Prescaler of the timer, which is set automatically. 73 | uint16_t prescaler_; 74 | double cpu_freq_prescaler_; 75 | 76 | private: 77 | // Function to determine prescaler based on timer size and frequency. 78 | uint16_t calculatePrescaler(const uint16_t rate_hz) const; 79 | }; 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Timer.cpp: -------------------------------------------------------------------------------- 1 | #include "Timer.h" 2 | #include "helper.h" 3 | #include "versavis_configuration.h" 4 | 5 | // Constructor for TCC timer type. 6 | Timer::Timer(Tcc *tcc_timer) : tcc_timer_(tcc_timer), is_tcc_(true) { 7 | if (tcc_timer == nullptr) { 8 | error("NO_TOPIC (Timer.cpp): TCC timer does not exist.", 201); 9 | } 10 | } 11 | 12 | // Constructor for TcCount16 timer type. 13 | Timer::Timer(TcCount16 *tc_timer) : tc_timer_(tc_timer), is_tcc_(false) { 14 | if (tc_timer == nullptr) { 15 | error("NO_TOPIC (Timer.cpp): TC timer does not exist.", 201); 16 | } 17 | } 18 | 19 | bool Timer::checkOverflow() const { 20 | if (is_tcc_) { 21 | return tcc_timer_->INTFLAG.bit.OVF == 1; 22 | } else { 23 | return tc_timer_->INTFLAG.bit.OVF == 1; 24 | } 25 | } 26 | 27 | void Timer::resetOverflow() { 28 | // Writing a one clears the ovf flag of the timer. 29 | if (is_tcc_) { 30 | tcc_timer_->INTFLAG.bit.OVF = 1; 31 | } else { 32 | tc_timer_->INTFLAG.bit.OVF = 1; 33 | } 34 | } 35 | 36 | void Timer::initialize(const uint16_t prescaler, const long &compare, 37 | const String &topic) { 38 | DEBUG_PRINTLN((topic + " (Timer.cpp): Initialize.").c_str()); 39 | if (is_tcc_) { 40 | DEBUG_PRINTLN((topic + " (Timer.cpp): is_tcc.").c_str()); 41 | // -------------- For TCC timers ----------------- 42 | tcc_timer_->CTRLA.reg &= ~TCC_CTRLA_ENABLE; // Disable the timers 43 | while (tcc_timer_->SYNCBUSY.bit.ENABLE == 1) { 44 | ; // wait for sync. 45 | } 46 | 47 | // Set bits for prescaler (32, 128 and 512 doesn’t work) 48 | switch (prescaler) { 49 | case 1: 50 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV1; 51 | break; 52 | case 2: 53 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV2; 54 | break; 55 | case 4: 56 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV4; 57 | break; 58 | case 8: 59 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV8; 60 | break; 61 | case 16: 62 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV16; 63 | break; 64 | case 64: 65 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV64; 66 | break; 67 | case 256: 68 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV256; 69 | break; 70 | case 1024: 71 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_PRESCALER_DIV1024; 72 | break; 73 | default: 74 | error((topic + " (Timer.cpp): Error 200: Prescaler is not available.") 75 | .c_str(), 76 | 200); 77 | break; 78 | } 79 | 80 | tcc_timer_->WAVE.reg |= 81 | TCC_WAVE_WAVEGEN_MFRQ; // Set wave form on match frequency configuration 82 | // -> CC0 match. 83 | while (tcc_timer_->SYNCBUSY.bit.WAVE == 1) { 84 | ; // wait for sync. 85 | } 86 | 87 | tcc_timer_->CC[0].reg = 88 | compare; // define period: rate_hz, also control the prescaler! 89 | while (tcc_timer_->SYNCBUSY.bit.CC0 == 1) { 90 | ; // wait for sync. 91 | } 92 | 93 | tcc_timer_->INTENSET.reg = 0; // disable all interrupts. 94 | tcc_timer_->INTENSET.bit.OVF = 1; // enable overfollow. 95 | 96 | tcc_timer_->CTRLA.reg |= TCC_CTRLA_ENABLE; // enable timer. 97 | while (tcc_timer_->SYNCBUSY.bit.ENABLE == 1) { 98 | ; 99 | } 100 | } else { 101 | DEBUG_PRINTLN((topic + " (Timer.cpp): is_tc.").c_str()); 102 | // -------------- For TC timers ----------------- 103 | tc_timer_->CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable the timers 104 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 105 | ; // wait for sync 106 | } 107 | // Set bits for prescaler (32, 128 and 512 doesn’t work) 108 | switch (prescaler) { 109 | case 1: 110 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1; 111 | break; 112 | case 2: 113 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; 114 | break; 115 | case 4: 116 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV4; 117 | break; 118 | case 8: 119 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV8; 120 | break; 121 | case 16: 122 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV16; 123 | break; 124 | case 64: 125 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV64; 126 | break; 127 | case 256: 128 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV256; 129 | break; 130 | case 1024: 131 | tc_timer_->CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1024; 132 | break; 133 | default: 134 | error((topic + " (Timer.cpp): Error 200: Prescaler is not available.") 135 | .c_str(), 136 | 200); 137 | break; 138 | } 139 | 140 | tc_timer_->CTRLA.reg |= TC_CTRLA_MODE_COUNT16; 141 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 142 | ; // wait for sync. 143 | } 144 | tc_timer_->CTRLA.reg |= 145 | TC_CTRLA_WAVEGEN_MFRQ; // Set wave form on match frequency 146 | // configuration -> CC0 match. 147 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 148 | ; // wait for sync. 149 | } 150 | 151 | tc_timer_->CC[0].reg = 152 | compare; // define period: rate_hz, also control the prescaler! 153 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 154 | ; // wait for sync. 155 | } 156 | 157 | tc_timer_->INTENSET.reg = 0; // disable all interrupts. 158 | tc_timer_->INTENSET.bit.OVF = 1; // enable overfollow. 159 | 160 | tc_timer_->CTRLA.reg |= TC_CTRLA_ENABLE; // enable timer. 161 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 162 | ; // wait for sync. 163 | } 164 | } 165 | } 166 | 167 | void Timer::setCompare(const long &compare) { 168 | if (is_tcc_) { 169 | tcc_timer_->CC[0].reg = compare; 170 | while (tcc_timer_->SYNCBUSY.bit.CC0 == 1) { 171 | ; // wait for sync. 172 | } 173 | } else { 174 | tc_timer_->CC[0].reg = compare; 175 | while (tc_timer_->STATUS.bit.SYNCBUSY == 1) { 176 | ; // wait for sync. 177 | } 178 | } 179 | } 180 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/Timer.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // Timer.h 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // This library provides interfacing functions for timers for ARM processors 9 | // (tested Ardnuino Zero). Refer to the parent package versavis for 10 | // license information. 11 | // 12 | //////////////////////////////////////////////////////////////////////////////// 13 | 14 | #ifndef Timer_h 15 | #define Timer_h 16 | 17 | #include "Arduino.h" 18 | 19 | // timers Class Definition 20 | class Timer { 21 | public: 22 | // Constructor for TCC timer type. 23 | Timer(Tcc *tcc_timer); 24 | 25 | // Constructor for TcCount16 timer type. 26 | Timer(TcCount16 *tc_timer); 27 | 28 | // Initialize 29 | void initialize(const uint16_t prescaler, const long &compare, 30 | const String &topic); 31 | 32 | // Check whether an overflow triggered the interrupt. 33 | bool checkOverflow() const; 34 | 35 | // Reset the overflow flag. 36 | void resetOverflow(); 37 | 38 | // Set a new compare register for the interupt. 39 | void setCompare(const long &compare); 40 | 41 | private: 42 | // Pointers to the actual timer. Depends on the type. 43 | Tcc *tcc_timer_; 44 | TcCount16 *tc_timer_; 45 | 46 | // Flag whether the timer is tcc or TcCount16 47 | bool is_tcc_; 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/VN100.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // May 2015 3 | // Author: light Dynamics and Control Lab 4 | // Adapted by Florian Tschopp for use in VersaVIS 5 | //////////////////////////////////////////////////////////////////////////////// 6 | // VN100.cpp 7 | //////////////////////////////////////////////////////////////////////////////// 8 | // 9 | // Copyright (c) 2018 Flight Dynamics and Control Lab 10 | 11 | // Permission is hereby granted, free of charge, to any person obtaining a copy 12 | // of this software and associated documentation files (the "Software"), to deal 13 | // in the Software without restriction, including without limitation the rights 14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | // copies of the Software, and to permit persons to whom the Software is 16 | // furnished to do so, subject to the following conditions: 17 | // 18 | // The above copyright notice and this permission notice shall be included in 19 | // all copies or substantial portions of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | // SOFTWARE. 28 | 29 | // NOTE: Before running this code, you must configure the IMU as 30 | // defined in the README file. 31 | //////////////////////////////////////////////////////////////////////////////// 32 | 33 | #include "VN100.h" 34 | #include "helper.h" 35 | #include "versavis_configuration.h" 36 | 37 | VN100::VN100(ros::NodeHandle *nh, const String &topic, const int rate_hz, 38 | Timer &timer) 39 | : Imu(nh, topic, rate_hz, timer), kMessageLength(30) { 40 | imu_accelerator_sensitivity_ = 1.0 / (0.00025 * 9.81); 41 | imu_gyro_sensitivity_ = 1.0 / (0.05 * M_PI / 180); 42 | in_ = new byte[kMessageLength]; 43 | } 44 | 45 | VN100::~VN100() { 46 | if (in_ != nullptr) { 47 | delete in_; 48 | } 49 | } 50 | 51 | void VN100::setup() { 52 | DEBUG_PRINTLN((topic_ + " (VN100.cpp): Setup.").c_str()); 53 | // Start Serial1 for IMU communication 54 | Serial1.begin(921600, SERIAL_8N1); 55 | delay(20); 56 | Serial.setTimeout(2); 57 | 58 | // Check whether IMU is setup correctly. Configuration is done on VectorNav 59 | // Control Center software. 60 | Serial1.print("$VNRRG,00*XX\r\n"); 61 | delay(20); 62 | byte response[100]; 63 | Serial1.readBytes(response, Serial1.available()); 64 | 65 | // Check that "User Tag" is set to 7665727361766973 (HEX for versavis). 66 | if (strcmp((char *)response, "$VNRRG,00,7665727361766973*51\r\n") != 0) { 67 | error((topic_ + " (VN100.cpp): IMU is not setup correctly.").c_str(), 11); 68 | } 69 | Imu::setupPublisher(); 70 | } 71 | 72 | //////////////////////////////////////////////////////////////////////////// 73 | // Read the IMU bytes. 74 | //////////////////////////////////////////////////////////////////////////// 75 | int16_t *VN100::readImuData() { 76 | clearBuffer(); 77 | Serial1.print("$VNBOM,1*XX\r\n"); 78 | const uint64_t local_tic = micros(); 79 | while (Serial1.available() < kMessageLength) { 80 | if (micros() - local_tic < kImuSyncTimeoutUs / 2) { 81 | delayMicroseconds(1); // Wait until full message is received. 82 | } else { 83 | DEBUG_PRINTLN(topic_ + 84 | " (VN100.cpp): Serial data never available (timeout?)."); 85 | return nullptr; 86 | } 87 | } 88 | size_t bytes = Serial1.readBytes(in_, kMessageLength); 89 | if (bytes != kMessageLength) { 90 | DEBUG_PRINTLN(topic_ + " (VN100.cpp): Not all data read (timeout?)."); 91 | return nullptr; 92 | } 93 | if (in_[0] != 0xFA) { 94 | DEBUG_PRINTLN(topic_ + " (VN100.cpp): Start byte not detected."); 95 | return nullptr; 96 | } 97 | checksum_.b[0] = in_[29]; 98 | checksum_.b[1] = in_[28]; 99 | for (size_t i = 0; i < 4; ++i) { 100 | a_x_.b[i] = in_[4 + i]; 101 | a_y_.b[i] = in_[8 + i]; 102 | a_z_.b[i] = in_[12 + i]; 103 | W_x_.b[i] = in_[16 + i]; 104 | W_y_.b[i] = in_[20 + i]; 105 | W_z_.b[i] = in_[24 + i]; 106 | } 107 | static int16_t scaled_sensor_data[7]; 108 | scaled_sensor_data[1] = W_x_.f * imu_gyro_sensitivity_; 109 | scaled_sensor_data[2] = W_y_.f * imu_gyro_sensitivity_; 110 | scaled_sensor_data[3] = W_z_.f * imu_gyro_sensitivity_; 111 | scaled_sensor_data[4] = a_x_.f * imu_accelerator_sensitivity_; 112 | scaled_sensor_data[5] = a_y_.f * imu_accelerator_sensitivity_; 113 | scaled_sensor_data[6] = a_z_.f * imu_accelerator_sensitivity_; 114 | return (scaled_sensor_data); // Return pointer with data 115 | } 116 | 117 | //////////////////////////////////////////////////////////////////////////// 118 | // Calculates checksum based on binary data. 119 | // Returns the calculated checksum. 120 | // 121 | // From the documentation: 122 | // The CRC consists of a 16-bit CRC of the packet. The CRC is calculated over 123 | // the packet starting just after the sync byte in the header (not including the 124 | // sync byte) and ending at the end of the payload. More information about the 125 | // CRC algorithm and example code for how to perform the calculation is shown in 126 | // the Checksum/CRC section of the Software Architecture chapter. The CRC is 127 | // selected such that if you compute the 16-bit CRC starting with the group byte 128 | // and include the CRC itself, a valid packet will result in 0x0000 computed by 129 | // the running CRC calculation over the entire packet. This provides a simple 130 | // way of detecting packet corruption by simply checking to see if the CRC 131 | // calculation of the entire packet (not including the sync byte) results in 132 | // zero. 133 | //////////////////////////////////////////////////////////////////////////// 134 | unsigned short VN100::checksum(byte data[], size_t length, 135 | bool ignore_first_byte) { 136 | unsigned short crc = 0; 137 | const size_t start_idx = ignore_first_byte ? 1 : 0; 138 | for (size_t i = start_idx; i < length; ++i) { 139 | crc = (byte)(crc >> 8) | (crc << 8); 140 | crc ^= data[i]; 141 | crc ^= (byte)(crc & 0xff) >> 4; 142 | crc ^= crc << 12; 143 | crc ^= (crc & 0x00ff) << 5; 144 | } 145 | return crc; 146 | } 147 | 148 | //////////////////////////////////////////////////////////////////////////// 149 | // Clear Serial buffer for any remaining data. 150 | //////////////////////////////////////////////////////////////////////////// 151 | void VN100::clearBuffer() { 152 | const uint64_t local_tic = micros(); 153 | while (Serial1.available()) { 154 | if (micros() - local_tic < kImuSyncTimeoutUs / 3) { 155 | Serial1.readBytes(in_, Serial1.available()); 156 | } else { 157 | return; 158 | } 159 | } 160 | } 161 | 162 | ///////////////////////////////////////////////////////////////////////////////////////////// 163 | // Method to update the sensor data without any validity checks (may result in 164 | // spikes). 165 | /////////////////////////////////////////////////////////////////////////////////////////////// 166 | bool VN100::updateData() { 167 | sensor_data_ = readImuData(); 168 | return sensor_data_ != nullptr; 169 | } 170 | 171 | ///////////////////////////////////////////////////////////////////////////////////////////// 172 | // Method to update the internally stored sensor data recusivelly by checking 173 | // the validity. 174 | /////////////////////////////////////////////////////////////////////////////////////////////// 175 | bool VN100::updateDataIterative() { 176 | uint64_t tic = micros(); 177 | bool success = false; 178 | for (size_t depth = 0; depth < kMaxRecursiveUpdateDepth; ++depth) { 179 | Sensor::setTimestampNow(); 180 | sensor_data_ = readImuData(); 181 | if (sensor_data_ == nullptr || checksum_.s != checksum(in_, 28, true)) { 182 | if (micros() - tic > kImuSyncTimeoutUs) { 183 | return false; 184 | } 185 | DEBUG_PRINTLN(topic_ + 186 | " (VN100.cpp): Failed IMU update detected, trying again " + 187 | (String)(kMaxRecursiveUpdateDepth - depth) + " times."); 188 | } else { 189 | return true; 190 | } 191 | } 192 | return false; 193 | } 194 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/VN100.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // May 2015 3 | // Author: light Dynamics and Control Lab 4 | // Adapted by Florian Tschopp for use in VersaVIS 5 | //////////////////////////////////////////////////////////////////////////////// 6 | // VN100.h 7 | //////////////////////////////////////////////////////////////////////////////// 8 | // 9 | // Copyright (c) 2018 Flight Dynamics and Control Lab 10 | 11 | // Permission is hereby granted, free of charge, to any person obtaining a copy 12 | // of this software and associated documentation files (the "Software"), to deal 13 | // in the Software without restriction, including without limitation the rights 14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | // copies of the Software, and to permit persons to whom the Software is 16 | // furnished to do so, subject to the following conditions: 17 | // 18 | // The above copyright notice and this permission notice shall be included in 19 | // all copies or substantial portions of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 26 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 27 | // SOFTWARE. 28 | 29 | // NOTE: Before running this code, you must configure the IMU as 30 | // defined in the README file. 31 | //////////////////////////////////////////////////////////////////////////////// 32 | 33 | #ifndef VN100_h 34 | #define VN100_h 35 | #include "Arduino.h" 36 | #include "Imu.h" 37 | #include 38 | #include 39 | 40 | // ADIS16448BMLZ Class Definition 41 | class VN100 : public Imu { 42 | public: 43 | // ADIS16448BMLZ Constructor (ChipSelect, DataReady output pin, HardwareReset) 44 | VN100(ros::NodeHandle *nh, const String &topic, const int rate_hz, 45 | Timer &timer); 46 | ~VN100(); 47 | void setup(); 48 | 49 | // Update data internally with validity checks. 50 | bool updateDataIterative(); 51 | 52 | // Update data withoput recursion. 53 | bool updateData(); 54 | 55 | private: 56 | // Read the IMU bytes 57 | int16_t *readImuData(); 58 | 59 | // Calculate the 16-bit CRC for the given ASCII or binary message. 60 | unsigned short checksum(byte data[], size_t length, bool ignore_first_byte); 61 | 62 | // Clear Serial buffer for any remaining data. 63 | void clearBuffer(); 64 | 65 | // Union functions for byte to float conversions 66 | // IMU sends data as bytes, the union functions are used to convert 67 | // these data into other data types 68 | 69 | // Angular rates 70 | union { 71 | float f; 72 | byte b[4]; 73 | } W_x_; 74 | union { 75 | float f; 76 | byte b[4]; 77 | } W_y_; 78 | union { 79 | float f; 80 | byte b[4]; 81 | } W_z_; 82 | 83 | // Acceleration 84 | union { 85 | float f; 86 | byte b[4]; 87 | } a_x_; 88 | union { 89 | float f; 90 | byte b[4]; 91 | } a_y_; 92 | union { 93 | float f; 94 | byte b[4]; 95 | } a_z_; 96 | 97 | // Checksum 98 | union { 99 | unsigned short s; 100 | byte b[2]; 101 | } checksum_; 102 | 103 | // Parameters 104 | //byte in_[30]; // array to save data send from the IMU 105 | const size_t kMessageLength; 106 | byte* in_; // array to save data send from the IMU 107 | float imu_accelerator_sensitivity_; 108 | float imu_gyro_sensitivity_; 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/helper.cpp: -------------------------------------------------------------------------------- 1 | #include "helper.h" 2 | 3 | void error(const char *string, const int number) { 4 | #ifdef ILLUMINATION_MODULE 5 | digitalWrite(ILLUMINATION_PIN, LOW); // Disable illumination if its activated. 6 | #endif 7 | DEBUG_PRINTLN(string); 8 | noInterrupts(); 9 | DEBUG_PRINT("ERROR: " + String(number)); 10 | while (true) { 11 | ; // TODO(floriantschopp) Find a better way to stop execution but still be 12 | // able to interrupt. 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/helper.h: -------------------------------------------------------------------------------- 1 | #ifndef helper_h 2 | #define helper_h 3 | 4 | #include "Arduino.h" 5 | #include "versavis_configuration.h" 6 | 7 | #ifdef DEBUG 8 | #define DEBUG_INIT(x) SerialUSB.begin(x) 9 | #define DEBUG_PRINT(x) SerialUSB.print(x) 10 | #define DEBUG_PRINTDEC(x) SerialUSB.print(x, DEC) 11 | #define DEBUG_PRINTHEX(x) SerialUSB.print(x, HEX) 12 | #define DEBUG_PRINTLN(x) SerialUSB.println(x) 13 | #define DEBUG_PRINTDECLN(x) SerialUSB.println(x, DEC) 14 | #else 15 | #define DEBUG_INIT(x) 16 | #define DEBUG_PRINT(x) 17 | #define DEBUG_PRINTDEC(x) 18 | #define DEBUG_PRINTHEX(x) 19 | #define DEBUG_PRINTLN(x) 20 | #define DEBUG_PRINTDECLN(x) 21 | #define DEBUG_PRINTLNT(x) 22 | #endif 23 | 24 | extern void error(const char *string, const int number); 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /firmware/libraries/versavis/src/versavis_configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef versavis_configuration_h 2 | #define versavis_configuration_h 3 | 4 | /* ----- General configuration -----*/ 5 | // Activate USB serial interface for ARM processor types. 6 | #define USE_USBCON 7 | 8 | // Specify the CPU frequency of the controller. 9 | #define CPU_FREQ_HZ 48e6 10 | 11 | // Specify the trigger pulse width; 12 | #define TRIGGER_PULSE_US 10 13 | 14 | /* ----- Camera configuration ----*/ 15 | // Camera 0 16 | #define CAM0_TOPIC "/versavis/cam0/" 17 | #define CAM0_RATE 20 18 | #define CAM0_TYPE trigger_type::NON_INVERTED 19 | #define CAM0_TRIGGER_PIN 14 20 | #define CAM0_EXPOSURE_PIN 5 21 | 22 | // Camera 1 23 | #define CAM1_TOPIC "" 24 | #define CAM1_RATE 20 25 | #define CAM1_TYPE trigger_type::NON_INVERTED 26 | #define CAM1_TRIGGER_PIN 15 27 | #define CAM1_EXPOSURE_PIN 6 28 | 29 | // Camera 2 30 | #define CAM2_TOPIC "" 31 | #define CAM2_RATE 5 32 | #define CAM2_TYPE trigger_type::NON_INVERTED 33 | #define CAM2_TRIGGER_PIN 16 34 | #define CAM2_EXPOSURE_PIN 7 35 | 36 | /* ----- IMU -----*/ 37 | // Possible values: USE_ADIS16445, USE_ADIS16448AMLZ, USE_ADIS16448BMLZ, 38 | // USE_ADIS16460 and USE_VN100 39 | #define USE_ADIS16448BMLZ 40 | #define IMU_TOPIC "/versavis/imu_micro" 41 | #define IMU_RATE 200 42 | 43 | /* ----- Additional triggers ----- */ 44 | // Define whether additional test outputs should be used. 45 | // #define ADD_TRIGGERS 46 | #ifdef ADD_TRIGGERS 47 | #define ADDITIONAL_TEST_PIN 2 48 | #endif 49 | 50 | /* ----- Illumination module ----- */ 51 | // Activation of the illumination module. 52 | // #define ILLUMINATION_MODULE 53 | #ifdef ILLUMINATION_MODULE 54 | #define ILLUMINATION_PWM_PIN 2 55 | #define ILLUMINATION_PIN 26 56 | #endif 57 | 58 | /* ----- Debug mode. ----- */ 59 | // Define whether debug mode should be used. This provides output on the 60 | // standard console but invalidates ROS communication. 61 | // #define DEBUG 62 | 63 | #endif // versavis_configuration_h 64 | -------------------------------------------------------------------------------- /firmware/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | catkin build versavis 4 | devel=/`catkin config | grep ^"Devel Space:" | cut -d'/' -f2-` 5 | source_path="$devel"/setup.bash 6 | 7 | echo $source_path 8 | source $source_path 9 | rm -rf libraries/ros_lib 10 | rosrun rosserial_arduino make_libraries.py libraries 11 | -------------------------------------------------------------------------------- /firmware/versavis/versavis.ino: -------------------------------------------------------------------------------- 1 | // Import all settings for the chosen sensor configuration. 2 | #include "versavis_configuration.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "Arduino.h" 9 | 10 | #ifdef USE_ADIS16445 11 | #include 12 | #elif defined(USE_ADIS16448AMLZ) 13 | #include 14 | #elif defined(USE_ADIS16448BMLZ) 15 | #include 16 | #elif defined(USE_ADIS16460) 17 | #include 18 | #elif defined(USE_VN100) 19 | #include 20 | #endif 21 | #include 22 | #include 23 | #include 24 | 25 | static void resetCb(const std_msgs::Bool & /*msg*/) { NVIC_SystemReset(); } 26 | 27 | #ifdef ILLUMINATION_MODULE 28 | static void pwmCb(const std_msgs::UInt8 &msg) { 29 | analogWrite(ILLUMINATION_PWM_PIN, msg.data); 30 | } 31 | #endif 32 | 33 | /* ----- ROS ----- */ 34 | ros::NodeHandle nh; 35 | ros::Subscriber reset_sub("/versavis/reset", &resetCb); 36 | #ifdef ILLUMINATION_MODULE 37 | ros::Subscriber pwm_sub("/versavis/illumination_pwm", &pwmCb); 38 | #endif 39 | 40 | /* ----- Timers ----- */ 41 | // In the current setup: TC5 -> IMU, TCC0 -> cam0, TCC1 -> cam1, TC3 -> cam2 42 | // (TCC2 is used for pwm on pin 11). 43 | // Be careful, there is NO bookkeeping whether the timer is already used or 44 | // not. Only use a timer once, otherwise there will be unexpected behavior. 45 | Timer timer_cam0 = Timer((Tcc *)TCC0); 46 | Timer timer_cam1 = Timer((Tcc *)TCC1); 47 | Timer timer_cam2 = Timer((TcCount16 *)TC3); 48 | Timer timer_imu = Timer((TcCount16 *)TC5); 49 | 50 | /* ----- IMU ----- */ 51 | #ifdef USE_ADIS16445 52 | ADIS16445 imu(&nh, IMU_TOPIC, IMU_RATE, timer_imu, 10, 2, 9); 53 | #elif defined(USE_ADIS16448AMLZ) 54 | ADIS16448AMLZ imu(&nh, IMU_TOPIC, IMU_RATE, timer_imu, 10, 2, 9); 55 | #elif defined(USE_ADIS16448BMLZ) 56 | ADIS16448BMLZ imu(&nh, IMU_TOPIC, IMU_RATE, timer_imu, 10, 2, 9); 57 | #elif defined(USE_ADIS16460) 58 | ADIS16460 imu(&nh, IMU_TOPIC, IMU_RATE, timer_imu, 10, 2, 9); 59 | #elif defined(USE_VN100) 60 | VN100 imu(&nh, IMU_TOPIC, IMU_RATE, timer_imu); 61 | #endif 62 | 63 | /* ----- Cameras ----- */ 64 | Camera cam0(&nh, CAM0_TOPIC, CAM0_RATE, timer_cam0, CAM0_TYPE, CAM0_TRIGGER_PIN, 65 | CAM0_EXPOSURE_PIN, true); 66 | Camera cam1(&nh, CAM1_TOPIC, CAM1_RATE, timer_cam1, CAM1_TYPE, CAM1_TRIGGER_PIN, 67 | CAM1_EXPOSURE_PIN, true); 68 | Camera cam2(&nh, CAM2_TOPIC, CAM2_RATE, timer_cam2, CAM2_TYPE, CAM2_TRIGGER_PIN, 69 | CAM2_EXPOSURE_PIN, true); 70 | 71 | void setup() { 72 | DEBUG_INIT(115200); 73 | 74 | /* ----- Define pins and initialize. ----- */ 75 | #ifdef ADD_TRIGGERS 76 | pinMode(ADDITIONAL_TEST_PIN, OUTPUT); 77 | digitalWrite(ADDITIONAL_TEST_PIN, LOW); 78 | #endif 79 | 80 | #ifdef ILLUMINATION_MODULE 81 | pinMode(ILLUMINATION_PWM_PIN, OUTPUT); 82 | pinMode(ILLUMINATION_PIN, OUTPUT); 83 | analogWrite(ILLUMINATION_PWM_PIN, 0); 84 | digitalWrite(ILLUMINATION_PIN, LOW); 85 | #endif 86 | 87 | delay(1000); 88 | 89 | /* ----- ROS ----- */ 90 | #ifndef DEBUG 91 | nh.getHardware()->setBaud(250000); 92 | nh.initNode(); 93 | nh.subscribe(reset_sub); 94 | #ifdef ILLUMINATION_MODULE 95 | nh.subscribe(pwm_sub); 96 | #endif 97 | #else 98 | while (!SerialUSB) { 99 | ; // wait for serial port to connect. Needed for native USB port only 100 | } 101 | #endif 102 | 103 | DEBUG_PRINTLN(F("Main: Start setup.")); 104 | 105 | imu.setup(); 106 | cam0.setup(); 107 | cam1.setup(); 108 | cam2.setup(); 109 | 110 | /* ----- Initialize all connected cameras. ----- */ 111 | while (!cam0.isInitialized() || !cam1.isInitialized() || 112 | !cam2.isInitialized()) { 113 | DEBUG_PRINTLN(F("Main: Initializing.")); 114 | cam0.initialize(); 115 | cam1.initialize(); 116 | cam2.initialize(); 117 | 118 | #ifndef DEBUG 119 | nh.spinOnce(); 120 | #endif 121 | delay(1000); 122 | } 123 | 124 | /* ----- Declare timers ----- */ 125 | // Enable TCC1 and TCC2 timers. 126 | REG_GCLK_CLKCTRL = static_cast( 127 | GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_TCC0_TCC1); 128 | while (GCLK->STATUS.bit.SYNCBUSY == 1) { 129 | ; // wait for sync 130 | } 131 | // Enable TCC2 (not used) and TC3 timers. 132 | REG_GCLK_CLKCTRL = static_cast( 133 | GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_TCC2_TC3); 134 | while (GCLK->STATUS.bit.SYNCBUSY == 1) { 135 | ; // wait for sync 136 | } 137 | 138 | // Enable TC4 (not used) and TC5 timers. 139 | REG_GCLK_CLKCTRL = static_cast( 140 | GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_TC4_TC5); 141 | while (GCLK->STATUS.bit.SYNCBUSY == 1) { 142 | ; // wait for sync 143 | } 144 | 145 | // enable InterruptVector. 146 | NVIC_EnableIRQ(TCC0_IRQn); 147 | NVIC_EnableIRQ(TCC1_IRQn); 148 | NVIC_EnableIRQ(TC3_IRQn); 149 | NVIC_EnableIRQ(TC5_IRQn); 150 | 151 | imu.begin(); 152 | cam0.begin(); 153 | cam1.begin(); 154 | cam2.begin(); 155 | 156 | /* ----- Interrupt for measuring the exposure time. ----- */ 157 | noInterrupts(); // Disable interrupts to configure them --> delay()'s 158 | // currently not working! 159 | 160 | DEBUG_PRINTLN(F("Main: Attach interrupts.")); 161 | attachInterrupt(digitalPinToInterrupt(cam0.exposurePin()), exposureEnd0, 162 | FALLING); 163 | attachInterrupt(digitalPinToInterrupt(cam1.exposurePin()), exposureEnd1, 164 | FALLING); 165 | attachInterrupt(digitalPinToInterrupt(cam2.exposurePin()), exposureEnd2, 166 | FALLING); 167 | interrupts(); 168 | 169 | DEBUG_PRINTLN(F("Main: Setup done.")); 170 | } 171 | 172 | void loop() { 173 | cam0.publish(); 174 | cam1.publish(); 175 | cam2.publish(); 176 | imu.publish(); 177 | 178 | #ifndef DEBUG 179 | nh.spinOnce(); 180 | #endif 181 | } 182 | 183 | void TCC0_Handler() { // Called by cam0_timer for camera 0 trigger. 184 | cam0.triggerMeasurement(); 185 | } 186 | 187 | void TCC1_Handler() { // Called by cam1_timer for camera 1 trigger. 188 | cam1.triggerMeasurement(); 189 | } 190 | 191 | void TC3_Handler() { // Called by cam2_timer for camera 2 trigger. 192 | cam2.triggerMeasurement(); 193 | } 194 | 195 | void TC5_Handler() { // Called by imu_timer for imu trigger. 196 | imu.triggerMeasurement(); 197 | } 198 | 199 | void exposureEnd0() { 200 | cam0.exposureEnd(); 201 | #ifdef ILLUMINATION_MODULE 202 | // Deactivate the LEDs with the last camera. 203 | if (!cam1.isExposing() && !cam2.isExposing()) { 204 | digitalWrite(ILLUMINATION_PIN, LOW); 205 | } 206 | #endif 207 | } 208 | 209 | void exposureEnd1() { 210 | cam1.exposureEnd(); 211 | #ifdef ILLUMINATION_MODULE 212 | // Deactivate the LEDs with the last camera. 213 | if (!cam0.isExposing() && !cam2.isExposing()) { 214 | digitalWrite(ILLUMINATION_PIN, LOW); 215 | } 216 | #endif 217 | } 218 | 219 | void exposureEnd2() { 220 | cam2.exposureEnd(); 221 | #ifdef ILLUMINATION_MODULE 222 | // Deactivate the LEDs with the last camera. 223 | if (!cam0.isExposing() && !cam1.isExposing()) { 224 | digitalWrite(ILLUMINATION_PIN, LOW); 225 | } 226 | #endif 227 | } 228 | -------------------------------------------------------------------------------- /image_numbered_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(image_numbered_msgs) 4 | 5 | # Set compile flags (-Ofast actually makes a big difference over -O3 here (maybe 20% faster) 6 | set(CMAKE_CXX_FLAGS "-std=c++11 -Ofast") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | message_generation 10 | sensor_msgs 11 | std_msgs 12 | ) 13 | 14 | 15 | add_message_files( 16 | FILES 17 | ImageNumbered.msg 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES 22 | sensor_msgs 23 | std_msgs 24 | ) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS message_runtime 28 | ) 29 | 30 | ############ 31 | ## EXPORT ## 32 | ############ 33 | # Install xml files 34 | install(FILES nodelet_plugin.xml 35 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 36 | ) 37 | -------------------------------------------------------------------------------- /image_numbered_msgs/msg/ImageNumbered.msg: -------------------------------------------------------------------------------- 1 | # This is a image message including a sequence number for association with a timestamp. 2 | # 3 | Header header # Had to add this for the TimeSynchronizer (see https://answers.ros.org/question/197811/time-synchronizer-message_filters-with-custom-message-type/) 4 | sensor_msgs/Image image 5 | uint64 number 6 | float32 exposure 7 | -------------------------------------------------------------------------------- /image_numbered_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_numbered_msgs 4 | 0.0.1 5 | The image_numbered_msgs package 6 | Florian Tschopp 7 | closed 8 | https://github.com/ethz-asl/versavis 9 | 10 | catkin 11 | 12 | message_generation 13 | message_runtime 14 | sensor_msgs 15 | std_msgs 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /scripts/calculate_img_delay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # Calculate and visualize timing accuracy of the VersaVIS 2.0. Used to calibrate 4 | # the time offset between end of integration time and time stamping by driver. 5 | # 6 | # Author: Andreas Pfrunder, Florian Tschopp 7 | # 8 | # Version: 2.0 Switched to more informative plots and added calculation 9 | # 1.0 Visualization script from Andreas 10 | 11 | import argparse 12 | 13 | import rosbag 14 | from std_msgs.msg import Time 15 | from sensor_msgs.msg import Image 16 | from tqdm import tqdm 17 | 18 | from decimal import * 19 | 20 | import matplotlib.pyplot as plt 21 | import numpy as np 22 | 23 | 24 | def main(): 25 | parser = argparse.ArgumentParser( 26 | description='Plot all timestamps of the bag topics.') 27 | parser.add_argument('--input_rosbag', default= '', 28 | help='Path to the rosbag file from which data is read.') 29 | parser.add_argument('--cam0', default= '', 30 | help='ROS topic name of cam0.') 31 | parser.add_argument('--cam1', default= '', 32 | help='ROS topic name of cam1.') 33 | parsed_args = parser.parse_args() 34 | 35 | input_path = parsed_args.input_rosbag 36 | 37 | input_bag = rosbag.Bag(input_path, 'r') 38 | 39 | ################# 40 | # Parse ROS bag # 41 | ################# 42 | img0_arrival_time_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam0])] 43 | img0_arrival_time_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam0])] 44 | img0_timestamp_nsec = [msg.header.stamp.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam0])] 45 | img0_timestamp_sec = [msg.header.stamp.secs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam0])] 46 | img1_arrival_time_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam1])] 47 | img1_arrival_time_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam1])] 48 | img1_timestamp_nsec = [msg.header.stamp.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam1])] 49 | img1_timestamp_sec = [msg.header.stamp.secs for (topic, msg, t) in input_bag.read_messages(topics=[parsed_args.cam1])] 50 | 51 | ard_img0_arrival_time_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam0'])] 52 | ard_img0_arrival_time_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam0'])] 53 | ard_img0_timestamp_nsec = [msg.time.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam0'])] 54 | ard_img0_timestamp_sec = [msg.time.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam0'])] 55 | ard_img1_arrival_time_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam1'])] 56 | ard_img1_arrival_time_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam1'])] 57 | ard_img1_timestamp_nsec = [msg.time.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam1'])] 58 | ard_img1_timestamp_sec = [msg.time.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/img_time_cam1'])] 59 | 60 | imu_arrival_time_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/imu_micro'])] 61 | imu_arrival_time_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/imu_micro'])] 62 | imu_timestamp_nsec = [msg.time.data.nsecs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/imu_micro'])] 63 | imu_timestamp_sec = [msg.time.data.secs for (topic, msg, t) in input_bag.read_messages(topics=['/flex_vi/imu_micro'])] 64 | 65 | getcontext().prec = 5 66 | 67 | index_cam = min(len(img0_arrival_time_sec), len(img0_timestamp_sec), len(img1_arrival_time_sec), len(img1_timestamp_sec), len(img1_arrival_time_sec), len(ard_img0_timestamp_sec), len(ard_img1_timestamp_sec)) 68 | index_imu = min(len(imu_arrival_time_sec), len(imu_timestamp_sec)) 69 | 70 | img0_arrival_time = [0] * (index_cam -1) 71 | img0_timestamp = [0] * (index_cam -1) 72 | img1_arrival_time = [0] * (index_cam -1) 73 | img1_timestamp = [0] * (index_cam -1) 74 | 75 | ard_img0_arrival_time = [0] * (index_cam -1) 76 | ard_img0_timestamp = [0] * (index_cam -1) 77 | ard_img1_arrival_time = [0] * (index_cam -1) 78 | ard_img1_timestamp = [0] * (index_cam -1) 79 | 80 | imu_arrival_time = [0] * (index_imu -1) 81 | imu_timestamp = [0] * (index_imu -1) 82 | 83 | ####################### 84 | # Compile time stamps # 85 | ####################### 86 | it_img0 = 0 87 | if ard_img0_timestamp_sec[1] + ard_img0_timestamp_nsec[1] / 1e9 > img0_timestamp_sec[1] + img0_timestamp_nsec[1] / 1e9: 88 | it_img0 = 1 89 | it_img1 = 0 90 | if ard_img1_timestamp_sec[1] + ard_img1_timestamp_nsec[1] / 1e9 > img1_timestamp_sec[1] + img1_timestamp_nsec[1] / 1e9: 91 | it_img1 = 1 92 | 93 | for x in range (1,(index_cam -2)): 94 | img0_arrival_time[x] = img0_arrival_time_sec[x + it_img0] + img0_arrival_time_nsec[x + it_img0] / 1e9 95 | img0_timestamp[x] = img0_timestamp_sec[x + it_img0] + img0_timestamp_nsec[x + it_img0] / 1e9 96 | img1_arrival_time[x] = img1_arrival_time_sec[x + it_img1] + img1_arrival_time_nsec[x + it_img1] / 1e9 97 | img1_timestamp[x] = img1_timestamp_sec[x + it_img1] + img1_timestamp_nsec[x + it_img1] / 1e9 98 | ard_img0_arrival_time[x] = ard_img0_arrival_time_sec[x] + ard_img0_arrival_time_nsec[x] / 1e9 99 | ard_img0_timestamp[x] = ard_img0_timestamp_sec[x] + ard_img0_timestamp_nsec[x] / 1e9 100 | ard_img1_arrival_time[x] = ard_img1_arrival_time_sec[x] + ard_img1_arrival_time_nsec[x] / 1e9 101 | ard_img1_timestamp[x] = ard_img1_timestamp_sec[x] + ard_img1_timestamp_nsec[x] / 1e9 102 | 103 | for x in range(1,(index_imu -2)): 104 | imu_arrival_time[x] = imu_arrival_time_sec[x] + imu_arrival_time_nsec[x] / 1e9 105 | imu_timestamp[x] = imu_timestamp_sec[x] + imu_timestamp_nsec[x] / 1e9 106 | 107 | ################### 108 | # Calculate delay # 109 | ################### 110 | # This delay is the time between the end of integration time and the time 111 | # stamp given from the ROS driver. Often, this corresponds to the highest 112 | # framerate of the sensor (may also be affected by the bandwidth of the 113 | # interface. 114 | integration_time_ms = 1 115 | time_dela_img0_ms = 1000 * np.mean(np.subtract(img0_timestamp, ard_img0_timestamp)) - integration_time_ms / 2 116 | time_dela_img1_ms = 1000 * np.mean(np.subtract(img1_timestamp, ard_img1_timestamp)) - integration_time_ms / 2 117 | time_delta = (time_dela_img0_ms + time_dela_img1_ms) / 2 # Weight both cameras equally. 118 | print("Time offset between end of integration and time stamping: " + str(time_delta) + " ms") 119 | 120 | ######## 121 | # Plot # 122 | ######## 123 | plt.figure(1) 124 | plt.plot(np.diff(img0_arrival_time[2:-1]), 'r.') 125 | plt.plot(np.diff(img0_timestamp[2:-1]), 'b.') 126 | plt.plot(np.diff(ard_img0_arrival_time[2:-1]), 'g.') 127 | plt.plot(np.diff(ard_img0_timestamp[2:-1]), 'm.') 128 | plt.xlabel('ROS message sequence sumber') 129 | plt.ylabel('Time difference [s]') 130 | plt.title('Time difference between consecutive messages.') 131 | plt.legend(['img0_arrival_time','img0_timestamp','ard_img0_arrival_time','ard_img0_timestamp']) 132 | 133 | plt.figure(2) 134 | plt.plot(np.diff(img1_arrival_time[2:-1]), 'r.') 135 | plt.plot(np.diff(img1_timestamp[2:-1]), 'b.') 136 | plt.plot(np.diff(ard_img1_arrival_time[2:-1]), 'g.') 137 | plt.plot(np.diff(ard_img1_timestamp[2:-1]), 'm.') 138 | plt.xlabel('ROS message sequence sumber') 139 | plt.ylabel('Time difference [s]') 140 | plt.title('Time difference between consecutive messages.') 141 | plt.legend(['img1_arrival_time','img1_timestamp','ard_img1_arrival_time','ard_img1_timestamp']) 142 | 143 | plt.figure(3) 144 | plt.plot(np.subtract(img0_arrival_time, img0_timestamp), 'r.') 145 | plt.plot(np.subtract(img1_arrival_time, img1_timestamp), 'b.') 146 | plt.plot(np.subtract(ard_img0_arrival_time, ard_img0_timestamp), '.g') 147 | plt.plot(np.subtract(ard_img1_arrival_time, ard_img1_timestamp), '.m') 148 | plt.xlabel('ROS message sequence number') 149 | plt.ylabel('Time difference [s]') 150 | plt.title('Time delay between timestamp and arrival time') 151 | plt.legend(['img_0','img1','ard_img0','ard_img1']) 152 | 153 | plt.figure(4) 154 | plt.plot(np.subtract(img0_timestamp, ard_img0_timestamp), '.r') 155 | plt.plot(np.subtract(img1_timestamp, ard_img1_timestamp), '.g') 156 | plt.xlabel('Message sequence number') 157 | plt.ylabel('Time difference [s]') 158 | plt.title('Delay between image stamp - arduino stamp.') 159 | plt.legend(['img0','img1']) 160 | 161 | plt.figure(5) 162 | plt.plot(np.diff(imu_arrival_time[2:-1]), 'r.') 163 | plt.plot(np.diff(imu_timestamp[2:-1]), 'm.') 164 | plt.xlabel('ROS message sequence sumber') 165 | plt.ylabel('Time difference [s]') 166 | plt.title('Time difference between consecutive messages (IMU).') 167 | plt.legend(['imu_arrival_time','imu_timestamp']) 168 | 169 | plt.figure(6) 170 | plt.plot(np.subtract(imu_arrival_time, imu_timestamp), 'r.') 171 | plt.xlabel('ROS message sequence number') 172 | plt.ylabel('Time difference [s]') 173 | plt.title('Time delay between arrival time and timestamp (IMU)') 174 | 175 | plt.show() 176 | 177 | if __name__ == '__main__': 178 | main() 179 | -------------------------------------------------------------------------------- /scripts/calculate_trigger_rates.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | close all 4 | 5 | CPU_freq = 16e6; % of Arduino UNO %% 48e6 for Arudino Zero 6 | Cam_rate_vec = []; 7 | IMU_rate_vec = []; 8 | Cam_compare_vec = []; 9 | IMU_compare_vec = []; 10 | Cam_prescale_vec = []; 11 | IMU_prescale_vec = []; 12 | 13 | for Cam_rate=1:300 14 | for IMU_rate=1:500 15 | for Cam_prescale = [1 8 64 256 1024] % timer1 of Arduino UNO 16 | for IMU_prescale = [1 8 32 64 128 256 1024] % timer2 of Arduino UNO 17 | IMU_compare = 1.0/IMU_rate*CPU_freq/IMU_prescale - 1; 18 | Cam_compare = 1.0/Cam_rate*CPU_freq/Cam_prescale - 1; 19 | if mod(IMU_compare,1)==0 && mod(Cam_compare,1) == 0 && IMU_compare < 2^8 && Cam_compare < 2^16 %&& (mod(IMU_rate / Cam_rate,1) == 0 || mod(Cam_rate / IMU_rate,1) == 0) 20 | Cam_rate_vec(end+1) = Cam_rate; 21 | IMU_rate_vec(end+1) = IMU_rate; 22 | Cam_compare_vec(end+1) = Cam_compare; 23 | IMU_compare_vec(end+1) = IMU_compare; 24 | Cam_prescale_vec(end+1) = Cam_prescale; 25 | IMU_prescale_vec(end+1) = IMU_prescale; 26 | end 27 | end 28 | end 29 | end 30 | end 31 | 32 | T = table(Cam_rate_vec',IMU_rate_vec',Cam_compare_vec',IMU_compare_vec',Cam_prescale_vec',IMU_prescale_vec','VariableNames',{'Cam_rate','IMU_rate','Cam_compare','IMU_compare','Cam_prescale','IMU_prescale'}) 33 | -------------------------------------------------------------------------------- /scripts/show_timings.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # Calculate and visualize timing accuracy of the VersaVIS 2.0. Used to calibrate 4 | # the time offset between end of integration time and time stamping by driver. 5 | # 6 | # Author: Andreas Pfrunder, Florian Tschopp 7 | # 8 | # Version: 2.0 Switched to more informative plots and added calculation 9 | # 1.0 Visualization script from Andreas 10 | 11 | import argparse 12 | 13 | import rosbag 14 | from std_msgs.msg import Time 15 | from sensor_msgs.msg import Image 16 | from tqdm import tqdm 17 | 18 | from decimal import * 19 | 20 | import matplotlib.pyplot as plt 21 | import numpy as np 22 | 23 | 24 | def main(): 25 | parser = argparse.ArgumentParser( 26 | description='Plot all timestamps of the bag topics.') 27 | parser.add_argument('--input_rosbag', default= '', 28 | help='Path to the rosbag file from which data is read.') 29 | parser.add_argument('--topic', default= '/VersaVIS/img_time', 30 | help='Topic to plot.') 31 | 32 | args = parser.parse_args() 33 | 34 | input_path = args.input_rosbag 35 | 36 | input_bag = rosbag.Bag(input_path, 'r') 37 | 38 | ################# 39 | # Parse ROS bag # 40 | ################# 41 | arduino_time_t_nsec = [t.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[args.topic])] 42 | arduino_time_t_sec = [t.secs for (topic, msg, t) in input_bag.read_messages(topics=[args.topic])] 43 | arduino_time_nsec = [msg.time.data.nsecs for (topic, msg, t) in input_bag.read_messages(topics=[args.topic])] 44 | arduino_time_sec = [msg.time.data.secs for (topic, msg, t) in input_bag.read_messages(topics=[args.topic])] 45 | index = len(arduino_time_t_nsec) 46 | arduino_time_t = [0] * (index -1) 47 | arduino_time = [0] * (index -1) 48 | 49 | ####################### 50 | # Compile time stamps # 51 | ####################### 52 | for x in range (1,(index -2)): 53 | arduino_time_t[x] = arduino_time_t_sec[x] + arduino_time_t_nsec[x] / 1e9 54 | arduino_time[x] = arduino_time_sec[x] + arduino_time_nsec[x] / 1e9 55 | 56 | ######## 57 | # Plot # 58 | ######## 59 | plt.figure(1) 60 | plt.plot(np.diff(arduino_time_t[2:-1]), 'b.') 61 | plt.plot(np.diff(arduino_time[2:-1]), 'c.') 62 | plt.xlabel('ROS message sequence number') 63 | plt.ylabel('Time difference [s]') 64 | plt.title('Time difference between consecutive messages.') 65 | plt.legend(['ard_t','ard']) 66 | 67 | plt.figure(2) 68 | plt.plot(np.subtract(arduino_time_t, arduino_time), '.b') 69 | plt.xlabel('ROS message sequence number') 70 | plt.ylabel('Time difference [s]') 71 | plt.title('Time delay between timestamp and arrival time') 72 | 73 | plt.figure(3) 74 | plt.plot(np.subtract(arduino_time_t, arduino_time_t[1]), '.r') 75 | plt.plot(np.subtract(arduino_time, arduino_time_t[1]), '.g') 76 | plt.xlabel('Message sequence number') 77 | plt.ylabel('Time [s]') 78 | plt.title('Time series.') 79 | plt.legend(['ard_t','ard']) 80 | 81 | plt.show() 82 | 83 | if __name__ == '__main__': 84 | main() 85 | -------------------------------------------------------------------------------- /versavis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(versavis) 4 | 5 | # Set compile flags (-Ofast actually makes a big difference over -O3 here (maybe 20% faster) 6 | set(CMAKE_CXX_FLAGS "-std=c++11 -Ofast") 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | image_transport 10 | message_generation 11 | nodelet 12 | roscpp 13 | rospy 14 | rosserial_arduino 15 | rosserial_client 16 | rosserial_python 17 | sensor_msgs 18 | std_msgs 19 | image_numbered_msgs 20 | ) 21 | 22 | add_message_files( 23 | FILES 24 | ImuMicro.msg 25 | TimeNumbered.msg 26 | ) 27 | 28 | generate_messages( 29 | DEPENDENCIES 30 | sensor_msgs 31 | std_msgs 32 | ) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include 36 | LIBRARIES ${PROJECT_NAME}_nodelet 37 | CATKIN_DEPENDS message_runtime 38 | ) 39 | 40 | include_directories( 41 | include 42 | ${catkin_INCLUDE_DIRS} 43 | ) 44 | 45 | ############# 46 | # LIBRARIES # 47 | ############# 48 | link_directories( 49 | ../firmware/libraries/ADIS16445/ 50 | ) 51 | 52 | add_library(${PROJECT_NAME}_nodelet 53 | src/versavis_synchronizer_nodelet.cpp 54 | src/versavis_synchronizer.cpp 55 | ) 56 | 57 | target_link_libraries(${PROJECT_NAME}_nodelet 58 | ${catkin_LIBRARIES} 59 | ) 60 | add_dependencies(${PROJECT_NAME}_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) 61 | 62 | add_executable(versavis_synchronizer_node src/versavis_synchronizer_node.cpp) 63 | target_link_libraries(versavis_synchronizer_node ${catkin_LIBRARIES}) 64 | 65 | add_executable(versavis_imu_receiver 66 | src/versavis_imu_receiver.cpp 67 | ) 68 | add_dependencies(versavis_imu_receiver ${${PROJECT_NAME}_EXPORTED_TARGETS}) 69 | target_link_libraries(versavis_imu_receiver ${catkin_LIBRARIES}) 70 | 71 | ############ 72 | ## EXPORT ## 73 | ############ 74 | # Install nodelet library 75 | install(TARGETS ${PROJECT_NAME}_nodelet 76 | DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | ) 78 | 79 | # Install header files 80 | install(DIRECTORY include/${PROJECT_NAME}/ 81 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 82 | ) 83 | 84 | # Install launch files 85 | install(DIRECTORY launch/ 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 87 | ) 88 | 89 | # Install xml files 90 | install(FILES nodelet_plugin.xml 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 92 | ) 93 | -------------------------------------------------------------------------------- /versavis/cfg/basler_acA1920-155uc_master.yaml: -------------------------------------------------------------------------------- 1 | # Config file for master 2 | 3 | ExposureAuto: "Continuous" 4 | ExposureMode: "Timed" 5 | ExposureTime: 1000.0 6 | AcquisitionFrameRateEnable: "0" 7 | AutoExposureTimeLowerLimit: 300.0 8 | AutoExposureTimeUpperLimit: 19000.0 9 | AutoTargetBrightness: 0.33 10 | GainAuto: "Continuous" 11 | BalanceWhiteAuto: "Continuous" 12 | TriggerMode: "On" 13 | TriggerSource: "Line1" 14 | LineSelector: "Line4" 15 | LineMode: "Output" 16 | LineSource: "ExposureActive" 17 | LineInverter: True 18 | -------------------------------------------------------------------------------- /versavis/cfg/basler_acA1920-155uc_slave.yaml: -------------------------------------------------------------------------------- 1 | # Config file for slave 2 | 3 | ExposureAuto: "Off" 4 | ExposureMode: "TriggerWidth" # For some random reason, TriggerWidth segfaults here... 5 | ExposureTime: 1000.0 6 | AcquisitionFrameRateEnable: "0" 7 | AutoExposureTimeLowerLimit: 300.0 8 | AutoExposureTimeUpperLimit: 19000.0 9 | AutoTargetBrightness: 0.33 10 | GainAuto: "Continuous" 11 | BalanceWhiteAuto: "Continuous" 12 | TriggerMode: "On" 13 | TriggerSource: "Line1" 14 | LineSelector: "Line4" 15 | LineMode: "Output" 16 | LineSource: "ExposureActive" 17 | LineInverter: True 18 | -------------------------------------------------------------------------------- /versavis/cfg/chameleon3.yaml: -------------------------------------------------------------------------------- 1 | # Set video mode. 2 | video_mode: format7_mode0 # Full resolution. 3 | # video_mode: format7_mode2 # Pixel binning. 4 | 5 | #see the launch file that overwrites this. 6 | #frame_rate: 20 7 | 8 | 9 | # Set shutter speed manually. 10 | auto_shutter: false 11 | #auto_shutter: false 12 | shutter_speed: 0.01 #10ms 13 | 14 | # Set exposure manually. 0.0 seems to work good for fairly sunny weather while 15 | # -0.5 may be advantageous for cloudy conditions. 16 | #auto_exposure: true 17 | auto_exposure: false 18 | exposure: 0.0 19 | #exposure: 10000.0 20 | 21 | # Set gamma (~contrast) and saturation. 22 | # Change of color coding allows to change gamma and saturation on the camera. 23 | # format7_color_coding: rgb8 24 | # gamma: 2.0 25 | # saturation: 125.0 26 | format7_color_coding: raw8 27 | 28 | # Set white balance manually. 760/740 are usually good values for sunny 29 | # conditions while 740/720 gives better results in cloudy conditions. 900/655 30 | # are good for the vicon room. 31 | # Launch mbzirc_histogram_viewer white_balance_tool.launch to tune the values to 32 | # a specific condition. 33 | #auto_white_balance: true 34 | auto_white_balance: true 35 | #white_balance_blue: 900 36 | #white_balance_red: 655 37 | 38 | auto_gain: false 39 | gain: 12.0 40 | brightness: 4.0 41 | 42 | #external_trigger: true 43 | enable_trigger: true 44 | trigger_mode: mode0 45 | trigger_source: gpio0 46 | -------------------------------------------------------------------------------- /versavis/include/versavis/versavis_synchronizer.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // versavis_synchronizer.h 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Used to merge image time stamps and image data. 9 | // 10 | //////////////////////////////////////////////////////////////////////////////// 11 | 12 | #ifndef ARDUINO_VI_SYNC_ARDUINO_VI_SYNCHRONIZER_H_ 13 | #define ARDUINO_VI_SYNC_ARDUINO_VI_SYNCHRONIZER_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace versavis { 38 | class VersaVISSynchronizer { 39 | public: 40 | VersaVISSynchronizer(const ros::NodeHandle &nh, 41 | const ros::NodeHandle &nh_private); 42 | ~VersaVISSynchronizer(); 43 | 44 | void imageCallback(const image_numbered_msgs::ImageNumbered &image_msg); 45 | void cameraInfoCallback(const sensor_msgs::CameraInfo& camera_info_msg); 46 | void imageTimeCallback(const versavis::TimeNumbered &image_time_msg); 47 | void publishImg(const image_numbered_msgs::ImageNumbered &image_msg); 48 | 49 | bool readParameters(); 50 | 51 | // Find nearest time stamp in a list of candidates (only newer are possible 52 | // due to driver logic). 53 | void associateTimeStampsAndCleanUp(); 54 | 55 | private: 56 | // ROS members. 57 | ros::NodeHandle nh_; 58 | ros::NodeHandle nh_private_; 59 | image_transport::ImageTransport image_transport_; 60 | ros::Subscriber image_sub_; 61 | ros::Subscriber camera_info_sub_; 62 | image_transport::Publisher image_fast_pub_; 63 | image_transport::Publisher image_slow_pub_; 64 | ros::Publisher camera_info_fast_pub_; 65 | ros::Publisher camera_info_slow_pub_; 66 | ros::Publisher initialized_pub_; 67 | ros::Subscriber image_time_sub_; 68 | 69 | // Topic names. 70 | std::string versavis_topic_; 71 | std::string driver_topic_; 72 | std::string camera_info_sub_topic_; 73 | std::string image_fast_pub_topic_; 74 | std::string image_slow_pub_topic_; 75 | std::string camera_info_fast_pub_topic_; 76 | std::string camera_info_slow_pub_topic_; 77 | std::string image_time_sub_topic_; 78 | std::string initialized_pub_topic_; 79 | 80 | // Association members. 81 | versavis::TimeNumbered init_time_; 82 | std::vector image_time_stamp_candidates_; 83 | std::vector image_candidates_; 84 | bool received_first_camera_info_; 85 | sensor_msgs::CameraInfo camera_info_msg_; 86 | ros::Time last_stamp_; 87 | ros::Time init_timestamp_; 88 | 89 | // Constants. 90 | const uint8_t kMaxImageCandidateLength; 91 | const uint8_t kMinSuccessfullConsecutiveMatches; 92 | const double kMaxImageDelayThreshold; 93 | 94 | // Image numbers and initialization. 95 | uint8_t init_number_; 96 | uint64_t last_image_number_; 97 | int64_t offset_; 98 | bool initialized_; 99 | 100 | // Configuration. 101 | uint8_t slow_publisher_image_counter_; 102 | bool publish_slow_images_; 103 | int publish_every_n_image_; 104 | bool forward_camera_info_; 105 | ros::Duration imu_offset_; 106 | 107 | std::mutex mutex_; 108 | }; 109 | } // namespace versavis 110 | 111 | #endif // ARDUINO_VI_SYNC_ARDUINO_VI_SYNCHRONIZER_H_ 112 | -------------------------------------------------------------------------------- /versavis/launch/run_versavis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 64 | 65 | 66 | 67 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 111 | 112 | 113 | 114 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 137 | 138 | 139 | 141 | 142 | 143 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | -------------------------------------------------------------------------------- /versavis/launch/run_versavis_bravo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 84 | 85 | 86 | 88 | 89 | 90 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /versavis/msg/ImuMicro.msg: -------------------------------------------------------------------------------- 1 | # This is a super-minimized message to hold data from an IMU (Inertial Measurement Unit). 2 | # 3 | # Accelerations and rotational velocities are raw data and have to be scaled! 4 | # 5 | # To minimize the amount of data transfered by this message (e.g. form an Arduino through rosserial) 6 | # the amount of data is truncated to its absolute minimum. 7 | 8 | std_msgs/Time time 9 | 10 | int16 ax 11 | int16 ay 12 | int16 az 13 | int16 gx 14 | int16 gy 15 | int16 gz 16 | 17 | -------------------------------------------------------------------------------- /versavis/msg/TimeNumbered.msg: -------------------------------------------------------------------------------- 1 | # This is a time message including a sequence number for association with a payload message. 2 | # 3 | 4 | time time 5 | uint64 number 6 | -------------------------------------------------------------------------------- /versavis/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | This is the VersaVIS synchronizer nodelet. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /versavis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | versavis 4 | 0.0.3 5 | The versavis package 6 | Florian Tschopp 7 | closed 8 | https://github.com/ethz-asl/versavis 9 | 10 | catkin 11 | image_transport 12 | message_generation 13 | message_runtime 14 | nodelet 15 | roscpp 16 | sensor_msgs 17 | std_msgs 18 | rosserial_client 19 | rosserial_arduino 20 | rosserial_python 21 | image_numbered_msgs 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /versavis/src/versavis_imu_receiver.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // versavis_imu_reciever.cpp 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Used to convert versavis/ImuMicro to sensor_msgs/Imu. Performs scaling and 9 | // setting the covarance constants. 10 | // 11 | //////////////////////////////////////////////////////////////////////////////// 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | class VersaVISImuReciever { 25 | public: 26 | VersaVISImuReciever(ros::NodeHandle &node_handle) 27 | : node_handle_(node_handle) { 28 | ROS_INFO("Versavis IMU message reciever. Version 1.0"); 29 | readParameters(); 30 | imu_sub_ = node_handle_.subscribe(imu_sub_topic_, 100u, 31 | &VersaVISImuReciever::imuCallback, this); 32 | imu_pub_ = node_handle_.advertise(imu_pub_topic_, 100u); 33 | last_msg_time_ = ros::Time(0); 34 | } 35 | 36 | ~VersaVISImuReciever() { node_handle_.shutdown(); } 37 | 38 | void imuCallback(const versavis::ImuMicro &imu_micro_msg) { 39 | ROS_INFO_ONCE("Received first IMU message."); 40 | if (imu_micro_msg.time.data.toNSec() < last_msg_time_.toNSec()) { 41 | ROS_WARN( 42 | "IMU message is not strictly increasing (%ld vs %ld). Dropping this " 43 | "message. This is normal during startup (< 1 min).", 44 | imu_micro_msg.time.data.toNSec(), last_msg_time_.toNSec()); 45 | return; 46 | } 47 | last_msg_time_ = imu_micro_msg.time.data; 48 | sensor_msgs::Imu imu_msg; 49 | 50 | imu_msg.header.stamp = imu_micro_msg.time.data; 51 | imu_msg.header.frame_id = "imu"; 52 | imu_msg.orientation.x = 0.0; 53 | imu_msg.orientation.y = 0.0; 54 | imu_msg.orientation.z = 0.0; 55 | imu_msg.orientation.w = 0.0; 56 | imu_msg.orientation_covariance[0] = -1; 57 | imu_msg.orientation_covariance[1] = 0.0; 58 | imu_msg.orientation_covariance[2] = 0.0; 59 | imu_msg.orientation_covariance[3] = 0.0; 60 | imu_msg.orientation_covariance[4] = 0; 61 | imu_msg.orientation_covariance[5] = 0.0; 62 | imu_msg.orientation_covariance[6] = 0.0; 63 | imu_msg.orientation_covariance[7] = 0.0; 64 | imu_msg.orientation_covariance[8] = 0; 65 | // --- Angular Velocity. 66 | imu_msg.angular_velocity.x = gyroScale(imu_micro_msg.gx); 67 | imu_msg.angular_velocity.y = gyroScale(imu_micro_msg.gy); 68 | imu_msg.angular_velocity.z = gyroScale(imu_micro_msg.gz); 69 | imu_msg.angular_velocity_covariance[0] = imu_gyro_covariance_; 70 | imu_msg.angular_velocity_covariance[1] = 0.0; 71 | imu_msg.angular_velocity_covariance[2] = 0.0; 72 | imu_msg.angular_velocity_covariance[3] = 0.0; 73 | imu_msg.angular_velocity_covariance[4] = imu_gyro_covariance_; 74 | imu_msg.angular_velocity_covariance[5] = 0.0; 75 | imu_msg.angular_velocity_covariance[6] = 0.0; 76 | imu_msg.angular_velocity_covariance[7] = 0.0; 77 | imu_msg.angular_velocity_covariance[8] = imu_gyro_covariance_; 78 | // --- Linear Acceleration. 79 | imu_msg.linear_acceleration.x = accelScale(imu_micro_msg.ax); 80 | imu_msg.linear_acceleration.y = accelScale(imu_micro_msg.ay); 81 | imu_msg.linear_acceleration.z = accelScale(imu_micro_msg.az); 82 | imu_msg.linear_acceleration_covariance[0] = imu_acceleration_covariance_; 83 | imu_msg.linear_acceleration_covariance[1] = 0.0; 84 | imu_msg.linear_acceleration_covariance[2] = 0.0; 85 | imu_msg.linear_acceleration_covariance[3] = 0.0; 86 | imu_msg.linear_acceleration_covariance[4] = imu_acceleration_covariance_; 87 | imu_msg.linear_acceleration_covariance[5] = 0.0; 88 | imu_msg.linear_acceleration_covariance[6] = 0.0; 89 | imu_msg.linear_acceleration_covariance[7] = 0.0; 90 | imu_msg.linear_acceleration_covariance[8] = imu_acceleration_covariance_; 91 | 92 | imu_pub_.publish(imu_msg); 93 | } 94 | 95 | void readParameters() { 96 | ROS_INFO("Read ROS parameters."); 97 | node_handle_.param("imu_sub_topic", imu_sub_topic_, 98 | std::string("/versavis/imu_micro")); 99 | node_handle_.param("imu_pub_topic", imu_pub_topic_, 100 | std::string("/versavis/imu")); 101 | node_handle_.param("imu_accelerator_sensitivity", 102 | imu_accelerator_sensitivity_, 0.00025); 103 | node_handle_.param("imu_gyro_sensitivity", imu_gyro_sensitivity_, 0.05); 104 | node_handle_.param("imu_acceleration_covariance", 105 | imu_acceleration_covariance_, 0.043864908); 106 | node_handle_.param("imu_gyro_covariance", imu_gyro_covariance_, 6e-9); 107 | } 108 | 109 | private: 110 | ros::NodeHandle node_handle_; 111 | ros::Subscriber imu_sub_; 112 | ros::Publisher imu_pub_; 113 | // ros::Subscriber img_time_sub_; 114 | std::string imu_sub_topic_; 115 | std::string imu_pub_topic_; 116 | ros::Time last_msg_time_; 117 | double imu_accelerator_sensitivity_; 118 | double imu_gyro_sensitivity_; 119 | double imu_acceleration_covariance_; 120 | double imu_gyro_covariance_; 121 | 122 | // Converts accelerometer data output from the regRead() function and 123 | // returns 124 | // acceleration in m2/s. 125 | float accelScale(const int16_t &sensor_data) { 126 | return (sensor_data * imu_accelerator_sensitivity_ * 127 | 9.81); // Multiply by accel sensitivity 128 | // (e.g. 4000 LSB/g for ADIS16445) 129 | // and acceleration of gravity. 130 | } 131 | 132 | // Converts gyro data output from the regRead() function and returns gyro rate 133 | // in rad/sec. 134 | float gyroScale(const int16_t &sensor_data) { 135 | return ( 136 | sensor_data * imu_gyro_sensitivity_ * M_PI / 137 | 180); // Multiply by gyro sensitivity (e.g. 100 LSB/dps for ADIS16445); 138 | } 139 | }; 140 | 141 | int main(int argc, char **argv) { 142 | ros::init(argc, argv, "versavis_imu_reciever"); 143 | ros::NodeHandle node_handle("~"); 144 | VersaVISImuReciever reciever(node_handle); 145 | 146 | ros::AsyncSpinner spinner(1); 147 | spinner.start(); 148 | ros::waitForShutdown(); 149 | 150 | return 0; 151 | } 152 | -------------------------------------------------------------------------------- /versavis/src/versavis_synchronizer.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // versavis_synchronizer.cpp 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Used to merge image time stamps and image data. 9 | // 10 | //////////////////////////////////////////////////////////////////////////////// 11 | 12 | #include "versavis/versavis_synchronizer.h" 13 | 14 | namespace versavis { 15 | 16 | VersaVISSynchronizer::VersaVISSynchronizer(const ros::NodeHandle &nh, 17 | const ros::NodeHandle &nh_private) 18 | : nh_(nh), nh_private_(nh_private), image_transport_(nh), 19 | received_first_camera_info_(false), kMaxImageCandidateLength(10), 20 | kMinSuccessfullConsecutiveMatches(4), kMaxImageDelayThreshold(0.1), 21 | slow_publisher_image_counter_(0), init_number_(0), initialized_(false), 22 | publish_slow_images_(false), publish_every_n_image_(10), 23 | forward_camera_info_(false) { 24 | ROS_INFO("Versavis message compiler started. Version 1.0"); 25 | readParameters(); 26 | 27 | // Subscriber of the non-corrected image directly from the ROS camera driver. 28 | image_sub_ = nh_.subscribe(driver_topic_, 10u, 29 | &VersaVISSynchronizer::imageCallback, this); 30 | ROS_INFO("Subscribing to %s.", driver_topic_.c_str()); 31 | 32 | // Subscriber to an exsternal publisher of a camera info topic, this can be 33 | // the driver or another node. It is not required that the camera info message 34 | // is synchrinized with the image. This node will simply take the most recent 35 | // camera info message and publish it in sync with the restamped image 36 | // messabge. 37 | if (forward_camera_info_) { 38 | camera_info_sub_ = 39 | nh_.subscribe(camera_info_sub_topic_, 10u, 40 | &VersaVISSynchronizer::cameraInfoCallback, this); 41 | ROS_INFO("Subscribing to camera info topic %s.", 42 | camera_info_sub_topic_.c_str()); 43 | } 44 | 45 | // Subscriber for the image time message containing both the corrected 46 | // timestamp and the sequence number. 47 | image_time_sub_ = 48 | nh_.subscribe(image_time_sub_topic_, 10u, 49 | &VersaVISSynchronizer::imageTimeCallback, this); 50 | ROS_INFO("Subscribing to %s.", image_time_sub_topic_.c_str()); 51 | 52 | // Publisher to let the triggering board know as soon as the camera is 53 | // initialized. 54 | initialized_pub_ = nh_.advertise(initialized_pub_topic_, 1u); 55 | ROS_INFO("Publishing initialization status to %s.", 56 | initialized_pub_topic_.c_str()); 57 | 58 | image_fast_pub_ = image_transport_.advertise(image_fast_pub_topic_, 10u); 59 | ROS_INFO("Publishing image to %s.", image_fast_pub_topic_.c_str()); 60 | 61 | if (publish_slow_images_) { 62 | image_slow_pub_ = image_transport_.advertise(image_slow_pub_topic_, 1u); 63 | ROS_INFO("Publishing (slow) image to %s.", image_slow_pub_topic_.c_str()); 64 | } 65 | 66 | if (forward_camera_info_) { 67 | camera_info_fast_pub_ = 68 | nh_.advertise(camera_info_fast_pub_topic_, 10u); 69 | ROS_INFO_STREAM("Publishing camera info to " << camera_info_fast_pub_topic_); 70 | 71 | if (publish_slow_images_) { 72 | camera_info_slow_pub_ = nh_.advertise( 73 | camera_info_slow_pub_topic_, 10u); 74 | ROS_INFO_STREAM("Publishing (slow) camera info to " 75 | << camera_info_slow_pub_topic_); 76 | } 77 | } 78 | } 79 | 80 | VersaVISSynchronizer::~VersaVISSynchronizer() { 81 | image_time_stamp_candidates_.clear(); 82 | image_candidates_.clear(); 83 | nh_.shutdown(); 84 | } 85 | 86 | void VersaVISSynchronizer::associateTimeStampsAndCleanUp() { 87 | if (image_candidates_.empty() || image_time_stamp_candidates_.empty()) { 88 | return; 89 | } 90 | // Association and cleanup. 91 | uint32_t last_image_time = 0u; 92 | uint32_t last_image = 0u; 93 | auto image_idx = image_candidates_.begin(); 94 | auto image_time_idx = image_time_stamp_candidates_.begin(); 95 | bool image_found = false; 96 | 97 | while (image_idx != image_candidates_.end()) { 98 | while (image_time_idx != image_time_stamp_candidates_.end()) { 99 | if (image_idx->number == image_time_idx->number + offset_) { 100 | image_idx->image.header.stamp = image_time_idx->time + imu_offset_; 101 | publishImg(*image_idx); 102 | last_image = image_idx->number; 103 | last_image_time = image_time_idx->number; 104 | image_idx = image_candidates_.erase(image_idx); 105 | image_time_idx = image_time_stamp_candidates_.erase(image_time_idx); 106 | image_found = true; 107 | break; 108 | } else { 109 | ++image_time_idx; 110 | } 111 | } 112 | image_time_idx = image_time_stamp_candidates_.begin(); 113 | if (!image_found) { 114 | ++image_idx; 115 | } else { 116 | image_found = false; 117 | } 118 | } 119 | 120 | // Delete old messages to prevent non-consecutive publishing. 121 | image_idx = image_candidates_.begin(); 122 | image_time_idx = image_time_stamp_candidates_.begin(); 123 | while (image_idx != image_candidates_.end()) { 124 | if (image_idx->number <= last_image) { 125 | ROS_WARN("%s: Deleted old image message %ld.", 126 | image_fast_pub_topic_.c_str(), image_idx->number); 127 | image_idx = image_candidates_.erase(image_idx); 128 | } else { 129 | ++image_idx; 130 | } 131 | } 132 | while (image_time_idx != image_time_stamp_candidates_.end()) { 133 | if (image_time_idx->number <= last_image_time) { 134 | ROS_WARN("%s: Deleted old image time message %ld.", 135 | image_fast_pub_topic_.c_str(), image_time_idx->number + offset_); 136 | image_time_idx = image_time_stamp_candidates_.erase(image_time_idx); 137 | } else { 138 | ++image_time_idx; 139 | } 140 | } 141 | } 142 | 143 | void VersaVISSynchronizer::imageCallback( 144 | const image_numbered_msgs::ImageNumbered &image_msg) { 145 | std::lock_guard mutex_lock(mutex_); 146 | ROS_INFO_ONCE("%s: Received first message with %s encoding.", 147 | image_fast_pub_topic_.c_str(), 148 | image_msg.image.encoding.c_str()); 149 | if (initialized_) { 150 | image_candidates_.emplace_back(image_msg); 151 | associateTimeStampsAndCleanUp(); 152 | if (image_candidates_.size() > kMaxImageCandidateLength) { 153 | image_candidates_.erase(image_candidates_.begin()); 154 | ROS_WARN("%s: Image candidates buffer overflow at %ld.", 155 | image_fast_pub_topic_.c_str(), image_msg.number); 156 | } 157 | } else { 158 | // Initialization procedure. 159 | ROS_INFO_ONCE("%s: Initializing...", image_fast_pub_topic_.c_str()); 160 | 161 | const int64_t offset = image_msg.number - init_time_.number; 162 | ROS_INFO("%s: Current offset %ld with time offset %0.5f", 163 | image_fast_pub_topic_.c_str(), offset, 164 | ros::Time::now().toSec() - init_timestamp_.toSec()); 165 | // Ensure that we match to the right image by enforcing a similar arrival 166 | // time and the same offset. 167 | // Note: The image_time message should arrive prior to the image. 168 | if (ros::Time::now().toSec() - init_timestamp_.toSec() < 169 | kMaxImageDelayThreshold && 170 | offset == offset_) { 171 | ++init_number_; 172 | if (init_number_ >= kMinSuccessfullConsecutiveMatches) { 173 | ROS_INFO("%s: Initialized with %ld offset.", 174 | image_fast_pub_topic_.c_str(), offset); 175 | initialized_ = true; 176 | std_msgs::Bool init_msg; 177 | init_msg.data = true; 178 | initialized_pub_.publish(init_msg); 179 | 180 | ROS_INFO_STREAM("Versavis node '" 181 | << nh_private_.getNamespace() 182 | << "' confirmed initialization of sync!"); 183 | } 184 | } else { 185 | init_number_ = 0; 186 | } 187 | offset_ = offset; 188 | } 189 | } 190 | 191 | void VersaVISSynchronizer::cameraInfoCallback( 192 | const sensor_msgs::CameraInfo &camera_info_msg) { 193 | std::lock_guard mutex_lock(mutex_); 194 | ROS_INFO_ONCE("%s: Received first camera info message.", 195 | camera_info_sub_topic_.c_str()); 196 | received_first_camera_info_ = true; 197 | camera_info_msg_ = camera_info_msg; 198 | } 199 | 200 | void VersaVISSynchronizer::imageTimeCallback( 201 | const versavis::TimeNumbered &image_triggered_time_msg) { 202 | std::lock_guard mutex_lock(mutex_); 203 | ROS_INFO_ONCE("%s: Received first image time stamp message.", 204 | image_fast_pub_topic_.c_str()); 205 | if (initialized_) { 206 | image_time_stamp_candidates_.emplace_back(image_triggered_time_msg); 207 | associateTimeStampsAndCleanUp(); 208 | if (image_time_stamp_candidates_.size() > kMaxImageCandidateLength) { 209 | image_time_stamp_candidates_.erase(image_time_stamp_candidates_.begin()); 210 | ROS_WARN("%s: Time candidates buffer overflow at %ld.", 211 | image_fast_pub_topic_.c_str(), image_triggered_time_msg.number); 212 | } 213 | } else { 214 | init_timestamp_ = ros::Time::now(); 215 | init_time_ = image_triggered_time_msg; 216 | } 217 | } 218 | 219 | void VersaVISSynchronizer::publishImg( 220 | const image_numbered_msgs::ImageNumbered &image_msg) { 221 | if (image_msg.image.header.stamp.toSec() == 0) { 222 | ROS_WARN("%s: Zero timestamp for %ld.", image_fast_pub_topic_.c_str(), 223 | image_msg.number); 224 | return; 225 | } 226 | 227 | if (image_msg.image.header.stamp.toSec() <= last_stamp_.toSec()) { 228 | ROS_WARN("%s: Non-increasing timestamp for %ld.", 229 | image_fast_pub_topic_.c_str(), image_msg.number); 230 | return; 231 | } 232 | if (image_msg.number > last_image_number_ + 1) { 233 | ROS_WARN("%s: Skipped %ld frame for unknown reasons.", 234 | image_fast_pub_topic_.c_str(), 235 | image_msg.number - 1 - last_image_number_); 236 | } 237 | last_image_number_ = image_msg.number; 238 | last_stamp_ = image_msg.image.header.stamp; 239 | 240 | if (forward_camera_info_) { 241 | if (received_first_camera_info_) { 242 | camera_info_msg_.header.stamp = image_msg.image.header.stamp; 243 | camera_info_fast_pub_.publish(camera_info_msg_); 244 | } else { 245 | ROS_WARN_THROTTLE(2.0, 246 | "No camera info received yet, will not publish image."); 247 | } 248 | } 249 | image_fast_pub_.publish(image_msg.image); 250 | 251 | 252 | if (publish_slow_images_) { 253 | // Publish color/raw image at lower rate. 254 | if (slow_publisher_image_counter_ >= 255 | publish_every_n_image_) { 256 | if (forward_camera_info_) { 257 | if (received_first_camera_info_) { 258 | camera_info_msg_.header.stamp = image_msg.image.header.stamp; 259 | camera_info_slow_pub_.publish(camera_info_msg_); 260 | } else { 261 | ROS_WARN_THROTTLE( 262 | 2.0, "No camera info received yet, will not publish image."); 263 | } 264 | } 265 | image_slow_pub_.publish(image_msg.image); 266 | slow_publisher_image_counter_ = 0u; 267 | } else { 268 | ++slow_publisher_image_counter_; 269 | } 270 | } 271 | } 272 | 273 | bool VersaVISSynchronizer::readParameters() { 274 | ROS_INFO("Read ROS parameters."); 275 | nh_private_.param("publish_slow_images", publish_slow_images_, 276 | publish_slow_images_); 277 | nh_private_.param("publish_every_n_image", publish_every_n_image_, 278 | publish_every_n_image_); 279 | 280 | if (!nh_private_.getParam("driver_topic", driver_topic_)) { 281 | ROS_ERROR("Define an image topic from the camera driver."); 282 | } 283 | 284 | if (!nh_private_.getParam("versavis_topic", versavis_topic_)) { 285 | ROS_ERROR("Define a topic range where the corrected image is published."); 286 | } else { 287 | if (versavis_topic_.back() != '/') { 288 | versavis_topic_ = versavis_topic_ + "/"; 289 | } 290 | image_time_sub_topic_ = versavis_topic_ + "image_time"; 291 | image_fast_pub_topic_ = versavis_topic_ + "image_raw"; 292 | camera_info_fast_pub_topic_ = versavis_topic_ + "camera_info"; 293 | initialized_pub_topic_ = versavis_topic_ + "init"; 294 | if (publish_slow_images_) { 295 | image_slow_pub_topic_ = versavis_topic_ + "slow/image_raw"; 296 | camera_info_slow_pub_topic_ = versavis_topic_ + "slow/camera_info"; 297 | } 298 | } 299 | 300 | if (nh_private_.getParam("camera_info_topic", camera_info_sub_topic_)) { 301 | if (!camera_info_sub_topic_.empty()) { 302 | ROS_INFO_STREAM("A camera info topic has been provided, versavis will " 303 | "synchronize and forward it to '" 304 | << camera_info_sub_topic_ << "'"); 305 | forward_camera_info_ = true; 306 | } 307 | } 308 | 309 | int imu_offset_us; 310 | nh_private_.param("imu_offset_us", imu_offset_us, 0); 311 | imu_offset_ = ros::Duration(imu_offset_us / 1e6); 312 | ROS_INFO("IMU has a calibrated dalay of %0.0f us.", 313 | imu_offset_.toSec() * 1e6); 314 | return true; 315 | } 316 | 317 | } // namespace versavis 318 | -------------------------------------------------------------------------------- /versavis/src/versavis_synchronizer_node.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // versavis_synchronizer_node.cpp 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Node wrapper for versavis_synchronizer. 9 | // 10 | //////////////////////////////////////////////////////////////////////////////// 11 | 12 | #include 13 | #include 14 | 15 | int main(int argc, char **argv) { 16 | ros::init(argc, argv, "versavis_synchronizer_node"); 17 | nodelet::Loader nodelet; 18 | nodelet::M_string remap(ros::names::getRemappings()); 19 | nodelet::V_string nargv; 20 | std::string nodelet_name = ros::this_node::getName(); 21 | ROS_INFO_STREAM("Started " << nodelet_name << " nodelet."); 22 | nodelet.load(nodelet_name, "versavis/VersaVISSynchronizerNodelet", remap, 23 | nargv); 24 | ros::spin(); 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /versavis/src/versavis_synchronizer_nodelet.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // April 2019 3 | // Author: Florian Tschopp 4 | //////////////////////////////////////////////////////////////////////////////// 5 | // versavis_synchronizer_nodelet.cpp 6 | //////////////////////////////////////////////////////////////////////////////// 7 | // 8 | // Nodelet wrapper for versavis_synchronizer. 9 | // 10 | //////////////////////////////////////////////////////////////////////////////// 11 | 12 | #include 13 | #include 14 | 15 | #include "versavis/versavis_synchronizer.h" 16 | 17 | namespace versavis { 18 | 19 | class VersaVISSynchronizerNodelet : public nodelet::Nodelet { 20 | virtual void onInit() { 21 | try { 22 | synchronizer_ = std::make_shared( 23 | getNodeHandle(), getPrivateNodeHandle()); 24 | } catch (std::runtime_error e) { 25 | ROS_ERROR("%s", e.what()); 26 | } 27 | } 28 | 29 | std::shared_ptr synchronizer_; 30 | }; 31 | } // namespace versavis 32 | 33 | PLUGINLIB_EXPORT_CLASS(versavis::VersaVISSynchronizerNodelet, nodelet::Nodelet); 34 | --------------------------------------------------------------------------------