christophe vermaelen / LIDARLite_v3HP_IUT_GEII

update LIDARLite_v3HP just to be functionnal. IUT GEII NICE

Dependents:   TEST_LIDARlitev3

LIDARLite_v3HP.cpp@1:bd9e4d198947, 2022-02-09 (annotated)

Committer:
vermaelen
Date:
Wed Feb 09 14:21:29 2022 +0000
Revision:
1:bd9e4d198947
Parent:
0:417c1bd45a3c
Child:
2:d6e45ac03963
first change of LIDARLitev3

Who changed what in which revision?

UserRevisionLine numberNew 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;
vermaelen 1:bd9e4d198947 264 uint8_t dataByteHigh;
vermaelen 1:bd9e4d198947 265 uint8_t dataByteLow;
hubercam 0:417c1bd45a3c 266
hubercam 0:417c1bd45a3c 267 // Read two bytes from register 0x0f and 0x10 (autoincrement)
vermaelen 1:bd9e4d198947 268 // ca marche pas !!!!
vermaelen 1:bd9e4d198947 269 // read(0x0f, dataBytes, 2, lidarliteAddress);
vermaelen 1:bd9e4d198947 270 read(0x0f, dataByteHigh, 1, lidarliteAddress);
vermaelen 1:bd9e4d198947 271 read(0x0f, dataByteLow, 1, lidarliteAddress);
vermaelen 1:bd9e4d198947 272 distance =(dataByteHigh<<8)+dataByteLow;
hubercam 0:417c1bd45a3c 273 return (distance);
hubercam 0:417c1bd45a3c 274 } /* LIDARLite_v3HP::readDistance */
hubercam 0:417c1bd45a3c 275
hubercam 0:417c1bd45a3c 276 /*------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 277 Reset Reference Filter
hubercam 0:417c1bd45a3c 278
hubercam 0:417c1bd45a3c 279 In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in
hubercam 0:417c1bd45a3c 280 initial measurements that may be a few centimeters short of actual distance.
hubercam 0:417c1bd45a3c 281 This symptom will eventually rectify itself after a few hundred measurements.
hubercam 0:417c1bd45a3c 282 This symptom can also be rectified more quickly by resetting the unit's internal
hubercam 0:417c1bd45a3c 283 reference filter. The process here illustrates how to disable the internal
hubercam 0:417c1bd45a3c 284 reference filter, trigger a measurement (forcing re-init of the reference
hubercam 0:417c1bd45a3c 285 filter), and then re-enable the filter.
hubercam 0:417c1bd45a3c 286
hubercam 0:417c1bd45a3c 287 Process
hubercam 0:417c1bd45a3c 288 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 289 1. Disable the LIDAR-Lite reference filter
hubercam 0:417c1bd45a3c 290 2. Set reference integration count to max
hubercam 0:417c1bd45a3c 291 3. Trigger a measurement
hubercam 0:417c1bd45a3c 292 4. Restore reference integration count
hubercam 0:417c1bd45a3c 293 5. Re-enable reference filter
hubercam 0:417c1bd45a3c 294
hubercam 0:417c1bd45a3c 295 Parameters
hubercam 0:417c1bd45a3c 296 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 297 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
hubercam 0:417c1bd45a3c 298 operating manual for instructions.
hubercam 0:417c1bd45a3c 299 ------------------------------------------------------------------------------*/
hubercam 0:417c1bd45a3c 300 void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress)
hubercam 0:417c1bd45a3c 301 {
hubercam 0:417c1bd45a3c 302 uint8_t dataBytes[2];
hubercam 0:417c1bd45a3c 303 uint8_t acqConfigReg;
hubercam 0:417c1bd45a3c 304 uint8_t refCountMax;
hubercam 0:417c1bd45a3c 305
hubercam 0:417c1bd45a3c 306 // Set bit 4 of the acquisition configuration register (disable reference filter)
hubercam 0:417c1bd45a3c 307 read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register)
hubercam 0:417c1bd45a3c 308 acqConfigReg = dataBytes[0]; // store for later restoration
hubercam 0:417c1bd45a3c 309 dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter
hubercam 0:417c1bd45a3c 310 write(0x04, dataBytes, 1, lidarliteAddress); // write it back
hubercam 0:417c1bd45a3c 311
hubercam 0:417c1bd45a3c 312 // Set reference integration count to max
hubercam 0:417c1bd45a3c 313 read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count)
hubercam 0:417c1bd45a3c 314 refCountMax = dataBytes[0]; // store for later restoration
hubercam 0:417c1bd45a3c 315 dataBytes[0] = 0xff; // we want to reference to overflow quickly
hubercam 0:417c1bd45a3c 316 write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count
hubercam 0:417c1bd45a3c 317
hubercam 0:417c1bd45a3c 318 // Trigger a measurement
hubercam 0:417c1bd45a3c 319 waitForBusy(lidarliteAddress);
hubercam 0:417c1bd45a3c 320 takeRange(lidarliteAddress);
hubercam 0:417c1bd45a3c 321 waitForBusy(lidarliteAddress);
hubercam 0:417c1bd45a3c 322 // ... no need to read the distance, it is immaterial
hubercam 0:417c1bd45a3c 323
hubercam 0:417c1bd45a3c 324 // Restore previous reference integration count
hubercam 0:417c1bd45a3c 325 dataBytes[0] = refCountMax;
hubercam 0:417c1bd45a3c 326 write(0x12, dataBytes, 1, lidarliteAddress);
hubercam 0:417c1bd45a3c 327
hubercam 0:417c1bd45a3c 328 // Restore previous acquisition configuration register (re-enabling reference filter)
hubercam 0:417c1bd45a3c 329 dataBytes[0] = acqConfigReg;
hubercam 0:417c1bd45a3c 330 write(0x04, dataBytes, 1, lidarliteAddress);
hubercam 0:417c1bd45a3c 331 } /* LIDARLite_v3HP::resetReferenceFilter */
hubercam 0:417c1bd45a3c 332
hubercam 0:417c1bd45a3c 333 /*------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 334 Write
hubercam 0:417c1bd45a3c 335
hubercam 0:417c1bd45a3c 336 Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP
hubercam 0:417c1bd45a3c 337 will receive multiple bytes in one I2C transmission. The first byte is
hubercam 0:417c1bd45a3c 338 always the register address. The the bytes that follow will be written
hubercam 0:417c1bd45a3c 339 into the specified register address first and then the internal address
hubercam 0:417c1bd45a3c 340 in the Lidar Lite will be auto-incremented for all following bytes.
hubercam 0:417c1bd45a3c 341
hubercam 0:417c1bd45a3c 342 Parameters
hubercam 0:417c1bd45a3c 343 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 344 regAddr: register address to write to
hubercam 0:417c1bd45a3c 345 dataBytes: pointer to array of bytes to write
hubercam 0:417c1bd45a3c 346 numBytes: number of bytes in 'dataBytes' array to write
hubercam 0:417c1bd45a3c 347 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
hubercam 0:417c1bd45a3c 348 operating manual for instructions.
hubercam 0:417c1bd45a3c 349 ------------------------------------------------------------------------------*/
hubercam 0:417c1bd45a3c 350 void LIDARLite_v3HP::write (const uint8_t &regAddr, uint8_t * dataBytes,
hubercam 0:417c1bd45a3c 351 const uint16_t &numBytes, const uint8_t &lidarliteAddress)
hubercam 0:417c1bd45a3c 352 {
hubercam 0:417c1bd45a3c 353 int nackCatcher;
hubercam 0:417c1bd45a3c 354
hubercam 0:417c1bd45a3c 355 // i2c Syntax
hubercam 0:417c1bd45a3c 356 // -----------------------------------------------------------------
hubercam 0:417c1bd45a3c 357 // I2C.write(i2cAddress, *data, length, repeated = false)
hubercam 0:417c1bd45a3c 358
hubercam 0:417c1bd45a3c 359
hubercam 0:417c1bd45a3c 360 nackCatcher = i2c_->write(lidarliteAddress<<1, (char *)dataBytes, numBytes, false);
hubercam 0:417c1bd45a3c 361
hubercam 0:417c1bd45a3c 362 // A nack means the device is not responding. Report the error over serial.
hubercam 0:417c1bd45a3c 363 if (nackCatcher != 0)
hubercam 0:417c1bd45a3c 364 {
hubercam 0:417c1bd45a3c 365 printf("> nack\n\r");
hubercam 0:417c1bd45a3c 366 }
hubercam 0:417c1bd45a3c 367
hubercam 0:417c1bd45a3c 368 wait_us(100); // 100 us delay for robustness with successive reads and writes
hubercam 0:417c1bd45a3c 369 } /* LIDARLite_v3HP::write */
hubercam 0:417c1bd45a3c 370
hubercam 0:417c1bd45a3c 371 /*------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 372 Read
hubercam 0:417c1bd45a3c 373
hubercam 0:417c1bd45a3c 374 Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP
hubercam 0:417c1bd45a3c 375 will send multiple bytes in one I2C transmission. The register address must
hubercam 0:417c1bd45a3c 376 be set up by a previous I2C write. The bytes that follow will be read
hubercam 0:417c1bd45a3c 377 from the specified register address first and then the internal address
hubercam 0:417c1bd45a3c 378 pointer in the Lidar Lite will be auto-incremented for following bytes.
hubercam 0:417c1bd45a3c 379
hubercam 0:417c1bd45a3c 380 Will detect an unresponsive device and report the error over serial.
hubercam 0:417c1bd45a3c 381
hubercam 0:417c1bd45a3c 382 Parameters
hubercam 0:417c1bd45a3c 383 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 384 regAddr: register address to write to
hubercam 0:417c1bd45a3c 385 dataBytes: pointer to array of bytes to write
hubercam 0:417c1bd45a3c 386 numBytes: number of bytes in 'dataBytes' array to write
hubercam 0:417c1bd45a3c 387 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
hubercam 0:417c1bd45a3c 388 operating manual for instructions.
hubercam 0:417c1bd45a3c 389 ------------------------------------------------------------------------------*/
hubercam 0:417c1bd45a3c 390 void LIDARLite_v3HP::read (const uint8_t &regAddr, uint8_t * dataBytes,
hubercam 0:417c1bd45a3c 391 const uint16_t &numBytes, const uint8_t &lidarliteAddress)
hubercam 0:417c1bd45a3c 392 {
hubercam 0:417c1bd45a3c 393 int nackCatcher = 0;
hubercam 0:417c1bd45a3c 394
hubercam 0:417c1bd45a3c 395 char regAddrC = char(regAddr);
hubercam 0:417c1bd45a3c 396
hubercam 0:417c1bd45a3c 397 // A nack means the device is not responding, report the error over serial
hubercam 0:417c1bd45a3c 398 nackCatcher = i2c_->write(lidarliteAddress<<1, &regAddrC, 1, false); // false means perform repeated start
hubercam 0:417c1bd45a3c 399
hubercam 0:417c1bd45a3c 400 if (nackCatcher != 0)
hubercam 0:417c1bd45a3c 401 {
hubercam 0:417c1bd45a3c 402 printf("> nack\n\r");
hubercam 0:417c1bd45a3c 403 }
hubercam 0:417c1bd45a3c 404
hubercam 0:417c1bd45a3c 405 // Perform read, save in dataBytes array
hubercam 0:417c1bd45a3c 406 i2c_->read(lidarliteAddress<<1, (char *)dataBytes, numBytes);
hubercam 0:417c1bd45a3c 407
hubercam 0:417c1bd45a3c 408 } /* LIDARLite_v3HP::read */
hubercam 0:417c1bd45a3c 409
hubercam 0:417c1bd45a3c 410 /*------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 411 Correlation Record To Serial
hubercam 0:417c1bd45a3c 412
hubercam 0:417c1bd45a3c 413 The correlation record used to calculate distance can be read from the device.
hubercam 0:417c1bd45a3c 414 It has a bipolar wave shape, transitioning from a positive going portion to a
hubercam 0:417c1bd45a3c 415 roughly symmetrical negative going pulse. The point where the signal crosses
hubercam 0:417c1bd45a3c 416 zero represents the effective delay for the reference and return signals.
hubercam 0:417c1bd45a3c 417
hubercam 0:417c1bd45a3c 418 Process
hubercam 0:417c1bd45a3c 419 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 420 1. Take a distance reading (there is no correlation record without at least
hubercam 0:417c1bd45a3c 421 one distance reading being taken)
hubercam 0:417c1bd45a3c 422 2. Set test mode select by writing 0x07 to register 0x40
hubercam 0:417c1bd45a3c 423 3. For as many readings as you want to take (max is 1024)
hubercam 0:417c1bd45a3c 424 1. Read two bytes from 0x52
hubercam 0:417c1bd45a3c 425 2. The Low byte is the value from the record
hubercam 0:417c1bd45a3c 426 3. The high byte is the sign from the record
hubercam 0:417c1bd45a3c 427
hubercam 0:417c1bd45a3c 428 Parameters
hubercam 0:417c1bd45a3c 429 ------------------------------------------------------------------------------
hubercam 0:417c1bd45a3c 430 separator: the separator between serial data words
hubercam 0:417c1bd45a3c 431 numberOfReadings: Default: 1024. Maximum of 1024
hubercam 0:417c1bd45a3c 432 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
hubercam 0:417c1bd45a3c 433 operating manual for instructions.
hubercam 0:417c1bd45a3c 434 ------------------------------------------------------------------------------*/
hubercam 0:417c1bd45a3c 435 void LIDARLite_v3HP::correlationRecordToSerial(
hubercam 0:417c1bd45a3c 436 const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress)
hubercam 0:417c1bd45a3c 437 {
hubercam 0:417c1bd45a3c 438 uint16_t i = 0;
hubercam 0:417c1bd45a3c 439 uint8_t dataBytes[2]; // Array to store read / write data
hubercam 0:417c1bd45a3c 440 int16_t correlationValue = 0; // Var to store value of correlation record
hubercam 0:417c1bd45a3c 441
hubercam 0:417c1bd45a3c 442 // Test mode enable
hubercam 0:417c1bd45a3c 443 dataBytes[0] = 0x07;
hubercam 0:417c1bd45a3c 444 write(0x40, dataBytes, 1, lidarliteAddress);
hubercam 0:417c1bd45a3c 445
hubercam 0:417c1bd45a3c 446 for (i=0 ; i<numberOfReadings ; i++)
hubercam 0:417c1bd45a3c 447 {
hubercam 0:417c1bd45a3c 448 read(0x52, dataBytes, 2, lidarliteAddress);
hubercam 0:417c1bd45a3c 449
hubercam 0:417c1bd45a3c 450 // Low byte is the value of the correlation record
hubercam 0:417c1bd45a3c 451 correlationValue = (uint16_t) dataBytes[0];
hubercam 0:417c1bd45a3c 452
hubercam 0:417c1bd45a3c 453 // if upper byte lsb is one, the value is negative
hubercam 0:417c1bd45a3c 454 // so here we test to artifically sign extend the data
hubercam 0:417c1bd45a3c 455 if ( (int) dataBytes[1] == 1)
hubercam 0:417c1bd45a3c 456 {
hubercam 0:417c1bd45a3c 457 correlationValue |= 0xff00;
hubercam 0:417c1bd45a3c 458 }
hubercam 0:417c1bd45a3c 459 printf("%d\n\r", correlationValue);
hubercam 0:417c1bd45a3c 460
hubercam 0:417c1bd45a3c 461 }
hubercam 0:417c1bd45a3c 462
hubercam 0:417c1bd45a3c 463 // test mode disable
hubercam 0:417c1bd45a3c 464 dataBytes[0] = 0;
hubercam 0:417c1bd45a3c 465 write(0x40, dataBytes, 1, lidarliteAddress);
hubercam 0:417c1bd45a3c 466 } /* LIDARLite_v3HP::correlationRecordToSerial */