Median of 3 filter for multiple time of flight sensors translated from arduino by pololu
Dependents: wheelchaircontrol wheelchaircontrol2 wheelchaircontrol3 wheelchaircontrol4 ... more
Diff: VL53L1X.cpp
- Revision:
- 2:ee9bc2e6dffd
- Parent:
- 1:0038ad0a63af
--- a/VL53L1X.cpp Mon Oct 08 23:01:21 2018 +0000 +++ b/VL53L1X.cpp Fri Apr 19 23:03:55 2019 +0000 @@ -44,7 +44,7 @@ { // check model ID and module type registers (values specified in datasheet) int tempRegister = readReg16Bit(IDENTIFICATION__MODEL_ID); - //printf("temporary %x\r\n", tempRegister); + printf("temporary %x\r\n", tempRegister); if (tempRegister != 0xEACC) { return false; } @@ -69,6 +69,7 @@ return false; } } + // VL53L1_poll_for_boot_completion() end // VL53L1_software_reset() end @@ -236,31 +237,33 @@ char data_read[2]; data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address data_write[1] = registerAddr & 0xFF; //LSB of register address + _i2c.write(address, data_write, 2, 0); + _i2c.read(address,data_read,2, 1); + data_high = data_read[0]; //Read Data from selected register + data_low = data_read[1]; //Read Data from selected register + data = (data_high << 8)|data_low; + + return data; +} +// Read a 32-bit register +uint32_t VL53L1X::readReg32Bit(uint16_t registerAddr) +{ + uint8_t data_low; + uint8_t data_high; + uint16_t data; + + char data_write[2]; + char data_read[2]; + data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address + data_write[1] = registerAddr & 0xFF; //LSB of register address _i2c.write(address, data_write, 2,0); _i2c.read(address,data_read,2,1); data_high = data_read[0]; //Read Data from selected register data_low = data_read[1]; //Read Data from selected register data = (data_high << 8)|data_low; - - return data; -} -// Read a 32-bit register -uint32_t VL53L1X::readReg32Bit(uint16_t reg) -{ - uint32_t value; -/* - _i2c.beginTransmission(address); - _i2c.write((reg >> 8) & 0xFF); // reg high byte - _i2c.write( reg & 0xFF); // reg low byte - last_status = _i2c.endTransmission(); - - _i2c.requestFrom(address, (uint8_t)4); - value = (uint32_t)_i2c.read() << 24; // value highest byte - value |= (uint32_t)_i2c.read() << 16; - value |= (uint16_t)_i2c.read() << 8; - value |= _i2c.read(); // value lowest byte -*/ - return value; + + return data; + } // set distance mode to Short, Medium, or Long @@ -836,7 +839,7 @@ didInitialize = false; return false; } - // setDistanceMode(VL53L1X::Short);//Short Medium Long + setDistanceMode(VL53L1X::Long);//Short Medium Long setAddress(addr);//change I2C address for next sensor setMeasurementTimingBudget(timing_budget);//min 20ms for Short, 33ms for Medium and Long startContinuous(50);