update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
LIDARLite_v3HP.cpp@2:d6e45ac03963, 2022-02-09 (annotated)
- Committer:
- vermaelen
- Date:
- Wed Feb 09 15:13:07 2022 +0000
- Revision:
- 2:d6e45ac03963
- Parent:
- 1:bd9e4d198947
plein de changement, ca marche
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 | { |
vermaelen | 2:d6e45ac03963 | 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) |
vermaelen | 2:d6e45ac03963 | 59 | { |
hubercam | 0:417c1bd45a3c | 60 | addr_ = addr; |
vermaelen | 2:d6e45ac03963 | 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 | |
vermaelen | 2:d6e45ac03963 | 71 | switch (configuration) { |
hubercam | 0:417c1bd45a3c | 72 | case 0: // Default mode, balanced performance |
hubercam | 0:417c1bd45a3c | 73 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 74 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 75 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 76 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 77 | break; |
hubercam | 0:417c1bd45a3c | 78 | |
hubercam | 0:417c1bd45a3c | 79 | case 1: // Short range, high speed |
hubercam | 0:417c1bd45a3c | 80 | sigCountMax = 0x1d; |
hubercam | 0:417c1bd45a3c | 81 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 82 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 83 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 84 | break; |
hubercam | 0:417c1bd45a3c | 85 | |
hubercam | 0:417c1bd45a3c | 86 | case 2: // Default range, higher speed short range |
hubercam | 0:417c1bd45a3c | 87 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 88 | acqConfigReg = 0x00; |
hubercam | 0:417c1bd45a3c | 89 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 90 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 91 | break; |
hubercam | 0:417c1bd45a3c | 92 | |
hubercam | 0:417c1bd45a3c | 93 | case 3: // Maximum range |
hubercam | 0:417c1bd45a3c | 94 | sigCountMax = 0xff; |
hubercam | 0:417c1bd45a3c | 95 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 96 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 97 | thresholdBypass = 0x00; // Default |
hubercam | 0:417c1bd45a3c | 98 | break; |
hubercam | 0:417c1bd45a3c | 99 | |
hubercam | 0:417c1bd45a3c | 100 | case 4: // High sensitivity detection, high erroneous measurements |
hubercam | 0:417c1bd45a3c | 101 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 102 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 103 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 104 | thresholdBypass = 0x80; |
hubercam | 0:417c1bd45a3c | 105 | break; |
hubercam | 0:417c1bd45a3c | 106 | |
hubercam | 0:417c1bd45a3c | 107 | case 5: // Low sensitivity detection, low erroneous measurements |
hubercam | 0:417c1bd45a3c | 108 | sigCountMax = 0x80; // Default |
hubercam | 0:417c1bd45a3c | 109 | acqConfigReg = 0x08; // Default |
hubercam | 0:417c1bd45a3c | 110 | refCountMax = 0x05; // Default |
hubercam | 0:417c1bd45a3c | 111 | thresholdBypass = 0xb0; |
hubercam | 0:417c1bd45a3c | 112 | break; |
hubercam | 0:417c1bd45a3c | 113 | |
hubercam | 0:417c1bd45a3c | 114 | case 6: // Short range, high speed, higher error |
hubercam | 0:417c1bd45a3c | 115 | sigCountMax = 0x04; |
hubercam | 0:417c1bd45a3c | 116 | acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode |
hubercam | 0:417c1bd45a3c | 117 | refCountMax = 0x03; |
hubercam | 0:417c1bd45a3c | 118 | thresholdBypass = 0x00; |
hubercam | 0:417c1bd45a3c | 119 | break; |
hubercam | 0:417c1bd45a3c | 120 | } |
hubercam | 0:417c1bd45a3c | 121 | |
vermaelen | 2:d6e45ac03963 | 122 | write(0x02, &sigCountMax, 1, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 123 | write(0x04, &acqConfigReg, 1, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 124 | write(0x12, &refCountMax, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 125 | write(0x1c, &thresholdBypass, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 126 | } /* LIDARLite_v3HP::configure */ |
hubercam | 0:417c1bd45a3c | 127 | |
hubercam | 0:417c1bd45a3c | 128 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 129 | Set I2C Address |
hubercam | 0:417c1bd45a3c | 130 | |
hubercam | 0:417c1bd45a3c | 131 | Set Alternate I2C Device Address. See Operation Manual for additional info. |
hubercam | 0:417c1bd45a3c | 132 | |
hubercam | 0:417c1bd45a3c | 133 | Parameters |
hubercam | 0:417c1bd45a3c | 134 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 135 | newAddress: desired secondary I2C device address |
hubercam | 0:417c1bd45a3c | 136 | disableDefault: a non-zero value here means the default 0x62 I2C device |
hubercam | 0:417c1bd45a3c | 137 | address will be disabled. |
hubercam | 0:417c1bd45a3c | 138 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 139 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 140 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 141 | void LIDARLite_v3HP::setI2Caddr (const uint8_t &newAddress, uint8_t &disableDefault, |
hubercam | 0:417c1bd45a3c | 142 | const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 143 | { |
hubercam | 0:417c1bd45a3c | 144 | uint8_t dataBytes[2]; |
hubercam | 0:417c1bd45a3c | 145 | |
hubercam | 0:417c1bd45a3c | 146 | // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations |
hubercam | 0:417c1bd45a3c | 147 | read (0x16, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 148 | write(0x18, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 149 | |
hubercam | 0:417c1bd45a3c | 150 | // Write the new I2C device address to registers |
hubercam | 0:417c1bd45a3c | 151 | // left shift by one to work around data alignment issue in v3HP |
hubercam | 0:417c1bd45a3c | 152 | dataBytes[0] = (newAddress << 1); |
hubercam | 0:417c1bd45a3c | 153 | write(0x1a, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 154 | |
hubercam | 0:417c1bd45a3c | 155 | // Enable the new I2C device address using the default I2C device address |
hubercam | 0:417c1bd45a3c | 156 | read (0x1e, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 157 | dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address |
hubercam | 0:417c1bd45a3c | 158 | write(0x1e, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 159 | |
hubercam | 0:417c1bd45a3c | 160 | // If desired, disable default I2C device address (using the new I2C device address) |
vermaelen | 2:d6e45ac03963 | 161 | if (disableDefault) { |
hubercam | 0:417c1bd45a3c | 162 | read (0x1e, dataBytes, 1, newAddress); |
hubercam | 0:417c1bd45a3c | 163 | dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address |
hubercam | 0:417c1bd45a3c | 164 | write(0x1e, dataBytes, 1, newAddress); |
hubercam | 0:417c1bd45a3c | 165 | } |
hubercam | 0:417c1bd45a3c | 166 | } /* LIDARLite_v3HP::setI2Caddr */ |
hubercam | 0:417c1bd45a3c | 167 | |
hubercam | 0:417c1bd45a3c | 168 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 169 | Take Range |
hubercam | 0:417c1bd45a3c | 170 | |
hubercam | 0:417c1bd45a3c | 171 | Initiate a distance measurement by writing to register 0x00. |
hubercam | 0:417c1bd45a3c | 172 | |
hubercam | 0:417c1bd45a3c | 173 | Parameters |
hubercam | 0:417c1bd45a3c | 174 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 175 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 176 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 177 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 178 | void LIDARLite_v3HP::takeRange(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 179 | { |
vermaelen | 2:d6e45ac03963 | 180 | // uint8_t dataByte= 0x04; |
vermaelen | 2:d6e45ac03963 | 181 | char start[2]={0,4}; |
vermaelen | 2:d6e45ac03963 | 182 | i2c_->write((lidarliteAddress<<1), start, 2); |
vermaelen | 2:d6e45ac03963 | 183 | // write(0x00, &dataByte, 1, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 184 | // i2c.write(0xC4,start,2); //on lance la mesure |
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 |
vermaelen | 2:d6e45ac03963 | 200 | char busyFlag ; // busyFlag monitors when the device is done with a measurement |
vermaelen | 2:d6e45ac03963 | 201 | char status=0x01; //status register |
vermaelen | 2:d6e45ac03963 | 202 | |
vermaelen | 2:d6e45ac03963 | 203 | /* i2c_->write((lidarliteAddress<<1), &status, 1);//choix de l'adresse à lire... |
hubercam | 0:417c1bd45a3c | 204 | |
vermaelen | 2:d6e45ac03963 | 205 | // Loop until device is not busy |
vermaelen | 2:d6e45ac03963 | 206 | do{ |
vermaelen | 2:d6e45ac03963 | 207 | // Handle timeout condition, exit while loop and goto bailout |
vermaelen | 2:d6e45ac03963 | 208 | if (busyCounter > 99) |
vermaelen | 2:d6e45ac03963 | 209 | { |
vermaelen | 2:d6e45ac03963 | 210 | break; |
vermaelen | 2:d6e45ac03963 | 211 | } |
vermaelen | 2:d6e45ac03963 | 212 | |
vermaelen | 2:d6e45ac03963 | 213 | busyFlag = getBusyFlag(lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 214 | printf("."); |
vermaelen | 2:d6e45ac03963 | 215 | |
vermaelen | 2:d6e45ac03963 | 216 | // Increment busyCounter for timeout |
vermaelen | 2:d6e45ac03963 | 217 | busyCounter++; |
vermaelen | 2:d6e45ac03963 | 218 | }while (busyFlag!=0) ;*/ |
vermaelen | 2:d6e45ac03963 | 219 | i2c_->write((lidarliteAddress<<1), &status, 1); |
vermaelen | 2:d6e45ac03963 | 220 | do { |
vermaelen | 2:d6e45ac03963 | 221 | i2c_->read((lidarliteAddress<<1), &busyFlag, 1); |
vermaelen | 2:d6e45ac03963 | 222 | busyCounter++; |
vermaelen | 2:d6e45ac03963 | 223 | if (busyCounter > 99) { |
hubercam | 0:417c1bd45a3c | 224 | break; |
hubercam | 0:417c1bd45a3c | 225 | } |
vermaelen | 2:d6e45ac03963 | 226 | putchar('.'); // pour voir le nombre de wait |
vermaelen | 2:d6e45ac03963 | 227 | } while((busyFlag&1)!=0); |
hubercam | 0:417c1bd45a3c | 228 | |
hubercam | 0:417c1bd45a3c | 229 | } /* LIDARLite_v3HP::waitForBusy */ |
hubercam | 0:417c1bd45a3c | 230 | |
hubercam | 0:417c1bd45a3c | 231 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 232 | Get Busy Flag |
hubercam | 0:417c1bd45a3c | 233 | |
hubercam | 0:417c1bd45a3c | 234 | Read BUSY flag from device registers. Function will return 0x00 if not busy. |
hubercam | 0:417c1bd45a3c | 235 | |
hubercam | 0:417c1bd45a3c | 236 | Parameters |
hubercam | 0:417c1bd45a3c | 237 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 238 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 239 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 240 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 241 | uint8_t LIDARLite_v3HP::getBusyFlag(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 242 | { |
vermaelen | 2:d6e45ac03963 | 243 | char busyFlag; // busyFlag monitors when the device is done with a measurement |
hubercam | 0:417c1bd45a3c | 244 | |
hubercam | 0:417c1bd45a3c | 245 | // Read status register to check busy flag |
vermaelen | 2:d6e45ac03963 | 246 | //read(0x01, &busyFlag, 1, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 247 | i2c_->read((lidarliteAddress<<1), &busyFlag, 1); |
hubercam | 0:417c1bd45a3c | 248 | |
hubercam | 0:417c1bd45a3c | 249 | // STATUS bit 0 is busyFlag |
vermaelen | 2:d6e45ac03963 | 250 | return (busyFlag&0x01); |
hubercam | 0:417c1bd45a3c | 251 | } /* LIDARLite_v3HP::getBusyFlag */ |
hubercam | 0:417c1bd45a3c | 252 | |
hubercam | 0:417c1bd45a3c | 253 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 254 | Read Distance |
hubercam | 0:417c1bd45a3c | 255 | |
hubercam | 0:417c1bd45a3c | 256 | Read and return result of distance measurement. |
hubercam | 0:417c1bd45a3c | 257 | |
hubercam | 0:417c1bd45a3c | 258 | Process |
hubercam | 0:417c1bd45a3c | 259 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 260 | 1. Read two bytes from register 0x8f and save |
hubercam | 0:417c1bd45a3c | 261 | 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f. |
hubercam | 0:417c1bd45a3c | 262 | The result is the measured distance in centimeters. |
hubercam | 0:417c1bd45a3c | 263 | |
hubercam | 0:417c1bd45a3c | 264 | Parameters |
hubercam | 0:417c1bd45a3c | 265 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 266 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 267 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 268 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 269 | uint16_t LIDARLite_v3HP::readDistance(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 270 | { |
hubercam | 0:417c1bd45a3c | 271 | uint16_t distance; |
vermaelen | 1:bd9e4d198947 | 272 | uint8_t dataByteHigh; |
vermaelen | 2:d6e45ac03963 | 273 | uint8_t dataByteLow; |
hubercam | 0:417c1bd45a3c | 274 | |
hubercam | 0:417c1bd45a3c | 275 | // Read two bytes from register 0x0f and 0x10 (autoincrement) |
vermaelen | 1:bd9e4d198947 | 276 | // ca marche pas !!!! |
vermaelen | 2:d6e45ac03963 | 277 | // read(0x0f, dataBytes, 2, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 278 | read(0x0f, &dataByteHigh, 1, lidarliteAddress); |
vermaelen | 2:d6e45ac03963 | 279 | read(0x10, &dataByteLow, 1, lidarliteAddress); |
vermaelen | 1:bd9e4d198947 | 280 | distance =(dataByteHigh<<8)+dataByteLow; |
hubercam | 0:417c1bd45a3c | 281 | return (distance); |
hubercam | 0:417c1bd45a3c | 282 | } /* LIDARLite_v3HP::readDistance */ |
hubercam | 0:417c1bd45a3c | 283 | |
hubercam | 0:417c1bd45a3c | 284 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 285 | Reset Reference Filter |
hubercam | 0:417c1bd45a3c | 286 | |
hubercam | 0:417c1bd45a3c | 287 | In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in |
hubercam | 0:417c1bd45a3c | 288 | initial measurements that may be a few centimeters short of actual distance. |
hubercam | 0:417c1bd45a3c | 289 | This symptom will eventually rectify itself after a few hundred measurements. |
hubercam | 0:417c1bd45a3c | 290 | This symptom can also be rectified more quickly by resetting the unit's internal |
hubercam | 0:417c1bd45a3c | 291 | reference filter. The process here illustrates how to disable the internal |
hubercam | 0:417c1bd45a3c | 292 | reference filter, trigger a measurement (forcing re-init of the reference |
hubercam | 0:417c1bd45a3c | 293 | filter), and then re-enable the filter. |
hubercam | 0:417c1bd45a3c | 294 | |
hubercam | 0:417c1bd45a3c | 295 | Process |
hubercam | 0:417c1bd45a3c | 296 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 297 | 1. Disable the LIDAR-Lite reference filter |
hubercam | 0:417c1bd45a3c | 298 | 2. Set reference integration count to max |
hubercam | 0:417c1bd45a3c | 299 | 3. Trigger a measurement |
hubercam | 0:417c1bd45a3c | 300 | 4. Restore reference integration count |
hubercam | 0:417c1bd45a3c | 301 | 5. Re-enable reference filter |
hubercam | 0:417c1bd45a3c | 302 | |
hubercam | 0:417c1bd45a3c | 303 | Parameters |
hubercam | 0:417c1bd45a3c | 304 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 305 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 306 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 307 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 308 | void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 309 | { |
hubercam | 0:417c1bd45a3c | 310 | uint8_t dataBytes[2]; |
hubercam | 0:417c1bd45a3c | 311 | uint8_t acqConfigReg; |
hubercam | 0:417c1bd45a3c | 312 | uint8_t refCountMax; |
hubercam | 0:417c1bd45a3c | 313 | |
hubercam | 0:417c1bd45a3c | 314 | // Set bit 4 of the acquisition configuration register (disable reference filter) |
hubercam | 0:417c1bd45a3c | 315 | read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register) |
hubercam | 0:417c1bd45a3c | 316 | acqConfigReg = dataBytes[0]; // store for later restoration |
hubercam | 0:417c1bd45a3c | 317 | dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter |
hubercam | 0:417c1bd45a3c | 318 | write(0x04, dataBytes, 1, lidarliteAddress); // write it back |
hubercam | 0:417c1bd45a3c | 319 | |
hubercam | 0:417c1bd45a3c | 320 | // Set reference integration count to max |
hubercam | 0:417c1bd45a3c | 321 | read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count) |
hubercam | 0:417c1bd45a3c | 322 | refCountMax = dataBytes[0]; // store for later restoration |
hubercam | 0:417c1bd45a3c | 323 | dataBytes[0] = 0xff; // we want to reference to overflow quickly |
hubercam | 0:417c1bd45a3c | 324 | write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count |
hubercam | 0:417c1bd45a3c | 325 | |
hubercam | 0:417c1bd45a3c | 326 | // Trigger a measurement |
hubercam | 0:417c1bd45a3c | 327 | waitForBusy(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 328 | takeRange(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 329 | waitForBusy(lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 330 | // ... no need to read the distance, it is immaterial |
hubercam | 0:417c1bd45a3c | 331 | |
hubercam | 0:417c1bd45a3c | 332 | // Restore previous reference integration count |
hubercam | 0:417c1bd45a3c | 333 | dataBytes[0] = refCountMax; |
hubercam | 0:417c1bd45a3c | 334 | write(0x12, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 335 | |
hubercam | 0:417c1bd45a3c | 336 | // Restore previous acquisition configuration register (re-enabling reference filter) |
hubercam | 0:417c1bd45a3c | 337 | dataBytes[0] = acqConfigReg; |
hubercam | 0:417c1bd45a3c | 338 | write(0x04, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 339 | } /* LIDARLite_v3HP::resetReferenceFilter */ |
hubercam | 0:417c1bd45a3c | 340 | |
hubercam | 0:417c1bd45a3c | 341 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 342 | Write |
hubercam | 0:417c1bd45a3c | 343 | |
hubercam | 0:417c1bd45a3c | 344 | Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP |
hubercam | 0:417c1bd45a3c | 345 | will receive multiple bytes in one I2C transmission. The first byte is |
hubercam | 0:417c1bd45a3c | 346 | always the register address. The the bytes that follow will be written |
hubercam | 0:417c1bd45a3c | 347 | into the specified register address first and then the internal address |
hubercam | 0:417c1bd45a3c | 348 | in the Lidar Lite will be auto-incremented for all following bytes. |
hubercam | 0:417c1bd45a3c | 349 | |
hubercam | 0:417c1bd45a3c | 350 | Parameters |
hubercam | 0:417c1bd45a3c | 351 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 352 | regAddr: register address to write to |
hubercam | 0:417c1bd45a3c | 353 | dataBytes: pointer to array of bytes to write |
hubercam | 0:417c1bd45a3c | 354 | numBytes: number of bytes in 'dataBytes' array to write |
hubercam | 0:417c1bd45a3c | 355 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 356 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 357 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 358 | void LIDARLite_v3HP::write (const uint8_t ®Addr, uint8_t * dataBytes, |
hubercam | 0:417c1bd45a3c | 359 | const uint16_t &numBytes, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 360 | { |
hubercam | 0:417c1bd45a3c | 361 | int nackCatcher; |
hubercam | 0:417c1bd45a3c | 362 | |
hubercam | 0:417c1bd45a3c | 363 | // i2c Syntax |
hubercam | 0:417c1bd45a3c | 364 | // ----------------------------------------------------------------- |
hubercam | 0:417c1bd45a3c | 365 | // I2C.write(i2cAddress, *data, length, repeated = false) |
hubercam | 0:417c1bd45a3c | 366 | |
vermaelen | 2:d6e45ac03963 | 367 | |
vermaelen | 2:d6e45ac03963 | 368 | nackCatcher = i2c_->write((lidarliteAddress<<1), (char *)dataBytes, numBytes, false); |
hubercam | 0:417c1bd45a3c | 369 | |
hubercam | 0:417c1bd45a3c | 370 | // A nack means the device is not responding. Report the error over serial. |
vermaelen | 2:d6e45ac03963 | 371 | if (nackCatcher != 0) { |
hubercam | 0:417c1bd45a3c | 372 | printf("> nack\n\r"); |
hubercam | 0:417c1bd45a3c | 373 | } |
hubercam | 0:417c1bd45a3c | 374 | |
hubercam | 0:417c1bd45a3c | 375 | wait_us(100); // 100 us delay for robustness with successive reads and writes |
hubercam | 0:417c1bd45a3c | 376 | } /* LIDARLite_v3HP::write */ |
hubercam | 0:417c1bd45a3c | 377 | |
hubercam | 0:417c1bd45a3c | 378 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 379 | Read |
hubercam | 0:417c1bd45a3c | 380 | |
hubercam | 0:417c1bd45a3c | 381 | Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP |
hubercam | 0:417c1bd45a3c | 382 | will send multiple bytes in one I2C transmission. The register address must |
hubercam | 0:417c1bd45a3c | 383 | be set up by a previous I2C write. The bytes that follow will be read |
hubercam | 0:417c1bd45a3c | 384 | from the specified register address first and then the internal address |
hubercam | 0:417c1bd45a3c | 385 | pointer in the Lidar Lite will be auto-incremented for following bytes. |
hubercam | 0:417c1bd45a3c | 386 | |
hubercam | 0:417c1bd45a3c | 387 | Will detect an unresponsive device and report the error over serial. |
hubercam | 0:417c1bd45a3c | 388 | |
hubercam | 0:417c1bd45a3c | 389 | Parameters |
hubercam | 0:417c1bd45a3c | 390 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 391 | regAddr: register address to write to |
hubercam | 0:417c1bd45a3c | 392 | dataBytes: pointer to array of bytes to write |
hubercam | 0:417c1bd45a3c | 393 | numBytes: number of bytes in 'dataBytes' array to write |
hubercam | 0:417c1bd45a3c | 394 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 395 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 396 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 397 | void LIDARLite_v3HP::read (const uint8_t ®Addr, uint8_t * dataBytes, |
hubercam | 0:417c1bd45a3c | 398 | const uint16_t &numBytes, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 399 | { |
hubercam | 0:417c1bd45a3c | 400 | int nackCatcher = 0; |
vermaelen | 2:d6e45ac03963 | 401 | |
hubercam | 0:417c1bd45a3c | 402 | char regAddrC = char(regAddr); |
vermaelen | 2:d6e45ac03963 | 403 | |
hubercam | 0:417c1bd45a3c | 404 | // A nack means the device is not responding, report the error over serial |
vermaelen | 2:d6e45ac03963 | 405 | nackCatcher = i2c_->write((lidarliteAddress<<1), ®AddrC, 1, false); // false means perform repeated start |
vermaelen | 2:d6e45ac03963 | 406 | |
vermaelen | 2:d6e45ac03963 | 407 | if (nackCatcher != 0) { |
hubercam | 0:417c1bd45a3c | 408 | printf("> nack\n\r"); |
hubercam | 0:417c1bd45a3c | 409 | } |
hubercam | 0:417c1bd45a3c | 410 | |
hubercam | 0:417c1bd45a3c | 411 | // Perform read, save in dataBytes array |
vermaelen | 2:d6e45ac03963 | 412 | i2c_->read((lidarliteAddress<<1), (char *)dataBytes, numBytes); |
vermaelen | 2:d6e45ac03963 | 413 | |
hubercam | 0:417c1bd45a3c | 414 | } /* LIDARLite_v3HP::read */ |
hubercam | 0:417c1bd45a3c | 415 | |
hubercam | 0:417c1bd45a3c | 416 | /*------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 417 | Correlation Record To Serial |
hubercam | 0:417c1bd45a3c | 418 | |
hubercam | 0:417c1bd45a3c | 419 | The correlation record used to calculate distance can be read from the device. |
hubercam | 0:417c1bd45a3c | 420 | It has a bipolar wave shape, transitioning from a positive going portion to a |
hubercam | 0:417c1bd45a3c | 421 | roughly symmetrical negative going pulse. The point where the signal crosses |
hubercam | 0:417c1bd45a3c | 422 | zero represents the effective delay for the reference and return signals. |
hubercam | 0:417c1bd45a3c | 423 | |
hubercam | 0:417c1bd45a3c | 424 | Process |
hubercam | 0:417c1bd45a3c | 425 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 426 | 1. Take a distance reading (there is no correlation record without at least |
hubercam | 0:417c1bd45a3c | 427 | one distance reading being taken) |
hubercam | 0:417c1bd45a3c | 428 | 2. Set test mode select by writing 0x07 to register 0x40 |
hubercam | 0:417c1bd45a3c | 429 | 3. For as many readings as you want to take (max is 1024) |
hubercam | 0:417c1bd45a3c | 430 | 1. Read two bytes from 0x52 |
hubercam | 0:417c1bd45a3c | 431 | 2. The Low byte is the value from the record |
hubercam | 0:417c1bd45a3c | 432 | 3. The high byte is the sign from the record |
hubercam | 0:417c1bd45a3c | 433 | |
hubercam | 0:417c1bd45a3c | 434 | Parameters |
hubercam | 0:417c1bd45a3c | 435 | ------------------------------------------------------------------------------ |
hubercam | 0:417c1bd45a3c | 436 | separator: the separator between serial data words |
hubercam | 0:417c1bd45a3c | 437 | numberOfReadings: Default: 1024. Maximum of 1024 |
hubercam | 0:417c1bd45a3c | 438 | lidarliteAddress: Default 0x62. Fill in new address here if changed. See |
hubercam | 0:417c1bd45a3c | 439 | operating manual for instructions. |
hubercam | 0:417c1bd45a3c | 440 | ------------------------------------------------------------------------------*/ |
hubercam | 0:417c1bd45a3c | 441 | void LIDARLite_v3HP::correlationRecordToSerial( |
hubercam | 0:417c1bd45a3c | 442 | const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress) |
hubercam | 0:417c1bd45a3c | 443 | { |
hubercam | 0:417c1bd45a3c | 444 | uint16_t i = 0; |
hubercam | 0:417c1bd45a3c | 445 | uint8_t dataBytes[2]; // Array to store read / write data |
hubercam | 0:417c1bd45a3c | 446 | int16_t correlationValue = 0; // Var to store value of correlation record |
hubercam | 0:417c1bd45a3c | 447 | |
hubercam | 0:417c1bd45a3c | 448 | // Test mode enable |
hubercam | 0:417c1bd45a3c | 449 | dataBytes[0] = 0x07; |
hubercam | 0:417c1bd45a3c | 450 | write(0x40, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 451 | |
vermaelen | 2:d6e45ac03963 | 452 | for (i=0 ; i<numberOfReadings ; i++) { |
hubercam | 0:417c1bd45a3c | 453 | read(0x52, dataBytes, 2, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 454 | |
hubercam | 0:417c1bd45a3c | 455 | // Low byte is the value of the correlation record |
hubercam | 0:417c1bd45a3c | 456 | correlationValue = (uint16_t) dataBytes[0]; |
hubercam | 0:417c1bd45a3c | 457 | |
hubercam | 0:417c1bd45a3c | 458 | // if upper byte lsb is one, the value is negative |
hubercam | 0:417c1bd45a3c | 459 | // so here we test to artifically sign extend the data |
vermaelen | 2:d6e45ac03963 | 460 | if ( (int) dataBytes[1] == 1) { |
hubercam | 0:417c1bd45a3c | 461 | correlationValue |= 0xff00; |
hubercam | 0:417c1bd45a3c | 462 | } |
hubercam | 0:417c1bd45a3c | 463 | printf("%d\n\r", correlationValue); |
vermaelen | 2:d6e45ac03963 | 464 | |
hubercam | 0:417c1bd45a3c | 465 | } |
hubercam | 0:417c1bd45a3c | 466 | |
hubercam | 0:417c1bd45a3c | 467 | // test mode disable |
hubercam | 0:417c1bd45a3c | 468 | dataBytes[0] = 0; |
hubercam | 0:417c1bd45a3c | 469 | write(0x40, dataBytes, 1, lidarliteAddress); |
hubercam | 0:417c1bd45a3c | 470 | } /* LIDARLite_v3HP::correlationRecordToSerial */ |