collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
64c6e28
tof
1#include <pigpio.h> 2#include <stdio.h> 3#include "I2Cdev.h" 4 5const short __VL6180X_IDENTIFICATION_MODEL_ID = 0x0000 ; 6const short __VL6180X_IDENTIFICATION_MODEL_REV_MAJOR = 0x0001 ; 7const short __VL6180X_IDENTIFICATION_MODEL_REV_MINOR = 0x0002 ; 8const short __VL6180X_IDENTIFICATION_MODULE_REV_MAJOR = 0x0003 ; 9const short __VL6180X_IDENTIFICATION_MODULE_REV_MINOR = 0x0004 ; 10const short __VL6180X_IDENTIFICATION_DATE = 0x0006 ; 11const short __VL6180X_IDENTIFICATION_TIME = 0x0008 ; 12 13const short __VL6180X_SYSTEM_MODE_GPIO0 = 0x0010 ; 14const short __VL6180X_SYSTEM_MODE_GPIO1 = 0x0011 ; 15const short __VL6180X_SYSTEM_HISTORY_CTRL = 0x0012 ; 16const short __VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0014 ; 17const short __VL6180X_SYSTEM_INTERRUPT_CLEAR = 0x0015 ; 18const short __VL6180X_SYSTEM_FRESH_OUT_OF_RESET = 0x0016 ; 19const short __VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD = 0x0017 ; 20 21const short __VL6180X_SYSRANGE_START = 0x0018 ; 22const short __VL6180X_SYSRANGE_THRESH_HIGH = 0x0019 ; 23const short __VL6180X_SYSRANGE_THRESH_LOW = 0x001A ; 24const short __VL6180X_SYSRANGE_INTERMEASUREMENT_PERIOD = 0x001B ; 25const short __VL6180X_SYSRANGE_MAX_CONVERGENCE_TIME = 0x001C ; 26const short __VL6180X_SYSRANGE_CROSSTALK_COMPENSATION_RATE = 0x001E ; 27const short __VL6180X_SYSRANGE_CROSSTALK_VALID_HEIGHT = 0x0021 ; 28const short __VL6180X_SYSRANGE_EARLY_CONVERGENCE_ESTIMATE = 0x0022 ; 29const short __VL6180X_SYSRANGE_PART_TO_PART_RANGE_OFFSET = 0x0024 ; 30const short __VL6180X_SYSRANGE_RANGE_IGNORE_VALID_HEIGHT = 0x0025 ; 31const short __VL6180X_SYSRANGE_RANGE_IGNORE_THRESHOLD = 0x0026 ; 32const short __VL6180X_SYSRANGE_MAX_AMBIENT_LEVEL_MULT = 0x002C ; 33const short __VL6180X_SYSRANGE_RANGE_CHECK_ENABLES = 0x002D ; 34const short __VL6180X_SYSRANGE_VHV_RECALIBRATE = 0x002E ; 35const short __VL6180X_SYSRANGE_VHV_REPEAT_RATE = 0x0031 ; 36 37const short __VL6180X_SYSALS_START = 0x0038 ; 38const short __VL6180X_SYSALS_THRESH_HIGH = 0x003A ; 39const short __VL6180X_SYSALS_THRESH_LOW = 0x003C ; 40const short __VL6180X_SYSALS_INTERMEASUREMENT_PERIOD = 0x003E ; 41const short __VL6180X_SYSALS_ANALOGUE_GAIN = 0x003F ; 42const short __VL6180X_SYSALS_INTEGRATION_PERIOD = 0x0040 ; 43 44const short __VL6180X_RESULT_RANGE_STATUS = 0x004D ; 45const short __VL6180X_RESULT_ALS_STATUS = 0x004E ; 46const short __VL6180X_RESULT_INTERRUPT_STATUS_GPIO = 0x004F ; 47const short __VL6180X_RESULT_ALS_VAL = 0x0050 ; 48const short __VL6180X_RESULT_HISTORY_BUFFER = 0x0052 ; 49const short __VL6180X_RESULT_RANGE_VAL = 0x0062 ; 50const short __VL6180X_RESULT_RANGE_RAW = 0x0064 ; 51const short __VL6180X_RESULT_RANGE_RETURN_RATE = 0x0066 ; 52const short __VL6180X_RESULT_RANGE_REFERENCE_RATE = 0x0068 ; 53const short __VL6180X_RESULT_RANGE_RETURN_SIGNAL_COUNT = 0x006C ; 54const short __VL6180X_RESULT_RANGE_REFERENCE_SIGNAL_COUNT = 0x0070 ; 55const short __VL6180X_RESULT_RANGE_RETURN_AMB_COUNT = 0x0074 ; 56const short __VL6180X_RESULT_RANGE_REFERENCE_AMB_COUNT = 0x0078 ; 57const short __VL6180X_RESULT_RANGE_RETURN_CONV_TIME = 0x007C ; 58const short __VL6180X_RESULT_RANGE_REFERENCE_CONV_TIME = 0x0080 ; 59 60const short __VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD = 0x010A ; 61const short __VL6180X_FIRMWARE_BOOTUP = 0x0119 ; 62const short __VL6180X_FIRMWARE_RESULT_SCALER = 0x0120 ; 63const short __VL6180X_I2C_SLAVE_DEVICE_ADDRESS = 0x0212 ; 64const short __VL6180X_INTERLEAVED_MODE_ENABLE = 0x02A3 ; 65 66int enablePins [4 ]= {12 ,23 ,24 ,24 }; 67int sensors [4 ]= {0x29 ,0x2a ,0x2b ,0x2c }; 68 69// first list is addresses, second is values 70int initPairs [2 ][29 ]= { 71 { 720x208 ,0x96 ,0x97 ,0xe3 ,0xe4 ,0xe5 ,0xe6 ,0xe7 ,0xf5 ,0xd9 , 730xdb ,0xdc ,0xdd ,0x9f ,0xa3 ,0xb7 ,0xbb ,0xb2 ,0xca ,0x198 , 740x1b0 ,0x1ad ,0xff ,0x100 ,0x199 ,0x1a6 ,0x1ac ,0x1a7 ,0x30 75 }, 76 { 770x01 ,0x00 ,0xfd ,0x00 ,0x04 ,0x02 ,0x01 ,0x03 ,0x02 ,0x05 , 780xce ,0x03 ,0xf8 ,0x00 ,0x3c ,0x00 ,0x3c ,0x09 ,0x09 ,0x01 , 790x17 ,0x00 ,0x05 ,0x05 ,0x05 ,0x1b ,0x3e ,0x1f ,0x00 80 } 81}; 82 83int WriteByte (unsigned addr ,uint16_t reg ,uint8_t data ) { 84//printf("register: %i, data: %i\n", reg, data); 85return I2Cdev ::writeBytes (addr ,reg ,1 ,& data ); 86 87} 88 89char ReadByte (unsigned addr ,uint16_t reg ) { 90uint8_t data_read ; 91I2Cdev ::readBytes (addr ,reg ,1 ,& data_read ); 92return data_read ; 93} 94 95//reinitialize all distance sensors to address 0x29 96void resetAddr () { 97for (int i = 0 ;i < 4 ;++ i ) { 98gpioWrite (enablePins [i ],1 ); 99 } 100for (int i = 0 ;i < 4 ;++ i ) { 101WriteByte (sensors [i ],__VL6180X_I2C_SLAVE_DEVICE_ADDRESS ,0x29 ); 102 } 103} 104 105void initSensor (sensor ) { 106//enable one sensor at a time 107printf ("Sensor %d enabling\n" ,sensor ); 108gpioWrite (enablePins [sensor ],1 ); 109//i2cReadByteData(sensor, __VL6180X_SYSTEM_FRESH_OUT_OF_RESET); 110// proprietary setup stuff 111for (int j = 0 ;j < 29 ;++ j ) { 112WriteByte (0x29 ,initPairs [0 ][sensor ],initPairs [1 ][sensor ]); 113 } 114// recommended setup stuff 115// WriteByte(sensor, __VL6180X_SYSTEM_MODE_GPIO1, 0x10); 116WriteByte (0x29 ,__VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD ,0x30 ); 117WriteByte (0x29 ,__VL6180X_SYSALS_ANALOGUE_GAIN ,0x46 ); 118WriteByte (0x29 ,__VL6180X_SYSRANGE_VHV_REPEAT_RATE ,0xff ); 119WriteByte (0x29 ,__VL6180X_SYSALS_INTEGRATION_PERIOD ,0x63 ); 120WriteByte (0x29 ,__VL6180X_SYSRANGE_VHV_RECALIBRATE ,0x01 ); 121// optional settings todo: add more settings 122WriteByte (0x29 ,0x001b ,0x09 ); 123WriteByte (0x29 ,0x003e ,0x31 ); 124WriteByte (0x29 ,0x0014 ,0x24 ); 125// assign new register 126WriteByte (0x29 ,__VL6180X_I2C_SLAVE_DEVICE_ADDRESS ,sensors [sensor ]); 127printf ("finished sensor %d\n" ,i ); 128} 129 130int main () { 131gpioInitialise (); 132I2Cdev ::initialize (); 133I2Cdev ::enable (true); 134resetAddr (); 135//must open address that tof is currently on 136for (int i = 0 ;i < 4 ;++ i ) { 137initSensor (i ); 138 } 139while (1 ) { 140for (int i = 0 ;i < 4 ;++ i ) { 141// wait until ready 142//while(!ReadByte(sensor1, 0x004d) & (1<<0)); 143// start measurment 1 144WriteByte (sensors [i ],0x0018 ,0x01 ); 145//while(!ReadByte(sensor1, 0x004f) & (1<<2)); 146int reading = ReadByte (sensors [i ],0x0062 ); 147printf ("%04dmm\t" ,reading ); 148 } 149printf ("\n" ); 150 } 151return 0 ; 152} 153