Hiroki Tanaka / Mbed 2 deprecated arrc_mbed

明石高専ロボ研 mbedライブラリ

Dependencies:   mbed

Dependents:   MDD_L432KC USB2RS485 pathtracking odometry ... more

lidar_lite.cpp@14:bdd394483434, 2021-10-13 (annotated)

Committer:
TanakaRobo
Date:
Wed Oct 13 09:12:48 2021 +0000
Revision:
14:bdd394483434
Parent:
6:678c6b604ac7 
first? commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 6:678c6b604ac7 1 #include "lidar_lite.h"
TanakaRobo 6:678c6b604ac7 2
TanakaRobo 6:678c6b604ac7 3 LIDARLite::LIDARLite(I2C &i2c):i2c_(i2c){}
TanakaRobo 6:678c6b604ac7 4
TanakaRobo 6:678c6b604ac7 5 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 6 Begin
TanakaRobo 6:678c6b604ac7 7 Starts the sensor and I2C.
TanakaRobo 6:678c6b604ac7 8 Parameters
TanakaRobo 6:678c6b604ac7 9 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 10 configuration: Default 0. Selects one of several preset configurations.
TanakaRobo 6:678c6b604ac7 11 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 12 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 13 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 14 void LIDARLite::begin(int configuration, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 15 {
TanakaRobo 6:678c6b604ac7 16 configure(configuration, lidarliteAddress); // Configuration settings
TanakaRobo 6:678c6b604ac7 17 } /* LIDARLite::begin */
TanakaRobo 6:678c6b604ac7 18
TanakaRobo 6:678c6b604ac7 19 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 20 Configure
TanakaRobo 6:678c6b604ac7 21 Selects one of several preset configurations.
TanakaRobo 6:678c6b604ac7 22 Parameters
TanakaRobo 6:678c6b604ac7 23 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 24 configuration: Default 0.
TanakaRobo 6:678c6b604ac7 25 0: Default mode, balanced performance.
TanakaRobo 6:678c6b604ac7 26 1: Short range, high speed. Uses 0x1d maximum acquisition count.
TanakaRobo 6:678c6b604ac7 27 2: Default range, higher speed short range. Turns on quick termination
TanakaRobo 6:678c6b604ac7 28 detection for faster measurements at short range (with decreased
TanakaRobo 6:678c6b604ac7 29 accuracy)
TanakaRobo 6:678c6b604ac7 30 3: Maximum range. Uses 0xff maximum acquisition count.
TanakaRobo 6:678c6b604ac7 31 4: High sensitivity detection. Overrides default valid measurement detection
TanakaRobo 6:678c6b604ac7 32 algorithm, and uses a threshold value for high sensitivity and noise.
TanakaRobo 6:678c6b604ac7 33 5: Low sensitivity detection. Overrides default valid measurement detection
TanakaRobo 6:678c6b604ac7 34 algorithm, and uses a threshold value for low sensitivity and noise.
TanakaRobo 6:678c6b604ac7 35 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 36 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 37 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 38 void LIDARLite::configure(int configuration, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 39 {
TanakaRobo 6:678c6b604ac7 40 switch (configuration)
TanakaRobo 6:678c6b604ac7 41 {
TanakaRobo 6:678c6b604ac7 42 case 0: // Default mode, balanced performance
TanakaRobo 6:678c6b604ac7 43 write(0x02,0x80,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 44 write(0x04,0x08,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 45 write(0x1c,0x00,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 46 break;
TanakaRobo 6:678c6b604ac7 47
TanakaRobo 6:678c6b604ac7 48 case 1: // Short range, high speed
TanakaRobo 6:678c6b604ac7 49 write(0x02,0x1d,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 50 write(0x04,0x08,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 51 write(0x1c,0x00,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 52 break;
TanakaRobo 6:678c6b604ac7 53
TanakaRobo 6:678c6b604ac7 54 case 2: // Default range, higher speed short range
TanakaRobo 6:678c6b604ac7 55 write(0x02,0x80,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 56 write(0x04,0x00,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 57 write(0x1c,0x00,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 58 break;
TanakaRobo 6:678c6b604ac7 59
TanakaRobo 6:678c6b604ac7 60 case 3: // Maximum range
TanakaRobo 6:678c6b604ac7 61 write(0x02,0xff,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 62 write(0x04,0x08,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 63 write(0x1c,0x00,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 64 break;
TanakaRobo 6:678c6b604ac7 65
TanakaRobo 6:678c6b604ac7 66 case 4: // High sensitivity detection, high erroneous measurements
TanakaRobo 6:678c6b604ac7 67 write(0x02,0x80,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 68 write(0x04,0x08,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 69 write(0x1c,0x80,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 70 break;
TanakaRobo 6:678c6b604ac7 71
TanakaRobo 6:678c6b604ac7 72 case 5: // Low sensitivity detection, low erroneous measurements
TanakaRobo 6:678c6b604ac7 73 write(0x02,0x80,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 74 write(0x04,0x08,lidarliteAddress); // Default
TanakaRobo 6:678c6b604ac7 75 write(0x1c,0xb0,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 76 break;
TanakaRobo 6:678c6b604ac7 77 }
TanakaRobo 6:678c6b604ac7 78 } /* LIDARLite::configure */
TanakaRobo 6:678c6b604ac7 79
TanakaRobo 6:678c6b604ac7 80 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 81 Set I2C Address
TanakaRobo 6:678c6b604ac7 82 Set Alternate I2C Device Address. See Operation Manual for additional info.
TanakaRobo 6:678c6b604ac7 83 Parameters
TanakaRobo 6:678c6b604ac7 84 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 85 newAddress: desired secondary I2C device address
TanakaRobo 6:678c6b604ac7 86 disableDefault: a non-zero value here means the default 0x62 I2C device
TanakaRobo 6:678c6b604ac7 87 address will be disabled.
TanakaRobo 6:678c6b604ac7 88 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 89 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 90 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 91 void LIDARLite::setI2Caddr(char newAddress, bool disableDefault, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 92 {
TanakaRobo 6:678c6b604ac7 93 char dataBytes[2];
TanakaRobo 6:678c6b604ac7 94
TanakaRobo 6:678c6b604ac7 95 // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations
TanakaRobo 6:678c6b604ac7 96 read ((0x16 | 0x80), 2, dataBytes, false, lidarliteAddress);
TanakaRobo 6:678c6b604ac7 97 write(0x18, dataBytes[0], lidarliteAddress);
TanakaRobo 6:678c6b604ac7 98 write(0x19, dataBytes[1], lidarliteAddress);
TanakaRobo 6:678c6b604ac7 99
TanakaRobo 6:678c6b604ac7 100 // Write the new I2C device address to registers
TanakaRobo 6:678c6b604ac7 101 dataBytes[0] = newAddress;
TanakaRobo 6:678c6b604ac7 102 write(0x1a, dataBytes[0], lidarliteAddress);
TanakaRobo 6:678c6b604ac7 103
TanakaRobo 6:678c6b604ac7 104 // Enable the new I2C device address using the default I2C device address
TanakaRobo 6:678c6b604ac7 105 dataBytes[0] = 0;
TanakaRobo 6:678c6b604ac7 106 write(0x1e, dataBytes[0], lidarliteAddress);
TanakaRobo 6:678c6b604ac7 107
TanakaRobo 6:678c6b604ac7 108 // If desired, disable default I2C device address (using the new I2C device address)
TanakaRobo 6:678c6b604ac7 109 if (disableDefault)
TanakaRobo 6:678c6b604ac7 110 {
TanakaRobo 6:678c6b604ac7 111 dataBytes[0] = (1 << 3); // set bit to disable default address
TanakaRobo 6:678c6b604ac7 112 write(0x1e, dataBytes[0], newAddress);
TanakaRobo 6:678c6b604ac7 113 }
TanakaRobo 6:678c6b604ac7 114 } /* LIDARLite::setI2Caddr */
TanakaRobo 6:678c6b604ac7 115
TanakaRobo 6:678c6b604ac7 116 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 117 Reset
TanakaRobo 6:678c6b604ac7 118 Reset device. The device reloads default register settings, including the
TanakaRobo 6:678c6b604ac7 119 default I2C address. Re-initialization takes approximately 22ms.
TanakaRobo 6:678c6b604ac7 120 Parameters
TanakaRobo 6:678c6b604ac7 121 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 122 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 123 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 124 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 125 void LIDARLite::reset( char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 126 {
TanakaRobo 6:678c6b604ac7 127 write(0x00,0x00,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 128 } /* LIDARLite::reset */
TanakaRobo 6:678c6b604ac7 129
TanakaRobo 6:678c6b604ac7 130 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 131 Distance
TanakaRobo 6:678c6b604ac7 132 Take a distance measurement and read the result.
TanakaRobo 6:678c6b604ac7 133 Process
TanakaRobo 6:678c6b604ac7 134 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 135 1. Write 0x04 or 0x03 to register 0x00 to initiate an aquisition.
TanakaRobo 6:678c6b604ac7 136 2. Read register 0x01 (this is handled in the read() command)
TanakaRobo 6:678c6b604ac7 137 - if the first bit is "1" then the sensor is busy, loop until the first
TanakaRobo 6:678c6b604ac7 138 bit is "0"
TanakaRobo 6:678c6b604ac7 139 - if the first bit is "0" then the sensor is ready
TanakaRobo 6:678c6b604ac7 140 3. Read two bytes from register 0x8f and save
TanakaRobo 6:678c6b604ac7 141 4. Shift the first value from 0x8f << 8 and add to second value from 0x8f.
TanakaRobo 6:678c6b604ac7 142 The result is the measured distance in centimeters.
TanakaRobo 6:678c6b604ac7 143 Parameters
TanakaRobo 6:678c6b604ac7 144 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 145 biasCorrection: Default true. Take aquisition with receiver bias
TanakaRobo 6:678c6b604ac7 146 correction. If set to false measurements will be faster. Receiver bias
TanakaRobo 6:678c6b604ac7 147 correction must be performed periodically. (e.g. 1 out of every 100
TanakaRobo 6:678c6b604ac7 148 readings).
TanakaRobo 6:678c6b604ac7 149 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 150 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 151 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 152 int LIDARLite::distance(bool biasCorrection, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 153 {
TanakaRobo 6:678c6b604ac7 154 if(biasCorrection)
TanakaRobo 6:678c6b604ac7 155 {
TanakaRobo 6:678c6b604ac7 156 // Take acquisition & correlation processing with receiver bias correction
TanakaRobo 6:678c6b604ac7 157 write(0x00,0x04,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 158 }
TanakaRobo 6:678c6b604ac7 159 else
TanakaRobo 6:678c6b604ac7 160 {
TanakaRobo 6:678c6b604ac7 161 // Take acquisition & correlation processing without receiver bias correction
TanakaRobo 6:678c6b604ac7 162 write(0x00,0x03,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 163 }
TanakaRobo 6:678c6b604ac7 164 // Array to store high and low bytes of distance
TanakaRobo 6:678c6b604ac7 165 char distanceArray[2];
TanakaRobo 6:678c6b604ac7 166 // Read two bytes from register 0x8f (autoincrement for reading 0x0f and 0x10)
TanakaRobo 6:678c6b604ac7 167 read(0x8f,2,distanceArray,true,lidarliteAddress);//0x8f
TanakaRobo 6:678c6b604ac7 168 // Shift high byte and add to low byte
TanakaRobo 6:678c6b604ac7 169 int distance = (distanceArray[0] << 8) + distanceArray[1];
TanakaRobo 6:678c6b604ac7 170 return(distance);
TanakaRobo 6:678c6b604ac7 171 } /* LIDARLite::distance */
TanakaRobo 6:678c6b604ac7 172
TanakaRobo 6:678c6b604ac7 173 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 174 Write
TanakaRobo 6:678c6b604ac7 175 Perform I2C write to device.
TanakaRobo 6:678c6b604ac7 176 Parameters
TanakaRobo 6:678c6b604ac7 177 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 178 myAddress: register address to write to.
TanakaRobo 6:678c6b604ac7 179 myValue: value to write.
TanakaRobo 6:678c6b604ac7 180 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 181 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 182 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 183 void LIDARLite::write(char myAddress, char myValue, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 184 {
TanakaRobo 6:678c6b604ac7 185 char data[2] = {myAddress,myValue};//register value
TanakaRobo 6:678c6b604ac7 186 int nackCatcher = i2c_.write((int)lidarliteAddress,data,2);
TanakaRobo 6:678c6b604ac7 187 // A nack means the device is not responding, report the error over serial
TanakaRobo 6:678c6b604ac7 188 /*
TanakaRobo 6:678c6b604ac7 189 if(nackCatcher != 0)
TanakaRobo 6:678c6b604ac7 190 {
TanakaRobo 6:678c6b604ac7 191 //printf("> nack");
TanakaRobo 6:678c6b604ac7 192 }*/
TanakaRobo 6:678c6b604ac7 193
TanakaRobo 6:678c6b604ac7 194 //wait_ms(1); // 1 ms delay for robustness with successive reads and writes
TanakaRobo 6:678c6b604ac7 195 } /* LIDARLite::write */
TanakaRobo 6:678c6b604ac7 196
TanakaRobo 6:678c6b604ac7 197 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 198 Read
TanakaRobo 6:678c6b604ac7 199 Perform I2C read from device. Will detect an unresponsive device and report
TanakaRobo 6:678c6b604ac7 200 the error over serial. The optional busy flag monitoring
TanakaRobo 6:678c6b604ac7 201 can be used to read registers that are updated at the end of a distance
TanakaRobo 6:678c6b604ac7 202 measurement to obtain the new data.
TanakaRobo 6:678c6b604ac7 203 Parameters
TanakaRobo 6:678c6b604ac7 204 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 205 myAddress: register address to read from.
TanakaRobo 6:678c6b604ac7 206 numOfBytes: numbers of bytes to read. Can be 1 or 2.
TanakaRobo 6:678c6b604ac7 207 arrayToSave: an array to store the read values.
TanakaRobo 6:678c6b604ac7 208 monitorBusyFlag: if true, the routine will repeatedly read the status
TanakaRobo 6:678c6b604ac7 209 register until the busy flag (LSB) is 0.
TanakaRobo 6:678c6b604ac7 210 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 211 void LIDARLite::read(char myAddress, int numOfBytes, char arrayToSave[2], bool monitorBusyFlag, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 212 {
TanakaRobo 6:678c6b604ac7 213 char data;
TanakaRobo 6:678c6b604ac7 214 int busyFlag = 0; // busyFlag monitors when the device is done with a measurement
TanakaRobo 6:678c6b604ac7 215 bool errer_flag =false;
TanakaRobo 6:678c6b604ac7 216 if(monitorBusyFlag)
TanakaRobo 6:678c6b604ac7 217 {
TanakaRobo 6:678c6b604ac7 218 busyFlag = 1; // Begin read immediately if not monitoring busy flag
TanakaRobo 6:678c6b604ac7 219 }
TanakaRobo 6:678c6b604ac7 220 int busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout
TanakaRobo 6:678c6b604ac7 221
TanakaRobo 6:678c6b604ac7 222 while(busyFlag != 0) // Loop until device is not busy
TanakaRobo 6:678c6b604ac7 223 {
TanakaRobo 6:678c6b604ac7 224 // Read status register to check busy flag
TanakaRobo 6:678c6b604ac7 225 data = 0x01;//Set the status register to be read
TanakaRobo 6:678c6b604ac7 226 int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
TanakaRobo 6:678c6b604ac7 227 // A nack means the device is not responding, report the error over serial
TanakaRobo 6:678c6b604ac7 228 if(nackCatcher != 0)
TanakaRobo 6:678c6b604ac7 229 {
TanakaRobo 6:678c6b604ac7 230 //printf("> nack");
TanakaRobo 6:678c6b604ac7 231 }
TanakaRobo 6:678c6b604ac7 232
TanakaRobo 6:678c6b604ac7 233 i2c_.read(lidarliteAddress,&data,1); // Read register 0x01
TanakaRobo 6:678c6b604ac7 234 busyFlag = data & 0x1; // Assign the LSB of the status register to busyFlag
TanakaRobo 6:678c6b604ac7 235
TanakaRobo 6:678c6b604ac7 236 busyCounter++; // Increment busyCounter for timeout
TanakaRobo 6:678c6b604ac7 237
TanakaRobo 6:678c6b604ac7 238 // Handle timeout condition, exit while loop and goto bailout
TanakaRobo 6:678c6b604ac7 239 if(busyCounter > 9999)
TanakaRobo 6:678c6b604ac7 240 {
TanakaRobo 6:678c6b604ac7 241 errer_flag = true;
TanakaRobo 6:678c6b604ac7 242 break;
TanakaRobo 6:678c6b604ac7 243 }
TanakaRobo 6:678c6b604ac7 244 }
TanakaRobo 6:678c6b604ac7 245
TanakaRobo 6:678c6b604ac7 246 // Device is not busy, begin read
TanakaRobo 6:678c6b604ac7 247 if(busyFlag == 0 && !errer_flag)
TanakaRobo 6:678c6b604ac7 248 {
TanakaRobo 6:678c6b604ac7 249 data = myAddress;// Set the register to be read
TanakaRobo 6:678c6b604ac7 250 int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
TanakaRobo 6:678c6b604ac7 251 // A nack means the device is not responding, report the error over serial
TanakaRobo 6:678c6b604ac7 252 if(nackCatcher != 0)
TanakaRobo 6:678c6b604ac7 253 {
TanakaRobo 6:678c6b604ac7 254 //printf("> nack");
TanakaRobo 6:678c6b604ac7 255 }
TanakaRobo 6:678c6b604ac7 256
TanakaRobo 6:678c6b604ac7 257 // Perform read of 1 or 2 bytes, save in arrayToSave
TanakaRobo 6:678c6b604ac7 258 i2c_.read(lidarliteAddress, arrayToSave, numOfBytes);
TanakaRobo 6:678c6b604ac7 259 }
TanakaRobo 6:678c6b604ac7 260
TanakaRobo 6:678c6b604ac7 261 // bailout reports error over serial
TanakaRobo 6:678c6b604ac7 262 if(busyCounter > 9999 || errer_flag)
TanakaRobo 6:678c6b604ac7 263 {
TanakaRobo 6:678c6b604ac7 264 busyCounter = 0;
TanakaRobo 6:678c6b604ac7 265 //printf("> read failed");
TanakaRobo 6:678c6b604ac7 266 }
TanakaRobo 6:678c6b604ac7 267 } /* LIDARLite::read */
TanakaRobo 6:678c6b604ac7 268
TanakaRobo 6:678c6b604ac7 269 /*------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 270 Correlation Record To Serial
TanakaRobo 6:678c6b604ac7 271 The correlation record used to calculate distance can be read from the device.
TanakaRobo 6:678c6b604ac7 272 It has a bipolar wave shape, transitioning from a positive going portion to a
TanakaRobo 6:678c6b604ac7 273 roughly symmetrical negative going pulse. The point where the signal crosses
TanakaRobo 6:678c6b604ac7 274 zero represents the effective delay for the reference and return signals.
TanakaRobo 6:678c6b604ac7 275 Process
TanakaRobo 6:678c6b604ac7 276 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 277 1. Take a distance reading (there is no correlation record without at least
TanakaRobo 6:678c6b604ac7 278 one distance reading being taken)
TanakaRobo 6:678c6b604ac7 279 2. Select memory bank by writing 0xc0 to register 0x5d
TanakaRobo 6:678c6b604ac7 280 3. Set test mode select by writing 0x07 to register 0x40
TanakaRobo 6:678c6b604ac7 281 4. For as many readings as you want to take (max is 1024)
TanakaRobo 6:678c6b604ac7 282 1. Read two bytes from 0xd2
TanakaRobo 6:678c6b604ac7 283 2. The Low byte is the value from the record
TanakaRobo 6:678c6b604ac7 284 3. The high byte is the sign from the record
TanakaRobo 6:678c6b604ac7 285 Parameters
TanakaRobo 6:678c6b604ac7 286 ------------------------------------------------------------------------------
TanakaRobo 6:678c6b604ac7 287 separator: the separator between serial data words
TanakaRobo 6:678c6b604ac7 288 numberOfReadings: Default: 256. Maximum of 1024
TanakaRobo 6:678c6b604ac7 289 lidarliteAddress: Default 0x62. Fill in new address here if changed. See
TanakaRobo 6:678c6b604ac7 290 operating manual for instructions.
TanakaRobo 6:678c6b604ac7 291 ------------------------------------------------------------------------------*/
TanakaRobo 6:678c6b604ac7 292 void LIDARLite::correlationRecordToSerial(char separator, int numberOfReadings, char lidarliteAddress)
TanakaRobo 6:678c6b604ac7 293 {
TanakaRobo 6:678c6b604ac7 294
TanakaRobo 6:678c6b604ac7 295 // Array to store read values
TanakaRobo 6:678c6b604ac7 296 char correlationArray[2];
TanakaRobo 6:678c6b604ac7 297 // Var to store value of correlation record
TanakaRobo 6:678c6b604ac7 298 int correlationValue = 0;
TanakaRobo 6:678c6b604ac7 299 // Selects memory bank
TanakaRobo 6:678c6b604ac7 300 write(0x5d,0xc0,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 301 // Test mode enable
TanakaRobo 6:678c6b604ac7 302 write(0x40, 0x07,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 303 for(int i = 0; i<numberOfReadings; i++){
TanakaRobo 6:678c6b604ac7 304 // Select single byte
TanakaRobo 6:678c6b604ac7 305 read(0xd2,2,correlationArray,false,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 306 // Low byte is the value of the correlation record
TanakaRobo 6:678c6b604ac7 307 correlationValue = correlationArray[0];
TanakaRobo 6:678c6b604ac7 308 // if upper byte lsb is set, the value is negative
TanakaRobo 6:678c6b604ac7 309 if((int)correlationArray[1] == 1){
TanakaRobo 6:678c6b604ac7 310 correlationValue |= 0xff00;
TanakaRobo 6:678c6b604ac7 311 }
TanakaRobo 6:678c6b604ac7 312 //printf((int)correlationValue);
TanakaRobo 6:678c6b604ac7 313 //printf(separator);
TanakaRobo 6:678c6b604ac7 314 }
TanakaRobo 6:678c6b604ac7 315 // test mode disable
TanakaRobo 6:678c6b604ac7 316 write(0x40,0x00,lidarliteAddress);
TanakaRobo 6:678c6b604ac7 317 } /* LIDARLite::correlationRecordToSerial */