update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
LIDARLite_v3HP.cpp
- Committer:
- hubercam
- Date:
- 2019-09-20
- Revision:
- 0:417c1bd45a3c
- Child:
- 1:bd9e4d198947
File content as of revision 0:417c1bd45a3c:
/*------------------------------------------------------------------------------ LIDARLite_v3HP Arduino Library LIDARLite_v3HP.cpp This library provides quick access to the basic functions of LIDAR-Lite via the Nucleo interface. Additionally, it can provide a user of any platform with a template for their own application code. Copyright (c) 2018 Garmin Ltd. or its subsidiaries. Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ------------------------------------------------------------------------------*/ #include <cstdarg> #include <cstdint> #include "LIDARLite_v3HP.h" #include "mbed.h" /*------------------------------------------------------------------------------ Configure Selects one of several preset configurations. Parameters ------------------------------------------------------------------------------ configuration: Default 0. 0: Default mode, balanced performance. 1: Short range, high speed. Uses 0x1d maximum acquisition count. 2: Default range, higher speed short range. Turns on quick termination detection for faster measurements at short range (with decreased accuracy) 3: Maximum range. Uses 0xff maximum acquisition count. 4: High sensitivity detection. Overrides default valid measurement detection algorithm, and uses a threshold value for high sensitivity and noise. 5: Low sensitivity detection. Overrides default valid measurement detection algorithm, and uses a threshold value for low sensitivity and noise. lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c):i2c_(i2c) { addr_ = LIDARLITE_ADDR_DEFAULT; } /* LIDARLite_v3HP::LIDARLite_v3HP */ LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c, uint8_t &addr):i2c_(i2c) { addr_ = addr; } /* LIDARLite_v3HP::LIDARLite_v3HP */ void LIDARLite_v3HP::configure(const uint8_t &configuration, const uint8_t &lidarliteAddress) { uint8_t sigCountMax; uint8_t acqConfigReg; uint8_t refCountMax; uint8_t thresholdBypass; switch (configuration) { case 0: // Default mode, balanced performance sigCountMax = 0x80; // Default acqConfigReg = 0x08; // Default refCountMax = 0x05; // Default thresholdBypass = 0x00; // Default break; case 1: // Short range, high speed sigCountMax = 0x1d; acqConfigReg = 0x08; // Default refCountMax = 0x03; thresholdBypass = 0x00; // Default break; case 2: // Default range, higher speed short range sigCountMax = 0x80; // Default acqConfigReg = 0x00; refCountMax = 0x03; thresholdBypass = 0x00; // Default break; case 3: // Maximum range sigCountMax = 0xff; acqConfigReg = 0x08; // Default refCountMax = 0x05; // Default thresholdBypass = 0x00; // Default break; case 4: // High sensitivity detection, high erroneous measurements sigCountMax = 0x80; // Default acqConfigReg = 0x08; // Default refCountMax = 0x05; // Default thresholdBypass = 0x80; break; case 5: // Low sensitivity detection, low erroneous measurements sigCountMax = 0x80; // Default acqConfigReg = 0x08; // Default refCountMax = 0x05; // Default thresholdBypass = 0xb0; break; case 6: // Short range, high speed, higher error sigCountMax = 0x04; acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode refCountMax = 0x03; thresholdBypass = 0x00; break; } write(0x02, &sigCountMax , 1, lidarliteAddress); write(0x04, &acqConfigReg , 1, lidarliteAddress); write(0x12, &refCountMax , 1, lidarliteAddress); write(0x1c, &thresholdBypass, 1, lidarliteAddress); } /* LIDARLite_v3HP::configure */ /*------------------------------------------------------------------------------ Set I2C Address Set Alternate I2C Device Address. See Operation Manual for additional info. Parameters ------------------------------------------------------------------------------ newAddress: desired secondary I2C device address disableDefault: a non-zero value here means the default 0x62 I2C device address will be disabled. lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::setI2Caddr (const uint8_t &newAddress, uint8_t &disableDefault, const uint8_t &lidarliteAddress) { uint8_t dataBytes[2]; // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations read (0x16, dataBytes, 2, lidarliteAddress); write(0x18, dataBytes, 2, lidarliteAddress); // Write the new I2C device address to registers // left shift by one to work around data alignment issue in v3HP dataBytes[0] = (newAddress << 1); write(0x1a, dataBytes, 1, lidarliteAddress); // Enable the new I2C device address using the default I2C device address read (0x1e, dataBytes, 1, lidarliteAddress); dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address write(0x1e, dataBytes, 1, lidarliteAddress); // If desired, disable default I2C device address (using the new I2C device address) if (disableDefault) { read (0x1e, dataBytes, 1, newAddress); dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address write(0x1e, dataBytes, 1, newAddress); } } /* LIDARLite_v3HP::setI2Caddr */ /*------------------------------------------------------------------------------ Take Range Initiate a distance measurement by writing to register 0x00. Parameters ------------------------------------------------------------------------------ lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::takeRange(const uint8_t &lidarliteAddress) { uint8_t dataByte = 0x01; write(0x00, &dataByte, 1, lidarliteAddress); } /* LIDARLite_v3HP::takeRange */ /*------------------------------------------------------------------------------ Wait for Busy Flag Blocking function to wait until the Lidar Lite's internal busy flag goes low Parameters ------------------------------------------------------------------------------ lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::waitForBusy(const uint8_t &lidarliteAddress) { uint16_t busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout uint8_t busyFlag = 1; // busyFlag monitors when the device is done with a measurement while (busyFlag) // Loop until device is not busy { // Handle timeout condition, exit while loop and goto bailout if (busyCounter > 9999) { break; } busyFlag = getBusyFlag(lidarliteAddress); // Increment busyCounter for timeout busyCounter++; } // bailout reports error over serial if (busyCounter > 9999) { } } /* LIDARLite_v3HP::waitForBusy */ /*------------------------------------------------------------------------------ Get Busy Flag Read BUSY flag from device registers. Function will return 0x00 if not busy. Parameters ------------------------------------------------------------------------------ lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ uint8_t LIDARLite_v3HP::getBusyFlag(const uint8_t &lidarliteAddress) { uint8_t busyFlag; // busyFlag monitors when the device is done with a measurement // Read status register to check busy flag read(0x01, &busyFlag, 1, lidarliteAddress); // STATUS bit 0 is busyFlag busyFlag &= 0x01; return busyFlag; } /* LIDARLite_v3HP::getBusyFlag */ /*------------------------------------------------------------------------------ Read Distance Read and return result of distance measurement. Process ------------------------------------------------------------------------------ 1. Read two bytes from register 0x8f and save 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f. The result is the measured distance in centimeters. Parameters ------------------------------------------------------------------------------ lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ uint16_t LIDARLite_v3HP::readDistance(const uint8_t &lidarliteAddress) { uint16_t distance; uint8_t dataBytes[2]; // Read two bytes from register 0x0f and 0x10 (autoincrement) read(0x0f, dataBytes, 2, lidarliteAddress); // Shift high byte and add to low byte distance = (dataBytes[0] << 8) | dataBytes[1]; return (distance); } /* LIDARLite_v3HP::readDistance */ /*------------------------------------------------------------------------------ Reset Reference Filter In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in initial measurements that may be a few centimeters short of actual distance. This symptom will eventually rectify itself after a few hundred measurements. This symptom can also be rectified more quickly by resetting the unit's internal reference filter. The process here illustrates how to disable the internal reference filter, trigger a measurement (forcing re-init of the reference filter), and then re-enable the filter. Process ------------------------------------------------------------------------------ 1. Disable the LIDAR-Lite reference filter 2. Set reference integration count to max 3. Trigger a measurement 4. Restore reference integration count 5. Re-enable reference filter Parameters ------------------------------------------------------------------------------ lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress) { uint8_t dataBytes[2]; uint8_t acqConfigReg; uint8_t refCountMax; // Set bit 4 of the acquisition configuration register (disable reference filter) read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register) acqConfigReg = dataBytes[0]; // store for later restoration dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter write(0x04, dataBytes, 1, lidarliteAddress); // write it back // Set reference integration count to max read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count) refCountMax = dataBytes[0]; // store for later restoration dataBytes[0] = 0xff; // we want to reference to overflow quickly write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count // Trigger a measurement waitForBusy(lidarliteAddress); takeRange(lidarliteAddress); waitForBusy(lidarliteAddress); // ... no need to read the distance, it is immaterial // Restore previous reference integration count dataBytes[0] = refCountMax; write(0x12, dataBytes, 1, lidarliteAddress); // Restore previous acquisition configuration register (re-enabling reference filter) dataBytes[0] = acqConfigReg; write(0x04, dataBytes, 1, lidarliteAddress); } /* LIDARLite_v3HP::resetReferenceFilter */ /*------------------------------------------------------------------------------ Write Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP will receive multiple bytes in one I2C transmission. The first byte is always the register address. The the bytes that follow will be written into the specified register address first and then the internal address in the Lidar Lite will be auto-incremented for all following bytes. Parameters ------------------------------------------------------------------------------ regAddr: register address to write to dataBytes: pointer to array of bytes to write numBytes: number of bytes in 'dataBytes' array to write lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::write (const uint8_t ®Addr, uint8_t * dataBytes, const uint16_t &numBytes, const uint8_t &lidarliteAddress) { int nackCatcher; // i2c Syntax // ----------------------------------------------------------------- // I2C.write(i2cAddress, *data, length, repeated = false) nackCatcher = i2c_->write(lidarliteAddress<<1, (char *)dataBytes, numBytes, false); // A nack means the device is not responding. Report the error over serial. if (nackCatcher != 0) { printf("> nack\n\r"); } wait_us(100); // 100 us delay for robustness with successive reads and writes } /* LIDARLite_v3HP::write */ /*------------------------------------------------------------------------------ Read Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP will send multiple bytes in one I2C transmission. The register address must be set up by a previous I2C write. The bytes that follow will be read from the specified register address first and then the internal address pointer in the Lidar Lite will be auto-incremented for following bytes. Will detect an unresponsive device and report the error over serial. Parameters ------------------------------------------------------------------------------ regAddr: register address to write to dataBytes: pointer to array of bytes to write numBytes: number of bytes in 'dataBytes' array to write lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::read (const uint8_t ®Addr, uint8_t * dataBytes, const uint16_t &numBytes, const uint8_t &lidarliteAddress) { int nackCatcher = 0; char regAddrC = char(regAddr); // A nack means the device is not responding, report the error over serial nackCatcher = i2c_->write(lidarliteAddress<<1, ®AddrC, 1, false); // false means perform repeated start if (nackCatcher != 0) { printf("> nack\n\r"); } // Perform read, save in dataBytes array i2c_->read(lidarliteAddress<<1, (char *)dataBytes, numBytes); } /* LIDARLite_v3HP::read */ /*------------------------------------------------------------------------------ Correlation Record To Serial The correlation record used to calculate distance can be read from the device. It has a bipolar wave shape, transitioning from a positive going portion to a roughly symmetrical negative going pulse. The point where the signal crosses zero represents the effective delay for the reference and return signals. Process ------------------------------------------------------------------------------ 1. Take a distance reading (there is no correlation record without at least one distance reading being taken) 2. Set test mode select by writing 0x07 to register 0x40 3. For as many readings as you want to take (max is 1024) 1. Read two bytes from 0x52 2. The Low byte is the value from the record 3. The high byte is the sign from the record Parameters ------------------------------------------------------------------------------ separator: the separator between serial data words numberOfReadings: Default: 1024. Maximum of 1024 lidarliteAddress: Default 0x62. Fill in new address here if changed. See operating manual for instructions. ------------------------------------------------------------------------------*/ void LIDARLite_v3HP::correlationRecordToSerial( const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress) { uint16_t i = 0; uint8_t dataBytes[2]; // Array to store read / write data int16_t correlationValue = 0; // Var to store value of correlation record // Test mode enable dataBytes[0] = 0x07; write(0x40, dataBytes, 1, lidarliteAddress); for (i=0 ; i<numberOfReadings ; i++) { read(0x52, dataBytes, 2, lidarliteAddress); // Low byte is the value of the correlation record correlationValue = (uint16_t) dataBytes[0]; // if upper byte lsb is one, the value is negative // so here we test to artifically sign extend the data if ( (int) dataBytes[1] == 1) { correlationValue |= 0xff00; } printf("%d\n\r", correlationValue); } // test mode disable dataBytes[0] = 0; write(0x40, dataBytes, 1, lidarliteAddress); } /* LIDARLite_v3HP::correlationRecordToSerial */