christophe vermaelen
update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
Embed:
(wiki syntax)
Show/hide line numbers
LIDARLite_v3HP.cpp
00001 /*------------------------------------------------------------------------------ 00002 00003 LIDARLite_v3HP Arduino Library 00004 LIDARLite_v3HP.cpp 00005 00006 This library provides quick access to the basic functions of LIDAR-Lite 00007 via the Nucleo interface. Additionally, it can provide a user of any 00008 platform with a template for their own application code. 00009 00010 Copyright (c) 2018 Garmin Ltd. or its subsidiaries. 00011 00012 Licensed under the Apache License, Version 2.0 (the "License"); 00013 you may not use this file except in compliance with the License. 00014 You may obtain a copy of the License at 00015 00016 http://www.apache.org/licenses/LICENSE-2.0 00017 00018 Unless required by applicable law or agreed to in writing, software 00019 distributed under the License is distributed on an "AS IS" BASIS, 00020 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00021 See the License for the specific language governing permissions and 00022 limitations under the License. 00023 00024 ------------------------------------------------------------------------------*/ 00025 00026 #include <cstdarg> 00027 #include <cstdint> 00028 #include "LIDARLite_v3HP.h" 00029 #include "mbed.h" 00030 00031 /*------------------------------------------------------------------------------ 00032 Configure 00033 00034 Selects one of several preset configurations. 00035 00036 Parameters 00037 ------------------------------------------------------------------------------ 00038 configuration: Default 0. 00039 0: Default mode, balanced performance. 00040 1: Short range, high speed. Uses 0x1d maximum acquisition count. 00041 2: Default range, higher speed short range. Turns on quick termination 00042 detection for faster measurements at short range (with decreased 00043 accuracy) 00044 3: Maximum range. Uses 0xff maximum acquisition count. 00045 4: High sensitivity detection. Overrides default valid measurement detection 00046 algorithm, and uses a threshold value for high sensitivity and noise. 00047 5: Low sensitivity detection. Overrides default valid measurement detection 00048 algorithm, and uses a threshold value for low sensitivity and noise. 00049 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00050 operating manual for instructions. 00051 ------------------------------------------------------------------------------*/ 00052 LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c):i2c_(i2c) 00053 { 00054 addr_ = LIDARLITE_ADDR_DEFAULT; 00055 00056 } /* LIDARLite_v3HP::LIDARLite_v3HP */ 00057 00058 LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c, uint8_t &addr):i2c_(i2c) 00059 { 00060 addr_ = addr; 00061 00062 } /* LIDARLite_v3HP::LIDARLite_v3HP */ 00063 00064 void LIDARLite_v3HP::configure(const uint8_t &configuration, const uint8_t &lidarliteAddress) 00065 { 00066 uint8_t sigCountMax; 00067 uint8_t acqConfigReg; 00068 uint8_t refCountMax; 00069 uint8_t thresholdBypass; 00070 00071 switch (configuration) { 00072 case 0: // Default mode, balanced performance 00073 sigCountMax = 0x80; // Default 00074 acqConfigReg = 0x08; // Default 00075 refCountMax = 0x05; // Default 00076 thresholdBypass = 0x00; // Default 00077 break; 00078 00079 case 1: // Short range, high speed 00080 sigCountMax = 0x1d; 00081 acqConfigReg = 0x08; // Default 00082 refCountMax = 0x03; 00083 thresholdBypass = 0x00; // Default 00084 break; 00085 00086 case 2: // Default range, higher speed short range 00087 sigCountMax = 0x80; // Default 00088 acqConfigReg = 0x00; 00089 refCountMax = 0x03; 00090 thresholdBypass = 0x00; // Default 00091 break; 00092 00093 case 3: // Maximum range 00094 sigCountMax = 0xff; 00095 acqConfigReg = 0x08; // Default 00096 refCountMax = 0x05; // Default 00097 thresholdBypass = 0x00; // Default 00098 break; 00099 00100 case 4: // High sensitivity detection, high erroneous measurements 00101 sigCountMax = 0x80; // Default 00102 acqConfigReg = 0x08; // Default 00103 refCountMax = 0x05; // Default 00104 thresholdBypass = 0x80; 00105 break; 00106 00107 case 5: // Low sensitivity detection, low erroneous measurements 00108 sigCountMax = 0x80; // Default 00109 acqConfigReg = 0x08; // Default 00110 refCountMax = 0x05; // Default 00111 thresholdBypass = 0xb0; 00112 break; 00113 00114 case 6: // Short range, high speed, higher error 00115 sigCountMax = 0x04; 00116 acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode 00117 refCountMax = 0x03; 00118 thresholdBypass = 0x00; 00119 break; 00120 } 00121 00122 write(0x02, &sigCountMax, 1, lidarliteAddress); 00123 write(0x04, &acqConfigReg, 1, lidarliteAddress); 00124 write(0x12, &refCountMax, 1, lidarliteAddress); 00125 write(0x1c, &thresholdBypass, 1, lidarliteAddress); 00126 } /* LIDARLite_v3HP::configure */ 00127 00128 /*------------------------------------------------------------------------------ 00129 Set I2C Address 00130 00131 Set Alternate I2C Device Address. See Operation Manual for additional info. 00132 00133 Parameters 00134 ------------------------------------------------------------------------------ 00135 newAddress: desired secondary I2C device address 00136 disableDefault: a non-zero value here means the default 0x62 I2C device 00137 address will be disabled. 00138 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00139 operating manual for instructions. 00140 ------------------------------------------------------------------------------*/ 00141 void LIDARLite_v3HP::setI2Caddr (const uint8_t &newAddress, uint8_t &disableDefault, 00142 const uint8_t &lidarliteAddress) 00143 { 00144 uint8_t dataBytes[2]; 00145 00146 // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations 00147 read (0x16, dataBytes, 2, lidarliteAddress); 00148 write(0x18, dataBytes, 2, lidarliteAddress); 00149 00150 // Write the new I2C device address to registers 00151 // left shift by one to work around data alignment issue in v3HP 00152 dataBytes[0] = (newAddress << 1); 00153 write(0x1a, dataBytes, 1, lidarliteAddress); 00154 00155 // Enable the new I2C device address using the default I2C device address 00156 read (0x1e, dataBytes, 1, lidarliteAddress); 00157 dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address 00158 write(0x1e, dataBytes, 1, lidarliteAddress); 00159 00160 // If desired, disable default I2C device address (using the new I2C device address) 00161 if (disableDefault) { 00162 read (0x1e, dataBytes, 1, newAddress); 00163 dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address 00164 write(0x1e, dataBytes, 1, newAddress); 00165 } 00166 } /* LIDARLite_v3HP::setI2Caddr */ 00167 00168 /*------------------------------------------------------------------------------ 00169 Take Range 00170 00171 Initiate a distance measurement by writing to register 0x00. 00172 00173 Parameters 00174 ------------------------------------------------------------------------------ 00175 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00176 operating manual for instructions. 00177 ------------------------------------------------------------------------------*/ 00178 void LIDARLite_v3HP::takeRange(const uint8_t &lidarliteAddress) 00179 { 00180 // uint8_t dataByte= 0x04; 00181 char start[2]={0,4}; 00182 i2c_->write((lidarliteAddress<<1), start, 2); 00183 // write(0x00, &dataByte, 1, lidarliteAddress); 00184 // i2c.write(0xC4,start,2); //on lance la mesure 00185 } /* LIDARLite_v3HP::takeRange */ 00186 00187 /*------------------------------------------------------------------------------ 00188 Wait for Busy Flag 00189 00190 Blocking function to wait until the Lidar Lite's internal busy flag goes low 00191 00192 Parameters 00193 ------------------------------------------------------------------------------ 00194 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00195 operating manual for instructions. 00196 ------------------------------------------------------------------------------*/ 00197 void LIDARLite_v3HP::waitForBusy(const uint8_t &lidarliteAddress) 00198 { 00199 uint16_t busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout 00200 char busyFlag ; // busyFlag monitors when the device is done with a measurement 00201 char status=0x01; //status register 00202 00203 /* i2c_->write((lidarliteAddress<<1), &status, 1);//choix de l'adresse à lire... 00204 00205 // Loop until device is not busy 00206 do{ 00207 // Handle timeout condition, exit while loop and goto bailout 00208 if (busyCounter > 99) 00209 { 00210 break; 00211 } 00212 00213 busyFlag = getBusyFlag(lidarliteAddress); 00214 printf("."); 00215 00216 // Increment busyCounter for timeout 00217 busyCounter++; 00218 }while (busyFlag!=0) ;*/ 00219 i2c_->write((lidarliteAddress<<1), &status, 1); 00220 do { 00221 i2c_->read((lidarliteAddress<<1), &busyFlag, 1); 00222 busyCounter++; 00223 if (busyCounter > 99) { 00224 break; 00225 } 00226 putchar('.'); // pour voir le nombre de wait 00227 } while((busyFlag&1)!=0); 00228 00229 } /* LIDARLite_v3HP::waitForBusy */ 00230 00231 /*------------------------------------------------------------------------------ 00232 Get Busy Flag 00233 00234 Read BUSY flag from device registers. Function will return 0x00 if not busy. 00235 00236 Parameters 00237 ------------------------------------------------------------------------------ 00238 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00239 operating manual for instructions. 00240 ------------------------------------------------------------------------------*/ 00241 uint8_t LIDARLite_v3HP::getBusyFlag(const uint8_t &lidarliteAddress) 00242 { 00243 char busyFlag; // busyFlag monitors when the device is done with a measurement 00244 00245 // Read status register to check busy flag 00246 //read(0x01, &busyFlag, 1, lidarliteAddress); 00247 i2c_->read((lidarliteAddress<<1), &busyFlag, 1); 00248 00249 // STATUS bit 0 is busyFlag 00250 return (busyFlag&0x01); 00251 } /* LIDARLite_v3HP::getBusyFlag */ 00252 00253 /*------------------------------------------------------------------------------ 00254 Read Distance 00255 00256 Read and return result of distance measurement. 00257 00258 Process 00259 ------------------------------------------------------------------------------ 00260 1. Read two bytes from register 0x8f and save 00261 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f. 00262 The result is the measured distance in centimeters. 00263 00264 Parameters 00265 ------------------------------------------------------------------------------ 00266 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00267 operating manual for instructions. 00268 ------------------------------------------------------------------------------*/ 00269 uint16_t LIDARLite_v3HP::readDistance(const uint8_t &lidarliteAddress) 00270 { 00271 uint16_t distance; 00272 uint8_t dataByteHigh; 00273 uint8_t dataByteLow; 00274 00275 // Read two bytes from register 0x0f and 0x10 (autoincrement) 00276 // ca marche pas !!!! 00277 // read(0x0f, dataBytes, 2, lidarliteAddress); 00278 read(0x0f, &dataByteHigh, 1, lidarliteAddress); 00279 read(0x10, &dataByteLow, 1, lidarliteAddress); 00280 distance =(dataByteHigh<<8)+dataByteLow; 00281 return (distance); 00282 } /* LIDARLite_v3HP::readDistance */ 00283 00284 /*------------------------------------------------------------------------------ 00285 Reset Reference Filter 00286 00287 In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in 00288 initial measurements that may be a few centimeters short of actual distance. 00289 This symptom will eventually rectify itself after a few hundred measurements. 00290 This symptom can also be rectified more quickly by resetting the unit's internal 00291 reference filter. The process here illustrates how to disable the internal 00292 reference filter, trigger a measurement (forcing re-init of the reference 00293 filter), and then re-enable the filter. 00294 00295 Process 00296 ------------------------------------------------------------------------------ 00297 1. Disable the LIDAR-Lite reference filter 00298 2. Set reference integration count to max 00299 3. Trigger a measurement 00300 4. Restore reference integration count 00301 5. Re-enable reference filter 00302 00303 Parameters 00304 ------------------------------------------------------------------------------ 00305 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00306 operating manual for instructions. 00307 ------------------------------------------------------------------------------*/ 00308 void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress) 00309 { 00310 uint8_t dataBytes[2]; 00311 uint8_t acqConfigReg; 00312 uint8_t refCountMax; 00313 00314 // Set bit 4 of the acquisition configuration register (disable reference filter) 00315 read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register) 00316 acqConfigReg = dataBytes[0]; // store for later restoration 00317 dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter 00318 write(0x04, dataBytes, 1, lidarliteAddress); // write it back 00319 00320 // Set reference integration count to max 00321 read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count) 00322 refCountMax = dataBytes[0]; // store for later restoration 00323 dataBytes[0] = 0xff; // we want to reference to overflow quickly 00324 write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count 00325 00326 // Trigger a measurement 00327 waitForBusy(lidarliteAddress); 00328 takeRange(lidarliteAddress); 00329 waitForBusy(lidarliteAddress); 00330 // ... no need to read the distance, it is immaterial 00331 00332 // Restore previous reference integration count 00333 dataBytes[0] = refCountMax; 00334 write(0x12, dataBytes, 1, lidarliteAddress); 00335 00336 // Restore previous acquisition configuration register (re-enabling reference filter) 00337 dataBytes[0] = acqConfigReg; 00338 write(0x04, dataBytes, 1, lidarliteAddress); 00339 } /* LIDARLite_v3HP::resetReferenceFilter */ 00340 00341 /*------------------------------------------------------------------------------ 00342 Write 00343 00344 Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP 00345 will receive multiple bytes in one I2C transmission. The first byte is 00346 always the register address. The the bytes that follow will be written 00347 into the specified register address first and then the internal address 00348 in the Lidar Lite will be auto-incremented for all following bytes. 00349 00350 Parameters 00351 ------------------------------------------------------------------------------ 00352 regAddr: register address to write to 00353 dataBytes: pointer to array of bytes to write 00354 numBytes: number of bytes in 'dataBytes' array to write 00355 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00356 operating manual for instructions. 00357 ------------------------------------------------------------------------------*/ 00358 void LIDARLite_v3HP::write (const uint8_t ®Addr, uint8_t * dataBytes, 00359 const uint16_t &numBytes, const uint8_t &lidarliteAddress) 00360 { 00361 int nackCatcher; 00362 00363 // i2c Syntax 00364 // ----------------------------------------------------------------- 00365 // I2C.write(i2cAddress, *data, length, repeated = false) 00366 00367 00368 nackCatcher = i2c_->write((lidarliteAddress<<1), (char *)dataBytes, numBytes, false); 00369 00370 // A nack means the device is not responding. Report the error over serial. 00371 if (nackCatcher != 0) { 00372 printf("> nack\n\r"); 00373 } 00374 00375 wait_us(100); // 100 us delay for robustness with successive reads and writes 00376 } /* LIDARLite_v3HP::write */ 00377 00378 /*------------------------------------------------------------------------------ 00379 Read 00380 00381 Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP 00382 will send multiple bytes in one I2C transmission. The register address must 00383 be set up by a previous I2C write. The bytes that follow will be read 00384 from the specified register address first and then the internal address 00385 pointer in the Lidar Lite will be auto-incremented for following bytes. 00386 00387 Will detect an unresponsive device and report the error over serial. 00388 00389 Parameters 00390 ------------------------------------------------------------------------------ 00391 regAddr: register address to write to 00392 dataBytes: pointer to array of bytes to write 00393 numBytes: number of bytes in 'dataBytes' array to write 00394 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00395 operating manual for instructions. 00396 ------------------------------------------------------------------------------*/ 00397 void LIDARLite_v3HP::read (const uint8_t ®Addr, uint8_t * dataBytes, 00398 const uint16_t &numBytes, const uint8_t &lidarliteAddress) 00399 { 00400 int nackCatcher = 0; 00401 00402 char regAddrC = char(regAddr); 00403 00404 // A nack means the device is not responding, report the error over serial 00405 nackCatcher = i2c_->write((lidarliteAddress<<1), ®AddrC, 1, false); // false means perform repeated start 00406 00407 if (nackCatcher != 0) { 00408 printf("> nack\n\r"); 00409 } 00410 00411 // Perform read, save in dataBytes array 00412 i2c_->read((lidarliteAddress<<1), (char *)dataBytes, numBytes); 00413 00414 } /* LIDARLite_v3HP::read */ 00415 00416 /*------------------------------------------------------------------------------ 00417 Correlation Record To Serial 00418 00419 The correlation record used to calculate distance can be read from the device. 00420 It has a bipolar wave shape, transitioning from a positive going portion to a 00421 roughly symmetrical negative going pulse. The point where the signal crosses 00422 zero represents the effective delay for the reference and return signals. 00423 00424 Process 00425 ------------------------------------------------------------------------------ 00426 1. Take a distance reading (there is no correlation record without at least 00427 one distance reading being taken) 00428 2. Set test mode select by writing 0x07 to register 0x40 00429 3. For as many readings as you want to take (max is 1024) 00430 1. Read two bytes from 0x52 00431 2. The Low byte is the value from the record 00432 3. The high byte is the sign from the record 00433 00434 Parameters 00435 ------------------------------------------------------------------------------ 00436 separator: the separator between serial data words 00437 numberOfReadings: Default: 1024. Maximum of 1024 00438 lidarliteAddress: Default 0x62. Fill in new address here if changed. See 00439 operating manual for instructions. 00440 ------------------------------------------------------------------------------*/ 00441 void LIDARLite_v3HP::correlationRecordToSerial( 00442 const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress) 00443 { 00444 uint16_t i = 0; 00445 uint8_t dataBytes[2]; // Array to store read / write data 00446 int16_t correlationValue = 0; // Var to store value of correlation record 00447 00448 // Test mode enable 00449 dataBytes[0] = 0x07; 00450 write(0x40, dataBytes, 1, lidarliteAddress); 00451 00452 for (i=0 ; i<numberOfReadings ; i++) { 00453 read(0x52, dataBytes, 2, lidarliteAddress); 00454 00455 // Low byte is the value of the correlation record 00456 correlationValue = (uint16_t) dataBytes[0]; 00457 00458 // if upper byte lsb is one, the value is negative 00459 // so here we test to artifically sign extend the data 00460 if ( (int) dataBytes[1] == 1) { 00461 correlationValue |= 0xff00; 00462 } 00463 printf("%d\n\r", correlationValue); 00464 00465 } 00466 00467 // test mode disable 00468 dataBytes[0] = 0; 00469 write(0x40, dataBytes, 1, lidarliteAddress); 00470 } /* LIDARLite_v3HP::correlationRecordToSerial */
Generated on Sun Jul 24 2022 00:03:47 by
1.7.2