Diff: VL53L0X.cpp

Revision:

12:81f37e50f8f8

Parent:

11:d8dbe3b87f9f

Child:

13:253cb4ea3fcc

--- a/VL53L0X.cpp	Tue Jul 02 12:38:07 2019 +0000
+++ b/VL53L0X.cpp	Mon Jul 08 14:07:22 2019 +0000
@@ -68,6 +68,104 @@
 #include <stdlib.h>
 #include "VL53L0X.h"
 
+void Report_Range_Infos(VL53L0X_RangingMeasurementData_t RangeResults)
+{    
+	printf("\n\r Reporting All  Fields of VL53L0X_RangingMeasurementData_t structure \n\r" ); 
+    printf(" .Range_mm = %dmm; Ranged distance. \n\r",  RangeResults.Range_mm ); 
+    printf(" .RangeDMax_mm = %dmm; maximum detection distance in current setup and environment conditions \n\r",  RangeResults.RangeDMax_mm ); 
+    printf(" .SignalRateRtn_MHz = %3.3fMHz; effectively a measure of target  reflectance \n\r",  RangeResults.SignalRateRtn_MHz / 65535.01); 
+    printf(" .AmbientRateRtn_MHz = %3.3fMHz; effectively a measure of the ambient light \n\r",  RangeResults.AmbientRateRtn_MHz / 65535.01 ); 
+    printf(" .EffectiveSpadRtnCount = %3.3f; effective SPAD count for the return signal \n\r",  RangeResults.EffectiveSpadRtnCount / 256.001 ); 
+    printf(" .RangeFractionalPart = %d; Fractional part of range distance. \n\r",  RangeResults.RangeFractionalPart >> 6 ); 
+    printf(" .RangeStatus = %d[u8];  Status for the current measurement, 0 = value is valid \n\r",  RangeResults.RangeStatus ); 
+    printf(" .SigmaEstimate = %3.2f;  Estimated Sigma - based on ambient & VCSEL rates and signal_total_events \n\r",  RangeResults.SigmaEstimate/ 65535.01 ); 
+};
+
+void Report_Deep_Infos(VL53L0X TOF1)
+{   
+    printf("\n\r Reporting All Top Level Infos of the class \n\r" ); 
+    printf("I2cDevAddr  = %d. \n\r",  TOF1.I2cDevAddr ); 
+    printf("comms_type  = %d. Type of comms: 1=VL53L0X_COMMS_I2C or VL53L0X_COMMS_SPI \n\r",  TOF1.comms_type ); 
+    printf("comms_speed = %d. Communication speed [kHz] : typically 400kHz for I2C   \n\r",  TOF1.comms_speed_khz ); 
+       
+    printf("\n\r Reporting All Infos of the Device_Info structure: \n\r" ); 
+    printf("Device_Info.ProductType = 0x%2X.  VL53L0X = 1, VL53L1 = 2 \n\r",  TOF1.Device_Info.ProductType );     
+    printf("Device_Info.ProductRevision = %d.%d. Revision NR, major.minor \n\r",  
+       TOF1.Device_Info.ProductRevisionMajor, TOF1.Device_Info.ProductRevisionMinor );
+    printf("Device_Info.Name = %s. Name of Device e.g. Left_Distance\n\r",   TOF1.Device_Info.Name );
+    printf("Device_Info.Type = %s. Type of Device e.g VL53L0X \n\r",   TOF1.Device_Info.Type );
+    printf("Device_Info.ProductId = %s. Product Identifier String \n\r",   TOF1.Device_Info.ProductId );
+
+    printf("\n\r Reporting All  Fields of CurrentParameters \n\r" ); 
+    printf(" .DeviceMode            = %d. Defines type of measurement to be done for the next measurement \n\r", 
+       TOF1.CurrentParameters.DeviceMode ); 
+    printf(" .Measure_Time_Budget_us= %dus.  Allowed total time for a single measurement \n\r", 
+       TOF1.CurrentParameters.MeasurementTimingBudget_us ); 
+    printf(" .Measure_Period_ms     = %dms. Time between two consecutive measurements \n\r", 
+       TOF1.CurrentParameters.InterMeasurementPeriod_ms ); 
+    printf(" .XTalk_Compens_En      = %d. Crosstalk compensation enable or not (0, default) \n\r", 
+       TOF1.CurrentParameters.XTalkCompensationEnable ); 
+    printf(" .XTalk_CompRange_mm    = %dmm. CrossTalk compensation range, seems never used \n\r", 
+       TOF1.CurrentParameters.XTalkCompensationRange_mm ); 
+    printf(" .XTalk_CompRate_MHz    = %3.2fMHz. CrossTalk compensation rate . \n\r", 
+       (float) TOF1.CurrentParameters.XTalkCompensationRate_MHz / 65536); 
+    printf(" .RangeOffset_um        = %d. Range offset adjustment (um) last programmed.\n\r", 
+       TOF1.CurrentParameters.RangeOffset_um ); 
+    printf(" .LimitChecks ... = SIGMA_FINAL, SIGNAL_RATE_FINAL, SIGNAL_REF_CLIP, IGNORE_THRESHOLD, SIGNAL_RATE_MSRC, SIGNAL_RATE_PRE.\n\r");
+    printf(" .LimitChecksEnable[x]  = %d %d %d %d %d %d. The Limit Checks enabled or not.\n\r", 
+       TOF1.CurrentParameters.LimitChecksEnable[0],TOF1.CurrentParameters.LimitChecksEnable[1] ,TOF1.CurrentParameters.LimitChecksEnable[2], 
+       TOF1.CurrentParameters.LimitChecksEnable[3],TOF1.CurrentParameters.LimitChecksEnable[4] ,TOF1.CurrentParameters.LimitChecksEnable[5]  ); 
+    printf(" .LimitChecksStatus[x]  = %d %d %d %d %d %d.  Status of checks of last measurement.\n\r", 
+       TOF1.CurrentParameters.LimitChecksStatus[0],TOF1.CurrentParameters.LimitChecksStatus[1] ,TOF1.CurrentParameters.LimitChecksStatus[2], 
+       TOF1.CurrentParameters.LimitChecksStatus[3],TOF1.CurrentParameters.LimitChecksStatus[4] ,TOF1.CurrentParameters.LimitChecksStatus[5]  ); 
+    printf(" .LimitChecksValue[x]   = %d %d %d %d %d %d [FP1616]. The Limit Check values \n\r", 
+       TOF1.CurrentParameters.LimitChecksValue[0],TOF1.CurrentParameters.LimitChecksValue[1] ,TOF1.CurrentParameters.LimitChecksValue[2], 
+       TOF1.CurrentParameters.LimitChecksValue[3],TOF1.CurrentParameters.LimitChecksValue[4] ,TOF1.CurrentParameters.LimitChecksValue[5]  ); 
+    printf(" .WrapAroundCheckEnable = %d. Wrap Around Check enabled or not \n\r", 
+       TOF1.CurrentParameters.WrapAroundCheckEnable ); 
+
+    printf("\n\r Reporting All Fields of VL53L0X_DevData_t Data structure \n\r" ); 
+    printf(" .OscFrequency_MHz          = %3.2fMHz; Frequency used \n\r",  (float) TOF1.Data.OscFrequency_MHz/65536 ); 
+    printf(" .LastEncodedTimeout        = %d[u16]; Last encoded Time out used for timing budget \n\r",  TOF1.Data.LastEncodedTimeout ); 
+    printf(" .Pin0GpioFunctionality     = %d[u8]; functionality of the GPIO: pin0  \n\r",  TOF1.Data.Pin0GpioFunctionality ); 
+    printf(" .FinalRangeTimeout_us      = %d[u32]; Execution time of the final ranging \n\r",  TOF1.Data.FinalRangeTimeout_us ); 
+    printf(" .FinalRangeVcselPulsePeriod= %d[u8]; Vcsel pulse period (pll clocks) for the final range measurement \n\r",  TOF1.Data.FinalRangeVcselPulsePeriod ); 
+    printf(" .PreRangeTimeout_us        = %d[u32]; Execution time of the final range \n\r",  TOF1.Data.PreRangeTimeout_us ); 
+    printf(" .PreRangeVcselPulsePeriod  = %d[u8]; Vcsel pulse period (pll clocks) for the pre-range measurement \n\r",  TOF1.Data.PreRangeVcselPulsePeriod ); 
+    printf(" .ReadDataFromDeviceDone    = %2d;  reads from device has been done (>0) or not. \n\r",  TOF1.Data.ReadDataFromDeviceDone ); 
+    printf(" .ModuleId                  = %X; Module ID \n\r",  TOF1.Data.ModuleId ); 
+    printf(" .Revision                  = %d[u8]; test Revision \n\r",  TOF1.Data.Revision ); 
+    printf(" .ProductId                 = %s[char*]; Product Identifier String  \n\r",  TOF1.Data.ProductId ); 
+    printf(" .ReferenceSpadCount        = %d[u8];  used for ref spad management \n\r",  TOF1.Data.ReferenceSpadCount ); 
+    printf(" .ReferenceSpadType         = %d[u8]; used for ref spad management \n\r",  TOF1.Data.ReferenceSpadType ); 
+    printf(" .RefSpadsInitialised       = %d[u8]; reports if ref spads are initialised. \n\r",  TOF1.Data.RefSpadsInitialised ); 
+    printf(" .PartUIDUpper              = %d[u32];  Unique Part ID Upper \n\r",  TOF1.Data.PartUIDUpper ); 
+    printf(" .PartUIDLower              = %d[u32]; Unique Part ID Lower \n\r",  TOF1.Data.PartUIDLower ); 
+    printf(" .SignalRateMeasFixed400mm  = %3.3f; Peak Signal rate at 400 mm \n\r",  1.0 / 65535.0 * TOF1.Data.SignalRateMeasFixed400mm ); 
+    printf(" .RefSpadEnables[x]         = %X %X %X %X %X %X[hex8]; Reference Spad Enables  \n\r",  
+        TOF1.Data.RefSpadEnables[0], TOF1.Data.RefSpadEnables[1], TOF1.Data.RefSpadEnables[2], 
+        TOF1.Data.RefSpadEnables[3], TOF1.Data.RefSpadEnables[4], TOF1.Data.RefSpadEnables[5] ); 
+    printf(" .RefGoodSpadMap[x]         = %X %X %X %X %X %X[hex8];  Reference Spad Good Spad Map\n\r",
+        TOF1.Data.RefGoodSpadMap[0], TOF1.Data.RefGoodSpadMap[1], TOF1.Data.RefGoodSpadMap[2], 
+        TOF1.Data.RefGoodSpadMap[3], TOF1.Data.RefGoodSpadMap[4], TOF1.Data.RefGoodSpadMap[5] );     
+    printf(" .Part2PartOffsetNVM_um     = %d[i32]; backed up NVM value \n\r",  TOF1.Data.Part2PartOffsetNVM_um ); 
+    printf(" .Part2PartOffsetAdjustNVM_um= %d[i32]; backed up NVM value of additional offset adjustment \n\r",  TOF1.Data.Part2PartOffsetAdjustNVM_um ); 
+    printf(" .SequenceConfig            = %d[u8]; Internal value for the sequence config  \n\r",   TOF1.Data.SequenceConfig ); 
+    printf(" .RangeFractionalEnable     = %d[u8]; Enable/Disable fractional part of range data  \n\r",  TOF1.Data.RangeFractionalEnable); 
+    printf(" .PalState                  = %d[u8]; Current state of the PAL \n\r",  TOF1.Data.PalState ); 
+    printf(" .PowerMode                 = %d[u8]; Current Power Mode; Stdby1/2, Idle1/2 \n\r",  TOF1.Data.PowerMode ); 
+    printf(" .SigmaEstRefArray          = %d[u16]; Reference array sigma value in 1/100th of [mm] \n\r",  TOF1.Data.SigmaEstRefArray ); 
+    printf(" .SigmaEstEffPulseWidth     = %d[u16]; Effective Pulse width for sigma estimate in 1/100th of ns \n\r", TOF1.Data.SigmaEstEffPulseWidth ); 
+    printf(" .SigmaEstEffAmbWidth       = %d. Effective Ambient width for sigma estimate in 1/100th of ns \n\r",  TOF1.Data.SigmaEstEffAmbWidth ); 
+    printf(" .StopVariable              = %d[u8]; StopVariable used during the stop sequence  \n\r",  TOF1.Data.StopVariable ); 
+    printf(" .targetRefRate             = %d. Target Ambient Rate for Ref spad management \n\r",   TOF1.Data.targetRefRate ); 
+    printf(" .LastSignalRef_MHz         = %3.3fMHz;  Latest Signal ref \n\r",  TOF1.Data.LastSignalRef_MHz / 65535.01 ); 
+    printf(" .UseInternalTuningSetting  = %d[u8]; Indicate if we use  Tuning Settings table  \n\r",    TOF1.Data.UseInternalTuningSettings ); 
+    printf(" .LinearityCorrectiveGain   = %d[u8]; Linearity Corrective Gain value in x1000 \n\r",  TOF1.Data.LinearityCorrectiveGain ); 
+    printf(" .DmaxCalRange_mm           = %dmm; Dmax Calibration Range \n\r",  TOF1.Data.DmaxCalRange_mm );               
+    printf(" .DmaxCalSignalRateRtn_MHz  = %3.3fMHz; Dmax Calibration Signal Rate Return \n\r",  TOF1.Data.DmaxCalSignalRateRtn_MHz / 65535.01 ); 
+}
+
 int VL53L0X::read_id(uint8_t *id)
 {   int status = 0;
     uint16_t rl_id = 0;
@@ -209,8 +307,7 @@
     }
     if (status == VL53L0X_ERROR_NONE) {
         status = VL53L0X_set_limit_check_value(VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
-                                               (FixPoint1616_t)(25 * 65536 / 100));
-        /* 0.25 * 65536 */
+                                               (FixPoint1616_t)(25 * 65536 / 100)); /* 0.25 * 65536 */
     }
 
     if (status == VL53L0X_ERROR_NONE) {
@@ -1892,7 +1989,8 @@
 
     loop_nb = 0;
 
-    do {status = VL53L0X_get_measurement_data_ready( &new_data_ready);
+    do {
+        status = VL53L0X_get_measurement_data_ready( &new_data_ready);
         if (status != 0) {
             break; /* the error is set */
         }
@@ -2811,8 +2909,7 @@
 }
 
 VL53L0X_Error VL53L0X::VL53L0X_calc_sigma_estimate(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
-        FixPoint1616_t *p_sigma_estimate,
-        uint32_t *p_dmax_mm)
+        FixPoint1616_t *p_sigma_estimate,  uint32_t *p_dmax_mm)
 {   /* Expressed in 100ths of a ns, i.e. centi-ns */
     const uint32_t c_pulse_effective_width_centi_ns   = 800;
     /* Expressed in 100ths of a ns, i.e. centi-ns */
@@ -2875,11 +2972,9 @@
      * confines of the 32 Fix1616 type.
      */
 
-    ambient_rate_kcps =
-        (p_ranging_measurement_data->AmbientRateRtn_MHz * 1000) >> 16;
-
-    corrected_signal_rate_MHz =
-        p_ranging_measurement_data->SignalRateRtn_MHz;
+    ambient_rate_kcps =  (p_ranging_measurement_data->AmbientRateRtn_MHz * 1000) >> 16;
+
+    corrected_signal_rate_MHz = p_ranging_measurement_data->SignalRateRtn_MHz;
 
     status = VL53L0X_get_total_signal_rate(p_ranging_measurement_data, &total_signal_rate_MHz);
     status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &x_talk_comp_rate_MHz);
@@ -2934,19 +3029,14 @@
         total_signal_rate_MHz <<= 8;
     }
 
