明石高専ロボ研 mbedライブラリ
Dependencies: mbed
lidar_lite.cpp@14:7ea663f79c81, 2022-03-05 (annotated)
- Committer:
- darkumatar
- Date:
- Sat Mar 05 05:00:50 2022 +0000
- Revision:
- 14:7ea663f79c81
- Parent:
- 6:678c6b604ac7
a
Who changed what in which revision?
User | Revision | Line number | New 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 */ |