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:
- 4:f6f0c2b9120a
- Parent:
- 3:0d2f2f31469b
- Child:
- 5:a75d46c735c7
diff -r 0d2f2f31469b -r f6f0c2b9120a VL53L1X.cpp --- a/VL53L1X.cpp Sun May 05 02:40:10 2019 +0000 +++ b/VL53L1X.cpp Thu Aug 08 16:15:17 2019 +0000 @@ -31,9 +31,9 @@ void VL53L1X::setAddress(uint8_t new_addr) { writeReg(I2C_SLAVE__DEVICE_ADDRESS, new_addr & 0x7F); - wait(.01); - ////printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS)); + wait(.02); address = new_addr << 1; + printf("%x\r\n", readReg(I2C_SLAVE__DEVICE_ADDRESS)); } // Initialize sensor using settings taken mostly from VL53L1_DataInit() and @@ -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; } @@ -59,10 +59,10 @@ startTimeout(); int firmware = (readReg16Bit(FIRMWARE__SYSTEM_STATUS)); - //printf("firmware : %x\r\n", firmware); + printf("firmware : %x\r\n", firmware); while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0) { - //printf("stuck\r\n"); + printf("stuck\r\n"); if (checkTimeoutExpired()) { did_timeout = true; @@ -151,7 +151,7 @@ // default to long range, 50 ms timing budget // note that this is different than what the API defaults to - setDistanceMode(Long); + setDistanceMode(Short); setMeasurementTimingBudget(50000); // VL53L1_StaticInit() end @@ -232,7 +232,7 @@ uint8_t data_high; uint16_t data; - /*char data_write[2]; + 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 @@ -241,7 +241,7 @@ 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