-    if (status != VL53L0X_ERROR_NONE) {
-        
-        return status;
-    }
+    if (status != VL53L0X_ERROR_NONE) { return status; }
 
     if (peak_signal_rate_kcps == 0) {
         *p_sigma_estimate = c_sigma_est_max;
-        Data.SigmaEstimate = c_sigma_est_max;
+        p_ranging_measurement_data->SigmaEstimate = c_sigma_est_max;
         *p_dmax_mm = 0;
     } else {
-        if (vcsel_total_events_rtn < 1) {
-            vcsel_total_events_rtn = 1;
-        }
+        if (vcsel_total_events_rtn < 1) { vcsel_total_events_rtn = 1; }
 
         sigma_estimate_p1 = c_pulse_effective_width_centi_ns;
 
@@ -2962,29 +3052,22 @@
         sigma_estimate_p3 = 2 * VL53L0X_isqrt(vcsel_total_events_rtn * 12);
 
         /* uint32 * FixPoint1616 = FixPoint1616 */
-        delta_t_ps = p_ranging_measurement_data->Range_mm *
-                     c_tof_per_mm_ps;
-
-        /*
-         * vcselRate - xtalkCompRate
+        delta_t_ps = p_ranging_measurement_data->Range_mm *  c_tof_per_mm_ps;
+
+        /* vcselRate - xtalkCompRate
          * (uint32 << 16) - FixPoint1616 = FixPoint1616.
          * Divide result by 1000 to convert to MHz.
-         * 500 is added to ensure rounding when integer division
-         * truncates.
-         */
+         * 500 is added to ensure rounding when integer division truncates. */
         diff1_MHz = (((peak_signal_rate_kcps << 16) -
                        2 * x_talk_comp_rate_kcps) + 500) / 1000;
 
         /* vcselRate + xtalkCompRate */
         diff2_MHz = ((peak_signal_rate_kcps << 16) + 500) / 1000;
 
-        /* Shift by 8 bits to increase resolution prior to the
-         * division */
+        /* Shift by 8 bits to increase resolution prior to the division */
         diff1_MHz <<= 8;
 
         /* FixPoint0824/FixPoint1616 = FixPoint2408 */
-//		xTalkCorrection	 = abs(diff1_MHz/diff2_MHz);
-// abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
         x_talk_correction	 = diff1_MHz / diff2_MHz;
 
         /* FixPoint2408 << 8 = FixPoint1616 */
@@ -2996,11 +3079,8 @@
             /* FixPoint1616/uint32 = FixPoint1616 */
             pw_mult = delta_t_ps / c_vcsel_pulse_width_ps; /* smaller than 1.0f */
 
-            /*
-             * FixPoint1616 * FixPoint1616 = FixPoint3232, however both
-             * values are small enough such that32 bits will not be
-             * exceeded.
-             */
+            /* FixPoint1616 * FixPoint1616 = FixPoint3232, however both
+             * values are small enough such that32 bits will not be exceeded. */
             pw_mult *= ((1 << 16) - x_talk_correction);
 
             /* (FixPoint3232 >> 16) = FixPoint1616 */
@@ -3009,11 +3089,9 @@
             /* FixPoint1616 + FixPoint1616 = FixPoint1616 */
             pw_mult += (1 << 16);
 
-            /*
-             * At this point the value will be 1.xx, therefore if we square
+            /* At this point the value will be 1.xx, therefore if we square
              * the value this will exceed 32 bits. To address this perform
-             * a single shift to the right before the multiplication.
-             */
+             * a single shift to the right before the multiplication. */
             pw_mult >>= 1;
             /* FixPoint1715 * FixPoint1715 = FixPoint3430 */
             pw_mult = pw_mult * pw_mult;
@@ -3048,8 +3126,7 @@
         /* (FixPoint3200 << 16) = FixPoint1616 */
         sqrt_result_centi_ns <<= 16;
 
-        /*
-         * Note that the Speed Of Light is expressed in um per 1E-10
+        /* Note that the Speed Of Light is expressed in um per 1E-10
          * seconds (2997) Therefore to get mm/ns we have to divide by
          * 10000
          */
@@ -3102,7 +3179,7 @@
         }
 
         *p_sigma_estimate = (uint32_t)(sigma_estimate);
