Diff: x_nucleo_6180xa1.cpp

Revision:

54:17f3bb228b13

Parent:

48:e799ad44dab7

--- a/x_nucleo_6180xa1.cpp	Tue Sep 27 12:18:09 2016 +0000
+++ b/x_nucleo_6180xa1.cpp	Wed Nov 30 08:25:04 2016 +0000
@@ -68,15 +68,21 @@
 {	
    int status, n_dev=0; uint8_t sensor_address;
 	
-     sensor_top->VL6180x_Off();
-     sensor_bottom->VL6180x_Off();
-     sensor_left->VL6180x_Off();
-     sensor_right->VL6180x_Off();
+     if (sensor_top)    sensor_top->VL6180x_Off();
+     if (sensor_bottom) sensor_bottom->VL6180x_Off();
+     if (sensor_left)   sensor_left->VL6180x_Off();
+     if (sensor_right)  sensor_right->VL6180x_Off();
      sensor_address = NEW_SENSOR_TOP_ADDRESS;     
      status=sensor_top->Init(&sensor_address);
      if(status)
      {
-        printf("Sensor top not present\n\r");
+        printf("Error: Mandatory top sensor fail, Init failed!\n\r");
+        if (sensor_top !=NULL)   { delete sensor_top; sensor_top=NULL; }
+        if (sensor_left !=NULL)  {delete sensor_left; sensor_left=NULL; }        
+        if (sensor_bottom !=NULL){ delete sensor_bottom; sensor_bottom=NULL; }
+        if (sensor_right !=NULL) { delete sensor_right; sensor_right=NULL; }
+        n_dev=0;
+        return 1;
      }
      else
      {
@@ -84,10 +90,12 @@
         n_dev++;
      }
      sensor_address = NEW_SENSOR_BOTTOM_ADDRESS;
-     status=sensor_bottom->Init(&sensor_address);
+     status=1;
+     if (sensor_bottom) status=sensor_bottom->Init(&sensor_address);
      if(status)
      {
         printf("Sensor bottom not present\n\r");
+        if (sensor_bottom !=NULL) { delete sensor_bottom; sensor_bottom=NULL; }
      }
      else
      {
@@ -95,10 +103,12 @@
         n_dev++;
      }
      sensor_address = NEW_SENSOR_LEFT_ADDRESS;
-     status=sensor_left->Init(&sensor_address);
+     status=1;
+     if (sensor_left) status=sensor_left->Init(&sensor_address);
      if(status)
      {
         printf("Sensor left not present\n\r");
+        if (sensor_left !=NULL) {delete sensor_left; sensor_left=NULL; }                
      }
      else
      {
@@ -106,10 +116,12 @@
         n_dev++;
      }
      sensor_address = NEW_SENSOR_RIGHT_ADDRESS;
-     status=sensor_right->Init(&sensor_address);
+     status=1;
+     if (sensor_right) status=sensor_right->Init(&sensor_address);
      if(status)
      {
         printf("Sensor right not present\n\r");
+        if (sensor_right !=NULL) { delete sensor_right; sensor_right=NULL; }        
      }
      else
      {