update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
LIDARLite_v3HP.cpp@0:417c1bd45a3c, 2019-09-20 (annotated)
- Committer:
- hubercam
- Date:
- Fri Sep 20 14:15:12 2019 +0000
- Revision:
- 0:417c1bd45a3c
- Child:
- 1:bd9e4d198947
+ LIDARLite_v3HP.cpp; + LIDARLite_v3HP.h
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hubercam | 0:417c1bd45a3c | 1 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 2 | |
hubercam | 0:417c1bd45a3c | 3 | LIDARLite_v3HP Arduino Library |
hubercam | 0:417c1bd45a3c | 4 | LIDARLite_v3HP.cpp |
hubercam | 0:417c1bd45a3c | 5 | |
hubercam | 0:417c1bd45a3c | 6 | This library provides quick access to the basic functions of LIDAR-Lite |
hubercam | 0:417c1bd45a3c | 7 | via the Nucleo interface. Additionally, it can provide a user of any |
hubercam | 0:417c1bd45a3c | 8 | platform with a template for their own application code. |
hubercam | 0:417c1bd45a3c | 9 | |
hubercam | 0:417c1bd45a3c | 10 | Copyright (c) 2018 Garmin Ltd. or its subsidiaries. |
hubercam | 0:417c1bd45a3c | 11 | |
hubercam | 0:417c1bd45a3c | 12 | Licensed under the Apache License, Version 2.0 (the "License"); |
hubercam | 0:417c1bd45a3c | 13 | you may not use this file except in compliance with the License. |
hubercam | 0:417c1bd45a3c | 14 | You may obtain a copy of the License at |
hubercam | 0:417c1bd45a3c | 15 | |
hubercam | 0:417c1bd45a3c | 16 | http://www.apache.org/licenses/LICENSE-2.0 |
hubercam | 0:417c1bd45a3c | 17 | |
hubercam | 0:417c1bd45a3c | 18 | Unless required by applicable law or agreed to in writing, software |
hubercam | 0:417c1bd45a3c | 19 | distributed under the License is distributed on an "AS IS" BASIS, |
hubercam | 0:417c1bd45a3c | 20 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
hubercam | 0:417c1bd45a3c | 21 | See the License for the specific language governing permissions and |
hubercam | 0:417c1bd45a3c | 22 | limitations under the License. |
hubercam | 0:417c1bd45a3c | 23 | |
hubercam | 0:417c1bd45a3c | 24 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 25 | |
hubercam | 0:417c1bd45a3c | 26 | #include <cstdarg> |
hubercam | 0:417c1bd45a3c | 27 | #include <cstdint> |
hubercam | 0:417c1bd45a3c | 28 | #include "LIDARLite_v3HP.h" |
hubercam | 0:417c1bd45a3c | 29 | #include "mbed.h" |
hubercam | 0:417c1bd45a3c | 30 | |
hubercam | 0:417c1bd45a3c | 31 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 32 | Configure |
hubercam | 0:417c1bd45a3c | 33 | |
hubercam | 0:417c1bd45a3c | 34 | Selects one of several preset configurations. |
hubercam | 0:417c1bd45a3c | 35 | |
hubercam | 0:417c1bd45a3c | 36 | Parameters |
hubercam | 0:417c1bd45a3c | 37 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 38 | configuration: Default 0. |
hubercam | 0:417c1bd45a3c | 39 | 0: Default mode, balanced performance. |
hubercam | 0:417c1bd45a3c | 40 | 1: Short range, high speed. Uses 0x1d maximum acquisition count. |
hubercam | 0:417c1bd45a3c | 41 | 2: Default range, higher speed short range. Turns on quick termination |
hubercam | 0:417c1bd45a3c | 42 | detection for faster measurements at short range (with decreased |
hubercam | 0:417c1bd45a3c | 43 | accuracy) |
hubercam | 0:417c1bd45a3c | 44 | 3: Maximum range. Uses 0xff maximum acquisition count. |
hubercam | 0:417c1bd45a3c | 45 | 4: High sensitivity detection. Overrides default valid measurement detection |
hubercam | 0:417c1bd45a3c | 46 | algorithm, and uses a threshold value for high sensitivity and noise. |
hubercam | 0:417c1bd45a3c | 47 | 5: Low sensitivity detection. Overrides default valid measurement detection |
hubercam | 0:417c1bd45a3c | 48 | algorithm, and uses a threshold value for low sensitivity and noise. |
hubercam | 0:417c1bd45a3c | 49 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 50 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 51 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 52 | LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c):i2c_(i2c) |
hubercam | 0:417c1bd45a3c | 53 | { |
hubercam | 0:417c1bd45a3c | 54 | addr_ = LIDARLITE_ADDR_DEFAULT; |
hubercam | 0:417c1bd45a3c | 55 | |
hubercam | 0:417c1bd45a3c | 56 | } /* LIDARLite_v3HP::LIDARLite_v3HP */ |
hubercam | 0:417c1bd45a3c | 57 | |
hubercam | 0:417c1bd45a3c | 58 | LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c, uint8_t &addr):i2c_(i2c) |
hubercam | 0:417c1bd45a3c | 59 | { |
hubercam | 0:417c1bd45a3c | 60 | addr_ = addr; |
hubercam | 0:417c1bd45a3c | 61 | |
hubercam | 0:417c1bd45a3c | 62 | } /* LIDARLite_v3HP::LIDARLite_v3HP */ |
hubercam | 0:417c1bd45a3c | 63 | |
hubercam | 0:417c1bd45a3c | 64 | void LIDARLite_v3HP::configure(const uint8_t &configuration, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 65 | { |
hubercam | 0:417c1bd45a3c | 66 | uint8_t sigCountMax; |
hubercam | 0:417c1bd45a3c | 67 | uint8_t acqConfigReg; |
hubercam | 0:417c1bd45a3c | 68 | uint8_t refCountMax; |
hubercam | 0:417c1bd45a3c | 69 | uint8_t thresholdBypass; |
hubercam | 0:417c1bd45a3c | 70 | |
hubercam | 0:417c1bd45a3c | 71 | switch (configuration) |
hubercam | 0:417c1bd45a3c | 72 | { |
hubercam | 0:417c1bd45a3c | 73 | case 0: // Default mode, balanced performance |
hubercam | 0:417c1bd45a3c | 74 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 75 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 76 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 77 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 78 | break; |
hubercam | 0:417c1bd45a3c | 79 | |
hubercam | 0:417c1bd45a3c | 80 | case 1: // Short range, high speed |
hubercam | 0:417c1bd45a3c | 81 | sigCountMax = 0x1d; |
hubercam | 0:417c1bd45a3c | 82 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 83 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 84 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 85 | break; |
hubercam | 0:417c1bd45a3c | 86 | |
hubercam | 0:417c1bd45a3c | 87 | case 2: // Default range, higher speed short range |
hubercam | 0:417c1bd45a3c | 88 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 89 | acqConfigReg = 0x00; |
hubercam | 0:417c1bd45a3c | 90 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 91 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 92 | break; |
hubercam | 0:417c1bd45a3c | 93 | |
hubercam | 0:417c1bd45a3c | 94 | case 3: // Maximum range |
hubercam | 0:417c1bd45a3c | 95 | sigCountMax = 0xff; |
hubercam | 0:417c1bd45a3c | 96 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 97 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 98 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 99 | break; |
hubercam | 0:417c1bd45a3c | 100 | |
hubercam | 0:417c1bd45a3c | 101 | case 4: // High sensitivity detection, high erroneous measurements |
hubercam | 0:417c1bd45a3c | 102 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 103 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 104 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 105 | thresholdBypass = 0x80; |
hubercam | 0:417c1bd45a3c | 106 | break; |
hubercam | 0:417c1bd45a3c | 107 | |
hubercam | 0:417c1bd45a3c | 108 | case 5: // Low sensitivity detection, low erroneous measurements |
hubercam | 0:417c1bd45a3c | 109 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 110 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 111 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 112 | thresholdBypass = 0xb0; |
hubercam | 0:417c1bd45a3c | 113 | break; |
hubercam | 0:417c1bd45a3c | 114 | |
hubercam | 0:417c1bd45a3c | 115 | case 6: // Short range, high speed, higher error |
hubercam | 0:417c1bd45a3c | 116 | sigCountMax = 0x04; |
hubercam | 0:417c1bd45a3c | 117 | acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode |
hubercam | 0:417c1bd45a3c | 118 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 119 | thresholdBypass = 0x00; |
hubercam | 0:417c1bd45a3c | 120 | break; |
hubercam | 0:417c1bd45a3c | 121 | } |
hubercam | 0:417c1bd45a3c | 122 | |
hubercam | 0:417c1bd45a3c | 123 | write(0x02, &sigCountMax , 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 124 | write(0x04, &acqConfigReg , 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 125 | write(0x12, &refCountMax , 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 126 | write(0x1c, &thresholdBypass, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 127 | } /* LIDARLite_v3HP::configure */ |
hubercam | 0:417c1bd45a3c | 128 | |
hubercam | 0:417c1bd45a3c | 129 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 130 | Set I2C Address |
hubercam | 0:417c1bd45a3c | 131 | |
hubercam | 0:417c1bd45a3c | 132 | Set Alternate I2C Device Address. See Operation Manual for additional info. |
hubercam | 0:417c1bd45a3c | 133 | |
hubercam | 0:417c1bd45a3c | 134 | Parameters |
hubercam | 0:417c1bd45a3c | 135 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 136 | newAddress: desired secondary I2C device address |
hubercam | 0:417c1bd45a3c | 137 | disableDefault: a non-zero value here means the default 0x62 I2C device |
hubercam | 0:417c1bd45a3c | 138 | address will be disabled. |
hubercam | 0:417c1bd45a3c | 139 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 140 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 141 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 142 | void LIDARLite_v3HP::setI2Caddr (const uint8_t &newAddress, uint8_t &disableDefault, |
hubercam | 0:417c1bd45a3c | 143 | const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 144 | { |
hubercam | 0:417c1bd45a3c | 145 | uint8_t dataBytes[2]; |
hubercam | 0:417c1bd45a3c | 146 | |
hubercam | 0:417c1bd45a3c | 147 | // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations |
hubercam | 0:417c1bd45a3c | 148 | read (0x16, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 149 | write(0x18, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 150 | |
hubercam | 0:417c1bd45a3c | 151 | // Write the new I2C device address to registers |
hubercam | 0:417c1bd45a3c | 152 | // left shift by one to work around data alignment issue in v3HP |
hubercam | 0:417c1bd45a3c | 153 | dataBytes[0] = (newAddress << 1); |
hubercam | 0:417c1bd45a3c | 154 | write(0x1a, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 155 | |
hubercam | 0:417c1bd45a3c | 156 | // Enable the new I2C device address using the default I2C device address |
hubercam | 0:417c1bd45a3c | 157 | read (0x1e, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 158 | dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address |
hubercam | 0:417c1bd45a3c | 159 | write(0x1e, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 160 | |
hubercam | 0:417c1bd45a3c | 161 | // If desired, disable default I2C device address (using the new I2C device address) |
hubercam | 0:417c1bd45a3c | 162 | if (disableDefault) |
hubercam | 0:417c1bd45a3c | 163 | { |
hubercam | 0:417c1bd45a3c | 164 | read (0x1e, dataBytes, 1, newAddress); |
hubercam | 0:417c1bd45a3c | 165 | dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address |
hubercam | 0:417c1bd45a3c | 166 | write(0x1e, dataBytes, 1, newAddress); |
hubercam | 0:417c1bd45a3c | 167 | } |
hubercam | 0:417c1bd45a3c | 168 | } /* LIDARLite_v3HP::setI2Caddr */ |
hubercam | 0:417c1bd45a3c | 169 | |
hubercam | 0:417c1bd45a3c | 170 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 171 | Take Range |
hubercam | 0:417c1bd45a3c | 172 | |
hubercam | 0:417c1bd45a3c | 173 | Initiate a distance measurement by writing to register 0x00. |
hubercam | 0:417c1bd45a3c | 174 | |
hubercam | 0:417c1bd45a3c | 175 | Parameters |
hubercam | 0:417c1bd45a3c | 176 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 177 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 178 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 179 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 180 | void LIDARLite_v3HP::takeRange(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 181 | { |
hubercam | 0:417c1bd45a3c | 182 | uint8_t dataByte = 0x01; |
hubercam | 0:417c1bd45a3c | 183 | |
hubercam | 0:417c1bd45a3c | 184 | write(0x00, &dataByte, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 185 | } /* LIDARLite_v3HP::takeRange */ |
hubercam | 0:417c1bd45a3c | 186 | |
hubercam | 0:417c1bd45a3c | 187 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 188 | Wait for Busy Flag |
hubercam | 0:417c1bd45a3c | 189 | |
hubercam | 0:417c1bd45a3c | 190 | Blocking function to wait until the Lidar Lite's internal busy flag goes low |
hubercam | 0:417c1bd45a3c | 191 | |
hubercam | 0:417c1bd45a3c | 192 | Parameters |
hubercam | 0:417c1bd45a3c | 193 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 194 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 195 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 196 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 197 | void LIDARLite_v3HP::waitForBusy(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 198 | { |
hubercam | 0:417c1bd45a3c | 199 | uint16_t busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout |
hubercam | 0:417c1bd45a3c | 200 | uint8_t busyFlag = 1; // busyFlag monitors when the device is done with a measurement |
hubercam | 0:417c1bd45a3c | 201 | |
hubercam | 0:417c1bd45a3c | 202 | while (busyFlag) // Loop until device is not busy |
hubercam | 0:417c1bd45a3c | 203 | { |
hubercam | 0:417c1bd45a3c | 204 | // Handle timeout condition, exit while loop and goto bailout |
hubercam | 0:417c1bd45a3c | 205 | if (busyCounter > 9999) |
hubercam | 0:417c1bd45a3c | 206 | { |
hubercam | 0:417c1bd45a3c | 207 | break; |
hubercam | 0:417c1bd45a3c | 208 | } |
hubercam | 0:417c1bd45a3c | 209 | |
hubercam | 0:417c1bd45a3c | 210 | busyFlag = getBusyFlag(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 211 | |
hubercam | 0:417c1bd45a3c | 212 | // Increment busyCounter for timeout |
hubercam | 0:417c1bd45a3c | 213 | busyCounter++; |
hubercam | 0:417c1bd45a3c | 214 | } |
hubercam | 0:417c1bd45a3c | 215 | |
hubercam | 0:417c1bd45a3c | 216 | // bailout reports error over serial |
hubercam | 0:417c1bd45a3c | 217 | if (busyCounter > 9999) |
hubercam | 0:417c1bd45a3c | 218 | { |
hubercam | 0:417c1bd45a3c | 219 | } |
hubercam | 0:417c1bd45a3c | 220 | } /* LIDARLite_v3HP::waitForBusy */ |
hubercam | 0:417c1bd45a3c | 221 | |
hubercam | 0:417c1bd45a3c | 222 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 223 | Get Busy Flag |
hubercam | 0:417c1bd45a3c | 224 | |
hubercam | 0:417c1bd45a3c | 225 | Read BUSY flag from device registers. Function will return 0x00 if not busy. |
hubercam | 0:417c1bd45a3c | 226 | |
hubercam | 0:417c1bd45a3c | 227 | Parameters |
hubercam | 0:417c1bd45a3c | 228 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 229 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 230 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 231 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 232 | uint8_t LIDARLite_v3HP::getBusyFlag(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 233 | { |
hubercam | 0:417c1bd45a3c | 234 | uint8_t busyFlag; // busyFlag monitors when the device is done with a measurement |
hubercam | 0:417c1bd45a3c | 235 | |
hubercam | 0:417c1bd45a3c | 236 | // Read status register to check busy flag |
hubercam | 0:417c1bd45a3c | 237 | read(0x01, &busyFlag, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 238 | |
hubercam | 0:417c1bd45a3c | 239 | // STATUS bit 0 is busyFlag |
hubercam | 0:417c1bd45a3c | 240 | busyFlag &= 0x01; |
hubercam | 0:417c1bd45a3c | 241 | |
hubercam | 0:417c1bd45a3c | 242 | return busyFlag; |
hubercam | 0:417c1bd45a3c | 243 | } /* LIDARLite_v3HP::getBusyFlag */ |
hubercam | 0:417c1bd45a3c | 244 | |
hubercam | 0:417c1bd45a3c | 245 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 246 | Read Distance |
hubercam | 0:417c1bd45a3c | 247 | |
hubercam | 0:417c1bd45a3c | 248 | Read and return result of distance measurement. |
hubercam | 0:417c1bd45a3c | 249 | |
hubercam | 0:417c1bd45a3c | 250 | Process |
hubercam | 0:417c1bd45a3c | 251 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 252 | 1. Read two bytes from register 0x8f and save |
hubercam | 0:417c1bd45a3c | 253 | 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f. |
hubercam | 0:417c1bd45a3c | 254 | The result is the measured distance in centimeters. |
hubercam | 0:417c1bd45a3c | 255 | |
hubercam | 0:417c1bd45a3c | 256 | Parameters |
hubercam | 0:417c1bd45a3c | 257 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 258 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 259 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 260 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 261 | uint16_t LIDARLite_v3HP::readDistance(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 262 | { |
hubercam | 0:417c1bd45a3c | 263 | uint16_t distance; |
hubercam | 0:417c1bd45a3c | 264 | uint8_t dataBytes[2]; |
hubercam | 0:417c1bd45a3c | 265 | |
hubercam | 0:417c1bd45a3c | 266 | // Read two bytes from register 0x0f and 0x10 (autoincrement) |
hubercam | 0:417c1bd45a3c | 267 | read(0x0f, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 268 | |
hubercam | 0:417c1bd45a3c | 269 | // Shift high byte and add to low byte |
hubercam | 0:417c1bd45a3c | 270 | distance = (dataBytes[0] << 8) | dataBytes[1]; |
hubercam | 0:417c1bd45a3c | 271 | |
hubercam | 0:417c1bd45a3c | 272 | return (distance); |
hubercam | 0:417c1bd45a3c | 273 | } /* LIDARLite_v3HP::readDistance */ |
hubercam | 0:417c1bd45a3c | 274 | |
hubercam | 0:417c1bd45a3c | 275 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 276 | Reset Reference Filter |
hubercam | 0:417c1bd45a3c | 277 | |
hubercam | 0:417c1bd45a3c | 278 | In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in |
hubercam | 0:417c1bd45a3c | 279 | initial measurements that may be a few centimeters short of actual distance. |
hubercam | 0:417c1bd45a3c | 280 | This symptom will eventually rectify itself after a few hundred measurements. |
hubercam | 0:417c1bd45a3c | 281 | This symptom can also be rectified more quickly by resetting the unit's internal |
hubercam | 0:417c1bd45a3c | 282 | reference filter. The process here illustrates how to disable the internal |
hubercam | 0:417c1bd45a3c | 283 | reference filter, trigger a measurement (forcing re-init of the reference |
hubercam | 0:417c1bd45a3c | 284 | filter), and then re-enable the filter. |
hubercam | 0:417c1bd45a3c | 285 | |
hubercam | 0:417c1bd45a3c | 286 | Process |
hubercam | 0:417c1bd45a3c | 287 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 288 | 1. Disable the LIDAR-Lite reference filter |
hubercam | 0:417c1bd45a3c | 289 | 2. Set reference integration count to max |
hubercam | 0:417c1bd45a3c | 290 | 3. Trigger a measurement |
hubercam | 0:417c1bd45a3c | 291 | 4. Restore reference integration count |
hubercam | 0:417c1bd45a3c | 292 | 5. Re-enable reference filter |
hubercam | 0:417c1bd45a3c | 293 | |
hubercam | 0:417c1bd45a3c | 294 | Parameters |
hubercam | 0:417c1bd45a3c | 295 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 296 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 297 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 298 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 299 | void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 300 | { |
hubercam | 0:417c1bd45a3c | 301 | uint8_t dataBytes[2]; |
hubercam | 0:417c1bd45a3c | 302 | uint8_t acqConfigReg; |
hubercam | 0:417c1bd45a3c | 303 | uint8_t refCountMax; |
hubercam | 0:417c1bd45a3c | 304 | |
hubercam | 0:417c1bd45a3c | 305 | // Set bit 4 of the acquisition configuration register (disable reference filter) |
hubercam | 0:417c1bd45a3c | 306 | read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register) |
hubercam | 0:417c1bd45a3c | 307 | acqConfigReg = dataBytes[0]; // store for later restoration |
hubercam | 0:417c1bd45a3c | 308 | dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter |
hubercam | 0:417c1bd45a3c | 309 | write(0x04, dataBytes, 1, lidarliteAddress); // write it back |
hubercam | 0:417c1bd45a3c | 310 | |
hubercam | 0:417c1bd45a3c | 311 | // Set reference integration count to max |
hubercam | 0:417c1bd45a3c | 312 | read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count) |
hubercam | 0:417c1bd45a3c | 313 | refCountMax = dataBytes[0]; // store for later restoration |
hubercam | 0:417c1bd45a3c | 314 | dataBytes[0] = 0xff; // we want to reference to overflow quickly |
hubercam | 0:417c1bd45a3c | 315 | write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count |
hubercam | 0:417c1bd45a3c | 316 | |
hubercam | 0:417c1bd45a3c | 317 | // Trigger a measurement |
hubercam | 0:417c1bd45a3c | 318 | waitForBusy(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 319 | takeRange(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 320 | waitForBusy(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 321 | // ... no need to read the distance, it is immaterial |
hubercam | 0:417c1bd45a3c | 322 | |
hubercam | 0:417c1bd45a3c | 323 | // Restore previous reference integration count |
hubercam | 0:417c1bd45a3c | 324 | dataBytes[0] = refCountMax; |
hubercam | 0:417c1bd45a3c | 325 | write(0x12, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 326 | |
hubercam | 0:417c1bd45a3c | 327 | // Restore previous acquisition configuration register (re-enabling reference filter) |
hubercam | 0:417c1bd45a3c | 328 | dataBytes[0] = acqConfigReg; |
hubercam | 0:417c1bd45a3c | 329 | write(0x04, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 330 | } /* LIDARLite_v3HP::resetReferenceFilter */ |
hubercam | 0:417c1bd45a3c | 331 | |
hubercam | 0:417c1bd45a3c | 332 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 333 | Write |
hubercam | 0:417c1bd45a3c | 334 | |
hubercam | 0:417c1bd45a3c | 335 | Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP |
hubercam | 0:417c1bd45a3c | 336 | will receive multiple bytes in one I2C transmission. The first byte is |
hubercam | 0:417c1bd45a3c | 337 | always the register address. The the bytes that follow will be written |
hubercam | 0:417c1bd45a3c | 338 | into the specified register address first and then the internal address |
hubercam | 0:417c1bd45a3c | 339 | in the Lidar Lite will be auto-incremented for all following bytes. |
hubercam | 0:417c1bd45a3c | 340 | |
hubercam | 0:417c1bd45a3c | 341 | Parameters |
hubercam | 0:417c1bd45a3c | 342 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 343 | regAddr: register address to write to |
hubercam | 0:417c1bd45a3c | 344 | dataBytes: pointer to array of bytes to write |
hubercam | 0:417c1bd45a3c | 345 | numBytes: number of bytes in 'dataBytes' array to write |
hubercam | 0:417c1bd45a3c | 346 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 347 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 348 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 349 | void LIDARLite_v3HP::write (const uint8_t ®Addr, uint8_t * dataBytes, |
hubercam | 0:417c1bd45a3c | 350 | const uint16_t &numBytes, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 351 | { |
hubercam | 0:417c1bd45a3c | 352 | int nackCatcher; |
hubercam | 0:417c1bd45a3c | 353 | |
hubercam | 0:417c1bd45a3c | 354 | // i2c Syntax |
hubercam | 0:417c1bd45a3c | 355 | // ----------------------------------------------------------------- |
hubercam | 0:417c1bd45a3c | 356 | // I2C.write(i2cAddress, *data, length, repeated = false) |
hubercam | 0:417c1bd45a3c | 357 | |
hubercam | 0:417c1bd45a3c | 358 | |
hubercam | 0:417c1bd45a3c | 359 | nackCatcher = i2c_->write(lidarliteAddress<<1, (char *)dataBytes, numBytes, false); |
hubercam | 0:417c1bd45a3c | 360 | |
hubercam | 0:417c1bd45a3c | 361 | // A nack means the device is not responding. Report the error over serial. |
hubercam | 0:417c1bd45a3c | 362 | if (nackCatcher != 0) |
hubercam | 0:417c1bd45a3c | 363 | { |
hubercam | 0:417c1bd45a3c | 364 | printf("> nack\n\r"); |
hubercam | 0:417c1bd45a3c | 365 | } |
hubercam | 0:417c1bd45a3c | 366 | |
hubercam | 0:417c1bd45a3c | 367 | wait_us(100); // 100 us delay for robustness with successive reads and writes |
hubercam | 0:417c1bd45a3c | 368 | } /* LIDARLite_v3HP::write */ |
hubercam | 0:417c1bd45a3c | 369 | |
hubercam | 0:417c1bd45a3c | 370 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 371 | Read |
hubercam | 0:417c1bd45a3c | 372 | |
hubercam | 0:417c1bd45a3c | 373 | Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP |
hubercam | 0:417c1bd45a3c | 374 | will send multiple bytes in one I2C transmission. The register address must |
hubercam | 0:417c1bd45a3c | 375 | be set up by a previous I2C write. The bytes that follow will be read |
hubercam | 0:417c1bd45a3c | 376 | from the specified register address first and then the internal address |
hubercam | 0:417c1bd45a3c | 377 | pointer in the Lidar Lite will be auto-incremented for following bytes. |
hubercam | 0:417c1bd45a3c | 378 | |
hubercam | 0:417c1bd45a3c | 379 | Will detect an unresponsive device and report the error over serial. |
hubercam | 0:417c1bd45a3c | 380 | |
hubercam | 0:417c1bd45a3c | 381 | Parameters |
hubercam | 0:417c1bd45a3c | 382 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 383 | regAddr: register address to write to |
hubercam | 0:417c1bd45a3c | 384 | dataBytes: pointer to array of bytes to write |
hubercam | 0:417c1bd45a3c | 385 | numBytes: number of bytes in 'dataBytes' array to write |
hubercam | 0:417c1bd45a3c | 386 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 387 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 388 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 389 | void LIDARLite_v3HP::read (const uint8_t ®Addr, uint8_t * dataBytes, |
hubercam | 0:417c1bd45a3c | 390 | const uint16_t &numBytes, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 391 | { |
hubercam | 0:417c1bd45a3c | 392 | int nackCatcher = 0; |
hubercam | 0:417c1bd45a3c | 393 | |
hubercam | 0:417c1bd45a3c | 394 | char regAddrC = char(regAddr); |
hubercam | 0:417c1bd45a3c | 395 | |
hubercam | 0:417c1bd45a3c | 396 | // A nack means the device is not responding, report the error over serial |
hubercam | 0:417c1bd45a3c | 397 | nackCatcher = i2c_->write(lidarliteAddress<<1, ®AddrC, 1, false); // false means perform repeated start |
hubercam | 0:417c1bd45a3c | 398 | |
hubercam | 0:417c1bd45a3c | 399 | if (nackCatcher != 0) |
hubercam | 0:417c1bd45a3c | 400 | { |
hubercam | 0:417c1bd45a3c | 401 | printf("> nack\n\r"); |
hubercam | 0:417c1bd45a3c | 402 | } |
hubercam | 0:417c1bd45a3c | 403 | |
hubercam | 0:417c1bd45a3c | 404 | // Perform read, save in dataBytes array |
hubercam | 0:417c1bd45a3c | 405 | i2c_->read(lidarliteAddress<<1, (char *)dataBytes, numBytes); |
hubercam | 0:417c1bd45a3c | 406 | |
hubercam | 0:417c1bd45a3c | 407 | } /* LIDARLite_v3HP::read */ |
hubercam | 0:417c1bd45a3c | 408 | |
hubercam | 0:417c1bd45a3c | 409 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 410 | Correlation Record To Serial |
hubercam | 0:417c1bd45a3c | 411 | |
hubercam | 0:417c1bd45a3c | 412 | The correlation record used to calculate distance can be read from the device. |
hubercam | 0:417c1bd45a3c | 413 | It has a bipolar wave shape, transitioning from a positive going portion to a |
hubercam | 0:417c1bd45a3c | 414 | roughly symmetrical negative going pulse. The point where the signal crosses |
hubercam | 0:417c1bd45a3c | 415 | zero represents the effective delay for the reference and return signals. |
hubercam | 0:417c1bd45a3c | 416 | |
hubercam | 0:417c1bd45a3c | 417 | Process |
hubercam | 0:417c1bd45a3c | 418 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 419 | 1. Take a distance reading (there is no correlation record without at least |
hubercam | 0:417c1bd45a3c | 420 | one distance reading being taken) |
hubercam | 0:417c1bd45a3c | 421 | 2. Set test mode select by writing 0x07 to register 0x40 |
hubercam | 0:417c1bd45a3c | 422 | 3. For as many readings as you want to take (max is 1024) |
hubercam | 0:417c1bd45a3c | 423 | 1. Read two bytes from 0x52 |
hubercam | 0:417c1bd45a3c | 424 | 2. The Low byte is the value from the record |
hubercam | 0:417c1bd45a3c | 425 | 3. The high byte is the sign from the record |
hubercam | 0:417c1bd45a3c | 426 | |
hubercam | 0:417c1bd45a3c | 427 | Parameters |
hubercam | 0:417c1bd45a3c | 428 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 429 | separator: the separator between serial data words |
hubercam | 0:417c1bd45a3c | 430 | numberOfReadings: Default: 1024. Maximum of 1024 |
hubercam | 0:417c1bd45a3c | 431 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 432 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 433 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 434 | void LIDARLite_v3HP::correlationRecordToSerial( |
hubercam | 0:417c1bd45a3c | 435 | const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 436 | { |
hubercam | 0:417c1bd45a3c | 437 | uint16_t i = 0; |
hubercam | 0:417c1bd45a3c | 438 | uint8_t dataBytes[2]; // Array to store read / write data |
hubercam | 0:417c1bd45a3c | 439 | int16_t correlationValue = 0; // Var to store value of correlation record |
hubercam | 0:417c1bd45a3c | 440 | |
hubercam | 0:417c1bd45a3c | 441 | // Test mode enable |
hubercam | 0:417c1bd45a3c | 442 | dataBytes[0] = 0x07; |
hubercam | 0:417c1bd45a3c | 443 | write(0x40, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 444 | |
hubercam | 0:417c1bd45a3c | 445 | for (i=0 ; i<numberOfReadings ; i++) |
hubercam | 0:417c1bd45a3c | 446 | { |
hubercam | 0:417c1bd45a3c | 447 | read(0x52, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 448 | |
hubercam | 0:417c1bd45a3c | 449 | // Low byte is the value of the correlation record |
hubercam | 0:417c1bd45a3c | 450 | correlationValue = (uint16_t) dataBytes[0]; |
hubercam | 0:417c1bd45a3c | 451 | |
hubercam | 0:417c1bd45a3c | 452 | // if upper byte lsb is one, the value is negative |
hubercam | 0:417c1bd45a3c | 453 | // so here we test to artifically sign extend the data |
hubercam | 0:417c1bd45a3c | 454 | if ( (int) dataBytes[1] == 1) |
hubercam | 0:417c1bd45a3c | 455 | { |
hubercam | 0:417c1bd45a3c | 456 | correlationValue |= 0xff00; |
hubercam | 0:417c1bd45a3c | 457 | } |
hubercam | 0:417c1bd45a3c | 458 | printf("%d\n\r", correlationValue); |
hubercam | 0:417c1bd45a3c | 459 | |
hubercam | 0:417c1bd45a3c | 460 | } |
hubercam | 0:417c1bd45a3c | 461 | |
hubercam | 0:417c1bd45a3c | 462 | // test mode disable |
hubercam | 0:417c1bd45a3c | 463 | dataBytes[0] = 0; |
hubercam | 0:417c1bd45a3c | 464 | write(0x40, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 465 | } /* LIDARLite_v3HP::correlationRecordToSerial */ |