#include "vl53l8cx_api.h" #include "Trajet.h" #include int masque[64]={ 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, 3000,3000,3000,3000,3000,3000,3000,3000, 350,350,350,350,350,350,350,350, 281,286,299,306,310,314,302,286, 195,200,202,206,213,200,200,202 }; float old_min_distance; void VL53L8_init(VL53L8CX_Configuration * Dev){ uint8_t status, isAlive, isReady, i; /*********************************/ /* Customer platform */ /*********************************/ /* Fill the platform structure with customer's implementation. For this * example, only the I2C address is used. */ Dev->platform.address = VL53L8CX_DEFAULT_I2C_ADDRESS; /* (Optional) Reset sensor toggling PINs (see platform, not in API) */ //Reset_Sensor(&(Dev.platform)); /* (Optional) Set a new I2C address if the wanted address is different * from the default one (filled with 0x20 for this example). */ //status = vl53l8cx_set_i2c_address(&Dev, 0x20); /*********************************/ /* Power on sensor and init */ /*********************************/ /* (Optional) Check if there is a VL53L8CX sensor connected */ do{ status = vl53l8cx_is_alive(Dev, &isAlive); if(status){ printf("VL53L8CX not detected at requested address\n"); WaitMs(&(Dev->platform), 1000); } if(!isAlive){ printf("VL53L8CX not alived\n"); WaitMs(&(Dev->platform), 1000); } }while(!isAlive || status); printf("VL53L8CX detected !\n"); /* (Mandatory) Init VL53L8CX sensor */ status = vl53l8cx_init(Dev); if(status) { while(1){ printf("VL53L8CX ULD Loading failed :%d\n", status); WaitMs(&(Dev->platform), 1000); } return; } printf("VL53L8CX ULD ready ! (Version : %s)\n", VL53L8CX_API_REVISION); status = vl53l8cx_set_resolution(Dev, VL53L8CX_RESOLUTION_8X8); if(status !=0){ while(1){ printf("vl53l8cx_set_resolution failed :%d\n", status); WaitMs(&(Dev->platform), 1000); } } status = vl53l8cx_set_ranging_frequency_hz(Dev, 15); if(status !=0){ while(1){ printf("vl53l8cx_set_ranging_frequency_hz (15hz) failed :%d\n", status); WaitMs(&(Dev->platform), 1000); } } //vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST); vl53l8cx_set_target_order(Dev, VL53L8CX_TARGET_ORDER_STRONGEST); /*********************************/ /* Ranging loop */ /*********************************/ status = vl53l8cx_start_ranging(Dev); } void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){ uint8_t error; error = vl53l8cx_get_ranging_data(Dev, Results); if(error){ printf("Error VL53L8\n"); VL53L8_init(Dev); } /* As the sensor is set in 4x4 mode by default, we have a total * of 16 zones to print. For this example, only the data of first zone are * print */ // Filtrage des données for(int row=0; row < 8; row++){ for(int col=0; col < 8; col++){ /*if(Results.target_status[col+ 8*row] == 255 ||// No Target detected Results.target_status[col+ 8*row] == 10 ||// Range valid, but no target detected at previous range Results.target_status[col+ 8*row] == 4 ||// Target consistency failed Results.target_status[col+ 8*row] == 3 ){// Sigma error too high Results.distance_mm[col+ 8*row] = 3000; }*/ // Seul status 100% valid if(Results->target_status[col+ 8*row] != 5){ Results->distance_mm[col+ 8*row] = 3000; } if(Results->distance_mm[col+ 8*row] > 0.9 * masque[col+ 8*row]){ Results->distance_mm[col+ 8*row] = 3000; } if(Results->distance_mm[col+ 8*row] < 0){ printf("Valeur négative %d, status:%d\n", Results->distance_mm[col+ 8*row], Results->target_status[col+ 8*row]); } } } // Affichage des données /* printf(">distance:"); for(int row=0; row < 8; row++){ for(int col=0; col < 8; col++){ printf("%d,", Results->distance_mm[col+ 8*row]); } } printf("\n"); */ } int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ uint16_t mesure1, mesure2; int min_distance = 2000; int col, row; for(col=0; col<8; col++){ for(row=0; row<=3; row++){ if(min_distance > Results.distance_mm[col + 8*row]){ min_distance = Results.distance_mm[col + 8*row]; } } } *distance = 0; if(min_distance-50 > 0){ *distance = min_distance-50; } old_min_distance = *distance; } float VL53L8_get_old_min_distance(){ return old_min_distance; }