Diff: x_nucleo_6180xa1.cpp

Revision:

15:454710d17358

Parent:

14:0effa0bbf192

Child:

16:0d4776564733

--- a/x_nucleo_6180xa1.cpp	Mon Oct 19 13:54:17 2015 +0200
+++ b/x_nucleo_6180xa1.cpp	Tue Oct 20 16:44:28 2015 +0200
@@ -48,16 +48,19 @@
 	if(_instance==NULL)
 	   _instance=new X_NUCLEO_6180XA1(ext_i2c);
 	else
-	   error("Failed to init X_NUCLEO_6180XA1 board!\n");
+	   VL6180x_ErrLog("Failed to init X_NUCLEO_6180XA1 board!\n");
 	return _instance;
 }
 
 
-int X_NUCLEO_6180XA1::InitBoard()
+int X_NUCLEO_6180XA1::InitBoard() //FIXME sistemare le stampe degli errori
 {	
-	int status;
-	//flag per verificare sensori presenti 
-	//sensor_top->VL6180x_Off() x4
+	int status, n_dev=0;
+	
+	sensor_top->VL6180x_Off();
+	sensor_bottom->VL6180x_Off();
+	sensor_left->VL6180x_Off();
+	sensor_right->VL6180x_Off();
 	status=sensor_top->InitSensor(NEW_SENSOR_TOP_ADDRESS);
 	if(status)
 	{
@@ -65,9 +68,59 @@
 		 delete gpio0_top;
 		 sensor_top=NULL;
  		 gpio0_top=NULL;
+		 //VL6180x_ErrLog("Fail to init sensor_top\n\r");
+		 printf("Failure to init sensor_top\n\r");
 	}
-		   
-  //stessa cosa per left, bottom e right		
+	else
+	{
+		 n_dev++;
+	}
+	status=sensor_bottom->InitSensor(NEW_SENSOR_BOTTOM_ADDRESS);
+	if(status)
+	{
+	   delete sensor_bottom;
+		 delete gpio0_bottom;
+		 sensor_bottom=NULL;
+ 		 gpio0_bottom=NULL;
+		 //VL6180x_ErrLog("Fail to init sensor_bottom\n\r");
+		 printf("Failed to init sensor_bottom\n\r");
+	}
+	else
+	{
+		 n_dev++;
+	}
+	status=sensor_left->InitSensor(NEW_SENSOR_LEFT_ADDRESS);
+	if(status)
+	{
+	   delete sensor_left;
+		 delete gpio0_left;
+		 sensor_left=NULL;
+ 		 gpio0_left=NULL;
+		 //VL6180x_ErrLog("Fail to init sensor_left\n\r");
+		 printf("Failed to init sensor_left\n\r");
+	}
+	else
+	{
+		 n_dev++;
+	}
+	status=sensor_right->InitSensor(NEW_SENSOR_RIGHT_ADDRESS);
+	if(status)
+	{
+	   delete sensor_right;
+		 delete gpio0_right;
+		 sensor_right=NULL;
+ 		 gpio0_right=NULL;
+		 //VL6180x_ErrLog("Fail to init sensor_right\n\r");
+		 printf("Failed to init sensor_right\n\r");
+	}
+	else
+	{
+		 n_dev++;
+	}
+	if(n_dev==0)
+		 return 1;
+	else
+		 return 0;
 }