Suitable for Lidar Lite V2
Dependencies: mbed
Fork of LidarLite by
Diff: LidarLite.cpp
- Revision:
- 1:b8d3e9e59703
- Parent:
- 0:8e6304ab38d2
--- a/LidarLite.cpp Tue Feb 17 12:01:04 2015 +0000 +++ b/LidarLite.cpp Thu Jun 16 10:26:33 2016 +0000 @@ -12,9 +12,15 @@ LidarLite::LidarLite(PinName sda, PinName scl) { i2c_ = new I2C(sda, scl); - i2c_->frequency(100000); //I2C @ 100kHz + i2c_->frequency(400000); //I2C @ 100kHz wait(0.5); } +LidarLite::LidarLite(PinName sda, PinName scl, char currentAddress) +{ + i2c_ = new I2C(sda, scl); + i2c_->frequency(400000); //I2C @ 100kHz + wait(0.5); +} int16_t LidarLite::getRange_cm() { @@ -29,12 +35,48 @@ return((velocity_LL-256)*10); } +void LidarLite::refreshRangeAdd(char currentAddres) +{ + uint8_t nackack; + + char write[2]={SET_CommandReg, AcqMode};//00,0x04 + char read_dist[1]={GET_Distance2BReg};//0x8f + char read_vel[1]={GET_VelocityReg}; + + char dist[2]; + char vel[1]; + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + //nackack = i2c_->write(LIDARLite_WriteAdr, write, 2); + nackack = i2c_->write((currentAddres<<1), write, 2); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); +// nackack = i2c_->write(LIDARLite_WriteAdr, read_dist, 1); + nackack = i2c_->write((currentAddres<<1), read_dist, 1); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->read(((currentAddres<<1)|0X01), dist, 2); +// nackack = i2c_->read(LIDARLite_ReadAdr, dist, 2); + } + distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1]; +} void LidarLite::refreshRange() { uint8_t nackack; - char write[2]={SET_CommandReg, AcqMode}; - char read_dist[1]={GET_Distance2BReg}; + char write[2]={SET_CommandReg, AcqMode};//00,0x04 + char read_dist[1]={GET_Distance2BReg};//0x8f char read_vel[1]={GET_VelocityReg}; char dist[2]; @@ -62,7 +104,6 @@ } distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1]; } - void LidarLite::refreshVelocity() { uint8_t nackack; @@ -96,6 +137,41 @@ } velocity_LL = (uint16_t)vel[0]; } +//For V2 sensor + +void LidarLite::refreshVelocityAdd(char currentAddres) +{ + uint8_t nackack; + + char write[2]={SET_CommandReg, AcqMode}; + char read_dist[1]={GET_Distance2BReg}; + char read_vel[1]={GET_VelocityReg}; + + char dist[2]; + char vel[1]; + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->write((currentAddres<<1), write, 2); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->write((currentAddres<<1), read_vel, 1); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1); + } + velocity_LL = (uint16_t)vel[0]; +} void LidarLite::refreshRangeVelocity() { @@ -145,4 +221,114 @@ nackack = i2c_->read(LIDARLite_ReadAdr, vel, 1); } velocity_LL = (uint16_t)vel[0]; +} + +//For V2 +void LidarLite::refreshRangeVelocityAdd(char currentAddres) +{ + uint8_t nackack; + + char write[2]={SET_CommandReg, AcqMode}; + char read_dist[1]={GET_Distance2BReg}; + char read_vel[1]={GET_VelocityReg}; + + char dist[2]; + char vel[1]; + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->write((currentAddres<<1), write, 2); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->write((currentAddres<<1), read_dist, 1); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->read(((currentAddres<<1)|0x01), dist, 2); + } + distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1]; + + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->write((currentAddres<<1), read_vel, 1); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1); + } + velocity_LL = (uint16_t)vel[0]; +} + +void LidarLite::changeAddress(char currentAddres,char newAddress) +{ + + char read_vel[1]={0x96}; + char write[2]={0x1a,}; + char write_sno[2]={0x18,}; + char write_sno1[2]={0x19,}; + char write_D[2]={0x1e,0x08}; //disable Primary Address + write[1]= newAddress; + + char s_no[2]; + uint8_t nackack=1; + + while(nackack !=0) + { + wait_ms(1); + nackack = i2c_->read(((currentAddres<<1)|0x01), s_no, 2); + } + + + write_sno[1]=s_no[0]; + write_sno1[1]=s_no[0]; + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + i2c_->write((currentAddres<<1), write_sno, 2);//write 0x18 + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + i2c_->write((currentAddres<<1), write_sno1, 2);//write 0x18 + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + i2c_->write((currentAddres<<1), write, 2); + } + + nackack=1; + while(nackack !=0) + { + wait_ms(1); + i2c_->write((currentAddres<<1), write, 2); + } + nackack=1; + while(nackack !=0) + { + wait_ms(1); + i2c_->write((currentAddres<<1), write_D, 2); + } + } \ No newline at end of file