├── LICENSE ├── README.md ├── examples ├── v3 │ ├── GetDistanceI2c │ │ └── GetDistanceI2c.ino │ ├── GetDistancePwm │ │ └── GetDistancePwm.ino │ └── ShortRangeHighSpeed │ │ └── ShortRangeHighSpeed.ino └── v3HP │ ├── v3HP_I2C │ └── v3HP_I2C.ino │ └── v3HP_MONITOR │ └── v3HP_MONITOR.ino ├── keywords.txt ├── library.properties └── src ├── LIDARLite.cpp ├── LIDARLite.h ├── LIDARLite_v3HP.cpp └── LIDARLite_v3HP.h /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LIDAR-Lite Arduino Library 2 | 3 | * [Product Page - LLV3](http://www.robotshop.com/en/lidar-lite-3-laser-rangefinder.html) [(Operating Manual and Technical Specifications)](http://www.robotshop.com/media/files/pdf2/pli-06-instruction.pdf) 4 | * [Product Page - LLV3HP](https://www.robotshop.com/en/lidar-lite-3-laser-rangefinder-high-performance-llv3hp.html) [(Operating Manual and Technical Specifications)](https://www.robotshop.com/media/files/pdf2/rb-pli-17_-_llv3hp_-_operation_manual_and_technical_specifications.pdf) 5 | 6 | This library provides quick access to basic functions of LIDAR-Lite 7 | via the Arduino interface. Additionally, it can provide a user of any 8 | platform with a template for their own application code. 9 | 10 | For detailed specifications, pinout, and connection diagrams, see the manuals linked at the above product pages. 11 | 12 | ***A Note on Compatibility:*** *Minor interface changes have occurred between LIDAR-Lite v3, v3HP, and previous versions. Backwards-compatibility of this library is largely preserved, though support is not directly provided for v1 and v2.* 13 | 14 | ## Installation instructions 15 | To install, download this repository and place in your Arduino libraries folder or use the Arduino Library Manager. If you need help, follow the instructions here: [http://arduino.cc/en/Guide/Libraries](http://arduino.cc/en/Guide/Libraries). 16 | 17 | ## Example Sketches 18 | ### [v3/GetDistancePWM](https://github.com/RobotShop/LIDARLite_v3_Arduino_Library/tree/master/examples/v3/GetDistanceI2c) 19 | This is the simplest demonstration of LIDAR-Lite. It shows how to read a distance using the PWM interface. 20 | 21 | ### [v3/GetDistanceI2c](https://github.com/RobotShop/LIDARLite_v3_Arduino_Library/tree/master/examples/v3/GetDistancePwm) 22 | This demonstration shows how to read distance using the I2C interface and choose preset configurations. 23 | 24 | ### [v3/ShortRangeHighSpeed](https://github.com/RobotShop/LIDARLite_v3_Arduino_Library/tree/master/examples/v3/ShortRangeHighSpeed) 25 | This example shows a method to run LIDAR-Lite at high speed for short range applications. It combines a variety of settings to trade off range and accuracy for very fast measurements. 26 | 27 | ### [v3HP/v3HP_I2C](https://github.com/RobotShop/LIDARLite_v3_Arduino_Library/blob/master/examples/v3HP/v3HP_I2C/v3HP_I2C.ino) 28 | This example shows various methods to run LIDAR-Lite v3HP. 29 | 30 | ### [v3HP/v3HP_MONITOR](https://github.com/RobotShop/LIDARLite_v3_Arduino_Library/blob/master/examples/v3HP/v3HP_MONITOR/v3HP_MONITOR.ino) 31 | Use the MODE pin to monitor STATUS of the LIDAR-Lite v3HP. 32 | 33 | ## Version History 34 | * 2.0.5 - Add v3 library function to set alternate I2C addr 35 | * 2.0.4 - Update short-range, high-error configuration 36 | * 2.0.3 - New example using MODE pin for STATUS output of v3HP 37 | * 2.0.2 - Updates to v3HP library 38 | * 2.0.0 - Support for LIDAR-Lite v3HP 39 | * 1.0.3 - Fix version convention 40 | * 1.0.2 - Library Manager Update 41 | * 1.0.1 - Release to Library Manager 42 | * 1.0.0 - Initial release 43 | 44 | ## License 45 | Copyright (c) 2018 Garmin Ltd. or its subsidiaries. Distributed under the Apache 2.0 License. 46 | See [LICENSE](LICENSE) for further details. 47 | -------------------------------------------------------------------------------- /examples/v3/GetDistanceI2c/GetDistanceI2c.ino: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | v3/GetDistanceI2c 5 | 6 | This example shows how to initialize, configure, and read distance from a 7 | LIDAR-Lite connected over the I2C interface. 8 | 9 | Connections: 10 | LIDAR-Lite 5 Vdc (red) to Arduino 5v 11 | LIDAR-Lite I2C SCL (green) to Arduino SCL 12 | LIDAR-Lite I2C SDA (blue) to Arduino SDA 13 | LIDAR-Lite Ground (black) to Arduino GND 14 | 15 | (Capacitor recommended to mitigate inrush current when device is enabled) 16 | 680uF capacitor (+) to Arduino 5v 17 | 680uF capacitor (-) to Arduino GND 18 | 19 | See the Operation Manual for wiring diagrams and more information: 20 | http://www.robotshop.com/media/files/pdf2/pli-06-instruction.pdf 21 | 22 | ------------------------------------------------------------------------------*/ 23 | 24 | #include 25 | #include 26 | 27 | LIDARLite myLidarLite; 28 | 29 | void setup() 30 | { 31 | Serial.begin(115200); // Initialize serial connection to display distance readings 32 | 33 | /* 34 | begin(int configuration, bool fasti2c, char lidarliteAddress) 35 | 36 | Starts the sensor and I2C. 37 | 38 | Parameters 39 | ---------------------------------------------------------------------------- 40 | configuration: Default 0. Selects one of several preset configurations. 41 | fasti2c: Default 100 kHz. I2C base frequency. 42 | If true I2C frequency is set to 400kHz. 43 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 44 | operating manual for instructions. 45 | */ 46 | myLidarLite.begin(0, true); // Set configuration to default and I2C to 400 kHz 47 | 48 | /* 49 | configure(int configuration, char lidarliteAddress) 50 | 51 | Selects one of several preset configurations. 52 | 53 | Parameters 54 | ---------------------------------------------------------------------------- 55 | configuration: Default 0. 56 | 0: Default mode, balanced performance. 57 | 1: Short range, high speed. Uses 0x1d maximum acquisition count. 58 | 2: Default range, higher speed short range. Turns on quick termination 59 | detection for faster measurements at short range (with decreased 60 | accuracy) 61 | 3: Maximum range. Uses 0xff maximum acquisition count. 62 | 4: High sensitivity detection. Overrides default valid measurement detection 63 | algorithm, and uses a threshold value for high sensitivity and noise. 64 | 5: Low sensitivity detection. Overrides default valid measurement detection 65 | algorithm, and uses a threshold value for low sensitivity and noise. 66 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 67 | operating manual for instructions. 68 | */ 69 | myLidarLite.configure(0); // Change this number to try out alternate configurations 70 | } 71 | 72 | void loop() 73 | { 74 | /* 75 | distance(bool biasCorrection, char lidarliteAddress) 76 | 77 | Take a distance measurement and read the result. 78 | 79 | Parameters 80 | ---------------------------------------------------------------------------- 81 | biasCorrection: Default true. Take aquisition with receiver bias 82 | correction. If set to false measurements will be faster. Receiver bias 83 | correction must be performed periodically. (e.g. 1 out of every 100 84 | readings). 85 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 86 | operating manual for instructions. 87 | */ 88 | 89 | // Take a measurement with receiver bias correction and print to serial terminal 90 | Serial.println(myLidarLite.distance()); 91 | 92 | // Take 99 measurements without receiver bias correction and print to serial terminal 93 | for(int i = 0; i < 99; i++) 94 | { 95 | Serial.println(myLidarLite.distance(false)); 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /examples/v3/GetDistancePwm/GetDistancePwm.ino: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | v3/GetDistancePwm 5 | 6 | This example shows how to read distance from a LIDAR-Lite connected over the 7 | PWM interface. 8 | 9 | Connections: 10 | LIDAR-Lite 5 Vdc (red) to Arduino 5v 11 | LIDAR-Lite Ground (black) to Arduino GND 12 | LIDAR-Lite Mode control (yellow) to Arduino digital input (pin 3) 13 | LIDAR-Lite Mode control (yellow) to 1 kOhm resistor lead 1 14 | 1 kOhm resistor lead 2 to Arduino digital output (pin 2) 15 | 16 | (Capacitor recommended to mitigate inrush current when device is enabled) 17 | 680uF capacitor (+) to Arduino 5v 18 | 680uF capacitor (-) to Arduino GND 19 | 20 | See the Operation Manual for wiring diagrams and more information: 21 | http://www.robotshop.com/media/files/pdf2/pli-06-instruction.pdf 22 | 23 | ------------------------------------------------------------------------------*/ 24 | 25 | unsigned long pulseWidth; 26 | 27 | void setup() 28 | { 29 | Serial.begin(115200); // Start serial communications 30 | 31 | pinMode(2, OUTPUT); // Set pin 2 as trigger pin 32 | digitalWrite(2, LOW); // Set trigger LOW for continuous read 33 | 34 | pinMode(3, INPUT); // Set pin 3 as monitor pin 35 | } 36 | 37 | void loop() 38 | { 39 | pulseWidth = pulseIn(3, HIGH); // Count how long the pulse is high in microseconds 40 | 41 | // If we get a reading that isn't zero, let's print it 42 | if(pulseWidth != 0) 43 | { 44 | pulseWidth = pulseWidth / 10; // 10usec = 1 cm of distance 45 | Serial.println(pulseWidth); // Print the distance 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /examples/v3/ShortRangeHighSpeed/ShortRangeHighSpeed.ino: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | v3/ShortRangeHighSpeed 5 | 6 | This example shows a method for running the LIDAR-Lite at high speeds for 7 | short range operation. It uses an different approach than the default 8 | LIDAR-Lite library, showing how to read and write to the device using 9 | Wire directly, as well as various custom settings. See GetDistancePWM and 10 | GetDistanceI2C for simpler demonstrations of the device. 11 | 12 | Connections: 13 | LIDAR-Lite 5 Vdc (red) to Arduino 5v 14 | LIDAR-Lite I2C SCL (green) to Arduino SCL 15 | LIDAR-Lite I2C SDA (blue) to Arduino SDA 16 | LIDAR-Lite Ground (black) to Arduino GND 17 | 18 | (Capacitor recommended to mitigate inrush current when device is enabled) 19 | 680uF capacitor (+) to Arduino 5v 20 | 680uF capacitor (-) to Arduino GND 21 | 22 | See the Operation Manual for wiring diagrams and more information: 23 | http://www.robotshop.com/media/files/pdf2/pli-06-instruction.pdf 24 | 25 | ------------------------------------------------------------------------------*/ 26 | 27 | #include 28 | #include 29 | 30 | LIDARLite myLidarLite; 31 | 32 | void setup() 33 | { 34 | Serial.begin(115200); // Initialize serial connection to display distance readings 35 | 36 | /* 37 | begin(int configuration, bool fasti2c, char lidarliteAddress) 38 | 39 | Starts the sensor and I2C. 40 | 41 | Parameters 42 | ---------------------------------------------------------------------------- 43 | configuration: Default 0. Selects one of several preset configurations. 44 | fasti2c: Default 100 kHz. I2C base frequency. 45 | If true I2C frequency is set to 400kHz. 46 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 47 | operating manual for instructions. 48 | */ 49 | myLidarLite.begin(0, true); // Set configuration to default and I2C to 400 kHz 50 | 51 | /* 52 | Write 53 | 54 | Perform I2C write to device. 55 | 56 | Parameters 57 | ---------------------------------------------------------------------------- 58 | myAddress: register address to write to. 59 | myValue: value to write. 60 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 61 | operating manual for instructions. 62 | */ 63 | myLidarLite.write(0x02, 0x0d); // Maximum acquisition count of 0x0d. (default is 0x80) 64 | myLidarLite.write(0x04, 0b00000100); // Use non-default reference acquisition count 65 | myLidarLite.write(0x12, 0x03); // Reference acquisition count of 3 (default is 5) 66 | } 67 | 68 | void loop() 69 | { 70 | // Take a measurement with receiver bias correction and print to serial terminal 71 | Serial.println(distanceFast(true)); 72 | 73 | // Take 99 measurements without receiver bias correction and print to serial terminal 74 | for(int i = 0; i < 99; i++) 75 | { 76 | Serial.println(distanceFast(false)); 77 | } 78 | } 79 | 80 | // Read distance. The approach is to poll the status register until the device goes 81 | // idle after finishing a measurement, send a new measurement command, then read the 82 | // previous distance data while it is performing the new command. 83 | int distanceFast(bool biasCorrection) 84 | { 85 | byte isBusy = 1; 86 | int distance; 87 | int loopCount; 88 | 89 | // Poll busy bit in status register until device is idle 90 | while(isBusy) 91 | { 92 | // Read status register 93 | Wire.beginTransmission(LIDARLITE_ADDR_DEFAULT); 94 | Wire.write(0x01); 95 | Wire.endTransmission(); 96 | Wire.requestFrom(LIDARLITE_ADDR_DEFAULT, 1); 97 | isBusy = Wire.read(); 98 | isBusy = bitRead(isBusy,0); // Take LSB of status register, busy bit 99 | 100 | loopCount++; // Increment loop counter 101 | // Stop status register polling if stuck in loop 102 | if(loopCount > 9999) 103 | { 104 | break; 105 | } 106 | } 107 | 108 | // Send measurement command 109 | Wire.beginTransmission(LIDARLITE_ADDR_DEFAULT); 110 | Wire.write(0X00); // Prepare write to register 0x00 111 | if(biasCorrection == true) 112 | { 113 | Wire.write(0X04); // Perform measurement with receiver bias correction 114 | } 115 | else 116 | { 117 | Wire.write(0X03); // Perform measurement without receiver bias correction 118 | } 119 | Wire.endTransmission(); 120 | 121 | // Immediately read previous distance measurement data. This is valid until the next measurement finishes. 122 | // The I2C transaction finishes before new distance measurement data is acquired. 123 | // Prepare 2 byte read from registers 0x0f and 0x10 124 | Wire.beginTransmission(LIDARLITE_ADDR_DEFAULT); 125 | Wire.write(0x8f); 126 | Wire.endTransmission(); 127 | 128 | // Perform the read and repack the 2 bytes into 16-bit word 129 | Wire.requestFrom(LIDARLITE_ADDR_DEFAULT, 2); 130 | distance = Wire.read(); 131 | distance <<= 8; 132 | distance |= Wire.read(); 133 | 134 | // Return the measured distance 135 | return distance; 136 | } 137 | -------------------------------------------------------------------------------- /examples/v3HP/v3HP_I2C/v3HP_I2C.ino: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | v3HP/v3HP_I2C 5 | 6 | This example shows methods for running the LIDAR-Lite v3 HP in various 7 | modes of operation. To exercise the examples open a serial terminal 8 | program (or the Serial Monitor in the Arduino IDE) and send ASCII 9 | characters to trigger the commands. See "loop" function for details. 10 | 11 | Connections: 12 | LIDAR-Lite 5 Vdc (red) to Arduino 5v 13 | LIDAR-Lite I2C SCL (green) to Arduino SCL 14 | LIDAR-Lite I2C SDA (blue) to Arduino SDA 15 | LIDAR-Lite Ground (black) to Arduino GND 16 | 17 | (Capacitor recommended to mitigate inrush current when device is enabled) 18 | 680uF capacitor (+) to Arduino 5v 19 | 680uF capacitor (-) to Arduino GND 20 | 21 | See the Operation Manual for wiring diagrams and more information: 22 | https://www.robotshop.com/media/files/pdf2/rb-pli-17_-_llv3hp_-_operation_manual_and_technical_specifications.pdf 23 | 24 | ------------------------------------------------------------------------------*/ 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | LIDARLite_v3HP myLidarLite; 31 | 32 | #define FAST_I2C 33 | 34 | enum rangeType_T 35 | { 36 | RANGE_NONE, 37 | RANGE_SINGLE, 38 | RANGE_CONTINUOUS, 39 | RANGE_TIMER 40 | }; 41 | 42 | void setup() 43 | { 44 | uint8_t dataByte; 45 | 46 | // Initialize Arduino serial port (for display of ASCII output to PC) 47 | Serial.begin(115200); 48 | 49 | // Initialize Arduino I2C (for communication to LidarLite) 50 | Wire.begin(); 51 | #ifdef FAST_I2C 52 | #if ARDUINO >= 157 53 | Wire.setClock(400000UL); // Set I2C frequency to 400kHz (for Arduino Due) 54 | #else 55 | TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz 56 | #endif 57 | #endif 58 | 59 | // Configure the LidarLite internal parameters so as to lend itself to 60 | // various modes of operation by altering 'configure' input integer to 61 | // anything in the range of 0 to 5. See LIDARLite_v3HP.cpp for details. 62 | myLidarLite.configure(0); 63 | } 64 | 65 | 66 | void loop() 67 | { 68 | uint16_t distance; 69 | uint8_t newDistance = 0; 70 | uint8_t c; 71 | rangeType_T rangeMode = RANGE_NONE; 72 | 73 | // Continuous loop 74 | while (1) 75 | { 76 | // Each time through the loop, look for a serial input character 77 | if (Serial.available() > 0) 78 | { 79 | // read input character ... 80 | c = (uint8_t) Serial.read(); 81 | 82 | // ... and parse 83 | switch (c) 84 | { 85 | case 'S': 86 | case 's': 87 | rangeMode = RANGE_SINGLE; 88 | break; 89 | 90 | case 'C': 91 | case 'c': 92 | rangeMode = RANGE_CONTINUOUS; 93 | break; 94 | 95 | case 'T': 96 | case 't': 97 | rangeMode = RANGE_TIMER; 98 | break; 99 | 100 | case '.': 101 | rangeMode = RANGE_NONE; 102 | break; 103 | 104 | case 'D': 105 | case 'd': 106 | rangeMode = RANGE_NONE; 107 | dumpCorrelationRecord(); 108 | break; 109 | 110 | case 0x0D: 111 | case 0x0A: 112 | break; 113 | 114 | default: 115 | Serial.println("====================================="); 116 | Serial.println("== Type a single character command =="); 117 | Serial.println("====================================="); 118 | Serial.println(" S - Single Measurement"); 119 | Serial.println(" C - Continuous Measurement"); 120 | Serial.println(" T - Timed Measurement"); 121 | Serial.println(" . - Stop Measurement"); 122 | Serial.println(" D - Dump Correlation Record"); 123 | break; 124 | } 125 | } 126 | 127 | switch (rangeMode) 128 | { 129 | case RANGE_NONE: 130 | newDistance = 0; 131 | break; 132 | 133 | case RANGE_SINGLE: 134 | newDistance = distanceSingle(&distance); 135 | break; 136 | 137 | case RANGE_CONTINUOUS: 138 | newDistance = distanceContinuous(&distance); 139 | break; 140 | 141 | case RANGE_TIMER: 142 | delay(250); // 4 Hz 143 | newDistance = distanceFast(&distance); 144 | break; 145 | 146 | default: 147 | newDistance = 0; 148 | break; 149 | } 150 | 151 | // When there is new distance data, print it to the serial port 152 | if (newDistance) 153 | { 154 | Serial.println(distance); 155 | } 156 | 157 | // Single measurements print once and then stop 158 | if (rangeMode == RANGE_SINGLE) 159 | { 160 | rangeMode = RANGE_NONE; 161 | } 162 | } 163 | } 164 | 165 | //--------------------------------------------------------------------- 166 | // Read Single Distance Measurement 167 | // 168 | // This is the simplest form of taking a measurement. This is a 169 | // blocking function as it will not return until a range has been 170 | // taken and a new distance measurement can be read. 171 | //--------------------------------------------------------------------- 172 | uint8_t distanceSingle(uint16_t * distance) 173 | { 174 | // 1. Wait for busyFlag to indicate device is idle. This must be 175 | // done before triggering a range measurement. 176 | myLidarLite.waitForBusy(); 177 | 178 | // 2. Trigger range measurement. 179 | myLidarLite.takeRange(); 180 | 181 | // 3. Wait for busyFlag to indicate device is idle. This should be 182 | // done before reading the distance data that was triggered above. 183 | myLidarLite.waitForBusy(); 184 | 185 | // 4. Read new distance data from device registers 186 | *distance = myLidarLite.readDistance(); 187 | 188 | return 1; 189 | } 190 | 191 | //--------------------------------------------------------------------- 192 | // Read Continuous Distance Measurements 193 | // 194 | // The most recent distance measurement can always be read from 195 | // device registers. Polling for the BUSY flag in the STATUS 196 | // register can alert the user that the distance measurement is new 197 | // and that the next measurement can be initiated. If the device is 198 | // BUSY this function does nothing and returns 0. If the device is 199 | // NOT BUSY this function triggers the next measurement, reads the 200 | // distance data from the previous measurement, and returns 1. 201 | //--------------------------------------------------------------------- 202 | uint8_t distanceContinuous(uint16_t * distance) 203 | { 204 | uint8_t newDistance = 0; 205 | 206 | // Check on busyFlag to indicate if device is idle 207 | // (meaning = it finished the previously triggered measurement) 208 | if (myLidarLite.getBusyFlag() == 0) 209 | { 210 | // Trigger the next range measurement 211 | myLidarLite.takeRange(); 212 | 213 | // Read new distance data from device registers 214 | *distance = myLidarLite.readDistance(); 215 | 216 | // Report to calling function that we have new data 217 | newDistance = 1; 218 | } 219 | 220 | return newDistance; 221 | } 222 | 223 | //--------------------------------------------------------------------- 224 | // Read Distance Measurement, Quickly 225 | // 226 | // Read distance. The approach is to poll the status register until the device goes 227 | // idle after finishing a measurement, send a new measurement command, then read the 228 | // previous distance data while it is performing the new command. 229 | //--------------------------------------------------------------------- 230 | uint8_t distanceFast(uint16_t * distance) 231 | { 232 | // 1. Wait for busyFlag to indicate device is idle. This must be 233 | // done before triggering a range measurement. 234 | myLidarLite.waitForBusy(); 235 | 236 | // 2. Trigger range measurement. 237 | myLidarLite.takeRange(); 238 | 239 | // 3. Read previous distance data from device registers. 240 | // After starting a measurement we can immediately read previous 241 | // distance measurement while the current range acquisition is 242 | // ongoing. This distance data is valid until the next 243 | // measurement finishes. The I2C transaction finishes before new 244 | // distance measurement data is acquired. 245 | *distance = myLidarLite.readDistance(); 246 | 247 | return 1; 248 | } 249 | 250 | //--------------------------------------------------------------------- 251 | // Print the correlation record for analysis 252 | //--------------------------------------------------------------------- 253 | void dumpCorrelationRecord() 254 | { 255 | myLidarLite.correlationRecordToSerial(256); 256 | } 257 | 258 | -------------------------------------------------------------------------------- /examples/v3HP/v3HP_MONITOR/v3HP_MONITOR.ino: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | v3HP/v3HP_MONITOR 5 | 6 | This example shows a method for running the LIDAR-Lite v3 HP while monitoring 7 | busy status via the MODE pin on the device. Using the MODE pin and the 8 | selected configuration, this example illustrates a higher speed implementation 9 | that can be used for shorter distances. 10 | 11 | Open a serial terminal program (or the Serial Monitor in the Arduino IDE) 12 | to view distance measurements. 13 | 14 | Connections: 15 | LIDAR-Lite 5 Vdc (red) to Arduino 5v 16 | LIDAR-Lite I2C SCL (green) to Arduino SCL 17 | LIDAR-Lite I2C SDA (blue) to Arduino SDA 18 | LIDAR-Lite Ground (black) to Arduino GND 19 | LIDAR-Lite MODE (yellow) to Arduino pin 3 20 | 21 | (Capacitor recommended to mitigate inrush current when device is enabled) 22 | 680uF capacitor (+) to Arduino 5v 23 | 680uF capacitor (-) to Arduino GND 24 | 25 | See the Operation Manual for wiring diagrams and more information: 26 | https://www.robotshop.com/media/files/pdf2/rb-pli-17_-_llv3hp_-_operation_manual_and_technical_specifications.pdf 27 | 28 | ------------------------------------------------------------------------------*/ 29 | 30 | #include 31 | #include 32 | #include "LIDARLite_v3HP.h" 33 | 34 | LIDARLite_v3HP myLidarLite; 35 | 36 | #define FAST_I2C 37 | //#define EXTERNAL_PULLUP 38 | 39 | #define MonitorPin 3 40 | #define TriggerPin 2 41 | 42 | void setup() 43 | { 44 | // Initialize Arduino serial port (for display of ASCII output to PC) 45 | Serial.begin(115200); 46 | 47 | // Initialize Arduino I2C (for communication to LidarLite) 48 | Wire.begin(); 49 | #ifdef FAST_I2C 50 | #if ARDUINO >= 157 51 | Wire.setClock(400000UL); // Set I2C frequency to 400kHz (for Arduino Due) 52 | #else 53 | TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz 54 | #endif 55 | #endif 56 | 57 | // Configuration '6' sets the mode select bits in the acquisition config 58 | // register to 01 and configures other settings to enable high rep rates. 59 | myLidarLite.configure(6); 60 | 61 | #ifdef EXTERNAL_PULLUP 62 | // Standard configuration of the monitor and trigger pins as 63 | // shown in the operation manual 64 | pinMode(MonitorPin, INPUT); 65 | pinMode(TriggerPin, OUTPUT); 66 | digitalWrite(TriggerPin, HIGH); 67 | #else 68 | // When no external pullup to the trigger pin is installed the 69 | // monitor pin can be used by itself using an internal pullup in the micro 70 | pinMode(MonitorPin, INPUT_PULLUP); 71 | #endif 72 | } 73 | 74 | void loop() 75 | { 76 | uint16_t distance = 0; 77 | uint8_t newDistance = 0; 78 | 79 | // Continuous loop 80 | while (1) 81 | { 82 | newDistance = distanceContinuous(&distance); 83 | 84 | // When there is new distance data, print it to the serial port 85 | if (newDistance) 86 | { 87 | Serial.println(distance); 88 | } 89 | } 90 | } 91 | 92 | //--------------------------------------------------------------------- 93 | // Read Continuous Distance Measurements 94 | // 95 | // To use the function, the MODE pin must first be configured for 96 | // status output mode via the acquisition configuration register. 97 | // 98 | // The most recent distance measurement can always be read from 99 | // device registers. Monitoring the BUSY flag via the MODE pin 100 | // can alert the user that the distance measurement is new 101 | // and that the next measurement can be initiated. If the device is 102 | // BUSY this function does nothing and returns 0. If the device is 103 | // NOT BUSY this function triggers the next measurement, reads the 104 | // distance data from the previous measurement, and returns 1. 105 | //--------------------------------------------------------------------- 106 | uint8_t distanceContinuous(uint16_t * distance) 107 | { 108 | uint8_t newDistance = 0; 109 | 110 | // Check on busyFlag to indicate if device is idle 111 | // (meaning = it finished the previously triggered measurement) 112 | if (digitalRead(MonitorPin)) 113 | { 114 | // Trigger the next range measurement 115 | myLidarLite.takeRange(); 116 | 117 | // Read new distance data from device registers 118 | *distance = myLidarLite.readDistance(); 119 | 120 | // Report to calling function that we have new data 121 | newDistance = 1; 122 | } 123 | 124 | return newDistance; 125 | } 126 | 127 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | LIDARLite KEYWORD1 10 | LIDARLite_v3HP KEYWORD1 11 | 12 | ####################################### 13 | # Methods and Functions (KEYWORD2) 14 | ####################################### 15 | 16 | begin KEYWORD2 17 | configure KEYWORD2 18 | reset KEYWORD2 19 | distance KEYWORD2 20 | write KEYWORD2 21 | read KEYWORD2 22 | correlationRecordToSerial KEYWORD2 23 | 24 | setI2Caddr KEYWORD2 25 | readDistance KEYWORD2 26 | waitForBusy KEYWORD2 27 | getBusyFlag KEYWORD2 28 | takeRange KEYWORD2 29 | 30 | ###################################### 31 | # Constants (LITERAL1) 32 | ####################################### 33 | 34 | LIDARLITE_ADDR_DEFAULT LITERAL1 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=LIDAR-Lite 2 | version=2.0.5 3 | author=Garmin 4 | maintainer=Garmin 5 | sentence=Arduino library for Garmin LIDAR-Lite. 6 | paragraph=High-performance optical distance sensing. See product page for specs, manual, and pinout. 7 | category=Sensors 8 | url=https://www.robotshop.com/en/garmin.html 9 | architectures=* 10 | -------------------------------------------------------------------------------- /src/LIDARLite.cpp: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite Arduino Library 4 | LIDARLite.cpp 5 | 6 | This library provides quick access to the basic functions of LIDAR-Lite 7 | via the Arduino interface. Additionally, it can provide a user of any 8 | platform with a template for their own application code. 9 | 10 | Copyright (c) 2016 Garmin Ltd. or its subsidiaries. 11 | 12 | Licensed under the Apache License, Version 2.0 (the "License"); 13 | you may not use this file except in compliance with the License. 14 | You may obtain a copy of the License at 15 | 16 | http://www.apache.org/licenses/LICENSE-2.0 17 | 18 | Unless required by applicable law or agreed to in writing, software 19 | distributed under the License is distributed on an "AS IS" BASIS, 20 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | See the License for the specific language governing permissions and 22 | limitations under the License. 23 | 24 | ------------------------------------------------------------------------------*/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include "LIDARLite.h" 30 | 31 | /*------------------------------------------------------------------------------ 32 | Constructor 33 | 34 | Use LIDARLite::begin to initialize. 35 | ------------------------------------------------------------------------------*/ 36 | LIDARLite::LIDARLite(){} 37 | 38 | /*------------------------------------------------------------------------------ 39 | Begin 40 | 41 | Starts the sensor and I2C. 42 | 43 | Parameters 44 | ------------------------------------------------------------------------------ 45 | configuration: Default 0. Selects one of several preset configurations. 46 | fasti2c: Default 100 kHz. I2C base frequency. 47 | If true I2C frequency is set to 400kHz. 48 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 49 | operating manual for instructions. 50 | ------------------------------------------------------------------------------*/ 51 | void LIDARLite::begin(int configuration, bool fasti2c, char lidarliteAddress) 52 | { 53 | Wire.begin(); // Start I2C 54 | if(fasti2c) 55 | { 56 | #if ARDUINO >= 157 57 | Wire.setClock(400000UL); // Set I2C frequency to 400kHz, for Arduino Due 58 | #else 59 | TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz 60 | #endif 61 | } 62 | configure(configuration, lidarliteAddress); // Configuration settings 63 | } /* LIDARLite::begin */ 64 | 65 | /*------------------------------------------------------------------------------ 66 | Configure 67 | 68 | Selects one of several preset configurations. 69 | 70 | Parameters 71 | ------------------------------------------------------------------------------ 72 | configuration: Default 0. 73 | 0: Default mode, balanced performance. 74 | 1: Short range, high speed. Uses 0x1d maximum acquisition count. 75 | 2: Default range, higher speed short range. Turns on quick termination 76 | detection for faster measurements at short range (with decreased 77 | accuracy) 78 | 3: Maximum range. Uses 0xff maximum acquisition count. 79 | 4: High sensitivity detection. Overrides default valid measurement detection 80 | algorithm, and uses a threshold value for high sensitivity and noise. 81 | 5: Low sensitivity detection. Overrides default valid measurement detection 82 | algorithm, and uses a threshold value for low sensitivity and noise. 83 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 84 | operating manual for instructions. 85 | ------------------------------------------------------------------------------*/ 86 | void LIDARLite::configure(int configuration, char lidarliteAddress) 87 | { 88 | switch (configuration) 89 | { 90 | case 0: // Default mode, balanced performance 91 | write(0x02,0x80,lidarliteAddress); // Default 92 | write(0x04,0x08,lidarliteAddress); // Default 93 | write(0x1c,0x00,lidarliteAddress); // Default 94 | break; 95 | 96 | case 1: // Short range, high speed 97 | write(0x02,0x1d,lidarliteAddress); 98 | write(0x04,0x08,lidarliteAddress); // Default 99 | write(0x1c,0x00,lidarliteAddress); // Default 100 | break; 101 | 102 | case 2: // Default range, higher speed short range 103 | write(0x02,0x80,lidarliteAddress); // Default 104 | write(0x04,0x00,lidarliteAddress); 105 | write(0x1c,0x00,lidarliteAddress); // Default 106 | break; 107 | 108 | case 3: // Maximum range 109 | write(0x02,0xff,lidarliteAddress); 110 | write(0x04,0x08,lidarliteAddress); // Default 111 | write(0x1c,0x00,lidarliteAddress); // Default 112 | break; 113 | 114 | case 4: // High sensitivity detection, high erroneous measurements 115 | write(0x02,0x80,lidarliteAddress); // Default 116 | write(0x04,0x08,lidarliteAddress); // Default 117 | write(0x1c,0x80,lidarliteAddress); 118 | break; 119 | 120 | case 5: // Low sensitivity detection, low erroneous measurements 121 | write(0x02,0x80,lidarliteAddress); // Default 122 | write(0x04,0x08,lidarliteAddress); // Default 123 | write(0x1c,0xb0,lidarliteAddress); 124 | break; 125 | } 126 | } /* LIDARLite::configure */ 127 | 128 | /*------------------------------------------------------------------------------ 129 | Set I2C Address 130 | 131 | Set Alternate I2C Device Address. See Operation Manual for additional info. 132 | 133 | Parameters 134 | ------------------------------------------------------------------------------ 135 | newAddress: desired secondary I2C device address 136 | disableDefault: a non-zero value here means the default 0x62 I2C device 137 | address will be disabled. 138 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 139 | operating manual for instructions. 140 | ------------------------------------------------------------------------------*/ 141 | void LIDARLite::setI2Caddr(char newAddress, char disableDefault, char lidarliteAddress) 142 | { 143 | byte dataBytes[2]; 144 | 145 | // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations 146 | read ((0x16 | 0x80), 2, dataBytes, false, lidarliteAddress); 147 | write(0x18, dataBytes[0], lidarliteAddress); 148 | write(0x19, dataBytes[1], lidarliteAddress); 149 | 150 | // Write the new I2C device address to registers 151 | dataBytes[0] = newAddress; 152 | write(0x1a, dataBytes[0], lidarliteAddress); 153 | 154 | // Enable the new I2C device address using the default I2C device address 155 | dataBytes[0] = 0; 156 | write(0x1e, dataBytes[0], lidarliteAddress); 157 | 158 | // If desired, disable default I2C device address (using the new I2C device address) 159 | if (disableDefault) 160 | { 161 | dataBytes[0] = (1 << 3); // set bit to disable default address 162 | write(0x1e, dataBytes[0], newAddress); 163 | } 164 | } /* LIDARLite::setI2Caddr */ 165 | 166 | /*------------------------------------------------------------------------------ 167 | Reset 168 | 169 | Reset device. The device reloads default register settings, including the 170 | default I2C address. Re-initialization takes approximately 22ms. 171 | 172 | Parameters 173 | ------------------------------------------------------------------------------ 174 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 175 | operating manual for instructions. 176 | ------------------------------------------------------------------------------*/ 177 | void LIDARLite::reset(char lidarliteAddress) 178 | { 179 | write(0x00,0x00,lidarliteAddress); 180 | } /* LIDARLite::reset */ 181 | 182 | /*------------------------------------------------------------------------------ 183 | Distance 184 | 185 | Take a distance measurement and read the result. 186 | 187 | Process 188 | ------------------------------------------------------------------------------ 189 | 1. Write 0x04 or 0x03 to register 0x00 to initiate an aquisition. 190 | 2. Read register 0x01 (this is handled in the read() command) 191 | - if the first bit is "1" then the sensor is busy, loop until the first 192 | bit is "0" 193 | - if the first bit is "0" then the sensor is ready 194 | 3. Read two bytes from register 0x8f and save 195 | 4. Shift the first value from 0x8f << 8 and add to second value from 0x8f. 196 | The result is the measured distance in centimeters. 197 | 198 | Parameters 199 | ------------------------------------------------------------------------------ 200 | biasCorrection: Default true. Take aquisition with receiver bias 201 | correction. If set to false measurements will be faster. Receiver bias 202 | correction must be performed periodically. (e.g. 1 out of every 100 203 | readings). 204 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 205 | operating manual for instructions. 206 | ------------------------------------------------------------------------------*/ 207 | int LIDARLite::distance(bool biasCorrection, char lidarliteAddress) 208 | { 209 | if(biasCorrection) 210 | { 211 | // Take acquisition & correlation processing with receiver bias correction 212 | write(0x00,0x04,lidarliteAddress); 213 | } 214 | else 215 | { 216 | // Take acquisition & correlation processing without receiver bias correction 217 | write(0x00,0x03,lidarliteAddress); 218 | } 219 | // Array to store high and low bytes of distance 220 | byte distanceArray[2]; 221 | // Read two bytes from register 0x8f (autoincrement for reading 0x0f and 0x10) 222 | read(0x8f,2,distanceArray,true,lidarliteAddress); 223 | // Shift high byte and add to low byte 224 | int distance = (distanceArray[0] << 8) + distanceArray[1]; 225 | return(distance); 226 | } /* LIDARLite::distance */ 227 | 228 | /*------------------------------------------------------------------------------ 229 | Write 230 | 231 | Perform I2C write to device. 232 | 233 | Parameters 234 | ------------------------------------------------------------------------------ 235 | myAddress: register address to write to. 236 | myValue: value to write. 237 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 238 | operating manual for instructions. 239 | ------------------------------------------------------------------------------*/ 240 | void LIDARLite::write(char myAddress, char myValue, char lidarliteAddress) 241 | { 242 | Wire.beginTransmission((int)lidarliteAddress); 243 | Wire.write((int)myAddress); // Set register for write 244 | Wire.write((int)myValue); // Write myValue to register 245 | 246 | // A nack means the device is not responding, report the error over serial 247 | int nackCatcher = Wire.endTransmission(); 248 | if(nackCatcher != 0) 249 | { 250 | Serial.println("> nack"); 251 | } 252 | 253 | delay(1); // 1 ms delay for robustness with successive reads and writes 254 | } /* LIDARLite::write */ 255 | 256 | /*------------------------------------------------------------------------------ 257 | Read 258 | 259 | Perform I2C read from device. Will detect an unresponsive device and report 260 | the error over serial. The optional busy flag monitoring 261 | can be used to read registers that are updated at the end of a distance 262 | measurement to obtain the new data. 263 | 264 | Parameters 265 | ------------------------------------------------------------------------------ 266 | myAddress: register address to read from. 267 | numOfBytes: numbers of bytes to read. Can be 1 or 2. 268 | arrayToSave: an array to store the read values. 269 | monitorBusyFlag: if true, the routine will repeatedly read the status 270 | register until the busy flag (LSB) is 0. 271 | ------------------------------------------------------------------------------*/ 272 | void LIDARLite::read(char myAddress, int numOfBytes, byte arrayToSave[2], bool monitorBusyFlag, char lidarliteAddress) 273 | { 274 | int busyFlag = 0; // busyFlag monitors when the device is done with a measurement 275 | if(monitorBusyFlag) 276 | { 277 | busyFlag = 1; // Begin read immediately if not monitoring busy flag 278 | } 279 | int busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout 280 | 281 | while(busyFlag != 0) // Loop until device is not busy 282 | { 283 | // Read status register to check busy flag 284 | Wire.beginTransmission((int)lidarliteAddress); 285 | Wire.write(0x01); // Set the status register to be read 286 | 287 | // A nack means the device is not responding, report the error over serial 288 | int nackCatcher = Wire.endTransmission(); 289 | if(nackCatcher != 0) 290 | { 291 | Serial.println("> nack"); 292 | } 293 | 294 | Wire.requestFrom((int)lidarliteAddress,1); // Read register 0x01 295 | busyFlag = bitRead(Wire.read(),0); // Assign the LSB of the status register to busyFlag 296 | 297 | busyCounter++; // Increment busyCounter for timeout 298 | 299 | // Handle timeout condition, exit while loop and goto bailout 300 | if(busyCounter > 9999) 301 | { 302 | goto bailout; 303 | } 304 | } 305 | 306 | // Device is not busy, begin read 307 | if(busyFlag == 0) 308 | { 309 | Wire.beginTransmission((int)lidarliteAddress); 310 | Wire.write((int)myAddress); // Set the register to be read 311 | 312 | // A nack means the device is not responding, report the error over serial 313 | int nackCatcher = Wire.endTransmission(); 314 | if(nackCatcher != 0) 315 | { 316 | Serial.println("> nack"); 317 | } 318 | 319 | // Perform read of 1 or 2 bytes, save in arrayToSave 320 | Wire.requestFrom((int)lidarliteAddress, numOfBytes); 321 | int i = 0; 322 | if(numOfBytes <= Wire.available()) 323 | { 324 | while(i < numOfBytes) 325 | { 326 | arrayToSave[i] = Wire.read(); 327 | i++; 328 | } 329 | } 330 | } 331 | 332 | // bailout reports error over serial 333 | if(busyCounter > 9999) 334 | { 335 | bailout: 336 | busyCounter = 0; 337 | Serial.println("> read failed"); 338 | } 339 | } /* LIDARLite::read */ 340 | 341 | /*------------------------------------------------------------------------------ 342 | Correlation Record To Serial 343 | 344 | The correlation record used to calculate distance can be read from the device. 345 | It has a bipolar wave shape, transitioning from a positive going portion to a 346 | roughly symmetrical negative going pulse. The point where the signal crosses 347 | zero represents the effective delay for the reference and return signals. 348 | 349 | Process 350 | ------------------------------------------------------------------------------ 351 | 1. Take a distance reading (there is no correlation record without at least 352 | one distance reading being taken) 353 | 2. Select memory bank by writing 0xc0 to register 0x5d 354 | 3. Set test mode select by writing 0x07 to register 0x40 355 | 4. For as many readings as you want to take (max is 1024) 356 | 1. Read two bytes from 0xd2 357 | 2. The Low byte is the value from the record 358 | 3. The high byte is the sign from the record 359 | 360 | Parameters 361 | ------------------------------------------------------------------------------ 362 | separator: the separator between serial data words 363 | numberOfReadings: Default: 256. Maximum of 1024 364 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 365 | operating manual for instructions. 366 | ------------------------------------------------------------------------------*/ 367 | void LIDARLite::correlationRecordToSerial(char separator, int numberOfReadings, char lidarliteAddress) 368 | { 369 | 370 | // Array to store read values 371 | byte correlationArray[2]; 372 | // Var to store value of correlation record 373 | int correlationValue = 0; 374 | // Selects memory bank 375 | write(0x5d,0xc0,lidarliteAddress); 376 | // Test mode enable 377 | write(0x40, 0x07,lidarliteAddress); 378 | for(int i = 0; i 31 | 32 | class LIDARLite 33 | { 34 | public: 35 | LIDARLite(); 36 | void begin(int = 0, bool = false, char = LIDARLITE_ADDR_DEFAULT); 37 | void configure(int = 0, char = LIDARLITE_ADDR_DEFAULT); 38 | void setI2Caddr(char, char, char = LIDARLITE_ADDR_DEFAULT); 39 | void reset(char = LIDARLITE_ADDR_DEFAULT); 40 | int distance(bool = true, char = LIDARLITE_ADDR_DEFAULT); 41 | void write(char, char, char = LIDARLITE_ADDR_DEFAULT); 42 | void read(char, int, byte*, bool, char); 43 | void correlationRecordToSerial(char = '\n', int = 256, char = LIDARLITE_ADDR_DEFAULT); 44 | }; 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /src/LIDARLite_v3HP.cpp: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------------ 2 | 3 | LIDARLite_v3HP Arduino Library 4 | LIDARLite_v3HP.cpp 5 | 6 | This library provides quick access to the basic functions of LIDAR-Lite 7 | via the Arduino interface. Additionally, it can provide a user of any 8 | platform with a template for their own application code. 9 | 10 | Copyright (c) 2018 Garmin Ltd. or its subsidiaries. 11 | 12 | Licensed under the Apache License, Version 2.0 (the "License"); 13 | you may not use this file except in compliance with the License. 14 | You may obtain a copy of the License at 15 | 16 | http://www.apache.org/licenses/LICENSE-2.0 17 | 18 | Unless required by applicable law or agreed to in writing, software 19 | distributed under the License is distributed on an "AS IS" BASIS, 20 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | See the License for the specific language governing permissions and 22 | limitations under the License. 23 | 24 | ------------------------------------------------------------------------------*/ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include "LIDARLite_v3HP.h" 31 | 32 | /*------------------------------------------------------------------------------ 33 | Configure 34 | 35 | Selects one of several preset configurations. 36 | 37 | Parameters 38 | ------------------------------------------------------------------------------ 39 | configuration: Default 0. 40 | 0: Default mode, balanced performance. 41 | 1: Short range, high speed. Uses 0x1d maximum acquisition count. 42 | 2: Default range, higher speed short range. Turns on quick termination 43 | detection for faster measurements at short range (with decreased 44 | accuracy) 45 | 3: Maximum range. Uses 0xff maximum acquisition count. 46 | 4: High sensitivity detection. Overrides default valid measurement detection 47 | algorithm, and uses a threshold value for high sensitivity and noise. 48 | 5: Low sensitivity detection. Overrides default valid measurement detection 49 | algorithm, and uses a threshold value for low sensitivity and noise. 50 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 51 | operating manual for instructions. 52 | ------------------------------------------------------------------------------*/ 53 | void LIDARLite_v3HP::configure(uint8_t configuration, uint8_t lidarliteAddress) 54 | { 55 | uint8_t sigCountMax; 56 | uint8_t acqConfigReg; 57 | uint8_t refCountMax; 58 | uint8_t thresholdBypass; 59 | 60 | switch (configuration) 61 | { 62 | case 0: // Default mode, balanced performance 63 | sigCountMax = 0x80; // Default 64 | acqConfigReg = 0x08; // Default 65 | refCountMax = 0x05; // Default 66 | thresholdBypass = 0x00; // Default 67 | break; 68 | 69 | case 1: // Short range, high speed 70 | sigCountMax = 0x1d; 71 | acqConfigReg = 0x08; // Default 72 | refCountMax = 0x03; 73 | thresholdBypass = 0x00; // Default 74 | break; 75 | 76 | case 2: // Default range, higher speed short range 77 | sigCountMax = 0x80; // Default 78 | acqConfigReg = 0x00; 79 | refCountMax = 0x03; 80 | thresholdBypass = 0x00; // Default 81 | break; 82 | 83 | case 3: // Maximum range 84 | sigCountMax = 0xff; 85 | acqConfigReg = 0x08; // Default 86 | refCountMax = 0x05; // Default 87 | thresholdBypass = 0x00; // Default 88 | break; 89 | 90 | case 4: // High sensitivity detection, high erroneous measurements 91 | sigCountMax = 0x80; // Default 92 | acqConfigReg = 0x08; // Default 93 | refCountMax = 0x05; // Default 94 | thresholdBypass = 0x80; 95 | break; 96 | 97 | case 5: // Low sensitivity detection, low erroneous measurements 98 | sigCountMax = 0x80; // Default 99 | acqConfigReg = 0x08; // Default 100 | refCountMax = 0x05; // Default 101 | thresholdBypass = 0xb0; 102 | break; 103 | 104 | case 6: // Short range, high speed, higher error 105 | sigCountMax = 0x04; 106 | acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode 107 | refCountMax = 0x03; 108 | thresholdBypass = 0x00; 109 | break; 110 | } 111 | 112 | write(0x02, &sigCountMax , 1, lidarliteAddress); 113 | write(0x04, &acqConfigReg , 1, lidarliteAddress); 114 | write(0x12, &refCountMax , 1, lidarliteAddress); 115 | write(0x1c, &thresholdBypass, 1, lidarliteAddress); 116 | } /* LIDARLite_v3HP::configure */ 117 | 118 | /*------------------------------------------------------------------------------ 119 | Set I2C Address 120 | 121 | Set Alternate I2C Device Address. See Operation Manual for additional info. 122 | 123 | Parameters 124 | ------------------------------------------------------------------------------ 125 | newAddress: desired secondary I2C device address 126 | disableDefault: a non-zero value here means the default 0x62 I2C device 127 | address will be disabled. 128 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 129 | operating manual for instructions. 130 | ------------------------------------------------------------------------------*/ 131 | void LIDARLite_v3HP::setI2Caddr (uint8_t newAddress, uint8_t disableDefault, 132 | uint8_t lidarliteAddress) 133 | { 134 | uint8_t dataBytes[2]; 135 | 136 | // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations 137 | read (0x16, dataBytes, 2, lidarliteAddress); 138 | write(0x18, dataBytes, 2, lidarliteAddress); 139 | 140 | // Write the new I2C device address to registers 141 | // left shift by one to work around data alignment issue in v3HP 142 | dataBytes[0] = (newAddress << 1); 143 | write(0x1a, dataBytes, 1, lidarliteAddress); 144 | 145 | // Enable the new I2C device address using the default I2C device address 146 | read (0x1e, dataBytes, 1, lidarliteAddress); 147 | dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address 148 | write(0x1e, dataBytes, 1, lidarliteAddress); 149 | 150 | // If desired, disable default I2C device address (using the new I2C device address) 151 | if (disableDefault) 152 | { 153 | read (0x1e, dataBytes, 1, newAddress); 154 | dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address 155 | write(0x1e, dataBytes, 1, newAddress); 156 | } 157 | } /* LIDARLite_v3HP::setI2Caddr */ 158 | 159 | /*------------------------------------------------------------------------------ 160 | Take Range 161 | 162 | Initiate a distance measurement by writing to register 0x00. 163 | 164 | Parameters 165 | ------------------------------------------------------------------------------ 166 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 167 | operating manual for instructions. 168 | ------------------------------------------------------------------------------*/ 169 | void LIDARLite_v3HP::takeRange(uint8_t lidarliteAddress) 170 | { 171 | uint8_t dataByte = 0x01; 172 | 173 | write(0x00, &dataByte, 1, lidarliteAddress); 174 | } /* LIDARLite_v3HP::takeRange */ 175 | 176 | /*------------------------------------------------------------------------------ 177 | Wait for Busy Flag 178 | 179 | Blocking function to wait until the Lidar Lite's internal busy flag goes low 180 | 181 | Parameters 182 | ------------------------------------------------------------------------------ 183 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 184 | operating manual for instructions. 185 | ------------------------------------------------------------------------------*/ 186 | void LIDARLite_v3HP::waitForBusy(uint8_t lidarliteAddress) 187 | { 188 | uint16_t busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout 189 | uint8_t busyFlag = 1; // busyFlag monitors when the device is done with a measurement 190 | 191 | while (busyFlag) // Loop until device is not busy 192 | { 193 | // Handle timeout condition, exit while loop and goto bailout 194 | if (busyCounter > 9999) 195 | { 196 | break; 197 | } 198 | 199 | busyFlag = getBusyFlag(lidarliteAddress); 200 | 201 | // Increment busyCounter for timeout 202 | busyCounter++; 203 | } 204 | 205 | // bailout reports error over serial 206 | if (busyCounter > 9999) 207 | { 208 | Serial.println("> bailing out of waitForBusy()"); 209 | } 210 | } /* LIDARLite_v3HP::waitForBusy */ 211 | 212 | /*------------------------------------------------------------------------------ 213 | Get Busy Flag 214 | 215 | Read BUSY flag from device registers. Function will return 0x00 if not busy. 216 | 217 | Parameters 218 | ------------------------------------------------------------------------------ 219 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 220 | operating manual for instructions. 221 | ------------------------------------------------------------------------------*/ 222 | uint8_t LIDARLite_v3HP::getBusyFlag(uint8_t lidarliteAddress) 223 | { 224 | uint8_t busyFlag; // busyFlag monitors when the device is done with a measurement 225 | 226 | // Read status register to check busy flag 227 | read(0x01, &busyFlag, 1, lidarliteAddress); 228 | 229 | // STATUS bit 0 is busyFlag 230 | busyFlag &= 0x01; 231 | 232 | return busyFlag; 233 | } /* LIDARLite_v3HP::getBusyFlag */ 234 | 235 | /*------------------------------------------------------------------------------ 236 | Read Distance 237 | 238 | Read and return result of distance measurement. 239 | 240 | Process 241 | ------------------------------------------------------------------------------ 242 | 1. Read two bytes from register 0x8f and save 243 | 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f. 244 | The result is the measured distance in centimeters. 245 | 246 | Parameters 247 | ------------------------------------------------------------------------------ 248 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 249 | operating manual for instructions. 250 | ------------------------------------------------------------------------------*/ 251 | uint16_t LIDARLite_v3HP::readDistance(uint8_t lidarliteAddress) 252 | { 253 | uint16_t distance; 254 | uint8_t dataBytes[2]; 255 | 256 | // Read two bytes from register 0x0f and 0x10 (autoincrement) 257 | read(0x0f, dataBytes, 2, lidarliteAddress); 258 | 259 | // Shift high byte and add to low byte 260 | distance = (dataBytes[0] << 8) | dataBytes[1]; 261 | 262 | return (distance); 263 | } /* LIDARLite_v3HP::readDistance */ 264 | 265 | /*------------------------------------------------------------------------------ 266 | Write 267 | 268 | Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP 269 | will receive multiple bytes in one I2C transmission. The first byte is 270 | always the register address. The the bytes that follow will be written 271 | into the specified register address first and then the internal address 272 | in the Lidar Lite will be auto-incremented for all following bytes. 273 | 274 | Parameters 275 | ------------------------------------------------------------------------------ 276 | regAddr: register address to write to 277 | dataBytes: pointer to array of bytes to write 278 | numBytes: number of bytes in 'dataBytes' array to write 279 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 280 | operating manual for instructions. 281 | ------------------------------------------------------------------------------*/ 282 | void LIDARLite_v3HP::write(uint8_t regAddr, uint8_t * dataBytes, 283 | uint16_t numBytes, uint8_t lidarliteAddress) 284 | { 285 | int nackCatcher; 286 | 287 | Wire.beginTransmission((int) lidarliteAddress); 288 | 289 | // Wire.write Syntax 290 | // ----------------------------------------------------------------- 291 | // Wire.write(value) - a value to send as a single byte 292 | // Wire.write(string) - a string to send as a series of bytes 293 | // Wire.write(data, length) - an array of data to send as bytes 294 | 295 | // First byte of every write sets the LidarLite's internal register address pointer 296 | Wire.write((int) regAddr); 297 | 298 | // Subsequent bytes are data writes 299 | Wire.write(dataBytes, (int) numBytes); 300 | 301 | // A nack means the device is not responding. Report the error over serial. 302 | nackCatcher = Wire.endTransmission(); 303 | if (nackCatcher != 0) 304 | { 305 | Serial.println("> nack"); 306 | } 307 | 308 | delayMicroseconds(100); // 100 us delay for robustness with successive reads and writes 309 | } /* LIDARLite_v3HP::write */ 310 | 311 | /*------------------------------------------------------------------------------ 312 | Read 313 | 314 | Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP 315 | will send multiple bytes in one I2C transmission. The register address must 316 | be set up by a previous I2C write. The bytes that follow will be read 317 | from the specified register address first and then the internal address 318 | pointer in the Lidar Lite will be auto-incremented for following bytes. 319 | 320 | Will detect an unresponsive device and report the error over serial. 321 | 322 | Parameters 323 | ------------------------------------------------------------------------------ 324 | regAddr: register address to write to 325 | dataBytes: pointer to array of bytes to write 326 | numBytes: number of bytes in 'dataBytes' array to write 327 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 328 | operating manual for instructions. 329 | ------------------------------------------------------------------------------*/ 330 | void LIDARLite_v3HP::read(uint8_t regAddr, uint8_t * dataBytes, 331 | uint16_t numBytes, uint8_t lidarliteAddress) 332 | { 333 | uint16_t i = 0; 334 | int nackCatcher = 0; 335 | 336 | // Set the internal register address pointer in the Lidar Lite 337 | Wire.beginTransmission((int) lidarliteAddress); 338 | Wire.write((int) regAddr); // Set the register to be read 339 | 340 | // A nack means the device is not responding, report the error over serial 341 | nackCatcher = Wire.endTransmission(false); // false means perform repeated start 342 | if (nackCatcher != 0) 343 | { 344 | Serial.println("> nack"); 345 | } 346 | 347 | // Perform read, save in dataBytes array 348 | Wire.requestFrom((int)lidarliteAddress, (int) numBytes); 349 | if ((int) numBytes <= Wire.available()) 350 | { 351 | while (i < numBytes) 352 | { 353 | dataBytes[i] = (uint8_t) Wire.read(); 354 | i++; 355 | } 356 | } 357 | 358 | } /* LIDARLite_v3HP::read */ 359 | 360 | /*------------------------------------------------------------------------------ 361 | Correlation Record To Serial 362 | 363 | The correlation record used to calculate distance can be read from the device. 364 | It has a bipolar wave shape, transitioning from a positive going portion to a 365 | roughly symmetrical negative going pulse. The point where the signal crosses 366 | zero represents the effective delay for the reference and return signals. 367 | 368 | Process 369 | ------------------------------------------------------------------------------ 370 | 1. Take a distance reading (there is no correlation record without at least 371 | one distance reading being taken) 372 | 2. Set test mode select by writing 0x07 to register 0x40 373 | 3. For as many readings as you want to take (max is 1024) 374 | 1. Read two bytes from 0x52 375 | 2. The Low byte is the value from the record 376 | 3. The high byte is the sign from the record 377 | 378 | Parameters 379 | ------------------------------------------------------------------------------ 380 | separator: the separator between serial data words 381 | numberOfReadings: Default: 1024. Maximum of 1024 382 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See 383 | operating manual for instructions. 384 | ------------------------------------------------------------------------------*/ 385 | void LIDARLite_v3HP::correlationRecordToSerial( 386 | uint16_t numberOfReadings, uint8_t lidarliteAddress) 387 | { 388 | uint16_t i = 0; 389 | uint8_t dataBytes[2]; // Array to store read / write data 390 | int16_t correlationValue = 0; // Var to store value of correlation record 391 | 392 | // Test mode enable 393 | dataBytes[0] = 0x07; 394 | write(0x40, dataBytes, 1, lidarliteAddress); 395 | 396 | for (i=0 ; i 31 | #include 32 | 33 | class LIDARLite_v3HP 34 | { 35 | public: 36 | void configure (uint8_t configuration = 0, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 37 | 38 | void setI2Caddr (uint8_t newAddress, uint8_t disableDefault, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 39 | uint16_t readDistance(uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 40 | void waitForBusy (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 41 | uint8_t getBusyFlag (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 42 | void takeRange (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 43 | 44 | void write (uint8_t regAddr, uint8_t * dataBytes, uint16_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 45 | void read (uint8_t regAddr, uint8_t * dataBytes, uint16_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 46 | 47 | void correlationRecordToSerial (uint16_t numberOfReadings = 1024, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT); 48 | }; 49 | 50 | #endif 51 | --------------------------------------------------------------------------------