-        Data.SigmaEstimate = *p_sigma_estimate;
+        p_ranging_measurement_data->SigmaEstimate = *p_sigma_estimate;
         status = VL53L0X_calc_dmax(total_signal_rate_MHz,
                      corrected_signal_rate_MHz,
                      pw_mult,
@@ -3161,42 +3238,29 @@
         none_flag = 0;
     }
 
-    /*
-     * Check if Sigma limit is enabled, if yes then do comparison with limit
-     * value and put the result back into pPalRangeStatus.
-     */
+    /* Check if Sigma limit is enabled, if yes then do comparison with limit
+     * value and put the result back into pPalRangeStatus.  */
     if (status == VL53L0X_ERROR_NONE) {
         status =  VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
                   &sigma_limit_check_enable);
     }
 
     if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
-        /*
-        * compute the Sigma and check with limit
-        */
-        status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data,
-                     &sigma_estimate,
-                     &dmax_mm);
-        if (status == VL53L0X_ERROR_NONE) {
-            p_ranging_measurement_data->RangeDMax_mm = dmax_mm;
-        }
+        /*  compute the Sigma and check with limit */
+        status = VL53L0X_calc_sigma_estimate(p_ranging_measurement_data, &sigma_estimate, &dmax_mm);
+        if (status == VL53L0X_ERROR_NONE) {  p_ranging_measurement_data->RangeDMax_mm = dmax_mm;  }
 
         if (status == VL53L0X_ERROR_NONE) {
             status = VL53L0X_get_limit_check_value(VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
                                                    &sigma_limit_value);
 
-            if ((sigma_limit_value > 0) &&
-                    (sigma_estimate > sigma_limit_value)) {
-                /* Limit Fail */
-                sigma_limitflag = 1;
-            }
+            if ((sigma_limit_value > 0) && (sigma_estimate > sigma_limit_value)) 
+               {  sigma_limitflag = 1; } /* Limit Fail */
         }
     }
 
-    /*
-     * Check if Signal ref clip limit is enabled, if yes then do comparison
-     * with limit value and put the result back into pPalRangeStatus.
-     */
+    /* Check if Signal ref clip limit is enabled, if yes then do comparison
+     * with limit value and put the result back into pPalRangeStatus.  */
     if (status == VL53L0X_ERROR_NONE) {
         status =  VL53L0X_get_limit_check_enable(VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
                   &signal_ref_clip_limit_check_enable);