PAMI_2024/VL53L8_2024.c

#include "vl53l8cx_api.h"
#include "Trajet.h"
#include <stdio.h>

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
};

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=4; row<=6; 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;
	}
	
	
}