Diff: main.cpp

Revision:

22:a16cc1505e61

Parent:

13:2cf61951ea62

--- a/main.cpp	Thu Jun 24 12:52:18 2021 +0000
+++ b/main.cpp	Thu Jul 22 11:44:20 2021 +0200
@@ -60,7 +60,6 @@
 
 #if (MBED_VERSION  > 60300)
 UnbufferedSerial  pc(USBTX, USBRX);
-extern "C" void wait_ms(int ms);
 #else
 Serial pc(SERIAL_TX, SERIAL_RX);
 #endif
@@ -87,25 +86,28 @@
 }
 
 /* Start the sensor ranging */
-int init_sensor()
+int configure_sensor()
 {
     int status = 0;
-
-    Dev=&devCentre;
-    Sensor=board->sensor_centre;
+    VL53L1_DeviceInfo_t device_info;
+    VL53L1_RoiConfig_t RoiConfig;
 
-    // configure the sensors
-    Dev->comms_speed_khz = 400;
-    Dev->comms_type = 1;
-    Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+    Dev = &devCentre;
+    Sensor = board->sensor_centre;
+
+    if (Sensor == NULL)
+        return -1;
 
     printf("configuring centre channel \n");
 
-    /* Device Initialization and setting */
-    status = Sensor->VL53L1CB_DataInit();
-    status = Sensor->VL53L1CB_StaticInit();
-
-    VL53L1_RoiConfig_t RoiConfig;
+    status = Sensor->VL53L1CB_GetDeviceInfo(&device_info);
+    if (status != 0) {
+        return status;
+    }
+    printf("device name %s \n",device_info.Name);
+    printf("device type %s \n",device_info.Type);
+    printf("device productID %s \n",device_info.ProductId);
+    printf("device productType %x \n",device_info.ProductType);
 
     RoiConfig.NumberOfRoi = 3;
 
@@ -125,34 +127,25 @@
     RoiConfig.UserRois[2].BotRightY = 6;
 
     status = Sensor->VL53L1CB_SetPresetMode(VL53L1_PRESETMODE_MULTIZONES_SCANNING);
+    if (status != 0) {
+        return status;
+    }
+
     status = Sensor->VL53L1CB_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
+    if (status != 0) {
+        return status;
+    }
 
     status = Sensor->VL53L1CB_SetROI(&RoiConfig);
-
-    status = Sensor->VL53L1CB_SetMeasurementTimingBudgetMicroSeconds(17000);
-	
-	
-	VL53L1_DeviceInfo_t  device_info;
-		
-	status = Sensor->VL53L1CB_GetDeviceInfo(&device_info);
-	printf("device name %s \n",device_info.Name);
-	printf("device type %s \n",device_info.Type);
-    printf("device productID %s \n",device_info.ProductId);
-	printf("device productType %x \n",device_info.ProductType);
-
+    if (status != 0) {
+        return status;
+    }
 
-    // create interrupt handler and start measurements
-    if (board->sensor_centre!= NULL) {
-        status = board->sensor_centre->stop_measurement();
-        if (status != 0) {
-            return status;
-        }
+    status = Sensor->VL53L1CB_SetMeasurementTimingBudgetMicroSeconds(60000);
+    if (status != 0) {
+        return status;
+    }
 
-        status = Sensor->VL53L1CB_StartMeasurement();
-        if (status != 0) {
-            return status;
-        }
-    }
     return status;
 }
 
@@ -167,7 +160,6 @@
 int main()
 {
     int status;
-    uint16_t distance = 0;
 
     pc.baud(115200);  // baud rate is important as printf statements take a lot of time
 
@@ -176,7 +168,7 @@
 // create i2c interface
     ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
     /* creates the 53L1A2 expansion board singleton obj */
-    board = XNucleo53L1A2::instance(dev_I2C, CentreIntPin, LeftIntPin, RightIntPin);    
+    board = XNucleo53L1A2::instance(dev_I2C, CentreIntPin, LeftIntPin, RightIntPin);
 
     printf("board created!\r\n");
 
@@ -190,26 +182,31 @@
     printf("board initiated! - %d\r\n", status);
 
     /* init an array with chars to id the sensors */
-    status = init_sensor();
+    status = configure_sensor();
     if (status != 0) {
         printf("Failed to init sensors!\r\n");
         return status;
     }
 
+    // start measurements
+    status = board->sensor_centre->VL53L1CB_StartMeasurement();
+    if (status != 0) {
+        return status;
+    }
+
     printf("loop forever\n");
 
     VL53L1_MultiRangingData_t MultiRangingData;
-    VL53L1_MultiRangingData_t *pMultiRangingData = NULL;
+    VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;
 
     while (true) {
-        pMultiRangingData = &MultiRangingData;
 
         status = board->sensor_centre->VL53L1CB_WaitMeasurementDataReady();
         status = board->sensor_centre->VL53L1CB_GetMultiRangingData( pMultiRangingData);
 
         print_results( devCentre.i2c_slave_address, pMultiRangingData );
 
-        status = board->sensor_centre->VL53L1CB_ClearInterrupt();
+        status = board->sensor_centre->VL53L1CB_ClearInterruptAndStartMeasurement();
     }
 
     printf("Terminating.\n");
@@ -220,41 +217,25 @@
 void print_results( int devNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
 {
     int no_of_object_found = pMultiRangingData->NumberOfObjectsFound;
-    int signal_rate = 0;
-    int ambient_rate = 0;
+    int signal_kcps = 0;
+    int ambient_kcps = 0;
 
     int RoiNumber = pMultiRangingData->RoiNumber;
 
     if (no_of_object_found <= 1)
         no_of_object_found = 1;
+    printf("i2cAddr=%d\tRoiNumber=%d",  devNumber, RoiNumber);
     for(int j=0; j<no_of_object_found; j++) {
-            signal_rate = pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535;
-            ambient_rate = pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535;
-            printf("\t i2cAddr=%d \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%d Mcps, \t Ambient=%d Mcps \n",
-                   devNumber, RoiNumber,
-                   pMultiRangingData->RangeData[j].RangeStatus,
-                   pMultiRangingData->RangeData[j].RangeMilliMeter,
-                   signal_rate,
-                   ambient_rate);
-/*
-// online compiler disables printf() / floating-point support, for code-size reasons.                        
-// offline compiler can switch it on.
-            printf("\t i2cAddr=%d \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
-                   devNumber, RoiNumber,
-                   pMultiRangingData->RangeData[j].RangeStatus,
-                   pMultiRangingData->RangeData[j].RangeMilliMeter,
-                   pMultiRangingData->RangeData[j].SignalRateRtnMegaCps / 65535.0,
-                   pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps / 65535.0);
-*/                       
+        signal_kcps = 1000*(pMultiRangingData->RangeData[j].SignalRateRtnMegaCps) / 65536;
+        ambient_kcps = 1000*(pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps) / 65536;
+        if (j > 0)
+            printf("\t\t\t");
+        printf("\trange[%d] status=%d, \t D=%5dmm, \t Signal=%d Kcps, \t Ambient=%d Kcps \n",
+               j,
+               pMultiRangingData->RangeData[j].RangeStatus,
+               pMultiRangingData->RangeData[j].RangeMilliMeter,
+               signal_kcps,
+               ambient_kcps);
     }
 }
 
-
-#if (MBED_VERSION  > 60300)
-extern "C" void wait_ms(int ms)
-{
-    thread_sleep_for(ms);
-}
-#endif
-
-