Tarek Lule
Greatly simplified Architecture, Identical Functions Removed: Platform Interfaces, STP6001 interface
Diff: VL53L0X.cpp
- Revision:
- 9:cb4c6d4e5030
- Parent:
- 8:2fd7cb217068
- Child:
- 10:cd1758e186a4
--- a/VL53L0X.cpp Thu Jun 20 15:19:03 2019 +0000
+++ b/VL53L0X.cpp Tue Jun 25 11:03:34 2019 +0000
@@ -2230,7 +2230,7 @@
}
VL53L0X_Error VL53L0X::set_ref_spad_map( uint8_t *p_ref_spad_array)
-{ VL53L0X_Error status = VL53L0X_write_multi(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
+{ VL53L0X_Error status = VL53L0X_i2c_write(VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
p_ref_spad_array, 6);
return status;
@@ -2392,7 +2392,7 @@
uint8_t msb;
uint8_t lsb;
uint8_t select_param;
- uint8_t number_of_writes;
+ uint16_t number_of_writes;
uint8_t address;
uint8_t local_buffer[4]; /* max */
uint16_t temp16;
@@ -2454,7 +2454,7 @@
index++;
}
- status = VL53L0X_write_multi( address, local_buffer, number_of_writes);
+ status = VL53L0X_i2c_write( address, local_buffer, number_of_writes);
} else { status = VL53L0X_ERROR_INVALID_PARAMS; }
}
@@ -2534,17 +2534,14 @@
if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
status = VL53L0X_ERROR_TIME_OUT;
}
-
}
break;
- case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
- /* Back-to-back mode */
+ case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING: /* Back-to-back mode */
/* Check if need to apply interrupt settings */
- if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_check_and_load_interrupt_settings( 1);
- }
+ if (status == VL53L0X_ERROR_NONE)
+ { status = VL53L0X_check_and_load_interrupt_settings( 1); }
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
@@ -2563,40 +2560,32 @@
status = VL53L0X_write_byte(VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_TIMED);
- if (status == VL53L0X_ERROR_NONE) {
- /* Set PAL State to Running */
- Data.PalState = VL53L0X_STATE_RUNNING;
- }
+ if (status == VL53L0X_ERROR_NONE)/* Set PAL State to Running */
+ { Data.PalState = VL53L0X_STATE_RUNNING; }
break;
default:
/* Selected mode not supported */
status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
-
-
+
return status;
}
/* Group PAL Measurement Functions */
VL53L0X_Error VL53L0X::VL53L0X_perform_single_measurement(void)
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
- VL53L0X_DeviceModes device_mode;
-
-
+ VL53L0X_DeviceModes device_mode;
/* Get Current DeviceMode */
status = VL53L0X_get_device_mode( &device_mode);
/* Start immediately to run a single ranging measurement in case of
* single ranging or single histogram */
- if (status == VL53L0X_ERROR_NONE
- && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
- status = VL53L0X_start_measurement();
- }
+ if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
+ status = VL53L0X_start_measurement(); }
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_measurement_poll_for_completion();
- }
+ status = VL53L0X_measurement_poll_for_completion(); }
/* Change PAL State in case of single ranging or single histogram */
if (status == VL53L0X_ERROR_NONE && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING) {
@@ -2642,7 +2631,6 @@
(total_xtalk_mega_cps + 0x80) >> 8;
}
}
-
return status;
}
@@ -2651,16 +2639,13 @@
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t total_xtalk_mega_cps;
-
-
*p_total_signal_rate_mcps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
status = VL53L0X_get_total_xtalk_rate(p_ranging_measurement_data, &total_xtalk_mega_cps);
if (status == VL53L0X_ERROR_NONE) {
- *p_total_signal_rate_mcps += total_xtalk_mega_cps;
- }
+ *p_total_signal_rate_mcps += total_xtalk_mega_cps; }
return status;
}
@@ -2675,41 +2660,31 @@
macro_period_ps = VL53L0X_calc_macro_period_ps( vcsel_period_pclks);
macro_period_ns = (macro_period_ps + 500) / 1000;
- timeout_period_mclks =
- (uint32_t)(((timeout_period_us * 1000)
+ timeout_period_mclks = (uint32_t)(((timeout_period_us * 1000)
+ (macro_period_ns / 2)) / macro_period_ns);
return timeout_period_mclks;
}
uint32_t VL53L0X::VL53L0X_isqrt(uint32_t num)
-{ /*
- * Implements an integer square root
- *
- * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
- */
+{ /* Implements an integer square root
+ * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots */
uint32_t res = 0;
uint32_t bit = 1 << 30;
- /* The second-to-top bit is set:
- * 1 << 14 for 16-bits, 1 << 30 for 32 bits */
+ /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */
/* "bit" starts at the highest power of four <= the argument. */
- while (bit > num) {
- bit >>= 2;
- }
+ while (bit > num) { bit >>= 2; }
while (bit != 0) {
if (num >= res + bit) {
num -= res + bit;
res = (res >> 1) + bit;
- } else {
- res >>= 1;
- }
-
+ }
+ else { res >>= 1; }
bit >>= 2;
}
-
return res;
}
@@ -2744,13 +2719,9 @@
VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
-
- dmax_cal_range_mm =
- Data.DmaxCalRangeMilliMeter;
-
- dmax_cal_signal_rate_rtn_mcps =
- Data.DmaxCalSignalRateRtnMegaCps;
+ dmax_cal_range_mm = Data.DmaxCalRangeMilliMeter;
+
+ dmax_cal_signal_rate_rtn_mcps = Data.DmaxCalSignalRateRtnMegaCps;
/* uint32 * FixPoint1616 = FixPoint1616 */
signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
@@ -2762,8 +2733,7 @@
min_signal_needed_p1 = 0;
if (total_corr_signal_rate_mcps > 0) {
- /* Shift by 10 bits to increase resolution prior to the
- * division */
+ /* Shift by 10 bits to increase resolution prior to the division */
signal_rate_temp_mcps = total_signal_rate_mcps << 10;
/* Add rounding value prior to division */
@@ -2783,7 +2753,6 @@
/* FixPoint1220 >> 16 = FixPoint2804 */
min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
}
-
min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
/* FixPoint1616 >> 16 = uint32 */
@@ -2795,8 +2764,7 @@
/* Check sigmaEstimateP2
* If this value is too high there is not enough signal rate
* to calculate dmax value so set a suitable value to ensure
- * a very small dmax.
- */
+ * a very small dmax. */
sigma_est_p2_tmp = (sigma_estimate_p2 + 0x8000) >> 16;
sigma_est_p2_tmp = (sigma_est_p2_tmp + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
@@ -2805,20 +2773,15 @@
if (sigma_est_p2_tmp > 0xffff) {
min_signal_needed_p3 = 0xfff00000;
} else {
-
- /* DMAX uses a different ambient width from sigma, so apply
- * correction.
- * Perform division before multiplication to prevent overflow.
- */
+ /* DMAX uses a different ambient width from sigma, so apply correction.
+ * Perform division before multiplication to prevent overflow. */
sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns / 2) /
c_amb_eff_width_sigma_est_ns;
sigma_estimate_p2 *= c_amb_eff_width_d_max_ns;
/* FixPoint1616 >> 16 = uint32 */
min_signal_needed_p3 = (sigma_estimate_p2 + 0x8000) >> 16;
-
min_signal_needed_p3 *= min_signal_needed_p3;
-
}
/* FixPoint1814 / uint32 = FixPoint1814 */
@@ -2878,9 +2841,7 @@
if (signal_limit_tmp != 0) {
dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
/ signal_limit_tmp;
- } else {
- dmax_dark_tmp = 0;
- }
+ } else { dmax_dark_tmp = 0; }
dmax_dark = VL53L0X_isqrt(dmax_dark_tmp);
@@ -2888,18 +2849,12 @@
if (min_signal_needed != 0) {
dmax_ambient = (signal_at0_mm + min_signal_needed / 2)
/ min_signal_needed;
- } else {
- dmax_ambient = 0;
- }
+ } else { dmax_ambient = 0; }
dmax_ambient = VL53L0X_isqrt(dmax_ambient);
*pd_max_mm = dmax_dark;
- if (dmax_dark > dmax_ambient) {
- *pd_max_mm = dmax_ambient;
- }
-
-
+ if (dmax_dark > dmax_ambient) { *pd_max_mm = dmax_ambient; }
return status;
}
@@ -2962,8 +2917,6 @@
* Estimates the range sigma
*/
-
-
x_talk_comp_rate_mcps = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
/*
@@ -3208,7 +3161,6 @@
p_dmax_mm);
}
-
return status;
}
@@ -3236,16 +3188,12 @@
uint8_t temp8;
uint32_t dmax_mm = 0;
FixPoint1616_t last_signal_ref_mcps;
-
-
-
- /*
- * VL53L0X has a good ranging when the value of the
+
+ /* VL53L0X has a good ranging when the value of the
* DeviceRangeStatus = 11. This function will replace the value 0 with
* the value 11 in the DeviceRangeStatus.
* In addition, the SigmaEstimator is not included in the VL53L0X
- * DeviceRangeStatus, this will be added in the PalRangeStatus.
- */
+ * DeviceRangeStatus, this will be added in the PalRangeStatus. */
device_range_status_internal = ((device_range_status & 0x78) >> 3);
@@ -3433,10 +3381,8 @@
Data.CurrentParameters.LimitChecksStatus[VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD] = temp8;
}
-
return status;
-
}
VL53L0X_Error VL53L0X::VL53L0X_get_ranging_measurement_data(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
@@ -3455,13 +3401,9 @@
uint8_t localBuffer[12];
VL53L0X_RangingMeasurementData_t last_range_data_buffer;
-
-
/* use multi read even if some registers are not useful, result will
- * be more efficient
- * start reading at 0x14 dec20
- * end reading at 0x21 dec33 total 14 bytes to read
- */
+ * be more efficient start reading at 0x14 dec20
+ * end reading at 0x21 dec33 total 14 bytes to read */
status = VL53L0X_read_multi( 0x14, localBuffer, 12);
if (status == VL53L0X_ERROR_NONE) {
@@ -3494,8 +3436,7 @@
if (linearity_corrective_gain != 1000) {
- tmpuint16 = (uint16_t)((linearity_corrective_gain
- * tmpuint16 + 500) / 1000);
+ tmpuint16 = (uint16_t)((linearity_corrective_gain * tmpuint16 + 500) / 1000);
/* Implement Xtalk */
x_talk_compensation_rate_mega_cps = Data.CurrentParameters.XTalkCompensationRateMegaCps ;
@@ -3520,10 +3461,8 @@
* effective_spad_rtn_count)
>> 8));
}
-
tmpuint16 = xtalk_range_milli_meter;
}
-
}
if (range_fractional_enable) {
@@ -3536,8 +3475,7 @@
p_ranging_measurement_data->RangeFractionalPart = 0;
}
- /*
- * For a standard definition of RangeStatus, this should
+ /* For a standard definition of RangeStatus, this should
* return 0 in case of good result after a ranging
* The range status depends on the device so call a device
* specific function to obtain the right Status.
@@ -3572,33 +3510,26 @@
Data.LastRangeMeasure = last_range_data_buffer;
}
-
-
+
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_perform_single_ranging_measurement(VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
-
/* This function will do a complete single ranging
* Here we fix the mode! */
status = VL53L0X_set_device_mode( VL53L0X_DEVICEMODE_SINGLE_RANGING);
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_perform_single_measurement();
- }
+ status = VL53L0X_perform_single_measurement(); }
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data);
- }
+ status = VL53L0X_get_ranging_measurement_data(p_ranging_measurement_data); }
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_clear_interrupt_mask( 0);
- }
-
-
+ status = VL53L0X_clear_interrupt_mask( 0);}
+
return status;
}
@@ -3609,34 +3540,27 @@
uint8_t sequence_config = 0;
/* store the value of the sequence config,
- * this will be reset before the end of the function
- */
-
+ * this will be reset before the end of the function*/
sequence_config = Data.SequenceConfig;
/*
* This function performs a reference signal rate measurement.
*/
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
- }
+ status = VL53L0X_write_byte(VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);}
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data);
- }
+ status = VL53L0X_perform_single_ranging_measurement(&ranging_measurement_data); }
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_write_byte( 0xFF, 0x01);
- }
+ status = VL53L0X_write_byte( 0xFF, 0x01); }
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_read_word(VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
- p_ref_signal_rate);
- }
+ p_ref_signal_rate);}
if (status == VL53L0X_ERROR_NONE) {
- status = VL53L0X_write_byte( 0xFF, 0x00);
- }
+ status = VL53L0X_write_byte( 0xFF, 0x00);}
if (status == VL53L0X_ERROR_NONE) {
/* restore the previous Sequence Config */
@@ -3646,7 +3570,6 @@
Data.SequenceConfig = sequence_config;
}
}
-
return status;
}
@@ -3894,10 +3817,9 @@
if (status == VL53L0X_ERROR_NONE) {
*ref_spad_count = ref_spad_count_int;
*is_aperture_spads = is_aperture_spads_int;
-
Data.RefSpadsInitialised = 1;
Data.ReferenceSpadCount = (uint8_t)(*ref_spad_count);
- Data.ReferenceSpadType = *is_aperture_spads;
+ Data.ReferenceSpadType = *is_aperture_spads;
}
return status;
@@ -4070,17 +3992,13 @@
if (status == VL53L0X_ERROR_NONE) { status = VL53L0X_clear_interrupt_mask( 0); }
}
}
-
return status;
}
VL53L0X_Error VL53L0X::VL53L0X_get_fraction_enable( uint8_t *p_enabled)
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
status = VL53L0X_read_byte( VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled);
-
if (status == VL53L0X_ERROR_NONE) { *p_enabled = (*p_enabled & 1); }
-
return status;
}
@@ -4100,7 +4018,6 @@
}
encoded_timeout = (ms_byte << 8) + (uint16_t)(ls_byte & 0x000000FF);
}
-
return encoded_timeout;
}
@@ -4151,7 +4068,6 @@
VL53L0X_calc_timeout_mclks(timeout_micro_secs,
(uint8_t)current_vcsel_pulse_period_p_clk);
pre_range_encoded_time_out = VL53L0X_encode_timeout(pre_range_time_out_m_clks);
-
Data.LastEncodedTimeout = pre_range_encoded_time_out;
}
@@ -4168,8 +4084,7 @@
/* For the final range timeout, the pre-range timeout
* must be added. To do this both final and pre-range
* timeouts must be expressed in macro periods MClks
- * because they have different vcsel periods.
- */
+ * because they have different vcsel periods. */
VL53L0X_get_sequence_step_enables(&scheduler_sequence_steps);
pre_range_time_out_m_clks = 0;
@@ -4201,7 +4116,6 @@
(uint8_t) current_vcsel_pulse_period_p_clk);
final_range_time_out_m_clks += pre_range_time_out_m_clks;
-
final_range_encoded_time_out =
VL53L0X_encode_timeout(final_range_time_out_m_clks);
@@ -4216,7 +4130,6 @@
} else {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
-
}
return status;
}
@@ -4308,13 +4221,9 @@
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
-
}
- if (status != VL53L0X_ERROR_NONE) {
-
- return status;
- }
+ if (status != VL53L0X_ERROR_NONE) { return status; }
if (scheduler_sequence_steps.PreRangeOn) {
@@ -4357,9 +4266,7 @@
VL53L0X_Error VL53L0X::VL53L0X_set_measurement_timing_budget_micro_seconds(uint32_t measurement_timing_budget_micro_seconds)
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
status = wrapped_VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
-
return status;
}
@@ -4432,7 +4339,6 @@
/* Recalculate timing budget */
if (status == VL53L0X_ERROR_NONE) {
measurement_timing_budget_micro_seconds = Data.CurrentParameters.MeasurementTimingBudgetMicroSeconds ;
-
VL53L0X_set_measurement_timing_budget_micro_seconds(measurement_timing_budget_micro_seconds);
}
}
@@ -4652,7 +4558,6 @@
VL53L0X_Error VL53L0X::VL53L0X_stop_measurement(void)
{ VL53L0X_Error status = VL53L0X_ERROR_NONE;
-
status = VL53L0X_write_byte( VL53L0X_REG_SYSRANGE_START,
VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
@@ -4672,8 +4577,6 @@
if (status == VL53L0X_ERROR_NONE) {
status = VL53L0X_check_and_load_interrupt_settings( 0);
}
-
-
return status;
}
@@ -4710,29 +4613,14 @@
/****************** Write and read functions from I2C *************************/
-VL53L0X_Error VL53L0X::VL53L0X_write_multi( uint8_t index, uint8_t *p_data, uint32_t count)
-{ int status;
-
- status = VL53L0X_i2c_write(index, p_data, (uint16_t)count);
- return status;
-}
-
VL53L0X_Error VL53L0X::VL53L0X_read_multi( uint8_t index, uint8_t *p_data, uint32_t count)
-{ int status;
-
- if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
- status = VL53L0X_ERROR_INVALID_PARAMS;}
-
- status = VL53L0X_i2c_read(index, p_data, (uint16_t)count);
-
- return status;
+{ if (count >= VL53L0X_MAX_I2C_XFER_SIZE) {
+ return VL53L0X_ERROR_INVALID_PARAMS;}
+ else { return VL53L0X_i2c_read(index, p_data, (uint16_t)count); }
}
VL53L0X_Error VL53L0X::VL53L0X_write_byte( uint8_t index, uint8_t data)
-{ int status;
-
- status = VL53L0X_i2c_write(index, &data, 1);
- return status;
+{ return VL53L0X_i2c_write(index, &data, 1);
}
VL53L0X_Error VL53L0X::VL53L0X_write_word( uint8_t index, uint16_t data)
@@ -4758,27 +4646,15 @@
}
VL53L0X_Error VL53L0X::VL53L0X_read_byte( uint8_t index, uint8_t *p_data)
-{ int status;
-
- status = VL53L0X_i2c_read(index, p_data, 1);
-
- if (status) {
- return -1;
- }
-
- return 0;
-}
+{ return VL53L0X_i2c_read(index, p_data, 1); }
VL53L0X_Error VL53L0X::VL53L0X_read_word( uint8_t index, uint16_t *p_data)
{ int status;
uint8_t buffer[2] = {0, 0};
status = VL53L0X_i2c_read(index, buffer, 2);
- if (!status) {
- *p_data = (buffer[0] << 8) + buffer[1];
- }
+ if (!status) {*p_data = (buffer[0] << 8) + buffer[1];}
return status;
-
}
VL53L0X_Error VL53L0X::VL53L0X_read_dword( uint8_t index, uint32_t *p_data)
@@ -4786,11 +4662,8 @@
uint8_t buffer[4] = {0, 0, 0, 0};
status = VL53L0X_i2c_read(index, buffer, 4);
- if (!status) {
- *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
- }
+ if (!status) { *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3]; }
return status;
-
}
VL53L0X_Error VL53L0X::VL53L0X_update_byte( uint8_t index, uint8_t and_data, uint8_t or_data)
@@ -4829,7 +4702,7 @@
int ret = _dev_i2c->write(I2cDevAddr, (const char*)&RegisterAddr, 1, true);
/* all ok ? then Read data, with STOP condition */
- if(ret == 0) { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
+ if (ret == 0) { ret = _dev_i2c->read(I2cDevAddr, (char*)p_data, NumByteToRead, false); }
if (ret != 0 ){ return -1; }
return 0;