update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
Diff: LIDARLite_v3HP.cpp
- Revision:
- 0:417c1bd45a3c
- Child:
- 1:bd9e4d198947
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LIDARLite_v3HP.cpp Fri Sep 20 14:15:12 2019 +0000 @@ -0,0 +1,465 @@ +/*------------------------------------------------------------------------------ + + 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 */