ST Expansion SW Team
Updates to follow mbed SDK coding style guidelines.
Dependencies: ST_INTERFACES X_NUCLEO_COMMON
Dependents: 53L0A1_Satellites_with_Interrupts_OS5 Display_53L0A1_OS5
Fork of
X_NUCLEO_53L0A1
by 
ST
Diff: Components/VL53L0X/vl53l0x_class.cpp
- Revision:
- 20:cf211a3b3d9e
- Parent:
- 17:1b842521063a
- Child:
- 21:627b65069e6d
--- a/Components/VL53L0X/vl53l0x_class.cpp Mon Jun 26 13:40:27 2017 +0000
+++ b/Components/VL53L0X/vl53l0x_class.cpp Wed Jun 28 12:07:45 2017 +0000
@@ -80,40 +80,40 @@
-VL53L0X_Error VL53L0X::vl53l0x_device_read_strobe(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::vl53l0x_device_read_strobe( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t strobe;
uint32_t loop_nb;
- LOG_FUNCTION_START("");
-
- status |= vl53l0x_write_byte(dev, 0x83, 0x00);
+ LOG_FUNCTION_START( "" );
+
+ status |= vl53l0x_write_byte( dev, 0x83, 0x00 );
/* polling
* use timeout to avoid deadlock*/
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
loop_nb = 0;
do {
- status = vl53l0x_read_byte(dev, 0x83, &strobe);
- if ((strobe != 0x00) || status != VL53L0X_ERROR_NONE)
+ status = vl53l0x_read_byte( dev, 0x83, &strobe );
+ if ( ( strobe != 0x00 ) || status != VL53L0X_ERROR_NONE )
break;
loop_nb = loop_nb + 1;
- } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
-
- if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP)
+ } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
+
+ if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP )
status = VL53L0X_ERROR_TIME_OUT;
}
- status |= vl53l0x_write_byte(dev, 0x83, 0x01);
-
- LOG_FUNCTION_END(status);
- return status;
-
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_info_from_device(VL53L0X_DEV dev, uint8_t option)
+ status |= vl53l0x_write_byte( dev, 0x83, 0x01 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_info_from_device( VL53L0X_DEV dev, uint8_t option )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
@@ -138,249 +138,249 @@
int i;
- LOG_FUNCTION_START("");
-
- read_data_from_device_done = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- ReadDataFromDeviceDone);
+ LOG_FUNCTION_START( "" );
+
+ read_data_from_device_done = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ReadDataFromDeviceDone );
/* This access is done only once after that a GetDeviceInfo or
* datainit is done*/
- if (read_data_from_device_done != 7) {
-
- status |= vl53l0x_write_byte(dev, 0x80, 0x01);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x00);
-
- status |= vl53l0x_write_byte(dev, 0xFF, 0x06);
- status |= vl53l0x_read_byte(dev, 0x83, &byte);
- status |= vl53l0x_write_byte(dev, 0x83, byte|4);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x07);
- status |= vl53l0x_write_byte(dev, 0x81, 0x01);
-
- status |= vl53l0x_polling_delay(dev);
-
- status |= vl53l0x_write_byte(dev, 0x80, 0x01);
-
- if (((option & 1) == 1) &&
- ((read_data_from_device_done & 1) == 0)) {
- status |= vl53l0x_write_byte(dev, 0x94, 0x6b);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- reference_spad_count = (uint8_t)((tmp_dword >> 8) & 0x07f);
- reference_spad_type = (uint8_t)((tmp_dword >> 15) & 0x01);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x24);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
-
- nvm_ref_good_spad_map[0] = (uint8_t)((tmp_dword >> 24)
- & 0xff);
- nvm_ref_good_spad_map[1] = (uint8_t)((tmp_dword >> 16)
- & 0xff);
- nvm_ref_good_spad_map[2] = (uint8_t)((tmp_dword >> 8)
- & 0xff);
- nvm_ref_good_spad_map[3] = (uint8_t)(tmp_dword & 0xff);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x25);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- nvm_ref_good_spad_map[4] = (uint8_t)((tmp_dword >> 24)
- & 0xff);
- nvm_ref_good_spad_map[5] = (uint8_t)((tmp_dword >> 16)
- & 0xff);
+ if ( read_data_from_device_done != 7 ) {
+
+ status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
+
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x06 );
+ status |= vl53l0x_read_byte( dev, 0x83, &byte );
+ status |= vl53l0x_write_byte( dev, 0x83, byte | 4 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x07 );
+ status |= vl53l0x_write_byte( dev, 0x81, 0x01 );
+
+ status |= vl53l0x_polling_delay( dev );
+
+ status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
+
+ if ( ( ( option & 1 ) == 1 ) &&
+ ( ( read_data_from_device_done & 1 ) == 0 ) ) {
+ status |= vl53l0x_write_byte( dev, 0x94, 0x6b );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ reference_spad_count = ( uint8_t )( ( tmp_dword >> 8 ) & 0x07f );
+ reference_spad_type = ( uint8_t )( ( tmp_dword >> 15 ) & 0x01 );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x24 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+
+ nvm_ref_good_spad_map[0] = ( uint8_t )( ( tmp_dword >> 24 )
+ & 0xff );
+ nvm_ref_good_spad_map[1] = ( uint8_t )( ( tmp_dword >> 16 )
+ & 0xff );
+ nvm_ref_good_spad_map[2] = ( uint8_t )( ( tmp_dword >> 8 )
+ & 0xff );
+ nvm_ref_good_spad_map[3] = ( uint8_t )( tmp_dword & 0xff );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x25 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ nvm_ref_good_spad_map[4] = ( uint8_t )( ( tmp_dword >> 24 )
+ & 0xff );
+ nvm_ref_good_spad_map[5] = ( uint8_t )( ( tmp_dword >> 16 )
+ & 0xff );
}
- if (((option & 2) == 2) &&
- ((read_data_from_device_done & 2) == 0)) {
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x02);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_byte(dev, 0x90, &module_id);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x7B);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_byte(dev, 0x90, &revision);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x77);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- product_id[0] = (char)((tmp_dword >> 25) & 0x07f);
- product_id[1] = (char)((tmp_dword >> 18) & 0x07f);
- product_id[2] = (char)((tmp_dword >> 11) & 0x07f);
- product_id[3] = (char)((tmp_dword >> 4) & 0x07f);
-
- byte = (uint8_t)((tmp_dword & 0x00f) << 3);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x78);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- product_id[4] = (char)(byte +
- ((tmp_dword >> 29) & 0x07f));
- product_id[5] = (char)((tmp_dword >> 22) & 0x07f);
- product_id[6] = (char)((tmp_dword >> 15) & 0x07f);
- product_id[7] = (char)((tmp_dword >> 8) & 0x07f);
- product_id[8] = (char)((tmp_dword >> 1) & 0x07f);
-
- byte = (uint8_t)((tmp_dword & 0x001) << 6);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x79);
-
- status |= vl53l0x_device_read_strobe(dev);
-
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- product_id[9] = (char)(byte +
- ((tmp_dword >> 26) & 0x07f));
- product_id[10] = (char)((tmp_dword >> 19) & 0x07f);
- product_id[11] = (char)((tmp_dword >> 12) & 0x07f);
- product_id[12] = (char)((tmp_dword >> 5) & 0x07f);
-
- byte = (uint8_t)((tmp_dword & 0x01f) << 2);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x7A);
-
- status |= vl53l0x_device_read_strobe(dev);
-
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- product_id[13] = (char)(byte +
- ((tmp_dword >> 30) & 0x07f));
- product_id[14] = (char)((tmp_dword >> 23) & 0x07f);
- product_id[15] = (char)((tmp_dword >> 16) & 0x07f);
- product_id[16] = (char)((tmp_dword >> 9) & 0x07f);
- product_id[17] = (char)((tmp_dword >> 2) & 0x07f);
+ if ( ( ( option & 2 ) == 2 ) &&
+ ( ( read_data_from_device_done & 2 ) == 0 ) ) {
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x02 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_byte( dev, 0x90, &module_id );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x7B );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_byte( dev, 0x90, &revision );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x77 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ product_id[0] = ( char )( ( tmp_dword >> 25 ) & 0x07f );
+ product_id[1] = ( char )( ( tmp_dword >> 18 ) & 0x07f );
+ product_id[2] = ( char )( ( tmp_dword >> 11 ) & 0x07f );
+ product_id[3] = ( char )( ( tmp_dword >> 4 ) & 0x07f );
+
+ byte = ( uint8_t )( ( tmp_dword & 0x00f ) << 3 );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x78 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ product_id[4] = ( char )( byte +
+ ( ( tmp_dword >> 29 ) & 0x07f ) );
+ product_id[5] = ( char )( ( tmp_dword >> 22 ) & 0x07f );
+ product_id[6] = ( char )( ( tmp_dword >> 15 ) & 0x07f );
+ product_id[7] = ( char )( ( tmp_dword >> 8 ) & 0x07f );
+ product_id[8] = ( char )( ( tmp_dword >> 1 ) & 0x07f );
+
+ byte = ( uint8_t )( ( tmp_dword & 0x001 ) << 6 );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x79 );
+
+ status |= vl53l0x_device_read_strobe( dev );
+
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ product_id[9] = ( char )( byte +
+ ( ( tmp_dword >> 26 ) & 0x07f ) );
+ product_id[10] = ( char )( ( tmp_dword >> 19 ) & 0x07f );
+ product_id[11] = ( char )( ( tmp_dword >> 12 ) & 0x07f );
+ product_id[12] = ( char )( ( tmp_dword >> 5 ) & 0x07f );
+
+ byte = ( uint8_t )( ( tmp_dword & 0x01f ) << 2 );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x7A );
+
+ status |= vl53l0x_device_read_strobe( dev );
+
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ product_id[13] = ( char )( byte +
+ ( ( tmp_dword >> 30 ) & 0x07f ) );
+ product_id[14] = ( char )( ( tmp_dword >> 23 ) & 0x07f );
+ product_id[15] = ( char )( ( tmp_dword >> 16 ) & 0x07f );
+ product_id[16] = ( char )( ( tmp_dword >> 9 ) & 0x07f );
+ product_id[17] = ( char )( ( tmp_dword >> 2 ) & 0x07f );
product_id[18] = '\0';
}
- if (((option & 4) == 4) &&
- ((read_data_from_device_done & 4) == 0)) {
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x7B);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &part_uid_upper);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x7C);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &part_uid_lower);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x73);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- signal_rate_meas_fixed1104_400_mm = (tmp_dword &
- 0x0000000ff) << 8;
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x74);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- signal_rate_meas_fixed1104_400_mm |= ((tmp_dword &
- 0xff000000) >> 24);
-
- status |= vl53l0x_write_byte(dev, 0x94, 0x75);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- dist_meas_fixed1104_400_mm = (tmp_dword & 0x0000000ff)
+ if ( ( ( option & 4 ) == 4 ) &&
+ ( ( read_data_from_device_done & 4 ) == 0 ) ) {
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x7B );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &part_uid_upper );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x7C );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &part_uid_lower );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x73 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ signal_rate_meas_fixed1104_400_mm = ( tmp_dword &
+ 0x0000000ff ) << 8;
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x74 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ signal_rate_meas_fixed1104_400_mm |= ( ( tmp_dword &
+ 0xff000000 ) >> 24 );
+
+ status |= vl53l0x_write_byte( dev, 0x94, 0x75 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ dist_meas_fixed1104_400_mm = ( tmp_dword & 0x0000000ff )
<< 8;
- status |= vl53l0x_write_byte(dev, 0x94, 0x76);
- status |= vl53l0x_device_read_strobe(dev);
- status |= vl53l0x_read_dword(dev, 0x90, &tmp_dword);
-
- dist_meas_fixed1104_400_mm |= ((tmp_dword & 0xff000000)
- >> 24);
+ status |= vl53l0x_write_byte( dev, 0x94, 0x76 );
+ status |= vl53l0x_device_read_strobe( dev );
+ status |= vl53l0x_read_dword( dev, 0x90, &tmp_dword );
+
+ dist_meas_fixed1104_400_mm |= ( ( tmp_dword & 0xff000000 )
+ >> 24 );
}
- status |= vl53l0x_write_byte(dev, 0x81, 0x00);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x06);
- status |= vl53l0x_read_byte(dev, 0x83, &byte);
- status |= vl53l0x_write_byte(dev, 0x83, byte&0xfb);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x01);
-
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
- status |= vl53l0x_write_byte(dev, 0x80, 0x00);
- }
-
- if ((status == VL53L0X_ERROR_NONE) &&
- (read_data_from_device_done != 7)) {
+ status |= vl53l0x_write_byte( dev, 0x81, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x06 );
+ status |= vl53l0x_read_byte( dev, 0x83, &byte );
+ status |= vl53l0x_write_byte( dev, 0x83, byte & 0xfb );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
+
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
+ }
+
+ if ( ( status == VL53L0X_ERROR_NONE ) &&
+ ( read_data_from_device_done != 7 ) ) {
/* Assign to variable if status is ok */
- if (((option & 1) == 1) &&
- ((read_data_from_device_done & 1) == 0)) {
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadCount, reference_spad_count);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadType, reference_spad_type);
-
- for (i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++) {
+ if ( ( ( option & 1 ) == 1 ) &&
+ ( ( read_data_from_device_done & 1 ) == 0 ) ) {
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount, reference_spad_count );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType, reference_spad_type );
+
+ for ( i = 0; i < VL53L0X_REF_SPAD_BUFFER_SIZE; i++ ) {
dev->Data.SpadData.RefGoodSpadMap[i] =
nvm_ref_good_spad_map[i];
}
}
- if (((option & 2) == 2) &&
- ((read_data_from_device_done & 2) == 0)) {
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ModuleId, module_id);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- Revision, revision);
-
- product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- ProductId);
- VL53L0X_COPYSTRING(product_id_tmp, product_id);
+ if ( ( ( option & 2 ) == 2 ) &&
+ ( ( read_data_from_device_done & 2 ) == 0 ) ) {
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ModuleId, module_id );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ Revision, revision );
+
+ product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ProductId );
+ VL53L0X_COPYSTRING( product_id_tmp, product_id );
}
- if (((option & 4) == 4) &&
- ((read_data_from_device_done & 4) == 0)) {
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- PartUIDUpper, part_uid_upper);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- PartUIDLower, part_uid_lower);
+ if ( ( ( option & 4 ) == 4 ) &&
+ ( ( read_data_from_device_done & 4 ) == 0 ) ) {
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ PartUIDUpper, part_uid_upper );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ PartUIDLower, part_uid_lower );
signal_rate_meas_fixed400_mm_fix =
VL53L0X_FIXPOINT97TOFIXPOINT1616(
- signal_rate_meas_fixed1104_400_mm);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- SignalRateMeasFixed400mm,
- signal_rate_meas_fixed400_mm_fix);
+ signal_rate_meas_fixed1104_400_mm );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ SignalRateMeasFixed400mm,
+ signal_rate_meas_fixed400_mm_fix );
offset_micro_meters = 0;
- if (dist_meas_fixed1104_400_mm != 0) {
+ if ( dist_meas_fixed1104_400_mm != 0 ) {
offset_fixed1104_mm =
dist_meas_fixed1104_400_mm -
dist_meas_tgt_fixed1104_mm;
- offset_micro_meters = (offset_fixed1104_mm
- * 1000) >> 4;
+ offset_micro_meters = ( offset_fixed1104_mm
+ * 1000 ) >> 4;
offset_micro_meters *= -1;
}
- PALDevDataSet(dev,
- Part2PartOffsetAdjustmentNVMMicroMeter,
- offset_micro_meters);
+ PALDevDataSet( dev,
+ Part2PartOffsetAdjustmentNVMMicroMeter,
+ offset_micro_meters );
}
- byte = (uint8_t)(read_data_from_device_done|option);
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone,
- byte);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
- int32_t *p_offset_calibration_data_micro_meter)
+ byte = ( uint8_t )( read_data_from_device_done | option );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, ReadDataFromDeviceDone,
+ byte );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
+ int32_t *p_offset_calibration_data_micro_meter )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t range_offset_register;
@@ -389,42 +389,42 @@
/* Note that offset has 10.2 format */
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
- &range_offset_register);
-
- if (status == VL53L0X_ERROR_NONE) {
- range_offset_register = (range_offset_register & 0x0fff);
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
+ &range_offset_register );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ range_offset_register = ( range_offset_register & 0x0fff );
/* Apply 12 bit 2's compliment conversion */
- if (range_offset_register > c_max_offset)
+ if ( range_offset_register > c_max_offset )
*p_offset_calibration_data_micro_meter =
- (int16_t)(range_offset_register - c_offset_range)
+ ( int16_t )( range_offset_register - c_offset_range )
* 250;
else
*p_offset_calibration_data_micro_meter =
- (int16_t)range_offset_register * 250;
-
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
- int32_t *p_offset_calibration_data_micro_meter)
+ ( int16_t )range_offset_register * 250;
+
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
+ int32_t *p_offset_calibration_data_micro_meter )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_get_offset_calibration_data_micro_meter(dev,
- p_offset_calibration_data_micro_meter);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
- int32_t offset_calibration_data_micro_meter)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_offset_calibration_data_micro_meter( dev,
+ p_offset_calibration_data_micro_meter );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
+ int32_t offset_calibration_data_micro_meter )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int32_t c_max_offset_micro_meter = 511000;
@@ -432,48 +432,48 @@
int16_t c_offset_range = 4096;
uint32_t encoded_offset_val;
- LOG_FUNCTION_START("");
-
- if (offset_calibration_data_micro_meter > c_max_offset_micro_meter)
+ LOG_FUNCTION_START( "" );
+
+ if ( offset_calibration_data_micro_meter > c_max_offset_micro_meter )
offset_calibration_data_micro_meter = c_max_offset_micro_meter;
- else if (offset_calibration_data_micro_meter < c_min_offset_micro_meter)
+ else if ( offset_calibration_data_micro_meter < c_min_offset_micro_meter )
offset_calibration_data_micro_meter = c_min_offset_micro_meter;
/* The offset register is 10.2 format and units are mm
* therefore conversion is applied by a division of
* 250.
*/
- if (offset_calibration_data_micro_meter >= 0) {
+ if ( offset_calibration_data_micro_meter >= 0 ) {
encoded_offset_val =
- offset_calibration_data_micro_meter/250;
+ offset_calibration_data_micro_meter / 250;
} else {
encoded_offset_val =
c_offset_range +
- offset_calibration_data_micro_meter/250;
- }
-
- status = vl53l0x_write_word(dev,
- VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
- encoded_offset_val);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_offset_calibration_data_micro_meter(VL53L0X_DEV dev,
- int32_t offset_calibration_data_micro_meter)
+ offset_calibration_data_micro_meter / 250;
+ }
+
+ status = vl53l0x_write_word( dev,
+ VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM,
+ encoded_offset_val );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_offset_calibration_data_micro_meter( VL53L0X_DEV dev,
+ int32_t offset_calibration_data_micro_meter )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_set_offset_calibration_data_micro_meter(dev,
- offset_calibration_data_micro_meter);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_apply_offset_adjustment(VL53L0X_DEV dev)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_set_offset_calibration_data_micro_meter( dev,
+ offset_calibration_data_micro_meter );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_apply_offset_adjustment( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int32_t corrected_offset_micro_meters;
@@ -481,140 +481,140 @@
/* if we run on this function we can read all the NVM info
* used by the API */
- status = vl53l0x_get_info_from_device(dev, 7);
+ status = vl53l0x_get_info_from_device( dev, 7 );
/* Read back current device offset */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_offset_calibration_data_micro_meter(dev,
- ¤t_offset_micro_meters);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_offset_calibration_data_micro_meter( dev,
+ ¤t_offset_micro_meters );
}
/* Apply Offset Adjustment derived from 400mm measurements */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Store initial device offset */
- PALDevDataSet(dev, Part2PartOffsetNVMMicroMeter,
- current_offset_micro_meters);
+ PALDevDataSet( dev, Part2PartOffsetNVMMicroMeter,
+ current_offset_micro_meters );
corrected_offset_micro_meters = current_offset_micro_meters +
- (int32_t)PALDevDataGet(dev,
- Part2PartOffsetAdjustmentNVMMicroMeter);
-
- status = vl53l0x_set_offset_calibration_data_micro_meter(dev,
- corrected_offset_micro_meters);
+ ( int32_t )PALDevDataGet( dev,
+ Part2PartOffsetAdjustmentNVMMicroMeter );
+
+ status = vl53l0x_set_offset_calibration_data_micro_meter( dev,
+ corrected_offset_micro_meters );
/* store current, adjusted offset */
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETPARAMETERFIELD(dev, RangeOffsetMicroMeters,
- corrected_offset_micro_meters);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev, RangeOffsetMicroMeters,
+ corrected_offset_micro_meters );
}
}
return status;
}
-VL53L0X_Error VL53L0X::vl53l0x_get_device_mode(VL53L0X_DEV dev,
- VL53L0X_DeviceModes *p_device_mode)
+VL53L0X_Error VL53L0X::vl53l0x_get_device_mode( VL53L0X_DEV dev,
+ VL53L0X_DeviceModes *p_device_mode )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- VL53L0X_GETPARAMETERFIELD(dev, DeviceMode, *p_device_mode);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_inter_measurement_period_milli_seconds(VL53L0X_DEV dev,
- uint32_t *p_inter_measurement_period_milli_seconds)
+ LOG_FUNCTION_START( "" );
+
+ VL53L0X_GETPARAMETERFIELD( dev, DeviceMode, *p_device_mode );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_inter_measurement_period_milli_seconds( VL53L0X_DEV dev,
+ uint32_t *p_inter_measurement_period_milli_seconds )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t osc_calibrate_val;
uint32_t im_period_milli_seconds;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_word(dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
- &osc_calibrate_val);
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_dword(dev,
- VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
- &im_period_milli_seconds);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- if (osc_calibrate_val != 0) {
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_word( dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
+ &osc_calibrate_val );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_dword( dev,
+ VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
+ &im_period_milli_seconds );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( osc_calibrate_val != 0 ) {
*p_inter_measurement_period_milli_seconds =
im_period_milli_seconds / osc_calibrate_val;
}
- VL53L0X_SETPARAMETERFIELD(dev,
- InterMeasurementPeriodMilliSeconds,
- *p_inter_measurement_period_milli_seconds);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_rate_mega_cps(VL53L0X_DEV dev,
- FixPoint1616_t *p_xtalk_compensation_rate_mega_cps)
+ VL53L0X_SETPARAMETERFIELD( dev,
+ InterMeasurementPeriodMilliSeconds,
+ *p_inter_measurement_period_milli_seconds );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_rate_mega_cps( VL53L0X_DEV dev,
+ FixPoint1616_t *p_xtalk_compensation_rate_mega_cps )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t value;
FixPoint1616_t temp_fix1616;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, (uint16_t *)&value);
- if (status == VL53L0X_ERROR_NONE) {
- if (value == 0) {
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, ( uint16_t * )&value );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( value == 0 ) {
/* the Xtalk is disabled return value from memory */
- VL53L0X_GETPARAMETERFIELD(dev,
- XTalkCompensationRateMegaCps, temp_fix1616);
+ VL53L0X_GETPARAMETERFIELD( dev,
+ XTalkCompensationRateMegaCps, temp_fix1616 );
*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
- VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable,
- 0);
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
+ 0 );
} else {
- temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616(value);
+ temp_fix1616 = VL53L0X_FIXPOINT313TOFIXPOINT1616( value );
*p_xtalk_compensation_rate_mega_cps = temp_fix1616;
- VL53L0X_SETPARAMETERFIELD(dev,
- XTalkCompensationRateMegaCps, temp_fix1616);
- VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationEnable,
- 1);
+ VL53L0X_SETPARAMETERFIELD( dev,
+ XTalkCompensationRateMegaCps, temp_fix1616 );
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
+ 1 );
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id,
- FixPoint1616_t *p_limit_check_value)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_value( VL53L0X_DEV dev, uint16_t limit_check_id,
+ FixPoint1616_t *p_limit_check_value )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t enable_zero_value = 0;
uint16_t temp16;
FixPoint1616_t temp_fix1616;
- LOG_FUNCTION_START("");
-
- switch (limit_check_id) {
+ LOG_FUNCTION_START( "" );
+
+ switch ( limit_check_id ) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp_fix1616);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp_fix1616 );
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
- &temp16);
- if (status == VL53L0X_ERROR_NONE)
- temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
+ &temp16 );
+ if ( status == VL53L0X_ERROR_NONE )
+ temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616( temp16 );
enable_zero_value = 1;
@@ -622,25 +622,25 @@
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp_fix1616);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp_fix1616 );
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, temp_fix1616);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, temp_fix1616 );
enable_zero_value = 0;
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
- &temp16);
- if (status == VL53L0X_ERROR_NONE)
- temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616(temp16);
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
+ &temp16 );
+ if ( status == VL53L0X_ERROR_NONE )
+ temp_fix1616 = VL53L0X_FIXPOINT97TOFIXPOINT1616( temp16 );
enable_zero_value = 0;
@@ -651,155 +651,155 @@
}
- if (status == VL53L0X_ERROR_NONE) {
-
- if (enable_zero_value == 1) {
-
- if (temp_fix1616 == 0) {
+ if ( status == VL53L0X_ERROR_NONE ) {
+
+ if ( enable_zero_value == 1 ) {
+
+ if ( temp_fix1616 == 0 ) {
/* disabled: return value from memory */
- VL53L0X_GETARRAYPARAMETERFIELD(dev,
- LimitChecksValue, limit_check_id,
- temp_fix1616);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev,
+ LimitChecksValue, limit_check_id,
+ temp_fix1616 );
*p_limit_check_value = temp_fix1616;
- VL53L0X_SETARRAYPARAMETERFIELD(dev,
- LimitChecksEnable, limit_check_id, 0);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev,
+ LimitChecksEnable, limit_check_id, 0 );
} else {
*p_limit_check_value = temp_fix1616;
- VL53L0X_SETARRAYPARAMETERFIELD(dev,
- LimitChecksValue, limit_check_id,
- temp_fix1616);
- VL53L0X_SETARRAYPARAMETERFIELD(dev,
- LimitChecksEnable, limit_check_id, 1);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev,
+ LimitChecksValue, limit_check_id,
+ temp_fix1616 );
+ VL53L0X_SETARRAYPARAMETERFIELD( dev,
+ LimitChecksEnable, limit_check_id, 1 );
}
} else {
*p_limit_check_value = temp_fix1616;
}
}
- LOG_FUNCTION_END(status);
- return status;
-
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id,
- uint8_t *p_limit_check_enable)
+ LOG_FUNCTION_END( status );
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_enable( VL53L0X_DEV dev, uint16_t limit_check_id,
+ uint8_t *p_limit_check_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
- LOG_FUNCTION_START("");
-
- if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
+ LOG_FUNCTION_START( "" );
+
+ if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
*p_limit_check_enable = 0;
} else {
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- limit_check_id, temp8);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ limit_check_id, temp8 );
*p_limit_check_enable = temp8;
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_wrap_around_check_enable(VL53L0X_DEV dev,
- uint8_t *p_wrap_around_check_enable)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_wrap_around_check_enable( VL53L0X_DEV dev,
+ uint8_t *p_wrap_around_check_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t data;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data);
- if (status == VL53L0X_ERROR_NONE) {
- PALDevDataSet(dev, SequenceConfig, data);
- if (data & (0x01 << 7))
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ PALDevDataSet( dev, SequenceConfig, data );
+ if ( data & ( 0x01 << 7 ) )
*p_wrap_around_check_enable = 0x01;
else
*p_wrap_around_check_enable = 0x00;
}
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETPARAMETERFIELD(dev, WrapAroundCheckEnable,
- *p_wrap_around_check_enable);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::sequence_step_enabled(VL53L0X_DEV dev,
- VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config,
- uint8_t *p_sequence_step_enabled)
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev, WrapAroundCheckEnable,
+ *p_wrap_around_check_enable );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::sequence_step_enabled( VL53L0X_DEV dev,
+ VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_config,
+ uint8_t *p_sequence_step_enabled )
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
*p_sequence_step_enabled = 0;
- LOG_FUNCTION_START("");
-
- switch (sequence_step_id) {
+ LOG_FUNCTION_START( "" );
+
+ switch ( sequence_step_id ) {
case VL53L0X_SEQUENCESTEP_TCC:
- *p_sequence_step_enabled = (sequence_config & 0x10) >> 4;
+ *p_sequence_step_enabled = ( sequence_config & 0x10 ) >> 4;
break;
case VL53L0X_SEQUENCESTEP_DSS:
- *p_sequence_step_enabled = (sequence_config & 0x08) >> 3;
+ *p_sequence_step_enabled = ( sequence_config & 0x08 ) >> 3;
break;
case VL53L0X_SEQUENCESTEP_MSRC:
- *p_sequence_step_enabled = (sequence_config & 0x04) >> 2;
+ *p_sequence_step_enabled = ( sequence_config & 0x04 ) >> 2;
break;
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
- *p_sequence_step_enabled = (sequence_config & 0x40) >> 6;
+ *p_sequence_step_enabled = ( sequence_config & 0x40 ) >> 6;
break;
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
- *p_sequence_step_enabled = (sequence_config & 0x80) >> 7;
+ *p_sequence_step_enabled = ( sequence_config & 0x80 ) >> 7;
break;
default:
Status = VL53L0X_ERROR_INVALID_PARAMS;
}
- LOG_FUNCTION_END(status);
+ LOG_FUNCTION_END( status );
return Status;
}
-VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_enables(VL53L0X_DEV dev,
- VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps)
+VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_enables( VL53L0X_DEV dev,
+ VL53L0X_SchedulerSequenceSteps_t *p_scheduler_sequence_steps )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- &sequence_config);
-
- if (status == VL53L0X_ERROR_NONE) {
- status = sequence_step_enabled(dev,
- VL53L0X_SEQUENCESTEP_TCC, sequence_config,
- &p_scheduler_sequence_steps->TccOn);
- }
- if (status == VL53L0X_ERROR_NONE) {
- status = sequence_step_enabled(dev,
- VL53L0X_SEQUENCESTEP_DSS, sequence_config,
- &p_scheduler_sequence_steps->DssOn);
- }
- if (status == VL53L0X_ERROR_NONE) {
- status = sequence_step_enabled(dev,
- VL53L0X_SEQUENCESTEP_MSRC, sequence_config,
- &p_scheduler_sequence_steps->MsrcOn);
- }
- if (status == VL53L0X_ERROR_NONE) {
- status = sequence_step_enabled(dev,
- VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config,
- &p_scheduler_sequence_steps->PreRangeOn);
- }
- if (status == VL53L0X_ERROR_NONE) {
- status = sequence_step_enabled(dev,
- VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config,
- &p_scheduler_sequence_steps->FinalRangeOn);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-uint8_t VL53L0X::vl53l0x_decode_vcsel_period(uint8_t vcsel_period_reg)
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ &sequence_config );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev,
+ VL53L0X_SEQUENCESTEP_TCC, sequence_config,
+ &p_scheduler_sequence_steps->TccOn );
+ }
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev,
+ VL53L0X_SEQUENCESTEP_DSS, sequence_config,
+ &p_scheduler_sequence_steps->DssOn );
+ }
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev,
+ VL53L0X_SEQUENCESTEP_MSRC, sequence_config,
+ &p_scheduler_sequence_steps->MsrcOn );
+ }
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev,
+ VL53L0X_SEQUENCESTEP_PRE_RANGE, sequence_config,
+ &p_scheduler_sequence_steps->PreRangeOn );
+ }
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev,
+ VL53L0X_SEQUENCESTEP_FINAL_RANGE, sequence_config,
+ &p_scheduler_sequence_steps->FinalRangeOn );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+uint8_t VL53L0X::vl53l0x_decode_vcsel_period( uint8_t vcsel_period_reg )
{
/*!
* Converts the encoded VCSEL period register value into the real
@@ -808,12 +808,12 @@
uint8_t vcsel_period_pclks = 0;
- vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
+ vcsel_period_pclks = ( vcsel_period_reg + 1 ) << 1;
return vcsel_period_pclks;
}
-uint8_t VL53L0X::lv53l0x_encode_vcsel_period(uint8_t vcsel_period_pclks)
+uint8_t VL53L0X::lv53l0x_encode_vcsel_period( uint8_t vcsel_period_pclks )
{
/*!
* Converts the encoded VCSEL period register value into the real period
@@ -822,14 +822,14 @@
uint8_t vcsel_period_reg = 0;
- vcsel_period_reg = (vcsel_period_pclks >> 1) - 1;
+ vcsel_period_reg = ( vcsel_period_pclks >> 1 ) - 1;
return vcsel_period_reg;
}
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_vcsel_pulse_period(VL53L0X_DEV dev,
- VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk)
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_vcsel_pulse_period( VL53L0X_DEV dev,
+ VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period_pclk )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
@@ -845,149 +845,149 @@
/* Check if valid clock period requested */
- if ((vcsel_pulse_period_pclk % 2) != 0) {
+ if ( ( vcsel_pulse_period_pclk % 2 ) != 0 ) {
/* Value must be an even number */
status = VL53L0X_ERROR_INVALID_PARAMS;
- } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
- (vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
- vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk)) {
+ } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE &&
+ ( vcsel_pulse_period_pclk < min_pre_vcsel_period_pclk ||
+ vcsel_pulse_period_pclk > max_pre_vcsel_period_pclk ) ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
- } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
- (vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
- vcsel_pulse_period_pclk > max_final_vcsel_period_pclk)) {
+ } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE &&
+ ( vcsel_pulse_period_pclk < min_final_vcsel_period_pclk ||
+ vcsel_pulse_period_pclk > max_final_vcsel_period_pclk ) ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
/* Apply specific settings for the requested clock period */
- if (status != VL53L0X_ERROR_NONE)
+ if ( status != VL53L0X_ERROR_NONE )
return status;
- if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE) {
+ if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_PRE_RANGE ) {
/* Set phase check limits */
- if (vcsel_pulse_period_pclk == 12) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x18);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
- } else if (vcsel_pulse_period_pclk == 14) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x30);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
- } else if (vcsel_pulse_period_pclk == 16) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x40);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
- } else if (vcsel_pulse_period_pclk == 18) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x50);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
+ if ( vcsel_pulse_period_pclk == 12 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x18 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+ } else if ( vcsel_pulse_period_pclk == 14 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x30 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+ } else if ( vcsel_pulse_period_pclk == 16 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x40 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+ } else if ( vcsel_pulse_period_pclk == 18 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x50 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
}
- } else if (vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE) {
-
- if (vcsel_pulse_period_pclk == 8) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x10);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
-
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_LIM,
- 0x30);
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
- } else if (vcsel_pulse_period_pclk == 10) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x28);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
-
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_LIM,
- 0x20);
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
- } else if (vcsel_pulse_period_pclk == 12) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x38);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
-
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_LIM,
- 0x20);
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
- } else if (vcsel_pulse_period_pclk == 14) {
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
- 0x048);
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
- 0x08);
-
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_ALGO_PHASECAL_LIM,
- 0x20);
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
+ } else if ( vcsel_period_type == VL53L0X_VCSEL_PERIOD_FINAL_RANGE ) {
+
+ if ( vcsel_pulse_period_pclk == 8 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x10 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x02 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_LIM,
+ 0x30 );
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
+ } else if ( vcsel_pulse_period_pclk == 10 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x28 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_LIM,
+ 0x20 );
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
+ } else if ( vcsel_pulse_period_pclk == 12 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x38 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_LIM,
+ 0x20 );
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
+ } else if ( vcsel_pulse_period_pclk == 14 ) {
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH,
+ 0x048 );
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW,
+ 0x08 );
+
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH, 0x03 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_ALGO_PHASECAL_LIM,
+ 0x20 );
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
}
}
/* Re-calculate and apply timeouts, in macro periods */
- if (status == VL53L0X_ERROR_NONE) {
- vcsel_period_reg = lv53l0x_encode_vcsel_period((uint8_t)
- vcsel_pulse_period_pclk);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ vcsel_period_reg = lv53l0x_encode_vcsel_period( ( uint8_t )
+ vcsel_pulse_period_pclk );
/* When the VCSEL period for the pre or final range is changed,
* the corresponding timeout must be read from the device using
@@ -998,59 +998,59 @@
* For the MSRC timeout, the same applies - this timeout being
* dependant on the pre-range vcsel period.
*/
- switch (vcsel_period_type) {
+ switch ( vcsel_period_type ) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_PRE_RANGE,
- &pre_range_timeout_micro_seconds);
-
- if (status == VL53L0X_ERROR_NONE)
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_MSRC,
- &msrc_timeout_micro_seconds);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
- vcsel_period_reg);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = set_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_PRE_RANGE,
- pre_range_timeout_micro_seconds);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = set_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_MSRC,
- msrc_timeout_micro_seconds);
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_PRE_RANGE,
+ &pre_range_timeout_micro_seconds );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_MSRC,
+ &msrc_timeout_micro_seconds );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
+ vcsel_period_reg );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = set_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_PRE_RANGE,
+ pre_range_timeout_micro_seconds );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = set_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_MSRC,
+ msrc_timeout_micro_seconds );
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
PreRangeVcselPulsePeriod,
- vcsel_pulse_period_pclk);
+ vcsel_pulse_period_pclk );
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_FINAL_RANGE,
- &final_range_timeout_micro_seconds);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
- vcsel_period_reg);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = set_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_FINAL_RANGE,
- final_range_timeout_micro_seconds);
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_FINAL_RANGE,
+ &final_range_timeout_micro_seconds );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
+ vcsel_period_reg );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = set_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_FINAL_RANGE,
+ final_range_timeout_micro_seconds );
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
FinalRangeVcselPulsePeriod,
- vcsel_pulse_period_pclk);
+ vcsel_pulse_period_pclk );
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
@@ -1058,80 +1058,80 @@
}
/* Finally, the timing budget must be re-applied */
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_GETPARAMETERFIELD(dev,
- MeasurementTimingBudgetMicroSeconds,
- measurement_timing_budget_micro_seconds);
-
- status = vl53l0x_set_measurement_timing_budget_micro_seconds(dev,
- measurement_timing_budget_micro_seconds);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_GETPARAMETERFIELD( dev,
+ MeasurementTimingBudgetMicroSeconds,
+ measurement_timing_budget_micro_seconds );
+
+ status = vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
+ measurement_timing_budget_micro_seconds );
}
/* Perform the phase calibration. This is needed after changing on
* vcsel period.
* get_data_enable = 0, restore_config = 1 */
- if (status == VL53L0X_ERROR_NONE)
+ if ( status == VL53L0X_ERROR_NONE )
status = vl53l0x_perform_phase_calibration(
- dev, &phase_cal_int, 0, 1);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_vcsel_pulse_period(VL53L0X_DEV dev,
- VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period)
+ dev, &phase_cal_int, 0, 1 );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_vcsel_pulse_period( VL53L0X_DEV dev,
+ VL53L0X_VcselPeriod vcsel_period_type, uint8_t vcsel_pulse_period )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_set_vcsel_pulse_period(dev, vcsel_period_type,
- vcsel_pulse_period);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_vcsel_pulse_period(VL53L0X_DEV dev,
- VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_set_vcsel_pulse_period( dev, vcsel_period_type,
+ vcsel_pulse_period );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_vcsel_pulse_period( VL53L0X_DEV dev,
+ VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t vcsel_period_reg;
- switch (vcsel_period_type) {
+ switch ( vcsel_period_type ) {
case VL53L0X_VCSEL_PERIOD_PRE_RANGE:
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
- &vcsel_period_reg);
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD,
+ &vcsel_period_reg );
break;
case VL53L0X_VCSEL_PERIOD_FINAL_RANGE:
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
- &vcsel_period_reg);
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD,
+ &vcsel_period_reg );
break;
default:
status = VL53L0X_ERROR_INVALID_PARAMS;
}
- if (status == VL53L0X_ERROR_NONE)
+ if ( status == VL53L0X_ERROR_NONE )
*p_vcsel_pulse_period_pclk =
- vl53l0x_decode_vcsel_period(vcsel_period_reg);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_vcsel_pulse_period(VL53L0X_DEV dev,
- VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk)
+ vl53l0x_decode_vcsel_period( vcsel_period_reg );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_vcsel_pulse_period( VL53L0X_DEV dev,
+ VL53L0X_VcselPeriod vcsel_period_type, uint8_t *p_vcsel_pulse_period_pclk )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_get_vcsel_pulse_period(dev, vcsel_period_type,
- p_vcsel_pulse_period_pclk);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-uint32_t VL53L0X::vl53l0x_decode_timeout(uint16_t encoded_timeout)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_vcsel_pulse_period( dev, vcsel_period_type,
+ p_vcsel_pulse_period_pclk );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+uint32_t VL53L0X::vl53l0x_decode_timeout( uint16_t encoded_timeout )
{
/*!
* Decode 16-bit timeout register value - format (LSByte * 2^MSByte) + 1
@@ -1139,19 +1139,19 @@
uint32_t timeout_macro_clks = 0;
- timeout_macro_clks = ((uint32_t) (encoded_timeout & 0x00FF)
- << (uint32_t) ((encoded_timeout & 0xFF00) >> 8)) + 1;
+ timeout_macro_clks = ( ( uint32_t ) ( encoded_timeout & 0x00FF )
+ << ( uint32_t ) ( ( encoded_timeout & 0xFF00 ) >> 8 ) ) + 1;
return timeout_macro_clks;
}
-uint32_t VL53L0X::vl53l0x_calc_macro_period_ps(VL53L0X_DEV dev, uint8_t vcsel_period_pclks)
+uint32_t VL53L0X::vl53l0x_calc_macro_period_ps( VL53L0X_DEV dev, uint8_t vcsel_period_pclks )
{
uint64_t pll_period_ps;
uint32_t macro_period_vclks;
uint32_t macro_period_ps;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* The above calculation will produce rounding errors,
therefore set fixed value
@@ -1159,34 +1159,34 @@
pll_period_ps = 1655;
macro_period_vclks = 2304;
- macro_period_ps = (uint32_t)(macro_period_vclks
- * vcsel_period_pclks * pll_period_ps);
-
- LOG_FUNCTION_END("");
+ macro_period_ps = ( uint32_t )( macro_period_vclks
+ * vcsel_period_pclks * pll_period_ps );
+
+ LOG_FUNCTION_END( "" );
return macro_period_ps;
}
/* To convert register value into us */
-uint32_t VL53L0X::vl53l0x_calc_timeout_us(VL53L0X_DEV dev,
+uint32_t VL53L0X::vl53l0x_calc_timeout_us( VL53L0X_DEV dev,
uint16_t timeout_period_mclks,
- uint8_t vcsel_period_pclks)
+ uint8_t vcsel_period_pclks )
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t actual_timeout_period_us = 0;
- macro_period_ps = vl53l0x_calc_macro_period_ps(dev, vcsel_period_pclks);
- macro_period_ns = (macro_period_ps + 500) / 1000;
+ macro_period_ps = vl53l0x_calc_macro_period_ps( dev, vcsel_period_pclks );
+ macro_period_ns = ( macro_period_ps + 500 ) / 1000;
actual_timeout_period_us =
- ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
+ ( ( timeout_period_mclks * macro_period_ns ) + 500 ) / 1000;
return actual_timeout_period_us;
}
-VL53L0X_Error VL53L0X::get_sequence_step_timeout(VL53L0X_DEV dev,
+VL53L0X_Error VL53L0X::get_sequence_step_timeout( VL53L0X_DEV dev,
VL53L0X_SequenceStepId sequence_step_id,
- uint32_t *p_time_out_micro_secs)
+ uint32_t *p_time_out_micro_secs )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t current_vcsel_pulse_period_p_clk;
@@ -1199,92 +1199,92 @@
uint16_t final_range_encoded_time_out;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
- if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
- (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
- (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
-
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
- &encoded_time_out_byte);
+ if ( ( sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) ||
+ ( sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) ||
+ ( sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC ) ) {
+
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
+ &encoded_time_out_byte );
}
- msrc_time_out_m_clks = vl53l0x_decode_timeout(encoded_time_out_byte);
-
- timeout_micro_seconds = vl53l0x_calc_timeout_us(dev,
+ msrc_time_out_m_clks = vl53l0x_decode_timeout( encoded_time_out_byte );
+
+ timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
msrc_time_out_m_clks,
- current_vcsel_pulse_period_p_clk);
- } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
+ current_vcsel_pulse_period_p_clk );
+ } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE ) {
/* Retrieve PRE-RANGE VCSEL Period */
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
/* Retrieve PRE-RANGE Timeout in Macro periods (MCLKS) */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Retrieve PRE-RANGE VCSEL Period */
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
- &pre_range_encoded_time_out);
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
+ &pre_range_encoded_time_out );
}
pre_range_time_out_m_clks = vl53l0x_decode_timeout(
- pre_range_encoded_time_out);
-
- timeout_micro_seconds = vl53l0x_calc_timeout_us(dev,
+ pre_range_encoded_time_out );
+
+ timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
pre_range_time_out_m_clks,
- current_vcsel_pulse_period_p_clk);
+ current_vcsel_pulse_period_p_clk );
}
- } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
-
- vl53l0x_get_sequence_step_enables(dev, &scheduler_sequence_steps);
+ } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE ) {
+
+ vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
pre_range_time_out_m_clks = 0;
- if (scheduler_sequence_steps.PreRangeOn) {
+ if ( scheduler_sequence_steps.PreRangeOn ) {
/* Retrieve PRE-RANGE VCSEL Period */
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
- &pre_range_encoded_time_out);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
+ &pre_range_encoded_time_out );
pre_range_time_out_m_clks = vl53l0x_decode_timeout(
- pre_range_encoded_time_out);
+ pre_range_encoded_time_out );
}
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Retrieve FINAL-RANGE VCSEL Period */
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
}
/* Retrieve FINAL-RANGE Timeout in Macro periods (MCLKS) */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
- &final_range_encoded_time_out);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI,
+ &final_range_encoded_time_out );
final_range_time_out_m_clks = vl53l0x_decode_timeout(
- final_range_encoded_time_out);
+ final_range_encoded_time_out );
}
final_range_time_out_m_clks -= pre_range_time_out_m_clks;
- timeout_micro_seconds = vl53l0x_calc_timeout_us(dev,
+ timeout_micro_seconds = vl53l0x_calc_timeout_us( dev,
final_range_time_out_m_clks,
- current_vcsel_pulse_period_p_clk);
+ current_vcsel_pulse_period_p_clk );
}
*p_time_out_micro_secs = timeout_micro_seconds;
@@ -1292,8 +1292,8 @@
return status;
}
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
- uint32_t *p_measurement_timing_budget_micro_seconds)
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
+ uint32_t *p_measurement_timing_budget_micro_seconds )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
@@ -1308,40 +1308,40 @@
uint32_t final_range_overhead_micro_seconds = 550;
uint32_t pre_range_timeout_micro_seconds = 0;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* Start and end overhead times always present */
*p_measurement_timing_budget_micro_seconds
= start_overhead_micro_seconds + end_overhead_micro_seconds;
- status = vl53l0x_get_sequence_step_enables(dev, &scheduler_sequence_steps);
-
- if (status != VL53L0X_ERROR_NONE) {
- LOG_FUNCTION_END(status);
+ status = vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
+
+ if ( status != VL53L0X_ERROR_NONE ) {
+ LOG_FUNCTION_END( status );
return status;
}
- if (scheduler_sequence_steps.TccOn ||
+ if ( scheduler_sequence_steps.TccOn ||
scheduler_sequence_steps.MsrcOn ||
- scheduler_sequence_steps.DssOn) {
-
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_MSRC,
- &msrc_dcc_tcc_timeout_micro_seconds);
-
- if (status == VL53L0X_ERROR_NONE) {
- if (scheduler_sequence_steps.TccOn) {
+ scheduler_sequence_steps.DssOn ) {
+
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_MSRC,
+ &msrc_dcc_tcc_timeout_micro_seconds );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( scheduler_sequence_steps.TccOn ) {
*p_measurement_timing_budget_micro_seconds +=
msrc_dcc_tcc_timeout_micro_seconds +
tcc_overhead_micro_seconds;
}
- if (scheduler_sequence_steps.DssOn) {
+ if ( scheduler_sequence_steps.DssOn ) {
*p_measurement_timing_budget_micro_seconds +=
- 2 * (msrc_dcc_tcc_timeout_micro_seconds +
- dss_overhead_micro_seconds);
- } else if (scheduler_sequence_steps.MsrcOn) {
+ 2 * ( msrc_dcc_tcc_timeout_micro_seconds +
+ dss_overhead_micro_seconds );
+ } else if ( scheduler_sequence_steps.MsrcOn ) {
*p_measurement_timing_budget_micro_seconds +=
msrc_dcc_tcc_timeout_micro_seconds +
msrc_overhead_micro_seconds;
@@ -1349,174 +1349,174 @@
}
}
- if (status == VL53L0X_ERROR_NONE) {
- if (scheduler_sequence_steps.PreRangeOn) {
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_PRE_RANGE,
- &pre_range_timeout_micro_seconds);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( scheduler_sequence_steps.PreRangeOn ) {
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_PRE_RANGE,
+ &pre_range_timeout_micro_seconds );
*p_measurement_timing_budget_micro_seconds +=
pre_range_timeout_micro_seconds +
pre_range_overhead_micro_seconds;
}
}
- if (status == VL53L0X_ERROR_NONE) {
- if (scheduler_sequence_steps.FinalRangeOn) {
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_FINAL_RANGE,
- &final_range_timeout_micro_seconds);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( scheduler_sequence_steps.FinalRangeOn ) {
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_FINAL_RANGE,
+ &final_range_timeout_micro_seconds );
*p_measurement_timing_budget_micro_seconds +=
- (final_range_timeout_micro_seconds +
- final_range_overhead_micro_seconds);
+ ( final_range_timeout_micro_seconds +
+ final_range_overhead_micro_seconds );
}
}
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETPARAMETERFIELD(dev,
- MeasurementTimingBudgetMicroSeconds,
- *p_measurement_timing_budget_micro_seconds);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
- uint32_t *p_measurement_timing_budget_micro_seconds)
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev,
+ MeasurementTimingBudgetMicroSeconds,
+ *p_measurement_timing_budget_micro_seconds );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
+ uint32_t *p_measurement_timing_budget_micro_seconds )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds(dev,
- p_measurement_timing_budget_micro_seconds);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_device_parameters(VL53L0X_DEV dev,
- VL53L0X_DeviceParameters_t *p_device_parameters)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_measurement_timing_budget_micro_seconds( dev,
+ p_measurement_timing_budget_micro_seconds );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_device_parameters( VL53L0X_DEV dev,
+ VL53L0X_DeviceParameters_t *p_device_parameters )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int i;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_get_device_mode(dev, &(p_device_parameters->DeviceMode));
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_inter_measurement_period_milli_seconds(dev,
- &(p_device_parameters->InterMeasurementPeriodMilliSeconds));
-
-
- if (status == VL53L0X_ERROR_NONE)
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_get_device_mode( dev, &( p_device_parameters->DeviceMode ) );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_inter_measurement_period_milli_seconds( dev,
+ &( p_device_parameters->InterMeasurementPeriodMilliSeconds ) );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
p_device_parameters->XTalkCompensationEnable = 0;
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_x_talk_compensation_rate_mega_cps(dev,
- &(p_device_parameters->XTalkCompensationRateMegaCps));
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_offset_calibration_data_micro_meter(dev,
- &(p_device_parameters->RangeOffsetMicroMeters));
-
-
- if (status == VL53L0X_ERROR_NONE) {
- for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_x_talk_compensation_rate_mega_cps( dev,
+ &( p_device_parameters->XTalkCompensationRateMegaCps ) );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_offset_calibration_data_micro_meter( dev,
+ &( p_device_parameters->RangeOffsetMicroMeters ) );
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
/* get first the values, then the enables.
* VL53L0X_GetLimitCheckValue will modify the enable
* flags
*/
- if (status == VL53L0X_ERROR_NONE) {
- status |= vl53l0x_get_limit_check_value(dev, i,
- &(p_device_parameters->LimitChecksValue[i]));
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status |= vl53l0x_get_limit_check_value( dev, i,
+ &( p_device_parameters->LimitChecksValue[i] ) );
} else {
break;
}
- if (status == VL53L0X_ERROR_NONE) {
- status |= vl53l0x_get_limit_check_enable(dev, i,
- &(p_device_parameters->LimitChecksEnable[i]));
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status |= vl53l0x_get_limit_check_enable( dev, i,
+ &( p_device_parameters->LimitChecksEnable[i] ) );
} else {
break;
}
}
}
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_wrap_around_check_enable(dev,
- &(p_device_parameters->WrapAroundCheckEnable));
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_wrap_around_check_enable( dev,
+ &( p_device_parameters->WrapAroundCheckEnable ) );
}
/* Need to be done at the end as it uses VCSELPulsePeriod */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_measurement_timing_budget_micro_seconds(dev,
- &(p_device_parameters->MeasurementTimingBudgetMicroSeconds));
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_value(VL53L0X_DEV dev, uint16_t limit_check_id,
- FixPoint1616_t limit_check_value)
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_measurement_timing_budget_micro_seconds( dev,
+ &( p_device_parameters->MeasurementTimingBudgetMicroSeconds ) );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_value( VL53L0X_DEV dev, uint16_t limit_check_id,
+ FixPoint1616_t limit_check_value )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
- LOG_FUNCTION_START("");
-
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksEnable, limit_check_id,
- temp8);
-
- if (temp8 == 0) { /* disabled write only internal value */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- limit_check_id, limit_check_value);
+ LOG_FUNCTION_START( "" );
+
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksEnable, limit_check_id,
+ temp8 );
+
+ if ( temp8 == 0 ) { /* disabled write only internal value */
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ limit_check_id, limit_check_value );
} else {
- switch (limit_check_id) {
+ switch ( limit_check_id ) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
- limit_check_value);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
+ limit_check_value );
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
- status = vl53l0x_write_word(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
- VL53L0X_FIXPOINT1616TOFIXPOINT97(
- limit_check_value));
+ status = vl53l0x_write_word( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
+ VL53L0X_FIXPOINT1616TOFIXPOINT97(
+ limit_check_value ) );
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
- limit_check_value);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ limit_check_value );
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- limit_check_value);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ limit_check_value );
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
- status = vl53l0x_write_word(dev,
- VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
- VL53L0X_FIXPOINT1616TOFIXPOINT97(
- limit_check_value));
+ status = vl53l0x_write_word( dev,
+ VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT,
+ VL53L0X_FIXPOINT1616TOFIXPOINT97(
+ limit_check_value ) );
break;
@@ -1525,298 +1525,298 @@
}
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- limit_check_id, limit_check_value);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ limit_check_id, limit_check_value );
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_data_init(VL53L0X_DEV dev)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_data_init( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t CurrentParameters;
int i;
uint8_t StopVariable;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* by default the I2C is running at 1V8 if you want to change it you
* need to include this define at compilation level. */
#ifdef USE_I2C_2V8
- Status = VL53L0X_UpdateByte(Dev,
- VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
- 0xFE,
- 0x01);
+ Status = VL53L0X_UpdateByte( Dev,
+ VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
+ 0xFE,
+ 0x01 );
#endif
/* Set I2C standard mode */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0x88, 0x00);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, ReadDataFromDeviceDone, 0);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0x88, 0x00 );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, ReadDataFromDeviceDone, 0 );
#ifdef USE_IQC_STATION
- if (Status == VL53L0X_ERROR_NONE)
- Status = VL53L0X_apply_offset_adjustment(Dev);
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = VL53L0X_apply_offset_adjustment( Dev );
#endif
/* Default value is 1000 for Linearity Corrective Gain */
- PALDevDataSet(dev, LinearityCorrectiveGain, 1000);
+ PALDevDataSet( dev, LinearityCorrectiveGain, 1000 );
/* Dmax default Parameter */
- PALDevDataSet(dev, DmaxCalRangeMilliMeter, 400);
- PALDevDataSet(dev, DmaxCalSignalRateRtnMegaCps,
- (FixPoint1616_t)((0x00016B85))); /* 1.42 No Cover Glass*/
+ PALDevDataSet( dev, DmaxCalRangeMilliMeter, 400 );
+ PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
+ ( FixPoint1616_t )( ( 0x00016B85 ) ) ); /* 1.42 No Cover Glass*/
/* Set Default static parameters
*set first temporary values 9.44MHz * 65536 = 618660 */
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz, 618660);
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, OscFrequencyMHz, 618660 );
/* Set Default XTalkCompensationRateMegaCps to 0 */
- VL53L0X_SETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps, 0);
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps, 0 );
/* Get default parameters */
- status = vl53l0x_get_device_parameters(dev, &CurrentParameters);
- if (status == VL53L0X_ERROR_NONE) {
+ status = vl53l0x_get_device_parameters( dev, &CurrentParameters );
+ if ( status == VL53L0X_ERROR_NONE ) {
/* initialize PAL values */
CurrentParameters.DeviceMode = VL53L0X_DEVICEMODE_SINGLE_RANGING;
CurrentParameters.HistogramMode = VL53L0X_HISTOGRAMMODE_DISABLED;
- PALDevDataSet(dev, CurrentParameters, CurrentParameters);
+ PALDevDataSet( dev, CurrentParameters, CurrentParameters );
}
/* Sigma estimator variable */
- PALDevDataSet(dev, SigmaEstRefArray, 100);
- PALDevDataSet(dev, SigmaEstEffPulseWidth, 900);
- PALDevDataSet(dev, SigmaEstEffAmbWidth, 500);
- PALDevDataSet(dev, targetRefRate, 0x0A00); /* 20 MCPS in 9:7 format */
+ PALDevDataSet( dev, SigmaEstRefArray, 100 );
+ PALDevDataSet( dev, SigmaEstEffPulseWidth, 900 );
+ PALDevDataSet( dev, SigmaEstEffAmbWidth, 500 );
+ PALDevDataSet( dev, targetRefRate, 0x0A00 ); /* 20 MCPS in 9:7 format */
/* Use internal default settings */
- PALDevDataSet(dev, UseInternalTuningSettings, 1);
-
- status |= vl53l0x_write_byte(dev, 0x80, 0x01);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x00);
- status |= vl53l0x_read_byte(dev, 0x91, &StopVariable);
- PALDevDataSet(dev, StopVariable, StopVariable);
- status |= vl53l0x_write_byte(dev, 0x00, 0x01);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
- status |= vl53l0x_write_byte(dev, 0x80, 0x00);
+ PALDevDataSet( dev, UseInternalTuningSettings, 1 );
+
+ status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
+ status |= vl53l0x_read_byte( dev, 0x91, &StopVariable );
+ PALDevDataSet( dev, StopVariable, StopVariable );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
/* Enable all check */
- for (i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++) {
- if (status == VL53L0X_ERROR_NONE)
- status |= vl53l0x_set_limit_check_enable(dev, i, 1);
+ for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status |= vl53l0x_set_limit_check_enable( dev, i, 1 );
else
break;
}
/* Disable the following checks */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, 0 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC, 0 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE, 0 );
/* Limit default values */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_set_limit_check_value(dev,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
- (FixPoint1616_t)(18 * 65536));
- }
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_set_limit_check_value(dev,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
- (FixPoint1616_t)(25 * 65536 / 100));
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
+ ( FixPoint1616_t )( 18 * 65536 ) );
+ }
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
+ ( FixPoint1616_t )( 25 * 65536 / 100 ) );
/* 0.25 * 65536 */
}
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_set_limit_check_value(dev,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
- (FixPoint1616_t)(35 * 65536));
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_set_limit_check_value(dev,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- (FixPoint1616_t)(0 * 65536));
- }
-
- if (status == VL53L0X_ERROR_NONE) {
-
- PALDevDataSet(dev, SequenceConfig, 0xFF);
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- 0xFF);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ ( FixPoint1616_t )( 35 * 65536 ) );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ ( FixPoint1616_t )( 0 * 65536 ) );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+
+ PALDevDataSet( dev, SequenceConfig, 0xFF );
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ 0xFF );
/* Set PAL state to tell that we are waiting for call to
* VL53L0X_StaticInit */
- PALDevDataSet(dev, PalState, VL53L0X_STATE_WAIT_STATICINIT);
- }
-
- if (status == VL53L0X_ERROR_NONE)
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 0);
-
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_check_part_used(VL53L0X_DEV dev,
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_WAIT_STATICINIT );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE )
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 0 );
+
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_check_part_used( VL53L0X_DEV dev,
uint8_t *revision,
- VL53L0X_DeviceInfo_t *p_vl53l0x_device_info)
+ VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t module_id_int;
char *product_id_tmp;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_get_info_from_device(dev, 2);
-
- if (status == VL53L0X_ERROR_NONE) {
- module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, ModuleId);
-
- if (module_id_int == 0) {
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_get_info_from_device( dev, 2 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ module_id_int = VL53L0X_GETDEVICESPECIFICPARAMETER( dev, ModuleId );
+
+ if ( module_id_int == 0 ) {
*revision = 0;
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->ProductId, "");
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->ProductId, "" );
} else {
- *revision = VL53L0X_GETDEVICESPECIFICPARAMETER(dev, Revision);
- product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- ProductId);
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->ProductId, product_id_tmp);
+ *revision = VL53L0X_GETDEVICESPECIFICPARAMETER( dev, Revision );
+ product_id_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ProductId );
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->ProductId, product_id_tmp );
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_device_info(VL53L0X_DEV dev,
- VL53L0X_DeviceInfo_t *p_vl53l0x_device_info)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_device_info( VL53L0X_DEV dev,
+ VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t revision_id;
uint8_t revision;
- status = vl53l0x_check_part_used(dev, &revision, p_vl53l0x_device_info);
-
- if (status == VL53L0X_ERROR_NONE) {
- if (revision == 0) {
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->Name,
- VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
- } else if ((revision <= 34) && (revision != 32)) {
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->Name,
- VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
- } else if (revision < 39) {
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->Name,
- VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
+ status = vl53l0x_check_part_used( dev, &revision, p_vl53l0x_device_info );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( revision == 0 ) {
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name,
+ VL53L0X_STRING_DEVICE_INFO_NAME_TS0 );
+ } else if ( ( revision <= 34 ) && ( revision != 32 ) ) {
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name,
+ VL53L0X_STRING_DEVICE_INFO_NAME_TS1 );
+ } else if ( revision < 39 ) {
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name,
+ VL53L0X_STRING_DEVICE_INFO_NAME_TS2 );
} else {
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->Name,
- VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->Name,
+ VL53L0X_STRING_DEVICE_INFO_NAME_ES1 );
}
- VL53L0X_COPYSTRING(p_vl53l0x_device_info->Type,
- VL53L0X_STRING_DEVICE_INFO_TYPE);
-
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_byte(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
- &p_vl53l0x_device_info->ProductType);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_IDENTIFICATION_REVISION_ID,
- &revision_id);
+ VL53L0X_COPYSTRING( p_vl53l0x_device_info->Type,
+ VL53L0X_STRING_DEVICE_INFO_TYPE );
+
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
+ &p_vl53l0x_device_info->ProductType );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_IDENTIFICATION_REVISION_ID,
+ &revision_id );
p_vl53l0x_device_info->ProductRevisionMajor = 1;
p_vl53l0x_device_info->ProductRevisionMinor =
- (revision_id & 0xF0) >> 4;
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_device_info(VL53L0X_DEV dev,
- VL53L0X_DeviceInfo_t *p_vl53l0x_device_info)
+ ( revision_id & 0xF0 ) >> 4;
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_device_info( VL53L0X_DEV dev,
+ VL53L0X_DeviceInfo_t *p_vl53l0x_device_info )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_get_device_info(dev, p_vl53l0x_device_info);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_mask_status(VL53L0X_DEV dev,
- uint32_t *p_interrupt_mask_status)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_device_info( dev, p_vl53l0x_device_info );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_mask_status( VL53L0X_DEV dev,
+ uint32_t *p_interrupt_mask_status )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t byte;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_byte(dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte );
*p_interrupt_mask_status = byte & 0x07;
- if (byte & 0x18)
+ if ( byte & 0x18 )
status = VL53L0X_ERROR_RANGE_ERROR;
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_measurement_data_ready(VL53L0X_DEV dev,
- uint8_t *p_measurement_data_ready)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_measurement_data_ready( VL53L0X_DEV dev,
+ uint8_t *p_measurement_data_ready )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sys_range_status_register;
uint8_t interrupt_config;
uint32_t interrupt_mask;
- LOG_FUNCTION_START("");
-
- interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- Pin0GpioFunctionality);
-
- if (interrupt_config ==
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY) {
- status = vl53l0x_get_interrupt_mask_status(dev, &interrupt_mask);
- if (interrupt_mask ==
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY)
+ LOG_FUNCTION_START( "" );
+
+ interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ Pin0GpioFunctionality );
+
+ if ( interrupt_config ==
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY ) {
+ status = vl53l0x_get_interrupt_mask_status( dev, &interrupt_mask );
+ if ( interrupt_mask ==
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY )
*p_measurement_data_ready = 1;
else
*p_measurement_data_ready = 0;
} else {
- status = vl53l0x_read_byte(dev, VL53L0X_REG_RESULT_RANGE_STATUS,
- &sys_range_status_register);
- if (status == VL53L0X_ERROR_NONE) {
- if (sys_range_status_register & 0x01)
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_RESULT_RANGE_STATUS,
+ &sys_range_status_register );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( sys_range_status_register & 0x01 )
*p_measurement_data_ready = 1;
else
*p_measurement_data_ready = 0;
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_polling_delay(VL53L0X_DEV dev)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_polling_delay( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
@@ -1825,127 +1825,127 @@
return status;
}
-VL53L0X_Error VL53L0X::vl53l0x_measurement_poll_for_completion(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::vl53l0x_measurement_poll_for_completion( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t new_data_ready = 0;
uint32_t loop_nb;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
loop_nb = 0;
do {
- status = vl53l0x_get_measurement_data_ready(dev, &new_data_ready);
- if (status != 0)
+ status = vl53l0x_get_measurement_data_ready( dev, &new_data_ready );
+ if ( status != 0 )
break; /* the error is set */
- if (new_data_ready == 1)
+ if ( new_data_ready == 1 )
break; /* done note that status == 0 */
loop_nb++;
- if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
+ if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
status = VL53L0X_ERROR_TIME_OUT;
break;
}
- vl53l0x_polling_delay(dev);
- } while (1);
-
- LOG_FUNCTION_END(status);
+ vl53l0x_polling_delay( dev );
+ } while ( 1 );
+
+ LOG_FUNCTION_END( status );
return status;
}
/* Group PAL Interrupt Functions */
-VL53L0X_Error VL53L0X::vl53l0x_clear_interrupt_mask(VL53L0X_DEV dev, uint32_t interrupt_mask)
+VL53L0X_Error VL53L0X::vl53l0x_clear_interrupt_mask( VL53L0X_DEV dev, uint32_t interrupt_mask )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t loop_count;
uint8_t byte;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* clear bit 0 range interrupt, bit 1 error interrupt */
loop_count = 0;
do {
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01);
- status |= vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00);
- status |= vl53l0x_read_byte(dev,
- VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte);
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x01 );
+ status |= vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR, 0x00 );
+ status |= vl53l0x_read_byte( dev,
+ VL53L0X_REG_RESULT_INTERRUPT_STATUS, &byte );
loop_count++;
- } while (((byte & 0x07) != 0x00)
- && (loop_count < 3)
- && (status == VL53L0X_ERROR_NONE));
-
-
- if (loop_count >= 3)
+ } while ( ( ( byte & 0x07 ) != 0x00 )
+ && ( loop_count < 3 )
+ && ( status == VL53L0X_ERROR_NONE ) );
+
+
+ if ( loop_count >= 3 )
status = VL53L0X_ERROR_INTERRUPT_NOT_CLEARED;
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_single_ref_calibration(VL53L0X_DEV dev,
- uint8_t vhv_init_byte)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_single_ref_calibration( VL53L0X_DEV dev,
+ uint8_t vhv_init_byte )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
- VL53L0X_REG_SYSRANGE_MODE_START_STOP |
- vhv_init_byte);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_measurement_poll_for_completion(dev);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_clear_interrupt_mask(dev, 0);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x00);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_ref_calibration_io(VL53L0X_DEV dev, uint8_t read_not_write,
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START,
+ VL53L0X_REG_SYSRANGE_MODE_START_STOP |
+ vhv_init_byte );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_measurement_poll_for_completion( dev );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_clear_interrupt_mask( dev, 0 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START, 0x00 );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_ref_calibration_io( VL53L0X_DEV dev, uint8_t read_not_write,
uint8_t vhv_settings, uint8_t phase_cal,
uint8_t *p_vhv_settings, uint8_t *p_phase_cal,
- const uint8_t vhv_enable, const uint8_t phase_enable)
+ const uint8_t vhv_enable, const uint8_t phase_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t phase_calint = 0;
/* Read VHV from device */
- status |= vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x00);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- if (read_not_write) {
- if (vhv_enable)
- status |= vl53l0x_read_byte(dev, 0xCB, p_vhv_settings);
- if (phase_enable)
- status |= vl53l0x_read_byte(dev, 0xEE, &phase_calint);
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ if ( read_not_write ) {
+ if ( vhv_enable )
+ status |= vl53l0x_read_byte( dev, 0xCB, p_vhv_settings );
+ if ( phase_enable )
+ status |= vl53l0x_read_byte( dev, 0xEE, &phase_calint );
} else {
- if (vhv_enable)
- status |= vl53l0x_write_byte(dev, 0xCB, vhv_settings);
- if (phase_enable)
- status |= vl53l0x_update_byte(dev, 0xEE, 0x80, phase_cal);
- }
-
- status |= vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x01);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- *p_phase_cal = (uint8_t)(phase_calint&0xEF);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_vhv_calibration(VL53L0X_DEV dev,
+ if ( vhv_enable )
+ status |= vl53l0x_write_byte( dev, 0xCB, vhv_settings );
+ if ( phase_enable )
+ status |= vl53l0x_update_byte( dev, 0xEE, 0x80, phase_cal );
+ }
+
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ *p_phase_cal = ( uint8_t )( phase_calint & 0xEF );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_vhv_calibration( VL53L0X_DEV dev,
uint8_t *p_vhv_settings, const uint8_t get_data_enable,
- const uint8_t restore_config)
+ const uint8_t restore_config )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
@@ -1957,40 +1957,40 @@
* this will be reset before the end of the function
*/
- if (restore_config)
- sequence_config = PALDevDataGet(dev, SequenceConfig);
+ if ( restore_config )
+ sequence_config = PALDevDataGet( dev, SequenceConfig );
/* Run VHV */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_perform_single_ref_calibration(dev, 0x40);
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_perform_single_ref_calibration( dev, 0x40 );
/* Read VHV from device */
- if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
- status = vl53l0x_ref_calibration_io(dev, 1,
- vhv_settings, phase_cal, /* Not used here */
- p_vhv_settings, &phase_cal_int,
- 1, 0);
+ if ( ( status == VL53L0X_ERROR_NONE ) && ( get_data_enable == 1 ) ) {
+ status = vl53l0x_ref_calibration_io( dev, 1,
+ vhv_settings, phase_cal, /* Not used here */
+ p_vhv_settings, &phase_cal_int,
+ 1, 0 );
} else
*p_vhv_settings = 0;
- if ((status == VL53L0X_ERROR_NONE) && restore_config) {
+ if ( ( status == VL53L0X_ERROR_NONE ) && restore_config ) {
/* restore the previous Sequence Config */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- sequence_config);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, sequence_config);
-
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_phase_calibration(VL53L0X_DEV dev,
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ sequence_config );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, sequence_config );
+
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_phase_calibration( VL53L0X_DEV dev,
uint8_t *p_phase_cal, const uint8_t get_data_enable,
- const uint8_t restore_config)
+ const uint8_t restore_config )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
@@ -2002,39 +2002,39 @@
* this will be reset before the end of the function
*/
- if (restore_config)
- sequence_config = PALDevDataGet(dev, SequenceConfig);
+ if ( restore_config )
+ sequence_config = PALDevDataGet( dev, SequenceConfig );
/* Run PhaseCal */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_perform_single_ref_calibration(dev, 0x0);
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0x02 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_perform_single_ref_calibration( dev, 0x0 );
/* Read PhaseCal from device */
- if ((status == VL53L0X_ERROR_NONE) && (get_data_enable == 1)) {
- status = vl53l0x_ref_calibration_io(dev, 1,
- vhv_settings, phase_cal, /* Not used here */
- &vhv_settingsint, p_phase_cal,
- 0, 1);
+ if ( ( status == VL53L0X_ERROR_NONE ) && ( get_data_enable == 1 ) ) {
+ status = vl53l0x_ref_calibration_io( dev, 1,
+ vhv_settings, phase_cal, /* Not used here */
+ &vhv_settingsint, p_phase_cal,
+ 0, 1 );
} else
*p_phase_cal = 0;
- if ((status == VL53L0X_ERROR_NONE) && restore_config) {
+ if ( ( status == VL53L0X_ERROR_NONE ) && restore_config ) {
/* restore the previous Sequence Config */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- sequence_config);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, sequence_config);
-
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration(VL53L0X_DEV dev,
- uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable)
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ sequence_config );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, sequence_config );
+
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration( VL53L0X_DEV dev,
+ uint8_t *p_vhv_settings, uint8_t *p_phase_cal, uint8_t get_data_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
@@ -2043,33 +2043,33 @@
* this will be reset before the end of the function
*/
- sequence_config = PALDevDataGet(dev, SequenceConfig);
+ sequence_config = PALDevDataGet( dev, SequenceConfig );
/* In the following function we don't save the config to optimize
* writes on device. Config is saved and restored only once. */
status = vl53l0x_perform_vhv_calibration(
- dev, p_vhv_settings, get_data_enable, 0);
-
-
- if (status == VL53L0X_ERROR_NONE)
+ dev, p_vhv_settings, get_data_enable, 0 );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
status = vl53l0x_perform_phase_calibration(
- dev, p_phase_cal, get_data_enable, 0);
-
-
- if (status == VL53L0X_ERROR_NONE) {
+ dev, p_phase_cal, get_data_enable, 0 );
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* restore the previous Sequence Config */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- sequence_config);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, sequence_config);
-
- }
-
- return status;
-}
-
-void VL53L0X::get_next_good_spad(uint8_t good_spad_array[], uint32_t size,
- uint32_t curr, int32_t *p_next)
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ sequence_config );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, sequence_config );
+
+ }
+
+ return status;
+}
+
+void VL53L0X::get_next_good_spad( uint8_t good_spad_array[], uint32_t size,
+ uint32_t curr, int32_t *p_next )
{
uint32_t start_index;
uint32_t fine_offset;
@@ -2092,20 +2092,20 @@
start_index = curr / c_spads_per_byte;
fine_offset = curr % c_spads_per_byte;
- for (coarse_index = start_index; ((coarse_index < size) && !success);
- coarse_index++) {
+ for ( coarse_index = start_index; ( ( coarse_index < size ) && !success );
+ coarse_index++ ) {
fine_index = 0;
data_byte = good_spad_array[coarse_index];
- if (coarse_index == start_index) {
+ if ( coarse_index == start_index ) {
/* locate the bit position of the provided current
* spad bit before iterating */
data_byte >>= fine_offset;
fine_index = fine_offset;
}
- while (fine_index < c_spads_per_byte) {
- if ((data_byte & 0x1) == 1) {
+ while ( fine_index < c_spads_per_byte ) {
+ if ( ( data_byte & 0x1 ) == 1 ) {
success = 1;
*p_next = coarse_index * c_spads_per_byte + fine_index;
break;
@@ -2116,7 +2116,7 @@
}
}
-uint8_t VL53L0X::is_aperture(uint32_t spad_index)
+uint8_t VL53L0X::is_aperture( uint32_t spad_index )
{
/*
* This function reports if a given spad index is an aperture SPAD by
@@ -2125,14 +2125,14 @@
uint32_t quadrant;
uint8_t is_aperture = 1;
quadrant = spad_index >> 6;
- if (refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0)
+ if ( refArrayQuadrants[quadrant] == REF_ARRAY_SPAD_0 )
is_aperture = 0;
return is_aperture;
}
-VL53L0X_Error VL53L0X::enable_spad_bit(uint8_t spad_array[], uint32_t size,
- uint32_t spad_index)
+VL53L0X_Error VL53L0X::enable_spad_bit( uint8_t spad_array[], uint32_t size,
+ uint32_t spad_index )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t c_spads_per_byte = 8;
@@ -2141,29 +2141,29 @@
coarse_index = spad_index / c_spads_per_byte;
fine_index = spad_index % c_spads_per_byte;
- if (coarse_index >= size)
+ if ( coarse_index >= size )
status = VL53L0X_ERROR_REF_SPAD_INIT;
else
- spad_array[coarse_index] |= (1 << fine_index);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::set_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
-{
- VL53L0X_Error status = vl53l0x_write_multi(dev,
+ spad_array[coarse_index] |= ( 1 << fine_index );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::set_ref_spad_map( VL53L0X_DEV dev, uint8_t *p_ref_spad_array )
+{
+ VL53L0X_Error status = vl53l0x_write_multi( dev,
VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
- p_ref_spad_array, 6);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::get_ref_spad_map(VL53L0X_DEV dev, uint8_t *p_ref_spad_array)
-{
- VL53L0X_Error status = vl53l0x_read_multi(dev,
+ p_ref_spad_array, 6 );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::get_ref_spad_map( VL53L0X_DEV dev, uint8_t *p_ref_spad_array )
+{
+ VL53L0X_Error status = vl53l0x_read_multi( dev,
VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0,
p_ref_spad_array,
- 6);
+ 6 );
// VL53L0X_Error status = VL53L0X_ERROR_NONE;
// uint8_t count=0;
@@ -2172,15 +2172,15 @@
return status;
}
-VL53L0X_Error VL53L0X::enable_ref_spads(VL53L0X_DEV dev,
- uint8_t aperture_spads,
- uint8_t good_spad_array[],
- uint8_t spad_array[],
- uint32_t size,
- uint32_t start,
- uint32_t offset,
- uint32_t spad_count,
- uint32_t *p_last_spad)
+VL53L0X_Error VL53L0X::enable_ref_spads( VL53L0X_DEV dev,
+ uint8_t aperture_spads,
+ uint8_t good_spad_array[],
+ uint8_t spad_array[],
+ uint32_t size,
+ uint32_t start,
+ uint32_t offset,
+ uint32_t spad_count,
+ uint32_t *p_last_spad )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t index;
@@ -2200,41 +2200,41 @@
*/
current_spad = offset;
- for (index = 0; index < spad_count; index++) {
- get_next_good_spad(good_spad_array, size, current_spad,
- &next_good_spad);
-
- if (next_good_spad == -1) {
+ for ( index = 0; index < spad_count; index++ ) {
+ get_next_good_spad( good_spad_array, size, current_spad,
+ &next_good_spad );
+
+ if ( next_good_spad == -1 ) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
/* Confirm that the next good SPAD is non-aperture */
- if (is_aperture(start + next_good_spad) != aperture_spads) {
+ if ( is_aperture( start + next_good_spad ) != aperture_spads ) {
/* if we can't get the required number of good aperture
* spads from the current quadrant then this is an error
*/
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
- current_spad = (uint32_t)next_good_spad;
- enable_spad_bit(spad_array, size, current_spad);
+ current_spad = ( uint32_t )next_good_spad;
+ enable_spad_bit( spad_array, size, current_spad );
current_spad++;
}
*p_last_spad = current_spad;
- if (status == VL53L0X_ERROR_NONE)
- status = set_ref_spad_map(dev, spad_array);
-
-
- if (status == VL53L0X_ERROR_NONE) {
- status = get_ref_spad_map(dev, check_spad_array);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = set_ref_spad_map( dev, spad_array );
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = get_ref_spad_map( dev, check_spad_array );
i = 0;
/* Compare spad maps. If not equal report error. */
- while (i < size) {
- if (spad_array[i] != check_spad_array[i]) {
+ while ( i < size ) {
+ if ( spad_array[i] != check_spad_array[i] ) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
@@ -2244,82 +2244,82 @@
return status;
}
-VL53L0X_Error VL53L0X::vl53l0x_set_device_mode(VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode)
+VL53L0X_Error VL53L0X::vl53l0x_set_device_mode( VL53L0X_DEV dev, VL53L0X_DeviceModes device_mode )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("%d", (int)DeviceMode);
-
- switch (device_mode) {
+ LOG_FUNCTION_START( "%d", ( int )DeviceMode );
+
+ switch ( device_mode ) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_RANGING:
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
case VL53L0X_DEVICEMODE_GPIO_DRIVE:
case VL53L0X_DEVICEMODE_GPIO_OSC:
/* Supported modes */
- VL53L0X_SETPARAMETERFIELD(dev, DeviceMode, device_mode);
+ VL53L0X_SETPARAMETERFIELD( dev, DeviceMode, device_mode );
break;
default:
/* Unsupported mode */
status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_interrupt_thresholds(VL53L0X_DEV dev,
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_interrupt_thresholds( VL53L0X_DEV dev,
VL53L0X_DeviceModes device_mode, FixPoint1616_t threshold_low,
- FixPoint1616_t threshold_high)
+ FixPoint1616_t threshold_high )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t threshold16;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* no dependency on DeviceMode for Ewok */
/* Need to divide by 2 because the FW will apply a x2 */
- threshold16 = (uint16_t)((threshold_low >> 17) & 0x00fff);
- status = vl53l0x_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16);
-
- if (status == VL53L0X_ERROR_NONE) {
+ threshold16 = ( uint16_t )( ( threshold_low >> 17 ) & 0x00fff );
+ status = vl53l0x_write_word( dev, VL53L0X_REG_SYSTEM_THRESH_LOW, threshold16 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Need to divide by 2 because the FW will apply a x2 */
- threshold16 = (uint16_t)((threshold_high >> 17) & 0x00fff);
- status = vl53l0x_write_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
- threshold16);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_thresholds(VL53L0X_DEV dev,
+ threshold16 = ( uint16_t )( ( threshold_high >> 17 ) & 0x00fff );
+ status = vl53l0x_write_word( dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
+ threshold16 );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_interrupt_thresholds( VL53L0X_DEV dev,
VL53L0X_DeviceModes device_mode, FixPoint1616_t *p_threshold_low,
- FixPoint1616_t *p_threshold_high)
+ FixPoint1616_t *p_threshold_high )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint16_t threshold16;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* no dependency on DeviceMode for Ewok */
- status = vl53l0x_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16);
+ status = vl53l0x_read_word( dev, VL53L0X_REG_SYSTEM_THRESH_LOW, &threshold16 );
/* Need to multiply by 2 because the FW will apply a x2 */
- *p_threshold_low = (FixPoint1616_t)((0x00fff & threshold16) << 17);
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_word(dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
- &threshold16);
+ *p_threshold_low = ( FixPoint1616_t )( ( 0x00fff & threshold16 ) << 17 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_word( dev, VL53L0X_REG_SYSTEM_THRESH_HIGH,
+ &threshold16 );
/* Need to multiply by 2 because the FW will apply a x2 */
*p_threshold_high =
- (FixPoint1616_t)((0x00fff & threshold16) << 17);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_load_tuning_settings(VL53L0X_DEV dev,
- uint8_t *p_tuning_setting_buffer)
+ ( FixPoint1616_t )( ( 0x00fff & threshold16 ) << 17 );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_load_tuning_settings( VL53L0X_DEV dev,
+ uint8_t *p_tuning_setting_buffer )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
int i;
@@ -2332,112 +2332,112 @@
uint8_t local_buffer[4]; /* max */
uint16_t temp16;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
index = 0;
- while ((*(p_tuning_setting_buffer + index) != 0) &&
- (status == VL53L0X_ERROR_NONE)) {
- number_of_writes = *(p_tuning_setting_buffer + index);
+ while ( ( *( p_tuning_setting_buffer + index ) != 0 ) &&
+ ( status == VL53L0X_ERROR_NONE ) ) {
+ number_of_writes = *( p_tuning_setting_buffer + index );
index++;
- if (number_of_writes == 0xFF) {
+ if ( number_of_writes == 0xFF ) {
/* internal parameters */
- select_param = *(p_tuning_setting_buffer + index);
+ select_param = *( p_tuning_setting_buffer + index );
index++;
- switch (select_param) {
+ switch ( select_param ) {
case 0: /* uint16_t SigmaEstRefArray -> 2 bytes */
- msb = *(p_tuning_setting_buffer + index);
+ msb = *( p_tuning_setting_buffer + index );
index++;
- lsb = *(p_tuning_setting_buffer + index);
+ lsb = *( p_tuning_setting_buffer + index );
index++;
- temp16 = VL53L0X_MAKEUINT16(lsb, msb);
- PALDevDataSet(dev, SigmaEstRefArray, temp16);
+ temp16 = VL53L0X_MAKEUINT16( lsb, msb );
+ PALDevDataSet( dev, SigmaEstRefArray, temp16 );
break;
case 1: /* uint16_t SigmaEstEffPulseWidth -> 2 bytes */
- msb = *(p_tuning_setting_buffer + index);
+ msb = *( p_tuning_setting_buffer + index );
index++;
- lsb = *(p_tuning_setting_buffer + index);
+ lsb = *( p_tuning_setting_buffer + index );
index++;
- temp16 = VL53L0X_MAKEUINT16(lsb, msb);
- PALDevDataSet(dev, SigmaEstEffPulseWidth,
- temp16);
+ temp16 = VL53L0X_MAKEUINT16( lsb, msb );
+ PALDevDataSet( dev, SigmaEstEffPulseWidth,
+ temp16 );
break;
case 2: /* uint16_t SigmaEstEffAmbWidth -> 2 bytes */
- msb = *(p_tuning_setting_buffer + index);
+ msb = *( p_tuning_setting_buffer + index );
index++;
- lsb = *(p_tuning_setting_buffer + index);
+ lsb = *( p_tuning_setting_buffer + index );
index++;
- temp16 = VL53L0X_MAKEUINT16(lsb, msb);
- PALDevDataSet(dev, SigmaEstEffAmbWidth, temp16);
+ temp16 = VL53L0X_MAKEUINT16( lsb, msb );
+ PALDevDataSet( dev, SigmaEstEffAmbWidth, temp16 );
break;
case 3: /* uint16_t targetRefRate -> 2 bytes */
- msb = *(p_tuning_setting_buffer + index);
+ msb = *( p_tuning_setting_buffer + index );
index++;
- lsb = *(p_tuning_setting_buffer + index);
+ lsb = *( p_tuning_setting_buffer + index );
index++;
- temp16 = VL53L0X_MAKEUINT16(lsb, msb);
- PALDevDataSet(dev, targetRefRate, temp16);
+ temp16 = VL53L0X_MAKEUINT16( lsb, msb );
+ PALDevDataSet( dev, targetRefRate, temp16 );
break;
default: /* invalid parameter */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
- } else if (number_of_writes <= 4) {
- address = *(p_tuning_setting_buffer + index);
+ } else if ( number_of_writes <= 4 ) {
+ address = *( p_tuning_setting_buffer + index );
index++;
- for (i = 0; i < number_of_writes; i++) {
- local_buffer[i] = *(p_tuning_setting_buffer +
- index);
+ for ( i = 0; i < number_of_writes; i++ ) {
+ local_buffer[i] = *( p_tuning_setting_buffer +
+ index );
index++;
}
- status = vl53l0x_write_multi(dev, address, local_buffer,
- number_of_writes);
+ status = vl53l0x_write_multi( dev, address, local_buffer,
+ number_of_writes );
} else {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_check_and_load_interrupt_settings(VL53L0X_DEV dev,
- uint8_t start_not_stopflag)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_check_and_load_interrupt_settings( VL53L0X_DEV dev,
+ uint8_t start_not_stopflag )
{
uint8_t interrupt_config;
FixPoint1616_t threshold_low;
FixPoint1616_t threshold_high;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- Pin0GpioFunctionality);
-
- if ((interrupt_config ==
- VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW) ||
- (interrupt_config ==
- VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH) ||
- (interrupt_config ==
- VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT)) {
-
- status = vl53l0x_get_interrupt_thresholds(dev,
+ interrupt_config = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ Pin0GpioFunctionality );
+
+ if ( ( interrupt_config ==
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW ) ||
+ ( interrupt_config ==
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH ) ||
+ ( interrupt_config ==
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT ) ) {
+
+ status = vl53l0x_get_interrupt_thresholds( dev,
VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
- &threshold_low, &threshold_high);
-
- if (((threshold_low > 255*65536) ||
- (threshold_high > 255*65536)) &&
- (status == VL53L0X_ERROR_NONE)) {
-
- if (start_not_stopflag != 0) {
- status = vl53l0x_load_tuning_settings(dev,
- InterruptThresholdSettings);
+ &threshold_low, &threshold_high );
+
+ if ( ( ( threshold_low > 255 * 65536 ) ||
+ ( threshold_high > 255 * 65536 ) ) &&
+ ( status == VL53L0X_ERROR_NONE ) ) {
+
+ if ( start_not_stopflag != 0 ) {
+ status = vl53l0x_load_tuning_settings( dev,
+ InterruptThresholdSettings );
} else {
- status |= vl53l0x_write_byte(dev, 0xFF, 0x04);
- status |= vl53l0x_write_byte(dev, 0x70, 0x00);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
- status |= vl53l0x_write_byte(dev, 0x80, 0x00);
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x04 );
+ status |= vl53l0x_write_byte( dev, 0x70, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0x80, 0x00 );
}
}
@@ -2449,44 +2449,44 @@
}
-VL53L0X_Error VL53L0X::vl53l0x_start_measurement(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::vl53l0x_start_measurement( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes device_mode;
uint8_t byte;
uint8_t start_stop_byte = VL53L0X_REG_SYSRANGE_MODE_START_STOP;
uint32_t loop_nb;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* Get Current DeviceMode */
- vl53l0x_get_device_mode(dev, &device_mode);
-
- status = vl53l0x_write_byte(dev, 0x80, 0x01);
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
- status = vl53l0x_write_byte(dev, 0x00, 0x00);
- status = vl53l0x_write_byte(dev, 0x91, PALDevDataGet(dev, StopVariable));
- status = vl53l0x_write_byte(dev, 0x00, 0x01);
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
- status = vl53l0x_write_byte(dev, 0x80, 0x00);
-
- switch (device_mode) {
+ vl53l0x_get_device_mode( dev, &device_mode );
+
+ status = vl53l0x_write_byte( dev, 0x80, 0x01 );
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status = vl53l0x_write_byte( dev, 0x00, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x91, PALDevDataGet( dev, StopVariable ) );
+ status = vl53l0x_write_byte( dev, 0x00, 0x01 );
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x80, 0x00 );
+
+ switch ( device_mode ) {
case VL53L0X_DEVICEMODE_SINGLE_RANGING:
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSRANGE_START, 0x01);
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START, 0x01 );
byte = start_stop_byte;
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Wait until start bit has been cleared */
loop_nb = 0;
do {
- if (loop_nb > 0)
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_SYSRANGE_START, &byte);
+ if ( loop_nb > 0 )
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_SYSRANGE_START, &byte );
loop_nb = loop_nb + 1;
- } while (((byte & start_stop_byte) == start_stop_byte)
- && (status == VL53L0X_ERROR_NONE)
- && (loop_nb < VL53L0X_DEFAULT_MAX_LOOP));
-
- if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP)
+ } while ( ( ( byte & start_stop_byte ) == start_stop_byte )
+ && ( status == VL53L0X_ERROR_NONE )
+ && ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP ) );
+
+ if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP )
status = VL53L0X_ERROR_TIME_OUT;
}
@@ -2496,30 +2496,30 @@
/* Back-to-back mode */
/* Check if need to apply interrupt settings */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_check_and_load_interrupt_settings(dev, 1);
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSRANGE_START,
- VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK);
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_check_and_load_interrupt_settings( dev, 1 );
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSRANGE_START,
+ VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK );
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Set PAL State to Running */
- PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_RUNNING );
}
break;
case VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING:
/* Continuous mode */
/* Check if need to apply interrupt settings */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_check_and_load_interrupt_settings(dev, 1);
-
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSRANGE_START,
- VL53L0X_REG_SYSRANGE_MODE_TIMED);
-
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_check_and_load_interrupt_settings( dev, 1 );
+
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSRANGE_START,
+ VL53L0X_REG_SYSRANGE_MODE_TIMED );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Set PAL State to Running */
- PALDevDataSet(dev, PalState, VL53L0X_STATE_RUNNING);
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_RUNNING );
}
break;
default:
@@ -2528,59 +2528,59 @@
}
- LOG_FUNCTION_END(status);
+ LOG_FUNCTION_END( status );
return status;
}
/* Group PAL Measurement Functions */
-VL53L0X_Error VL53L0X::vl53l0x_perform_single_measurement(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::vl53l0x_perform_single_measurement( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceModes device_mode;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* Get Current DeviceMode */
- status = vl53l0x_get_device_mode(dev, &device_mode);
+ status = vl53l0x_get_device_mode( dev, &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(dev);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_measurement_poll_for_completion(dev);
+ if ( status == VL53L0X_ERROR_NONE
+ && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING )
+ status = vl53l0x_start_measurement( dev );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_measurement_poll_for_completion( dev );
/* Change PAL State in case of single ranging or single histogram */
- if (status == VL53L0X_ERROR_NONE
- && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING)
- PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
-
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_enable(VL53L0X_DEV dev,
- uint8_t *p_x_talk_compensation_enable)
+ if ( status == VL53L0X_ERROR_NONE
+ && device_mode == VL53L0X_DEVICEMODE_SINGLE_RANGING )
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
+
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_x_talk_compensation_enable( VL53L0X_DEV dev,
+ uint8_t *p_x_talk_compensation_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t temp8;
- LOG_FUNCTION_START("");
-
- VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable, temp8);
+ LOG_FUNCTION_START( "" );
+
+ VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable, temp8 );
*p_x_talk_compensation_enable = temp8;
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_total_xtalk_rate(VL53L0X_DEV dev,
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_total_xtalk_rate( VL53L0X_DEV dev,
VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
- FixPoint1616_t *p_total_xtalk_rate_mcps)
+ FixPoint1616_t *p_total_xtalk_rate_mcps )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
@@ -2590,15 +2590,15 @@
*p_total_xtalk_rate_mcps = 0;
- status = vl53l0x_get_x_talk_compensation_enable(dev, &xtalk_comp_enable);
- if (status == VL53L0X_ERROR_NONE) {
-
- if (xtalk_comp_enable) {
+ status = vl53l0x_get_x_talk_compensation_enable( dev, &xtalk_comp_enable );
+ if ( status == VL53L0X_ERROR_NONE ) {
+
+ if ( xtalk_comp_enable ) {
VL53L0X_GETPARAMETERFIELD(
dev,
XTalkCompensationRateMegaCps,
- xtalk_per_spad_mega_cps);
+ xtalk_per_spad_mega_cps );
/* FixPoint1616 * FixPoint 8:8 = FixPoint0824 */
total_xtalk_mega_cps =
@@ -2607,54 +2607,54 @@
/* FixPoint0824 >> 8 = FixPoint1616 */
*p_total_xtalk_rate_mcps =
- (total_xtalk_mega_cps + 0x80) >> 8;
+ ( total_xtalk_mega_cps + 0x80 ) >> 8;
}
}
return status;
}
-VL53L0X_Error VL53L0X::vl53l0x_get_total_signal_rate(VL53L0X_DEV dev,
+VL53L0X_Error VL53L0X::vl53l0x_get_total_signal_rate( VL53L0X_DEV dev,
VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
- FixPoint1616_t *p_total_signal_rate_mcps)
+ FixPoint1616_t *p_total_signal_rate_mcps )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t total_xtalk_mega_cps;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
*p_total_signal_rate_mcps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
status = vl53l0x_get_total_xtalk_rate(
- dev, p_ranging_measurement_data, &total_xtalk_mega_cps);
-
- if (status == VL53L0X_ERROR_NONE)
+ dev, p_ranging_measurement_data, &total_xtalk_mega_cps );
+
+ if ( status == VL53L0X_ERROR_NONE )
*p_total_signal_rate_mcps += total_xtalk_mega_cps;
return status;
}
/* To convert ms into register value */
-uint32_t VL53L0X::vl53l0x_calc_timeout_mclks(VL53L0X_DEV dev,
+uint32_t VL53L0X::vl53l0x_calc_timeout_mclks( VL53L0X_DEV dev,
uint32_t timeout_period_us,
- uint8_t vcsel_period_pclks)
+ uint8_t vcsel_period_pclks )
{
uint32_t macro_period_ps;
uint32_t macro_period_ns;
uint32_t timeout_period_mclks = 0;
- macro_period_ps = vl53l0x_calc_macro_period_ps(dev, vcsel_period_pclks);
- macro_period_ns = (macro_period_ps + 500) / 1000;
+ macro_period_ps = vl53l0x_calc_macro_period_ps( dev, vcsel_period_pclks );
+ macro_period_ns = ( macro_period_ps + 500 ) / 1000;
timeout_period_mclks =
- (uint32_t) (((timeout_period_us * 1000)
- + (macro_period_ns / 2)) / macro_period_ns);
+ ( 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)
+uint32_t VL53L0X::vl53l0x_isqrt( uint32_t num )
{
/*
* Implements an integer square root
@@ -2668,14 +2668,14 @@
* 1 << 14 for 16-bits, 1 << 30 for 32 bits */
/* "bit" starts at the highest power of four <= the argument. */
- while (bit > num)
+ while ( bit > num )
bit >>= 2;
- while (bit != 0) {
- if (num >= res + bit) {
+ while ( bit != 0 ) {
+ if ( num >= res + bit ) {
num -= res + bit;
- res = (res >> 1) + bit;
+ res = ( res >> 1 ) + bit;
} else
res >>= 1;
@@ -2685,6 +2685,28 @@
return res;
}
+uint32_t VL53L0X::vl53l0x_quadrature_sum( uint32_t a, uint32_t b )
+{
+ /*
+ * Implements a quadrature sum
+ *
+ * rea = sqrt(a^2 + b^2)
+ *
+ * Trap overflow case max input value is 65535 (16-bit value)
+ * as internal calc are 32-bit wide
+ *
+ * If overflow then seta output to maximum
+ */
+ uint32_t res = 0;
+
+ if ( a > 65535 || b > 65535 )
+ res = 65535;
+ else
+ res = vl53l0x_isqrt( a * a + b * b );
+
+ return res;
+}
+
VL53L0X_Error VL53L0X::vl53l0x_calc_dmax(
VL53L0X_DEV dev,
FixPoint1616_t total_signal_rate_mcps,
@@ -2693,7 +2715,7 @@
uint32_t sigma_estimate_p1,
FixPoint1616_t sigma_estimate_p2,
uint32_t peak_vcsel_duration_us,
- uint32_t *pd_max_mm)
+ uint32_t *pd_max_mm )
{
const uint32_t c_sigma_limit = 18;
const FixPoint1616_t c_signal_limit = 0x4000; /* 0.25 */
@@ -2719,23 +2741,23 @@
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
dmax_cal_range_mm =
- PALDevDataGet(dev, DmaxCalRangeMilliMeter);
+ PALDevDataGet( dev, DmaxCalRangeMilliMeter );
dmax_cal_signal_rate_rtn_mcps =
- PALDevDataGet(dev, DmaxCalSignalRateRtnMegaCps);
+ PALDevDataGet( dev, DmaxCalSignalRateRtnMegaCps );
/* uint32 * FixPoint1616 = FixPoint1616 */
signal_at0_mm = dmax_cal_range_mm * dmax_cal_signal_rate_rtn_mcps;
/* FixPoint1616 >> 8 = FixPoint2408 */
- signal_at0_mm = (signal_at0_mm + 0x80) >> 8;
+ signal_at0_mm = ( signal_at0_mm + 0x80 ) >> 8;
signal_at0_mm *= dmax_cal_range_mm;
min_signal_needed_p1 = 0;
- if (total_corr_signal_rate_mcps > 0) {
+ if ( total_corr_signal_rate_mcps > 0 ) {
/* Shift by 10 bits to increase resolution prior to the
* division */
@@ -2743,7 +2765,7 @@
/* Add rounding value prior to division */
min_signal_needed_p1 = signal_rate_temp_mcps +
- (total_corr_signal_rate_mcps/2);
+ ( total_corr_signal_rate_mcps / 2 );
/* FixPoint0626/FixPoint1616 = FixPoint2210 */
min_signal_needed_p1 /= total_corr_signal_rate_mcps;
@@ -2756,13 +2778,13 @@
min_signal_needed_p1 *= min_signal_needed_p1;
/* FixPoint1220 >> 16 = FixPoint2804 */
- min_signal_needed_p1 = (min_signal_needed_p1 + 0x8000) >> 16;
+ min_signal_needed_p1 = ( min_signal_needed_p1 + 0x8000 ) >> 16;
}
min_signal_needed_p2 = pw_mult * sigma_estimate_p1;
/* FixPoint1616 >> 16 = uint32 */
- min_signal_needed_p2 = (min_signal_needed_p2 + 0x8000) >> 16;
+ min_signal_needed_p2 = ( min_signal_needed_p2 + 0x8000 ) >> 16;
/* uint32 * uint32 = uint32 */
min_signal_needed_p2 *= min_signal_needed_p2;
@@ -2772,12 +2794,12 @@
* to calculate dmax value so set a suitable value to ensure
* 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)/
+ 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;
sigma_est_p2_tmp *= c_amb_eff_width_d_max_ns;
- if (sigma_est_p2_tmp > 0xffff) {
+ if ( sigma_est_p2_tmp > 0xffff ) {
min_signal_needed_p3 = 0xfff00000;
} else {
@@ -2785,19 +2807,19 @@
* correction.
* Perform division before multiplication to prevent overflow.
*/
- sigma_estimate_p2 = (sigma_estimate_p2 + c_amb_eff_width_sigma_est_ns/2)/
+ 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 = ( sigma_estimate_p2 + 0x8000 ) >> 16;
min_signal_needed_p3 *= min_signal_needed_p3;
}
/* FixPoint1814 / uint32 = FixPoint1814 */
- sigma_limit_tmp = ((c_sigma_limit << 14) + 500) / 1000;
+ sigma_limit_tmp = ( ( c_sigma_limit << 14 ) + 500 ) / 1000;
/* FixPoint1814 * FixPoint1814 = FixPoint3628 := FixPoint0428 */
sigma_limit_tmp *= sigma_limit_tmp;
@@ -2806,7 +2828,7 @@
sigma_est_sq_tmp = c_sigma_est_ref * c_sigma_est_ref;
/* FixPoint3232 >> 4 = FixPoint0428 */
- sigma_est_sq_tmp = (sigma_est_sq_tmp + 0x08) >> 4;
+ sigma_est_sq_tmp = ( sigma_est_sq_tmp + 0x08 ) >> 4;
/* FixPoint0428 - FixPoint0428 = FixPoint0428 */
sigma_limit_tmp -= sigma_est_sq_tmp;
@@ -2815,20 +2837,20 @@
min_signal_needed_p4 = 4 * 12 * sigma_limit_tmp;
/* FixPoint0428 >> 14 = FixPoint1814 */
- min_signal_needed_p4 = (min_signal_needed_p4 + 0x2000) >> 14;
+ min_signal_needed_p4 = ( min_signal_needed_p4 + 0x2000 ) >> 14;
/* uint32 + uint32 = uint32 */
- min_signal_needed = (min_signal_needed_p2 + min_signal_needed_p3);
+ min_signal_needed = ( min_signal_needed_p2 + min_signal_needed_p3 );
/* uint32 / uint32 = uint32 */
- min_signal_needed += (peak_vcsel_duration_us/2);
+ min_signal_needed += ( peak_vcsel_duration_us / 2 );
min_signal_needed /= peak_vcsel_duration_us;
/* uint32 << 14 = FixPoint1814 */
min_signal_needed <<= 14;
/* FixPoint1814 / FixPoint1814 = uint32 */
- min_signal_needed += (min_signal_needed_p4/2);
+ min_signal_needed += ( min_signal_needed_p4 / 2 );
min_signal_needed /= min_signal_needed_p4;
/* FixPoint3200 * FixPoint2804 := FixPoint2804*/
@@ -2841,45 +2863,45 @@
* handle the larger and smaller elements of this equation,
* i.e. speed of light and pulse widths.
*/
- min_signal_needed = (min_signal_needed + 500) / 1000;
+ min_signal_needed = ( min_signal_needed + 500 ) / 1000;
min_signal_needed <<= 4;
- min_signal_needed = (min_signal_needed + 500) / 1000;
+ min_signal_needed = ( min_signal_needed + 500 ) / 1000;
/* FixPoint1616 >> 8 = FixPoint2408 */
- signal_limit_tmp = (c_signal_limit + 0x80) >> 8;
+ signal_limit_tmp = ( c_signal_limit + 0x80 ) >> 8;
/* FixPoint2408/FixPoint2408 = uint32 */
- if (signal_limit_tmp != 0)
- dmax_dark_tmp = (signal_at0_mm + (signal_limit_tmp / 2))
+ if ( signal_limit_tmp != 0 )
+ dmax_dark_tmp = ( signal_at0_mm + ( signal_limit_tmp / 2 ) )
/ signal_limit_tmp;
else
dmax_dark_tmp = 0;
- dmax_dark = vl53l0x_isqrt(dmax_dark_tmp);
+ dmax_dark = vl53l0x_isqrt( dmax_dark_tmp );
/* FixPoint2408/FixPoint2408 = uint32 */
- if (min_signal_needed != 0)
- dmax_ambient = (signal_at0_mm + min_signal_needed/2)
+ if ( min_signal_needed != 0 )
+ dmax_ambient = ( signal_at0_mm + min_signal_needed / 2 )
/ min_signal_needed;
else
dmax_ambient = 0;
- dmax_ambient = vl53l0x_isqrt(dmax_ambient);
+ dmax_ambient = vl53l0x_isqrt( dmax_ambient );
*pd_max_mm = dmax_dark;
- if (dmax_dark > dmax_ambient)
+ if ( dmax_dark > dmax_ambient )
*pd_max_mm = dmax_ambient;
- LOG_FUNCTION_END(status);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_calc_sigma_estimate(VL53L0X_DEV dev,
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_calc_sigma_estimate( VL53L0X_DEV dev,
VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
FixPoint1616_t *p_sigma_estimate,
- uint32_t *p_dmax_mm)
+ 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;
@@ -2889,7 +2911,7 @@
const uint32_t c_vcsel_pulse_width_ps = 4700; /* pico secs */
const FixPoint1616_t c_sigma_est_max = 0x028F87AE;
const FixPoint1616_t c_sigma_est_rtn_max = 0xF000;
- const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000/
+ const FixPoint1616_t c_amb_to_signal_ratio_max = 0xF0000000 /
c_ambient_effective_width_centi_ns;
/* Time Of Flight per mm (6.6 pico secs) */
const FixPoint1616_t c_tof_per_mm_ps = 0x0006999A;
@@ -2936,10 +2958,10 @@
* Estimates the range sigma
*/
- LOG_FUNCTION_START("");
-
- VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationRateMegaCps,
- x_talk_comp_rate_mcps);
+ LOG_FUNCTION_START( "" );
+
+ VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
+ x_talk_comp_rate_mcps );
/*
* We work in kcps rather than mcps as this helps keep within the
@@ -2947,101 +2969,101 @@
*/
ambient_rate_kcps =
- (p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000) >> 16;
+ ( p_ranging_measurement_data->AmbientRateRtnMegaCps * 1000 ) >> 16;
corrected_signal_rate_mcps =
p_ranging_measurement_data->SignalRateRtnMegaCps;
status = vl53l0x_get_total_signal_rate(
- dev, p_ranging_measurement_data, &total_signal_rate_mcps);
+ dev, p_ranging_measurement_data, &total_signal_rate_mcps );
status = vl53l0x_get_total_xtalk_rate(
- dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps);
+ dev, p_ranging_measurement_data, &x_talk_comp_rate_mcps );
/* Signal rate measurement provided by device is the
* peak signal rate, not average.
*/
- peak_signal_rate_kcps = (total_signal_rate_mcps * 1000);
- peak_signal_rate_kcps = (peak_signal_rate_kcps + 0x8000) >> 16;
+ peak_signal_rate_kcps = ( total_signal_rate_mcps * 1000 );
+ peak_signal_rate_kcps = ( peak_signal_rate_kcps + 0x8000 ) >> 16;
x_talk_comp_rate_kcps = x_talk_comp_rate_mcps * 1000;
- if (x_talk_comp_rate_kcps > c_max_x_talk_kcps)
+ if ( x_talk_comp_rate_kcps > c_max_x_talk_kcps )
x_talk_comp_rate_kcps = c_max_x_talk_kcps;
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Calculate final range macro periods */
final_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
- dev, FinalRangeTimeoutMicroSecs);
+ dev, FinalRangeTimeoutMicroSecs );
final_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
- dev, FinalRangeVcselPulsePeriod);
+ dev, FinalRangeVcselPulsePeriod );
final_range_macro_pclks = vl53l0x_calc_timeout_mclks(
- dev, final_range_timeout_micro_secs, final_range_vcsel_pclks);
+ dev, final_range_timeout_micro_secs, final_range_vcsel_pclks );
/* Calculate pre-range macro periods */
pre_range_timeout_micro_secs = VL53L0X_GETDEVICESPECIFICPARAMETER(
- dev, PreRangeTimeoutMicroSecs);
+ dev, PreRangeTimeoutMicroSecs );
pre_range_vcsel_pclks = VL53L0X_GETDEVICESPECIFICPARAMETER(
- dev, PreRangeVcselPulsePeriod);
+ dev, PreRangeVcselPulsePeriod );
pre_range_macro_pclks = vl53l0x_calc_timeout_mclks(
- dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks);
+ dev, pre_range_timeout_micro_secs, pre_range_vcsel_pclks );
vcsel_width = 3;
- if (final_range_vcsel_pclks == 8)
+ if ( final_range_vcsel_pclks == 8 )
vcsel_width = 2;
peak_vcsel_duration_us = vcsel_width * 2048 *
- (pre_range_macro_pclks + final_range_macro_pclks);
- peak_vcsel_duration_us = (peak_vcsel_duration_us + 500)/1000;
+ ( pre_range_macro_pclks + final_range_macro_pclks );
+ peak_vcsel_duration_us = ( peak_vcsel_duration_us + 500 ) / 1000;
peak_vcsel_duration_us *= c_pll_period_ps;
- peak_vcsel_duration_us = (peak_vcsel_duration_us + 500)/1000;
+ peak_vcsel_duration_us = ( peak_vcsel_duration_us + 500 ) / 1000;
/* Fix1616 >> 8 = Fix2408 */
- total_signal_rate_mcps = (total_signal_rate_mcps + 0x80) >> 8;
+ total_signal_rate_mcps = ( total_signal_rate_mcps + 0x80 ) >> 8;
/* Fix2408 * uint32 = Fix2408 */
vcsel_total_events_rtn = total_signal_rate_mcps *
peak_vcsel_duration_us;
/* Fix2408 >> 8 = uint32 */
- vcsel_total_events_rtn = (vcsel_total_events_rtn + 0x80) >> 8;
+ vcsel_total_events_rtn = ( vcsel_total_events_rtn + 0x80 ) >> 8;
/* Fix2408 << 8 = Fix1616 = */
total_signal_rate_mcps <<= 8;
}
- if (status != VL53L0X_ERROR_NONE) {
- LOG_FUNCTION_END(status);
+ if ( status != VL53L0X_ERROR_NONE ) {
+ LOG_FUNCTION_END( status );
return status;
}
- if (peak_signal_rate_kcps == 0) {
+ if ( peak_signal_rate_kcps == 0 ) {
*p_sigma_estimate = c_sigma_est_max;
- PALDevDataSet(dev, SigmaEstimate, c_sigma_est_max);
+ PALDevDataSet( dev, SigmaEstimate, c_sigma_est_max );
*p_dmax_mm = 0;
} else {
- if (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;
/* ((FixPoint1616 << 16)* uint32)/uint32 = FixPoint1616 */
- sigma_estimate_p2 = (ambient_rate_kcps << 16)/peak_signal_rate_kcps;
- if (sigma_estimate_p2 > c_amb_to_signal_ratio_max) {
+ sigma_estimate_p2 = ( ambient_rate_kcps << 16 ) / peak_signal_rate_kcps;
+ if ( sigma_estimate_p2 > c_amb_to_signal_ratio_max ) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigma_estimate_p2 = c_amb_to_signal_ratio_max;
}
sigma_estimate_p2 *= c_ambient_effective_width_centi_ns;
- sigma_estimate_p3 = 2 * vl53l0x_isqrt(vcsel_total_events_rtn * 12);
+ sigma_estimate_p3 = 2 * vl53l0x_isqrt( vcsel_total_events_rtn * 12 );
/* uint32 * FixPoint1616 = FixPoint1616 */
delta_t_ps = p_ranging_measurement_data->RangeMilliMeter *
@@ -3054,11 +3076,11 @@
* 500 is added to ensure rounding when integer division
* truncates.
*/
- diff1_mcps = (((peak_signal_rate_kcps << 16) -
- 2 * x_talk_comp_rate_kcps) + 500)/1000;
+ diff1_mcps = ( ( ( peak_signal_rate_kcps << 16 ) -
+ 2 * x_talk_comp_rate_kcps ) + 500 ) / 1000;
/* vcselRate + xtalkCompRate */
- diff2_mcps = ((peak_signal_rate_kcps << 16) + 500)/1000;
+ diff2_mcps = ( ( peak_signal_rate_kcps << 16 ) + 500 ) / 1000;
/* Shift by 8 bits to increase resolution prior to the
* division */
@@ -3067,29 +3089,29 @@
/* FixPoint0824/FixPoint1616 = FixPoint2408 */
// xTalkCorrection = abs(diff1_mcps/diff2_mcps);
// abs is causing compiler overloading isue in C++, but unsigned types. So, redundant call anyway!
- x_talk_correction = diff1_mcps/diff2_mcps;
+ x_talk_correction = diff1_mcps / diff2_mcps;
/* FixPoint2408 << 8 = FixPoint1616 */
x_talk_correction <<= 8;
- if(p_ranging_measurement_data->RangeStatus != 0) {
+ if( p_ranging_measurement_data->RangeStatus != 0 ) {
pw_mult = 1 << 16;
} else {
/* FixPoint1616/uint32 = FixPoint1616 */
- pw_mult = delta_t_ps/c_vcsel_pulse_width_ps; /* smaller than 1.0f */
+ 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.
*/
- pw_mult *= ((1 << 16) - x_talk_correction);
+ pw_mult *= ( ( 1 << 16 ) - x_talk_correction );
/* (FixPoint3232 >> 16) = FixPoint1616 */
- pw_mult = (pw_mult + c_16bit_rounding_param) >> 16;
+ pw_mult = ( pw_mult + c_16bit_rounding_param ) >> 16;
/* FixPoint1616 + FixPoint1616 = FixPoint1616 */
- pw_mult += (1 << 16);
+ pw_mult += ( 1 << 16 );
/*
* At this point the value will be 1.xx, therefore if we square
@@ -3108,7 +3130,7 @@
sqr1 = pw_mult * sigma_estimate_p1;
/* (FixPoint1616 >> 16) = FixPoint3200 */
- sqr1 = (sqr1 + 0x8000) >> 16;
+ sqr1 = ( sqr1 + 0x8000 ) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr1 *= sqr1;
@@ -3116,7 +3138,7 @@
sqr2 = sigma_estimate_p2;
/* (FixPoint1616 >> 16) = FixPoint3200 */
- sqr2 = (sqr2 + 0x8000) >> 16;
+ sqr2 = ( sqr2 + 0x8000 ) >> 16;
/* FixPoint3200 * FixPoint3200 = FixPoint6400 */
sqr2 *= sqr2;
@@ -3125,7 +3147,7 @@
sqr_sum = sqr1 + sqr2;
/* SQRT(FixPoin6400) = FixPoint3200 */
- sqrt_result_centi_ns = vl53l0x_isqrt(sqr_sum);
+ sqrt_result_centi_ns = vl53l0x_isqrt( sqr_sum );
/* (FixPoint3200 << 16) = FixPoint1616 */
sqrt_result_centi_ns <<= 16;
@@ -3135,33 +3157,33 @@
* seconds (2997) Therefore to get mm/ns we have to divide by
* 10000
*/
- sigma_est_rtn = (((sqrt_result_centi_ns+50)/100) /
- sigma_estimate_p3);
+ sigma_est_rtn = ( ( ( sqrt_result_centi_ns + 50 ) / 100 ) /
+ sigma_estimate_p3 );
sigma_est_rtn *= VL53L0X_SPEED_OF_LIGHT_IN_AIR;
/* Add 5000 before dividing by 10000 to ensure rounding. */
sigma_est_rtn += 5000;
sigma_est_rtn /= 10000;
- if (sigma_est_rtn > c_sigma_est_rtn_max) {
+ if ( sigma_est_rtn > c_sigma_est_rtn_max ) {
/* Clip to prevent overflow. Will ensure safe
* max result. */
sigma_est_rtn = c_sigma_est_rtn_max;
}
final_range_integration_time_milli_secs =
- (final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500)/1000;
+ ( final_range_timeout_micro_secs + pre_range_timeout_micro_secs + 500 ) / 1000;
/* sigmaEstRef = 1mm * 25ms/final range integration time (inc pre-range)
* sqrt(FixPoint1616/int) = FixPoint2408)
*/
sigma_est_ref =
- vl53l0x_isqrt((c_dflt_final_range_integration_time_milli_secs +
- final_range_integration_time_milli_secs/2)/
- final_range_integration_time_milli_secs);
+ vl53l0x_isqrt( ( c_dflt_final_range_integration_time_milli_secs +
+ final_range_integration_time_milli_secs / 2 ) /
+ final_range_integration_time_milli_secs );
/* FixPoint2408 << 8 = FixPoint1616 */
sigma_est_ref <<= 8;
- sigma_est_ref = (sigma_est_ref + 500)/1000;
+ sigma_est_ref = ( sigma_est_ref + 500 ) / 1000;
/* FixPoint1616 * FixPoint1616 = FixPoint3232 */
sqr1 = sigma_est_rtn * sigma_est_rtn;
@@ -3169,7 +3191,7 @@
sqr2 = sigma_est_ref * sigma_est_ref;
/* sqrt(FixPoint3232) = FixPoint1616 */
- sqrt_result = vl53l0x_isqrt((sqr1 + sqr2));
+ sqrt_result = vl53l0x_isqrt( ( sqr1 + sqr2 ) );
/*
* Note that the Shift by 4 bits increases resolution prior to
* the sqrt, therefore the result must be shifted by 2 bits to
@@ -3178,13 +3200,13 @@
sigma_estimate = 1000 * sqrt_result;
- if ((peak_signal_rate_kcps < 1) || (vcsel_total_events_rtn < 1) ||
- (sigma_estimate > c_sigma_est_max)) {
+ if ( ( peak_signal_rate_kcps < 1 ) || ( vcsel_total_events_rtn < 1 ) ||
+ ( sigma_estimate > c_sigma_est_max ) ) {
sigma_estimate = c_sigma_est_max;
}
- *p_sigma_estimate = (uint32_t)(sigma_estimate);
- PALDevDataSet(dev, SigmaEstimate, *p_sigma_estimate);
+ *p_sigma_estimate = ( uint32_t )( sigma_estimate );
+ PALDevDataSet( dev, SigmaEstimate, *p_sigma_estimate );
status = vl53l0x_calc_dmax(
dev,
total_signal_rate_mcps,
@@ -3193,19 +3215,19 @@
sigma_estimate_p1,
sigma_estimate_p2,
peak_vcsel_duration_us,
- p_dmax_mm);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_pal_range_status(VL53L0X_DEV dev,
+ p_dmax_mm );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_pal_range_status( VL53L0X_DEV dev,
uint8_t device_range_status,
FixPoint1616_t signal_rate,
uint16_t effective_spad_rtn_count,
VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data,
- uint8_t *p_pal_range_status)
+ uint8_t *p_pal_range_status )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t none_flag;
@@ -3227,7 +3249,7 @@
uint32_t dmax_mm = 0;
FixPoint1616_t last_signal_ref_mcps;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/*
@@ -3238,9 +3260,9 @@
* DeviceRangeStatus, this will be added in the PalRangeStatus.
*/
- device_range_status_internal = ((device_range_status & 0x78) >> 3);
-
- if (device_range_status_internal == 0 ||
+ device_range_status_internal = ( ( device_range_status & 0x78 ) >> 3 );
+
+ if ( device_range_status_internal == 0 ||
device_range_status_internal == 5 ||
device_range_status_internal == 7 ||
device_range_status_internal == 12 ||
@@ -3257,12 +3279,12 @@
* 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(dev,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
- &sigma_limit_check_enable);
-
- if ((sigma_limit_check_enable != 0) && (status == VL53L0X_ERROR_NONE)) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_limit_check_enable( dev,
+ 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
*/
@@ -3270,17 +3292,17 @@
dev,
p_ranging_measurement_data,
&sigma_estimate,
- &dmax_mm);
- if (status == VL53L0X_ERROR_NONE)
+ &dmax_mm );
+ if ( status == VL53L0X_ERROR_NONE )
p_ranging_measurement_data->RangeDMaxMilliMeter = dmax_mm;
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_limit_check_value(dev,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
- &sigma_limit_value);
-
- if ((sigma_limit_value > 0) &&
- (sigma_estimate > sigma_limit_value))
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
+ &sigma_limit_value );
+
+ if ( ( sigma_limit_value > 0 ) &&
+ ( sigma_estimate > sigma_limit_value ) )
/* Limit Fail */
sigma_limitflag = 1;
}
@@ -3290,35 +3312,35 @@
* 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(dev,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
- &signal_ref_clip_limit_check_enable);
-
- if ((signal_ref_clip_limit_check_enable != 0) &&
- (status == VL53L0X_ERROR_NONE)) {
-
- status = vl53l0x_get_limit_check_value(dev,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
- &signal_ref_clip_value);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ &signal_ref_clip_limit_check_enable );
+
+ if ( ( signal_ref_clip_limit_check_enable != 0 ) &&
+ ( status == VL53L0X_ERROR_NONE ) ) {
+
+ status = vl53l0x_get_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ &signal_ref_clip_value );
/* Read LastSignalRefMcps from device */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
- &tmp_word);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616(tmp_word);
- PALDevDataSet(dev, LastSignalRefMcps, last_signal_ref_mcps);
-
- if ((signal_ref_clip_value > 0) &&
- (last_signal_ref_mcps > signal_ref_clip_value)) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
+ &tmp_word );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ last_signal_ref_mcps = VL53L0X_FIXPOINT97TOFIXPOINT1616( tmp_word );
+ PALDevDataSet( dev, LastSignalRefMcps, last_signal_ref_mcps );
+
+ if ( ( signal_ref_clip_value > 0 ) &&
+ ( last_signal_ref_mcps > signal_ref_clip_value ) ) {
/* Limit Fail */
signal_ref_clipflag = 1;
}
@@ -3330,51 +3352,51 @@
* EffectiveSpadRtnCount has a format 8.8
* If (Return signal rate < (1.5 x Xtalk x number of Spads)) : FAIL
*/
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- &range_ignore_threshold_limit_check_enable);
-
- if ((range_ignore_threshold_limit_check_enable != 0) &&
- (status == VL53L0X_ERROR_NONE)) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ &range_ignore_threshold_limit_check_enable );
+
+ if ( ( range_ignore_threshold_limit_check_enable != 0 ) &&
+ ( status == VL53L0X_ERROR_NONE ) ) {
/* Compute the signal rate per spad */
- if (effective_spad_rtn_count == 0) {
+ if ( effective_spad_rtn_count == 0 ) {
signal_rate_per_spad = 0;
} else {
- signal_rate_per_spad = (FixPoint1616_t)((256 * signal_rate)
- / effective_spad_rtn_count);
+ signal_rate_per_spad = ( FixPoint1616_t )( ( 256 * signal_rate )
+ / effective_spad_rtn_count );
}
- status = vl53l0x_get_limit_check_value(dev,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- &range_ignore_threshold_value);
-
- if ((range_ignore_threshold_value > 0) &&
- (signal_rate_per_spad < range_ignore_threshold_value)) {
+ status = vl53l0x_get_limit_check_value( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ &range_ignore_threshold_value );
+
+ if ( ( range_ignore_threshold_value > 0 ) &&
+ ( signal_rate_per_spad < range_ignore_threshold_value ) ) {
/* Limit Fail add 2^6 to range status */
range_ignore_thresholdflag = 1;
}
}
- if (status == VL53L0X_ERROR_NONE) {
- if (none_flag == 1) {
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( none_flag == 1 ) {
*p_pal_range_status = 255; /* NONE */
- } else if (device_range_status_internal == 1 ||
- device_range_status_internal == 2 ||
- device_range_status_internal == 3) {
+ } else if ( device_range_status_internal == 1 ||
+ device_range_status_internal == 2 ||
+ device_range_status_internal == 3 ) {
*p_pal_range_status = 5; /* HW fail */
- } else if (device_range_status_internal == 6 ||
- device_range_status_internal == 9) {
+ } else if ( device_range_status_internal == 6 ||
+ device_range_status_internal == 9 ) {
*p_pal_range_status = 4; /* Phase fail */
- } else if (device_range_status_internal == 8 ||
- device_range_status_internal == 10 ||
- signal_ref_clipflag == 1) {
+ } else if ( device_range_status_internal == 8 ||
+ device_range_status_internal == 10 ||
+ signal_ref_clipflag == 1 ) {
*p_pal_range_status = 3; /* Min range */
- } else if (device_range_status_internal == 4 ||
- range_ignore_thresholdflag == 1) {
+ } else if ( device_range_status_internal == 4 ||
+ range_ignore_thresholdflag == 1 ) {
*p_pal_range_status = 2; /* Signal Fail */
- } else if (sigma_limitflag == 1) {
+ } else if ( sigma_limitflag == 1 ) {
*p_pal_range_status = 1; /* Sigma Fail */
} else {
*p_pal_range_status = 0; /* Range Valid */
@@ -3382,59 +3404,59 @@
}
/* DMAX only relevant during range error */
- if (*p_pal_range_status == 0)
+ if ( *p_pal_range_status == 0 )
p_ranging_measurement_data->RangeDMaxMilliMeter = 0;
/* fill the Limit Check Status */
- status = vl53l0x_get_limit_check_enable(dev,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
- &signal_rate_final_range_limit_check_enable);
-
- if (status == VL53L0X_ERROR_NONE) {
- if ((sigma_limit_check_enable == 0) || (sigma_limitflag == 1))
+ status = vl53l0x_get_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
+ &signal_rate_final_range_limit_check_enable );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( ( sigma_limit_check_enable == 0 ) || ( sigma_limitflag == 1 ) )
temp8 = 1;
else
temp8 = 0;
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8);
-
- if ((device_range_status_internal == 4) ||
- (signal_rate_final_range_limit_check_enable == 0))
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, temp8 );
+
+ if ( ( device_range_status_internal == 4 ) ||
+ ( signal_rate_final_range_limit_check_enable == 0 ) )
temp8 = 1;
else
temp8 = 0;
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
- temp8);
-
- if ((signal_ref_clip_limit_check_enable == 0) ||
- (signal_ref_clipflag == 1))
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
+ temp8 );
+
+ if ( ( signal_ref_clip_limit_check_enable == 0 ) ||
+ ( signal_ref_clipflag == 1 ) )
temp8 = 1;
else
temp8 = 0;
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8);
-
- if ((range_ignore_threshold_limit_check_enable == 0) ||
- (range_ignore_thresholdflag == 1))
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP, temp8 );
+
+ if ( ( range_ignore_threshold_limit_check_enable == 0 ) ||
+ ( range_ignore_thresholdflag == 1 ) )
temp8 = 1;
else
temp8 = 0;
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksStatus,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- temp8);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_ranging_measurement_data(VL53L0X_DEV dev,
- VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ temp8 );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_ranging_measurement_data( VL53L0X_DEV dev,
+ VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t device_range_status;
@@ -3451,7 +3473,7 @@
uint8_t localBuffer[12];
VL53L0X_RangingMeasurementData_t last_range_data_buffer;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/*
* use multi read even if some registers are not useful, result will
@@ -3459,14 +3481,14 @@
* start reading at 0x14 dec20
* end reading at 0x21 dec33 total 14 bytes to read
*/
- status = vl53l0x_read_multi(dev, 0x14, localBuffer, 12);
-
- if (status == VL53L0X_ERROR_NONE) {
+ status = vl53l0x_read_multi( dev, 0x14, localBuffer, 12 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
p_ranging_measurement_data->ZoneId = 0; /* Only one zone */
p_ranging_measurement_data->TimeStamp = 0; /* Not Implemented */
- tmpuint16 = VL53L0X_MAKEUINT16(localBuffer[11], localBuffer[10]);
+ tmpuint16 = VL53L0X_MAKEUINT16( localBuffer[11], localBuffer[10] );
/* cut1.1 if SYSTEM__RANGE_CONFIG if 1 range is 2bits fractional
*(format 11.2) else no fractional
*/
@@ -3474,16 +3496,16 @@
p_ranging_measurement_data->MeasurementTimeUsec = 0;
signal_rate = VL53L0X_FIXPOINT97TOFIXPOINT1616(
- VL53L0X_MAKEUINT16(localBuffer[7], localBuffer[6]));
+ VL53L0X_MAKEUINT16( localBuffer[7], localBuffer[6] ) );
/* peak_signal_count_rate_rtn_mcps */
p_ranging_measurement_data->SignalRateRtnMegaCps = signal_rate;
- ambient_rate = VL53L0X_MAKEUINT16(localBuffer[9], localBuffer[8]);
+ ambient_rate = VL53L0X_MAKEUINT16( localBuffer[9], localBuffer[8] );
p_ranging_measurement_data->AmbientRateRtnMegaCps =
- VL53L0X_FIXPOINT97TOFIXPOINT1616(ambient_rate);
-
- effective_spad_rtn_count = VL53L0X_MAKEUINT16(localBuffer[3],
- localBuffer[2]);
+ VL53L0X_FIXPOINT97TOFIXPOINT1616( ambient_rate );
+
+ effective_spad_rtn_count = VL53L0X_MAKEUINT16( localBuffer[3],
+ localBuffer[2] );
/* EffectiveSpadRtnCount is 8.8 format */
p_ranging_measurement_data->EffectiveSpadRtnCount =
effective_spad_rtn_count;
@@ -3491,43 +3513,43 @@
device_range_status = localBuffer[0];
/* Get Linearity Corrective Gain */
- linearity_corrective_gain = PALDevDataGet(dev,
- LinearityCorrectiveGain);
+ linearity_corrective_gain = PALDevDataGet( dev,
+ LinearityCorrectiveGain );
/* Get ranging configuration */
- range_fractional_enable = PALDevDataGet(dev,
- RangeFractionalEnable);
-
- if (linearity_corrective_gain != 1000) {
-
- tmpuint16 = (uint16_t)((linearity_corrective_gain
- * tmpuint16 + 500) / 1000);
+ range_fractional_enable = PALDevDataGet( dev,
+ RangeFractionalEnable );
+
+ if ( linearity_corrective_gain != 1000 ) {
+
+ tmpuint16 = ( uint16_t )( ( linearity_corrective_gain
+ * tmpuint16 + 500 ) / 1000 );
/* Implement Xtalk */
- VL53L0X_GETPARAMETERFIELD(dev,
- XTalkCompensationRateMegaCps,
- x_talk_compensation_rate_mega_cps);
- VL53L0X_GETPARAMETERFIELD(dev, XTalkCompensationEnable,
- x_talk_compensation_enable);
-
- if (x_talk_compensation_enable) {
-
- if ((signal_rate
- - ((x_talk_compensation_rate_mega_cps
- * effective_spad_rtn_count) >> 8))
- <= 0) {
- if (range_fractional_enable)
+ VL53L0X_GETPARAMETERFIELD( dev,
+ XTalkCompensationRateMegaCps,
+ x_talk_compensation_rate_mega_cps );
+ VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable,
+ x_talk_compensation_enable );
+
+ if ( x_talk_compensation_enable ) {
+
+ if ( ( signal_rate
+ - ( ( x_talk_compensation_rate_mega_cps
+ * effective_spad_rtn_count ) >> 8 ) )
+ <= 0 ) {
+ if ( range_fractional_enable )
xtalk_range_milli_meter = 8888;
else
xtalk_range_milli_meter = 8888
<< 2;
} else {
xtalk_range_milli_meter =
- (tmpuint16 * signal_rate)
- / (signal_rate
- - ((x_talk_compensation_rate_mega_cps
- * effective_spad_rtn_count)
- >> 8));
+ ( tmpuint16 * signal_rate )
+ / ( signal_rate
+ - ( ( x_talk_compensation_rate_mega_cps
+ * effective_spad_rtn_count )
+ >> 8 ) );
}
tmpuint16 = xtalk_range_milli_meter;
@@ -3535,11 +3557,11 @@
}
- if (range_fractional_enable) {
+ if ( range_fractional_enable ) {
p_ranging_measurement_data->RangeMilliMeter =
- (uint16_t)((tmpuint16) >> 2);
+ ( uint16_t )( ( tmpuint16 ) >> 2 );
p_ranging_measurement_data->RangeFractionalPart =
- (uint8_t)((tmpuint16 & 0x03) << 6);
+ ( uint8_t )( ( tmpuint16 & 0x03 ) << 6 );
} else {
p_ranging_measurement_data->RangeMilliMeter = tmpuint16;
p_ranging_measurement_data->RangeFractionalPart = 0;
@@ -3551,18 +3573,18 @@
* The range status depends on the device so call a device
* specific function to obtain the right Status.
*/
- status |= vl53l0x_get_pal_range_status(dev, device_range_status,
- signal_rate, effective_spad_rtn_count,
- p_ranging_measurement_data, &pal_range_status);
-
- if (status == VL53L0X_ERROR_NONE)
+ status |= vl53l0x_get_pal_range_status( dev, device_range_status,
+ signal_rate, effective_spad_rtn_count,
+ p_ranging_measurement_data, &pal_range_status );
+
+ if ( status == VL53L0X_ERROR_NONE )
p_ranging_measurement_data->RangeStatus = pal_range_status;
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Copy last read data into Dev buffer */
- last_range_data_buffer = PALDevDataGet(dev, LastRangeMeasure);
+ last_range_data_buffer = PALDevDataGet( dev, LastRangeMeasure );
last_range_data_buffer.RangeMilliMeter =
p_ranging_measurement_data->RangeMilliMeter;
@@ -3581,43 +3603,43 @@
last_range_data_buffer.RangeStatus =
p_ranging_measurement_data->RangeStatus;
- PALDevDataSet(dev, LastRangeMeasure, last_range_data_buffer);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_single_ranging_measurement(VL53L0X_DEV dev,
- VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data)
+ PALDevDataSet( dev, LastRangeMeasure, last_range_data_buffer );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_single_ranging_measurement( VL53L0X_DEV dev,
+ VL53L0X_RangingMeasurementData_t *p_ranging_measurement_data )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* This function will do a complete single ranging
* Here we fix the mode! */
- status = vl53l0x_set_device_mode(dev, VL53L0X_DEVICEMODE_SINGLE_RANGING);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_perform_single_measurement(dev);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_ranging_measurement_data(dev,
- p_ranging_measurement_data);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_clear_interrupt_mask(dev, 0);
-
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::perform_ref_signal_measurement(VL53L0X_DEV dev,
- uint16_t *p_ref_signal_rate)
+ status = vl53l0x_set_device_mode( dev, VL53L0X_DEVICEMODE_SINGLE_RANGING );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_perform_single_measurement( dev );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_ranging_measurement_data( dev,
+ p_ranging_measurement_data );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_clear_interrupt_mask( dev, 0 );
+
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::perform_ref_signal_measurement( VL53L0X_DEV dev,
+ uint16_t *p_ref_signal_rate )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_RangingMeasurementData_t ranging_measurement_data;
@@ -3628,44 +3650,44 @@
* this will be reset before the end of the function
*/
- sequence_config = PALDevDataGet(dev, SequenceConfig);
+ sequence_config = PALDevDataGet( dev, SequenceConfig );
/*
* This function performs a reference signal rate measurement.
*/
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_perform_single_ranging_measurement(dev,
- &ranging_measurement_data);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_read_word(dev,
- VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
- p_ref_signal_rate);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, 0xC0 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_perform_single_ranging_measurement( dev,
+ &ranging_measurement_data );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_read_word( dev,
+ VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF,
+ p_ref_signal_rate );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* restore the previous Sequence Config */
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- sequence_config);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, sequence_config);
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_perform_ref_spad_management(VL53L0X_DEV dev,
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ sequence_config );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, sequence_config );
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_perform_ref_spad_management( VL53L0X_DEV dev,
uint32_t *ref_spad_count,
- uint8_t *is_aperture_spads)
+ uint8_t *is_aperture_spads )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t last_spad_array[6];
@@ -3711,7 +3733,7 @@
*/
- target_ref_rate = PALDevDataGet(dev, targetRefRate);
+ target_ref_rate = PALDevDataGet( dev, targetRefRate );
/*
* Initialize Spad arrays.
@@ -3721,93 +3743,93 @@
* Note that there are 6 bytes. Only the first 44 bits will be used to
* represent spads.
*/
- for (index = 0; index < spad_array_size; index++)
+ for ( index = 0; index < spad_array_size; index++ )
dev->Data.SpadData.RefSpadEnables[index] = 0;
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
- start_select);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0);
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
+ start_select );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE, 0 );
/* Perform ref calibration */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_perform_ref_calibration(dev, &vhv_settings,
- &phase_cal, 0);
-
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_perform_ref_calibration( dev, &vhv_settings,
+ &phase_cal, 0 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Enable Minimum NON-APERTURE Spads */
current_spad_index = 0;
last_spad_index = current_spad_index;
need_apt_spads = 0;
- status = enable_ref_spads(dev,
- need_apt_spads,
- dev->Data.SpadData.RefGoodSpadMap,
- dev->Data.SpadData.RefSpadEnables,
- spad_array_size,
- start_select,
- current_spad_index,
- minimum_spad_count,
- &last_spad_index);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
+ status = enable_ref_spads( dev,
+ need_apt_spads,
+ dev->Data.SpadData.RefGoodSpadMap,
+ dev->Data.SpadData.RefSpadEnables,
+ spad_array_size,
+ start_select,
+ current_spad_index,
+ minimum_spad_count,
+ &last_spad_index );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
current_spad_index = last_spad_index;
- status = perform_ref_signal_measurement(dev,
- &peak_signal_rate_ref);
- if ((status == VL53L0X_ERROR_NONE) &&
- (peak_signal_rate_ref > target_ref_rate)) {
+ status = perform_ref_signal_measurement( dev,
+ &peak_signal_rate_ref );
+ if ( ( status == VL53L0X_ERROR_NONE ) &&
+ ( peak_signal_rate_ref > target_ref_rate ) ) {
/* Signal rate measurement too high,
* switch to APERTURE SPADs */
- for (index = 0; index < spad_array_size; index++)
+ for ( index = 0; index < spad_array_size; index++ )
dev->Data.SpadData.RefSpadEnables[index] = 0;
/* Increment to the first APERTURE spad */
- while ((is_aperture(start_select + current_spad_index)
- == 0) && (current_spad_index < max_spad_count)) {
+ while ( ( is_aperture( start_select + current_spad_index )
+ == 0 ) && ( current_spad_index < max_spad_count ) ) {
current_spad_index++;
}
need_apt_spads = 1;
- status = enable_ref_spads(dev,
- need_apt_spads,
- dev->Data.SpadData.RefGoodSpadMap,
- dev->Data.SpadData.RefSpadEnables,
- spad_array_size,
- start_select,
- current_spad_index,
- minimum_spad_count,
- &last_spad_index);
-
- if (status == VL53L0X_ERROR_NONE) {
+ status = enable_ref_spads( dev,
+ need_apt_spads,
+ dev->Data.SpadData.RefGoodSpadMap,
+ dev->Data.SpadData.RefSpadEnables,
+ spad_array_size,
+ start_select,
+ current_spad_index,
+ minimum_spad_count,
+ &last_spad_index );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
current_spad_index = last_spad_index;
- status = perform_ref_signal_measurement(dev,
- &peak_signal_rate_ref);
-
- if ((status == VL53L0X_ERROR_NONE) &&
- (peak_signal_rate_ref > target_ref_rate)) {
+ status = perform_ref_signal_measurement( dev,
+ &peak_signal_rate_ref );
+
+ if ( ( status == VL53L0X_ERROR_NONE ) &&
+ ( peak_signal_rate_ref > target_ref_rate ) ) {
/* Signal rate still too high after
* setting the minimum number of
* APERTURE spads. Can do no more
@@ -3823,8 +3845,8 @@
}
}
- if ((status == VL53L0X_ERROR_NONE) &&
- (peak_signal_rate_ref < target_ref_rate)) {
+ if ( ( status == VL53L0X_ERROR_NONE ) &&
+ ( peak_signal_rate_ref < target_ref_rate ) ) {
/* At this point, the minimum number of either aperture
* or non-aperture spads have been set. Proceed to add
* spads and perform measurements until the target
@@ -3833,19 +3855,19 @@
is_aperture_spads_int = need_apt_spads;
ref_spad_count_int = minimum_spad_count;
- memcpy(last_spad_array, dev->Data.SpadData.RefSpadEnables,
- spad_array_size);
- last_signal_rate_diff = abs(peak_signal_rate_ref -
- target_ref_rate);
+ memcpy( last_spad_array, dev->Data.SpadData.RefSpadEnables,
+ spad_array_size );
+ last_signal_rate_diff = abs( peak_signal_rate_ref -
+ target_ref_rate );
complete = 0;
- while (!complete) {
+ while ( !complete ) {
get_next_good_spad(
dev->Data.SpadData.RefGoodSpadMap,
spad_array_size, current_spad_index,
- &next_good_spad);
-
- if (next_good_spad == -1) {
+ &next_good_spad );
+
+ if ( next_good_spad == -1 ) {
status = VL53L0X_ERROR_REF_SPAD_INIT;
break;
}
@@ -3853,8 +3875,8 @@
/* Cannot combine Aperture and Non-Aperture spads, so
* ensure the current spad is of the correct type.
*/
- if (is_aperture((uint32_t)start_select + next_good_spad) !=
- need_apt_spads) {
+ if ( is_aperture( ( uint32_t )start_select + next_good_spad ) !=
+ need_apt_spads ) {
/* At this point we have enabled the maximum
* number of Aperture spads.
*/
@@ -3862,76 +3884,76 @@
break;
}
- (ref_spad_count_int)++;
+ ( ref_spad_count_int )++;
current_spad_index = next_good_spad;
status = enable_spad_bit(
dev->Data.SpadData.RefSpadEnables,
- spad_array_size, current_spad_index);
-
- if (status == VL53L0X_ERROR_NONE) {
+ spad_array_size, current_spad_index );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
current_spad_index++;
/* Proceed to apply the additional spad and
* perform measurement. */
- status = set_ref_spad_map(dev,
- dev->Data.SpadData.RefSpadEnables);
+ status = set_ref_spad_map( dev,
+ dev->Data.SpadData.RefSpadEnables );
}
- if (status != VL53L0X_ERROR_NONE)
+ if ( status != VL53L0X_ERROR_NONE )
break;
- status = perform_ref_signal_measurement(dev,
- &peak_signal_rate_ref);
-
- if (status != VL53L0X_ERROR_NONE)
+ status = perform_ref_signal_measurement( dev,
+ &peak_signal_rate_ref );
+
+ if ( status != VL53L0X_ERROR_NONE )
break;
- signal_rate_diff = abs(peak_signal_rate_ref - target_ref_rate);
-
- if (peak_signal_rate_ref > target_ref_rate) {
+ signal_rate_diff = abs( peak_signal_rate_ref - target_ref_rate );
+
+ if ( peak_signal_rate_ref > target_ref_rate ) {
/* Select the spad map that provides the
* measurement closest to the target rate,
* either above or below it.
*/
- if (signal_rate_diff > last_signal_rate_diff) {
+ if ( signal_rate_diff > last_signal_rate_diff ) {
/* Previous spad map produced a closer
* measurement, so choose this. */
- status = set_ref_spad_map(dev,
- last_spad_array);
+ status = set_ref_spad_map( dev,
+ last_spad_array );
memcpy(
dev->Data.SpadData.RefSpadEnables,
- last_spad_array, spad_array_size);
-
- (ref_spad_count_int)--;
+ last_spad_array, spad_array_size );
+
+ ( ref_spad_count_int )--;
}
complete = 1;
} else {
/* Continue to add spads */
last_signal_rate_diff = signal_rate_diff;
- memcpy(last_spad_array,
- dev->Data.SpadData.RefSpadEnables,
- spad_array_size);
+ memcpy( last_spad_array,
+ dev->Data.SpadData.RefSpadEnables,
+ spad_array_size );
}
} /* while */
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
*ref_spad_count = ref_spad_count_int;
*is_aperture_spads = is_aperture_spads_int;
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadCount, (uint8_t)(*ref_spad_count));
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadType, *is_aperture_spads);
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_reference_spads(VL53L0X_DEV dev,
- uint32_t count, uint8_t is_aperture_spads)
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 1 );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount, ( uint8_t )( *ref_spad_count ) );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType, *is_aperture_spads );
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_reference_spads( VL53L0X_DEV dev,
+ uint32_t count, uint8_t is_aperture_spads )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t current_spad_index = 0;
@@ -3948,153 +3970,153 @@
* The good spad map will be applied.
*/
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
- start_select);
-
- for (index = 0; index < spad_array_size; index++)
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT,
+ start_select );
+
+ for ( index = 0; index < spad_array_size; index++ )
dev->Data.SpadData.RefSpadEnables[index] = 0;
- if (is_aperture_spads) {
+ if ( is_aperture_spads ) {
/* Increment to the first APERTURE spad */
- while ((is_aperture(start_select + current_spad_index) == 0) &&
- (current_spad_index < max_spad_count)) {
+ while ( ( is_aperture( start_select + current_spad_index ) == 0 ) &&
+ ( current_spad_index < max_spad_count ) ) {
current_spad_index++;
}
}
- status = enable_ref_spads(dev,
- is_aperture_spads,
- dev->Data.SpadData.RefGoodSpadMap,
- dev->Data.SpadData.RefSpadEnables,
- spad_array_size,
- start_select,
- current_spad_index,
- count,
- &last_spad_index);
-
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, RefSpadsInitialised, 1);
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadCount, (uint8_t)(count));
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadType, is_aperture_spads);
- }
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_wait_device_booted(VL53L0X_DEV dev)
+ status = enable_ref_spads( dev,
+ is_aperture_spads,
+ dev->Data.SpadData.RefGoodSpadMap,
+ dev->Data.SpadData.RefSpadEnables,
+ spad_array_size,
+ start_select,
+ current_spad_index,
+ count,
+ &last_spad_index );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, RefSpadsInitialised, 1 );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount, ( uint8_t )( count ) );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType, is_aperture_spads );
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_wait_device_booted( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
- LOG_FUNCTION_START("");
+ LOG_FUNCTION_START( "" );
/* not implemented on VL53L0X */
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration(VL53L0X_DEV dev, uint8_t *p_vhv_settings,
- uint8_t *p_phase_cal)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_ref_calibration( VL53L0X_DEV dev, uint8_t *p_vhv_settings,
+ uint8_t *p_phase_cal )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_perform_ref_calibration(dev, p_vhv_settings,
- p_phase_cal, 1);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_perform_ref_spad_management(VL53L0X_DEV dev,
- uint32_t *ref_spad_count, uint8_t *is_aperture_spads)
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_perform_ref_calibration( dev, p_vhv_settings,
+ p_phase_cal, 1 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_ref_spad_management( VL53L0X_DEV dev,
+ uint32_t *ref_spad_count, uint8_t *is_aperture_spads )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_perform_ref_spad_management(dev, ref_spad_count,
- is_aperture_spads);
-
- LOG_FUNCTION_END(status);
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_perform_ref_spad_management( dev, ref_spad_count,
+ is_aperture_spads );
+
+ LOG_FUNCTION_END( status );
return status;
}
/* Group PAL Init Functions */
-VL53L0X_Error VL53L0X::vl53l0x_set_device_address(VL53L0X_DEV dev, uint8_t device_address)
+VL53L0X_Error VL53L0X::vl53l0x_set_device_address( VL53L0X_DEV dev, uint8_t device_address )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_write_byte(dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
- device_address / 2);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_gpio_config(VL53L0X_DEV dev, uint8_t pin,
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS,
+ device_address / 2 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_gpio_config( VL53L0X_DEV dev, uint8_t pin,
VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
- VL53L0X_InterruptPolarity polarity)
+ VL53L0X_InterruptPolarity polarity )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t data;
- LOG_FUNCTION_START("");
-
- if (pin != 0) {
+ LOG_FUNCTION_START( "" );
+
+ if ( pin != 0 ) {
status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
- } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE) {
- if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
+ } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE ) {
+ if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
data = 0x10;
else
data = 1;
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data);
-
- } else if (device_mode == VL53L0X_DEVICEMODE_GPIO_OSC) {
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x00);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
- status |= vl53l0x_write_byte(dev, 0x80, 0x01);
- status |= vl53l0x_write_byte(dev, 0x85, 0x02);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x04);
- status |= vl53l0x_write_byte(dev, 0xcd, 0x00);
- status |= vl53l0x_write_byte(dev, 0xcc, 0x11);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x07);
- status |= vl53l0x_write_byte(dev, 0xbe, 0x00);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x06);
- status |= vl53l0x_write_byte(dev, 0xcc, 0x09);
-
- status |= vl53l0x_write_byte(dev, 0xff, 0x00);
- status |= vl53l0x_write_byte(dev, 0xff, 0x01);
- status |= vl53l0x_write_byte(dev, 0x00, 0x00);
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data );
+
+ } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_OSC ) {
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0x80, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x85, 0x02 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x04 );
+ status |= vl53l0x_write_byte( dev, 0xcd, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0xcc, 0x11 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x07 );
+ status |= vl53l0x_write_byte( dev, 0xbe, 0x00 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x06 );
+ status |= vl53l0x_write_byte( dev, 0xcc, 0x09 );
+
+ status |= vl53l0x_write_byte( dev, 0xff, 0x00 );
+ status |= vl53l0x_write_byte( dev, 0xff, 0x01 );
+ status |= vl53l0x_write_byte( dev, 0x00, 0x00 );
} else {
- if (status == VL53L0X_ERROR_NONE) {
- switch (functionality) {
+ if ( status == VL53L0X_ERROR_NONE ) {
+ switch ( functionality ) {
case VL53L0X_GPIOFUNCTIONALITY_OFF:
data = 0x00;
break;
@@ -4116,47 +4138,47 @@
}
}
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data);
-
- if (status == VL53L0X_ERROR_NONE) {
- if (polarity == VL53L0X_INTERRUPTPOLARITY_LOW)
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
data = 0;
else
- data = (uint8_t)(1 << 4);
-
- status = vl53l0x_update_byte(dev,
- VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data);
+ data = ( uint8_t )( 1 << 4 );
+
+ status = vl53l0x_update_byte( dev,
+ VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data );
}
- if (status == VL53L0X_ERROR_NONE)
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- Pin0GpioFunctionality, functionality);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_clear_interrupt_mask(dev, 0);
-
- }
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_fraction_enable(VL53L0X_DEV dev, uint8_t *p_enabled)
+ if ( status == VL53L0X_ERROR_NONE )
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ Pin0GpioFunctionality, functionality );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_clear_interrupt_mask( dev, 0 );
+
+ }
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_fraction_enable( VL53L0X_DEV dev, uint8_t *p_enabled )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_byte(dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled);
-
- if (status == VL53L0X_ERROR_NONE)
- *p_enabled = (*p_enabled & 1);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-uint16_t VL53L0X::vl53l0x_encode_timeout(uint32_t timeout_macro_clks)
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, p_enabled );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ *p_enabled = ( *p_enabled & 1 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+uint16_t VL53L0X::vl53l0x_encode_timeout( uint32_t timeout_macro_clks )
{
/*!
* Encode timeout in macro periods in (LSByte * 2^MSByte) + 1 format
@@ -4166,25 +4188,25 @@
uint32_t ls_byte = 0;
uint16_t ms_byte = 0;
- if (timeout_macro_clks > 0) {
+ if ( timeout_macro_clks > 0 ) {
ls_byte = timeout_macro_clks - 1;
- while ((ls_byte & 0xFFFFFF00) > 0) {
+ while ( ( ls_byte & 0xFFFFFF00 ) > 0 ) {
ls_byte = ls_byte >> 1;
ms_byte++;
}
- encoded_timeout = (ms_byte << 8)
- + (uint16_t) (ls_byte & 0x000000FF);
+ encoded_timeout = ( ms_byte << 8 )
+ + ( uint16_t ) ( ls_byte & 0x000000FF );
}
return encoded_timeout;
}
-VL53L0X_Error VL53L0X::set_sequence_step_timeout(VL53L0X_DEV dev,
+VL53L0X_Error VL53L0X::set_sequence_step_timeout( VL53L0X_DEV dev,
VL53L0X_SequenceStepId sequence_step_id,
- uint32_t timeout_micro_secs)
+ uint32_t timeout_micro_secs )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t current_vcsel_pulse_period_p_clk;
@@ -4196,68 +4218,68 @@
uint16_t final_range_encoded_time_out;
VL53L0X_SchedulerSequenceSteps_t scheduler_sequence_steps;
- if ((sequence_step_id == VL53L0X_SEQUENCESTEP_TCC) ||
- (sequence_step_id == VL53L0X_SEQUENCESTEP_DSS) ||
- (sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC)) {
-
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
-
- if (status == VL53L0X_ERROR_NONE) {
- msrc_range_time_out_m_clks = vl53l0x_calc_timeout_mclks(dev,
+ if ( ( sequence_step_id == VL53L0X_SEQUENCESTEP_TCC ) ||
+ ( sequence_step_id == VL53L0X_SEQUENCESTEP_DSS ) ||
+ ( sequence_step_id == VL53L0X_SEQUENCESTEP_MSRC ) ) {
+
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ msrc_range_time_out_m_clks = vl53l0x_calc_timeout_mclks( dev,
timeout_micro_secs,
- (uint8_t)current_vcsel_pulse_period_p_clk);
-
- if (msrc_range_time_out_m_clks > 256)
+ ( uint8_t )current_vcsel_pulse_period_p_clk );
+
+ if ( msrc_range_time_out_m_clks > 256 )
msrc_encoded_time_out = 255;
else
msrc_encoded_time_out =
- (uint8_t)msrc_range_time_out_m_clks - 1;
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- LastEncodedTimeout,
- msrc_encoded_time_out);
+ ( uint8_t )msrc_range_time_out_m_clks - 1;
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ LastEncodedTimeout,
+ msrc_encoded_time_out );
}
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
- msrc_encoded_time_out);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP,
+ msrc_encoded_time_out );
}
} else {
- if (sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE) {
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ if ( sequence_step_id == VL53L0X_SEQUENCESTEP_PRE_RANGE ) {
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
pre_range_time_out_m_clks =
- vl53l0x_calc_timeout_mclks(dev,
- timeout_micro_secs,
- (uint8_t)current_vcsel_pulse_period_p_clk);
+ vl53l0x_calc_timeout_mclks( dev,
+ 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);
-
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev,
- LastEncodedTimeout,
- pre_range_encoded_time_out);
+ pre_range_time_out_m_clks );
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ LastEncodedTimeout,
+ pre_range_encoded_time_out );
}
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_write_word(dev,
- VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
- pre_range_encoded_time_out);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_write_word( dev,
+ VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI,
+ pre_range_encoded_time_out );
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
PreRangeTimeoutMicroSecs,
- timeout_micro_secs);
+ timeout_micro_secs );
}
- } else if (sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE) {
+ } else if ( sequence_step_id == VL53L0X_SEQUENCESTEP_FINAL_RANGE ) {
/* For the final range timeout, the pre-range timeout
* must be added. To do this both final and pre-range
@@ -4265,58 +4287,58 @@
* because they have different vcsel periods.
*/
- vl53l0x_get_sequence_step_enables(dev,
- &scheduler_sequence_steps);
+ vl53l0x_get_sequence_step_enables( dev,
+ &scheduler_sequence_steps );
pre_range_time_out_m_clks = 0;
- if (scheduler_sequence_steps.PreRangeOn) {
+ if ( scheduler_sequence_steps.PreRangeOn ) {
/* Retrieve PRE-RANGE VCSEL Period */
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
/* Retrieve PRE-RANGE Timeout in Macro periods
* (MCLKS) */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_word(dev, 0x51,
- &pre_range_encoded_time_out);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_word( dev, 0x51,
+ &pre_range_encoded_time_out );
pre_range_time_out_m_clks =
vl53l0x_decode_timeout(
- pre_range_encoded_time_out);
+ pre_range_encoded_time_out );
}
}
/* Calculate FINAL RANGE Timeout in Macro Periods
* (MCLKS) and add PRE-RANGE value
*/
- if (status == VL53L0X_ERROR_NONE) {
-
- status = vl53l0x_get_vcsel_pulse_period(dev,
- VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
- ¤t_vcsel_pulse_period_p_clk);
+ if ( status == VL53L0X_ERROR_NONE ) {
+
+ status = vl53l0x_get_vcsel_pulse_period( dev,
+ VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
+ ¤t_vcsel_pulse_period_p_clk );
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
final_range_time_out_m_clks =
- vl53l0x_calc_timeout_mclks(dev,
- timeout_micro_secs,
- (uint8_t) current_vcsel_pulse_period_p_clk);
+ vl53l0x_calc_timeout_mclks( dev,
+ timeout_micro_secs,
+ ( 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);
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_write_word(dev, 0x71,
- final_range_encoded_time_out);
+ vl53l0x_encode_timeout( final_range_time_out_m_clks );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_write_word( dev, 0x71,
+ final_range_encoded_time_out );
}
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
FinalRangeTimeoutMicroSecs,
- timeout_micro_secs);
+ timeout_micro_secs );
}
}
} else
@@ -4326,8 +4348,8 @@
return status;
}
-VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
- uint32_t measurement_timing_budget_micro_seconds)
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
+ uint32_t measurement_timing_budget_micro_seconds )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint32_t final_range_timing_budget_micro_seconds;
@@ -4344,44 +4366,44 @@
uint32_t c_min_timing_budget_micro_seconds = 20000;
uint32_t sub_timeout = 0;
- LOG_FUNCTION_START("");
-
- if (measurement_timing_budget_micro_seconds
- < c_min_timing_budget_micro_seconds) {
+ LOG_FUNCTION_START( "" );
+
+ if ( measurement_timing_budget_micro_seconds
+ < c_min_timing_budget_micro_seconds ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
return status;
}
final_range_timing_budget_micro_seconds =
measurement_timing_budget_micro_seconds -
- (start_overhead_micro_seconds + end_overhead_micro_seconds);
-
- status = vl53l0x_get_sequence_step_enables(dev, &scheduler_sequence_steps);
-
- if (status == VL53L0X_ERROR_NONE &&
- (scheduler_sequence_steps.TccOn ||
- scheduler_sequence_steps.MsrcOn ||
- scheduler_sequence_steps.DssOn)) {
+ ( start_overhead_micro_seconds + end_overhead_micro_seconds );
+
+ status = vl53l0x_get_sequence_step_enables( dev, &scheduler_sequence_steps );
+
+ if ( status == VL53L0X_ERROR_NONE &&
+ ( scheduler_sequence_steps.TccOn ||
+ scheduler_sequence_steps.MsrcOn ||
+ scheduler_sequence_steps.DssOn ) ) {
/* TCC, MSRC and DSS all share the same timeout */
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_MSRC,
- &msrc_dcc_tcc_timeout_micro_seconds);
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_MSRC,
+ &msrc_dcc_tcc_timeout_micro_seconds );
/* Subtract the TCC, MSRC and DSS timeouts if they are
* enabled. */
- if (status != VL53L0X_ERROR_NONE)
+ if ( status != VL53L0X_ERROR_NONE )
return status;
/* TCC */
- if (scheduler_sequence_steps.TccOn) {
+ if ( scheduler_sequence_steps.TccOn ) {
sub_timeout = msrc_dcc_tcc_timeout_micro_seconds
+ tcc_overhead_micro_seconds;
- if (sub_timeout <
- final_range_timing_budget_micro_seconds) {
+ if ( sub_timeout <
+ final_range_timing_budget_micro_seconds ) {
final_range_timing_budget_micro_seconds -=
sub_timeout;
} else {
@@ -4390,30 +4412,30 @@
}
}
- if (status != VL53L0X_ERROR_NONE) {
- LOG_FUNCTION_END(status);
+ if ( status != VL53L0X_ERROR_NONE ) {
+ LOG_FUNCTION_END( status );
return status;
}
/* DSS */
- if (scheduler_sequence_steps.DssOn) {
-
- sub_timeout = 2 * (msrc_dcc_tcc_timeout_micro_seconds +
- dss_overhead_micro_seconds);
-
- if (sub_timeout < final_range_timing_budget_micro_seconds) {
+ if ( scheduler_sequence_steps.DssOn ) {
+
+ sub_timeout = 2 * ( msrc_dcc_tcc_timeout_micro_seconds +
+ dss_overhead_micro_seconds );
+
+ if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
final_range_timing_budget_micro_seconds
-= sub_timeout;
} else {
/* Requested timeout too big. */
status = VL53L0X_ERROR_INVALID_PARAMS;
}
- } else if (scheduler_sequence_steps.MsrcOn) {
+ } else if ( scheduler_sequence_steps.MsrcOn ) {
/* MSRC */
sub_timeout = msrc_dcc_tcc_timeout_micro_seconds +
msrc_overhead_micro_seconds;
- if (sub_timeout < final_range_timing_budget_micro_seconds) {
+ if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
final_range_timing_budget_micro_seconds
-= sub_timeout;
} else {
@@ -4424,23 +4446,23 @@
}
- if (status != VL53L0X_ERROR_NONE) {
- LOG_FUNCTION_END(status);
+ if ( status != VL53L0X_ERROR_NONE ) {
+ LOG_FUNCTION_END( status );
return status;
}
- if (scheduler_sequence_steps.PreRangeOn) {
+ if ( scheduler_sequence_steps.PreRangeOn ) {
/* Subtract the Pre-range timeout if enabled. */
- status = get_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_PRE_RANGE,
- &pre_range_timeout_micro_seconds);
+ status = get_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_PRE_RANGE,
+ &pre_range_timeout_micro_seconds );
sub_timeout = pre_range_timeout_micro_seconds +
pre_range_overhead_micro_seconds;
- if (sub_timeout < final_range_timing_budget_micro_seconds) {
+ if ( sub_timeout < final_range_timing_budget_micro_seconds ) {
final_range_timing_budget_micro_seconds -= sub_timeout;
} else {
/* Requested timeout too big. */
@@ -4449,8 +4471,8 @@
}
- if (status == VL53L0X_ERROR_NONE &&
- scheduler_sequence_steps.FinalRangeOn) {
+ if ( status == VL53L0X_ERROR_NONE &&
+ scheduler_sequence_steps.FinalRangeOn ) {
final_range_timing_budget_micro_seconds -=
final_range_overhead_micro_seconds;
@@ -4462,54 +4484,54 @@
* will be set. Otherwise the remaining time will be applied to
* the final range.
*/
- status = set_sequence_step_timeout(dev,
- VL53L0X_SEQUENCESTEP_FINAL_RANGE,
- final_range_timing_budget_micro_seconds);
-
- VL53L0X_SETPARAMETERFIELD(dev,
- MeasurementTimingBudgetMicroSeconds,
- measurement_timing_budget_micro_seconds);
- }
-
- LOG_FUNCTION_END(status);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_measurement_timing_budget_micro_seconds(VL53L0X_DEV dev,
- uint32_t measurement_timing_budget_micro_seconds)
+ status = set_sequence_step_timeout( dev,
+ VL53L0X_SEQUENCESTEP_FINAL_RANGE,
+ final_range_timing_budget_micro_seconds );
+
+ VL53L0X_SETPARAMETERFIELD( dev,
+ MeasurementTimingBudgetMicroSeconds,
+ measurement_timing_budget_micro_seconds );
+ }
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_measurement_timing_budget_micro_seconds( VL53L0X_DEV dev,
+ uint32_t measurement_timing_budget_micro_seconds )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds(dev,
- measurement_timing_budget_micro_seconds);
-
- LOG_FUNCTION_END(status);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_sequence_step_enable(VL53L0X_DEV dev,
- VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled)
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
+ measurement_timing_budget_micro_seconds );
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_sequence_step_enable( VL53L0X_DEV dev,
+ VL53L0X_SequenceStepId sequence_step_id, uint8_t sequence_step_enabled )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t sequence_config = 0;
uint8_t sequence_config_new = 0;
uint32_t measurement_timing_budget_micro_seconds;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_read_byte(dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
- &sequence_config);
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_read_byte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ &sequence_config );
sequence_config_new = sequence_config;
- if (status == VL53L0X_ERROR_NONE) {
- if (sequence_step_enabled == 1) {
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( sequence_step_enabled == 1 ) {
/* Enable requested sequence step
*/
- switch (sequence_step_id) {
+ switch ( sequence_step_id ) {
case VL53L0X_SEQUENCESTEP_TCC:
sequence_config_new |= 0x10;
break;
@@ -4531,7 +4553,7 @@
} else {
/* Disable requested sequence step
*/
- switch (sequence_step_id) {
+ switch ( sequence_step_id ) {
case VL53L0X_SEQUENCESTEP_TCC:
sequence_config_new &= 0xef;
break;
@@ -4553,34 +4575,34 @@
}
}
- if (sequence_config_new != sequence_config) {
+ if ( sequence_config_new != sequence_config ) {
/* Apply New Setting */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_write_byte(dev,
- VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_write_byte( dev,
+ VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, sequence_config_new );
}
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, sequence_config_new);
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, sequence_config_new );
/* Recalculate timing budget */
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_GETPARAMETERFIELD(dev,
- MeasurementTimingBudgetMicroSeconds,
- measurement_timing_budget_micro_seconds);
-
- vl53l0x_set_measurement_timing_budget_micro_seconds(dev,
- measurement_timing_budget_micro_seconds);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_GETPARAMETERFIELD( dev,
+ MeasurementTimingBudgetMicroSeconds,
+ measurement_timing_budget_micro_seconds );
+
+ vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
+ measurement_timing_budget_micro_seconds );
}
}
- LOG_FUNCTION_END(status);
-
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_enable(VL53L0X_DEV dev, uint16_t limit_check_id,
- uint8_t limit_check_enable)
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_limit_check_enable( VL53L0X_DEV dev, uint16_t limit_check_id,
+ uint8_t limit_check_enable )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
FixPoint1616_t temp_fix1616 = 0;
@@ -4588,75 +4610,75 @@
uint8_t limit_check_disable = 0;
uint8_t temp8;
- LOG_FUNCTION_START("");
-
- if (limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS) {
+ LOG_FUNCTION_START( "" );
+
+ if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
} else {
- if (limit_check_enable == 0) {
+ if ( limit_check_enable == 0 ) {
temp_fix1616 = 0;
limit_check_enable_int = 0;
limit_check_disable = 1;
} else {
- VL53L0X_GETARRAYPARAMETERFIELD(dev, LimitChecksValue,
- limit_check_id, temp_fix1616);
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksValue,
+ limit_check_id, temp_fix1616 );
limit_check_disable = 0;
/* this to be sure to have either 0 or 1 */
limit_check_enable_int = 1;
}
- switch (limit_check_id) {
+ switch ( limit_check_id ) {
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
- limit_check_enable_int);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
+ limit_check_enable_int );
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
- status = vl53l0x_write_word(dev,
- VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
- VL53L0X_FIXPOINT1616TOFIXPOINT97(temp_fix1616));
+ status = vl53l0x_write_word( dev,
+ VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT,
+ VL53L0X_FIXPOINT1616TOFIXPOINT97( temp_fix1616 ) );
break;
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
- limit_check_enable_int);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ limit_check_enable_int );
break;
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
/* internal computation: */
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
- limit_check_enable_int);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD,
+ limit_check_enable_int );
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
- temp8 = (uint8_t)(limit_check_disable << 1);
- status = vl53l0x_update_byte(dev,
- VL53L0X_REG_MSRC_CONFIG_CONTROL,
- 0xFE, temp8);
+ temp8 = ( uint8_t )( limit_check_disable << 1 );
+ status = vl53l0x_update_byte( dev,
+ VL53L0X_REG_MSRC_CONFIG_CONTROL,
+ 0xFE, temp8 );
break;
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
- temp8 = (uint8_t)(limit_check_disable << 4);
- status = vl53l0x_update_byte(dev,
- VL53L0X_REG_MSRC_CONFIG_CONTROL,
- 0xEF, temp8);
+ temp8 = ( uint8_t )( limit_check_disable << 4 );
+ status = vl53l0x_update_byte( dev,
+ VL53L0X_REG_MSRC_CONFIG_CONTROL,
+ 0xEF, temp8 );
break;
@@ -4668,21 +4690,21 @@
}
- if (status == VL53L0X_ERROR_NONE) {
- if (limit_check_enable == 0) {
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- limit_check_id, 0);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( limit_check_enable == 0 ) {
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ limit_check_id, 0 );
} else {
- VL53L0X_SETARRAYPARAMETERFIELD(dev, LimitChecksEnable,
- limit_check_id, 1);
+ VL53L0X_SETARRAYPARAMETERFIELD( dev, LimitChecksEnable,
+ limit_check_id, 1 );
}
}
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_static_init(VL53L0X_DEV dev)
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_static_init( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
VL53L0X_DeviceParameters_t current_parameters = {0};
@@ -4697,394 +4719,396 @@
uint8_t vcsel_pulse_period_pclk;
uint32_t seq_timeout_micro_secs;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_get_info_from_device(dev, 1);
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_get_info_from_device( dev, 1 );
/* set the ref spad from NVM */
- count = (uint32_t)VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadCount);
- aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER(dev,
- ReferenceSpadType);
+ count = ( uint32_t )VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount );
+ aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType );
/* NVM value invalid */
- if ((aperture_spads > 1) ||
- ((aperture_spads == 1) && (count > 32)) ||
- ((aperture_spads == 0) && (count > 12)))
- status = wrapped_vl53l0x_perform_ref_spad_management(dev, &ref_spad_count,
- &is_aperture_spads);
+ if ( ( aperture_spads > 1 ) ||
+ ( ( aperture_spads == 1 ) && ( count > 32 ) ) ||
+ ( ( aperture_spads == 0 ) && ( count > 12 ) ) )
+ status = wrapped_vl53l0x_perform_ref_spad_management( dev, &ref_spad_count,
+ &is_aperture_spads );
else
- status = vl53l0x_set_reference_spads(dev, count, aperture_spads);
+ status = wrapped_vl53l0x_set_reference_spads( dev, count, aperture_spads );
/* Initialize tuning settings buffer to prevent compiler warning. */
p_tuning_setting_buffer = DefaultTuningSettings;
- if (status == VL53L0X_ERROR_NONE) {
- use_internal_tuning_settings = PALDevDataGet(dev,
- UseInternalTuningSettings);
-
- if (use_internal_tuning_settings == 0)
- p_tuning_setting_buffer = PALDevDataGet(dev,
- pTuningSettingsPointer);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ use_internal_tuning_settings = PALDevDataGet( dev,
+ UseInternalTuningSettings );
+
+ if ( use_internal_tuning_settings == 0 )
+ p_tuning_setting_buffer = PALDevDataGet( dev,
+ pTuningSettingsPointer );
else
p_tuning_setting_buffer = DefaultTuningSettings;
}
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_load_tuning_settings(dev, p_tuning_setting_buffer);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_load_tuning_settings( dev, p_tuning_setting_buffer );
/* Set interrupt config to new sample ready */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_set_gpio_config(dev, 0, 0,
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
- VL53L0X_INTERRUPTPOLARITY_LOW);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
- status |= vl53l0x_read_word(dev, 0x84, &tempword);
- status |= vl53l0x_write_byte(dev, 0xFF, 0x00);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
- VL53L0X_SETDEVICESPECIFICPARAMETER(dev, OscFrequencyMHz,
- VL53L0X_FIXPOINT412TOFIXPOINT1616(tempword));
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_gpio_config( dev, 0, 0,
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
+ VL53L0X_INTERRUPTPOLARITY_LOW );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status |= vl53l0x_read_word( dev, 0x84, &tempword );
+ status |= vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, OscFrequencyMHz,
+ VL53L0X_FIXPOINT412TOFIXPOINT1616( tempword ) );
}
/* After static init, some device parameters may be changed,
* so update them */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_get_device_parameters(dev, ¤t_parameters);
-
-
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_get_fraction_enable(dev, &tempbyte);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, RangeFractionalEnable, tempbyte);
-
- }
-
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, CurrentParameters, current_parameters);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_device_parameters( dev, ¤t_parameters );
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_get_fraction_enable( dev, &tempbyte );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, RangeFractionalEnable, tempbyte );
+
+ }
+
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, CurrentParameters, current_parameters );
/* read the sequence config and save it */
- if (status == VL53L0X_ERROR_NONE) {
- status = vl53l0x_read_byte(dev,
- VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte);
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, SequenceConfig, tempbyte);
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_read_byte( dev,
+ VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &tempbyte );
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, SequenceConfig, tempbyte );
}
/* Disable MSRC and TCC by default */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_sequence_step_enable(dev,
- VL53L0X_SEQUENCESTEP_TCC, 0);
-
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_set_sequence_step_enable(dev,
- VL53L0X_SEQUENCESTEP_MSRC, 0);
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_sequence_step_enable( dev,
+ VL53L0X_SEQUENCESTEP_TCC, 0 );
+
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_sequence_step_enable( dev,
+ VL53L0X_SEQUENCESTEP_MSRC, 0 );
/* Set PAL State to standby */
- if (status == VL53L0X_ERROR_NONE)
- PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
/* Store pre-range vcsel period */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
status = vl53l0x_get_vcsel_pulse_period(
- dev,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE,
- &vcsel_pulse_period_pclk);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
+ dev,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE,
+ &vcsel_pulse_period_pclk );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
PreRangeVcselPulsePeriod,
- vcsel_pulse_period_pclk);
+ vcsel_pulse_period_pclk );
}
/* Store final-range vcsel period */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
status = vl53l0x_get_vcsel_pulse_period(
- dev,
- VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
- &vcsel_pulse_period_pclk);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
+ dev,
+ VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
+ &vcsel_pulse_period_pclk );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
FinalRangeVcselPulsePeriod,
- vcsel_pulse_period_pclk);
+ vcsel_pulse_period_pclk );
}
/* Store pre-range timeout */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
status = get_sequence_step_timeout(
dev,
VL53L0X_SEQUENCESTEP_PRE_RANGE,
- &seq_timeout_micro_secs);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
+ &seq_timeout_micro_secs );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
PreRangeTimeoutMicroSecs,
- seq_timeout_micro_secs);
+ seq_timeout_micro_secs );
}
/* Store final-range timeout */
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
status = get_sequence_step_timeout(
dev,
VL53L0X_SEQUENCESTEP_FINAL_RANGE,
- &seq_timeout_micro_secs);
- }
-
- if (status == VL53L0X_ERROR_NONE) {
+ &seq_timeout_micro_secs );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
VL53L0X_SETDEVICESPECIFICPARAMETER(
dev,
FinalRangeTimeoutMicroSecs,
- seq_timeout_micro_secs);
- }
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-
-VL53L0X_Error VL53L0X::vl53l0x_stop_measurement(VL53L0X_DEV dev)
+ seq_timeout_micro_secs );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_stop_measurement( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_write_byte(dev, VL53L0X_REG_SYSRANGE_START,
- VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT);
-
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
- status = vl53l0x_write_byte(dev, 0x00, 0x00);
- status = vl53l0x_write_byte(dev, 0x91, 0x00);
- status = vl53l0x_write_byte(dev, 0x00, 0x01);
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
-
- if (status == VL53L0X_ERROR_NONE) {
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_write_byte( dev, VL53L0X_REG_SYSRANGE_START,
+ VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT );
+
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status = vl53l0x_write_byte( dev, 0x00, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x91, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x00, 0x01 );
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
/* Set PAL State to Idle */
- PALDevDataSet(dev, PalState, VL53L0X_STATE_IDLE);
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_IDLE );
}
/* Check if need to apply interrupt settings */
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_check_and_load_interrupt_settings(dev, 0);
-
- LOG_FUNCTION_END(status);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_get_stop_completed_status(VL53L0X_DEV dev,
- uint32_t *p_stop_status)
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_check_and_load_interrupt_settings( dev, 0 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_stop_completed_status( VL53L0X_DEV dev,
+ uint32_t *p_stop_status )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
uint8_t byte = 0;
- LOG_FUNCTION_START("");
-
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_read_byte(dev, 0x04, &byte);
-
- if (status == VL53L0X_ERROR_NONE)
- status = vl53l0x_write_byte(dev, 0xFF, 0x0);
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_read_byte( dev, 0x04, &byte );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_write_byte( dev, 0xFF, 0x0 );
*p_stop_status = byte;
- if (byte == 0) {
- status = vl53l0x_write_byte(dev, 0x80, 0x01);
- status = vl53l0x_write_byte(dev, 0xFF, 0x01);
- status = vl53l0x_write_byte(dev, 0x00, 0x00);
- status = vl53l0x_write_byte(dev, 0x91,
- PALDevDataGet(dev, StopVariable));
- status = vl53l0x_write_byte(dev, 0x00, 0x01);
- status = vl53l0x_write_byte(dev, 0xFF, 0x00);
- status = vl53l0x_write_byte(dev, 0x80, 0x00);
- }
-
- LOG_FUNCTION_END(status);
+ if ( byte == 0 ) {
+ status = vl53l0x_write_byte( dev, 0x80, 0x01 );
+ status = vl53l0x_write_byte( dev, 0xFF, 0x01 );
+ status = vl53l0x_write_byte( dev, 0x00, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x91,
+ PALDevDataGet( dev, StopVariable ) );
+ status = vl53l0x_write_byte( dev, 0x00, 0x01 );
+ status = vl53l0x_write_byte( dev, 0xFF, 0x00 );
+ status = vl53l0x_write_byte( dev, 0x80, 0x00 );
+ }
+
+ LOG_FUNCTION_END( status );
return status;
}
/****************** Write and read functions from I2C *************************/
-VL53L0X_Error VL53L0X::vl53l0x_write_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
+VL53L0X_Error VL53L0X::vl53l0x_write_multi( VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count )
{
int status;
- status = vl53l0x_i2c_write(dev->I2cDevAddr, index, p_data, (uint16_t)count);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_read_multi(VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count)
+ status = vl53l0x_i2c_write( dev->I2cDevAddr, index, p_data, ( uint16_t )count );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_read_multi( VL53L0X_DEV dev, uint8_t index, uint8_t *p_data, uint32_t count )
{
int status;
- if (count>=VL53L0X_MAX_I2C_XFER_SIZE) {
+ if ( count >= VL53L0X_MAX_I2C_XFER_SIZE ) {
status = VL53L0X_ERROR_INVALID_PARAMS;
}
- status = vl53l0x_i2c_read(dev->I2cDevAddr, index, p_data, (uint16_t)count);
-
- return status;
-}
-
-
-VL53L0X_Error VL53L0X::vl53l0x_write_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
+ status = vl53l0x_i2c_read( dev->I2cDevAddr, index, p_data, ( uint16_t )count );
+
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_write_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t data )
{
int status;
- status=vl53l0x_i2c_write(Dev->I2cDevAddr, index, &data, 1);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_write_word(VL53L0X_DEV dev, uint8_t index, uint16_t data)
+ status = vl53l0x_i2c_write( Dev->I2cDevAddr, index, &data, 1 );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_write_word( VL53L0X_DEV dev, uint8_t index, uint16_t data )
{
int status;
uint8_t buffer[2];
buffer[0] = data >> 8;
buffer[1] = data & 0x00FF;
- status=vl53l0x_i2c_write(dev->I2cDevAddr, index, (uint8_t *)buffer, 2);
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_write_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
+ status = vl53l0x_i2c_write( dev->I2cDevAddr, index, ( uint8_t * )buffer, 2 );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_write_dword( VL53L0X_DEV Dev, uint8_t index, uint32_t data )
{
int status;
uint8_t buffer[4];
- buffer[0] = (data >> 24) & 0xFF;
- buffer[1] = (data >> 16) & 0xFF;
- buffer[2] = (data >> 8) & 0xFF;
- buffer[3] = (data >> 0) & 0xFF;
- status=vl53l0x_i2c_write(Dev->I2cDevAddr, index, (uint8_t *)buffer, 4);
- return status;
-}
-
-
-VL53L0X_Error VL53L0X::vl53l0x_read_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data)
+ buffer[0] = ( data >> 24 ) & 0xFF;
+ buffer[1] = ( data >> 16 ) & 0xFF;
+ buffer[2] = ( data >> 8 ) & 0xFF;
+ buffer[3] = ( data >> 0 ) & 0xFF;
+ status = vl53l0x_i2c_write( Dev->I2cDevAddr, index, ( uint8_t * )buffer, 4 );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_read_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t *p_data )
{
int status;
- status = vl53l0x_i2c_read(Dev->I2cDevAddr, index, p_data, 1);
-
- if(status)
+ status = vl53l0x_i2c_read( Dev->I2cDevAddr, index, p_data, 1 );
+
+ if( status )
return -1;
return 0;
}
-VL53L0X_Error VL53L0X::vl53l0x_read_word(VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data)
+VL53L0X_Error VL53L0X::vl53l0x_read_word( VL53L0X_DEV Dev, uint8_t index, uint16_t *p_data )
{
int status;
- uint8_t buffer[2] = {0,0};
-
- status = vl53l0x_i2c_read(Dev->I2cDevAddr, index, buffer, 2);
- if (!status) {
- *p_data = (buffer[0] << 8) + buffer[1];
- }
- return status;
-
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_read_dword(VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data)
+ uint8_t buffer[2] = {0, 0};
+
+ status = vl53l0x_i2c_read( Dev->I2cDevAddr, index, buffer, 2 );
+ if ( !status ) {
+ *p_data = ( buffer[0] << 8 ) + buffer[1];
+ }
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_read_dword( VL53L0X_DEV Dev, uint8_t index, uint32_t *p_data )
{
int status;
- uint8_t buffer[4] = {0,0,0,0};
-
- status = vl53l0x_i2c_read(Dev->I2cDevAddr, index, buffer, 4);
- if(!status) {
- *p_data = (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
- }
- return status;
-
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_update_byte(VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data)
+ uint8_t buffer[4] = {0, 0, 0, 0};
+
+ status = vl53l0x_i2c_read( Dev->I2cDevAddr, index, buffer, 4 );
+ if( !status ) {
+ *p_data = ( buffer[0] << 24 ) + ( buffer[1] << 16 ) + ( buffer[2] << 8 ) + buffer[3];
+ }
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_update_byte( VL53L0X_DEV Dev, uint8_t index, uint8_t and_data, uint8_t or_data )
{
int status;
uint8_t buffer = 0;
/* read data direct onto buffer */
- status = vl53l0x_i2c_read(Dev->I2cDevAddr, index, &buffer,1);
- if (!status) {
- buffer = (buffer & and_data) | or_data;
- status = vl53l0x_i2c_write(Dev->I2cDevAddr, index, &buffer, (uint8_t)1);
- }
- return status;
-}
-
-VL53L0X_Error VL53L0X::vl53l0x_i2c_write(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* p_data, uint16_t NumByteToWrite)
+ status = vl53l0x_i2c_read( Dev->I2cDevAddr, index, &buffer, 1 );
+ if ( !status ) {
+ buffer = ( buffer & and_data ) | or_data;
+ status = vl53l0x_i2c_write( Dev->I2cDevAddr, index, &buffer, ( uint8_t )1 );
+ }
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_i2c_write( uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
+ uint16_t NumByteToWrite )
{
int ret;
- ret = dev_i2c.i2c_write(p_data, DeviceAddr, RegisterAddr, NumByteToWrite);
-
- if(ret)
+ ret = dev_i2c.i2c_write( p_data, DeviceAddr, RegisterAddr, NumByteToWrite );
+
+ if( ret )
return -1;
return 0;
}
-VL53L0X_Error VL53L0X::vl53l0x_i2c_read(uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t* p_data, uint16_t NumByteToRead)
+VL53L0X_Error VL53L0X::vl53l0x_i2c_read( uint8_t DeviceAddr, uint8_t RegisterAddr, uint8_t *p_data,
+ uint16_t NumByteToRead )
{
int ret;
- ret = dev_i2c.i2c_read(p_data, DeviceAddr, RegisterAddr, NumByteToRead);
-
- if(ret)
+ ret = dev_i2c.i2c_read( p_data, DeviceAddr, RegisterAddr, NumByteToRead );
+
+ if( ret )
return -1;
return 0;
}
-int VL53L0X::read_id(uint8_t *id)
+int VL53L0X::read_id( uint8_t *id )
{
int status = 0;
- uint16_t rl_id=0;
-
- status = vl53l0x_read_word(_device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id);
- if (rl_id == 0xEEAA)
+ uint16_t rl_id = 0;
+
+ status = vl53l0x_read_word( _device, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &rl_id );
+ if ( rl_id == 0xEEAA )
return status;
return -1;
}
-VL53L0X_Error VL53L0X::wait_measurement_data_ready(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::wait_measurement_data_ready( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- uint8_t new_dat_ready=0;
+ uint8_t new_dat_ready = 0;
uint32_t loop_nb;
// Wait until it finished
// use timeout to avoid deadlock
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
loop_nb = 0;
do {
- status = vl53l0x_get_measurement_data_ready(dev, &new_dat_ready);
- if ((new_dat_ready == 0x01) || status != VL53L0X_ERROR_NONE) {
+ status = vl53l0x_get_measurement_data_ready( dev, &new_dat_ready );
+ if ( ( new_dat_ready == 0x01 ) || status != VL53L0X_ERROR_NONE ) {
break;
}
loop_nb = loop_nb + 1;
- vl53l0x_polling_delay(dev);
- } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
-
- if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
+ vl53l0x_polling_delay( dev );
+ } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
+
+ if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
status = VL53L0X_ERROR_TIME_OUT;
}
}
@@ -5092,26 +5116,26 @@
return status;
}
-VL53L0X_Error VL53L0X::wait_stop_completed(VL53L0X_DEV dev)
+VL53L0X_Error VL53L0X::wait_stop_completed( VL53L0X_DEV dev )
{
VL53L0X_Error status = VL53L0X_ERROR_NONE;
- uint32_t stop_completed=0;
+ uint32_t stop_completed = 0;
uint32_t loop_nb;
// Wait until it finished
// use timeout to avoid deadlock
- if (status == VL53L0X_ERROR_NONE) {
+ if ( status == VL53L0X_ERROR_NONE ) {
loop_nb = 0;
do {
- status = vl53l0x_get_stop_completed_status(dev, &stop_completed);
- if ((stop_completed == 0x00) || status != VL53L0X_ERROR_NONE) {
+ status = vl53l0x_get_stop_completed_status( dev, &stop_completed );
+ if ( ( stop_completed == 0x00 ) || status != VL53L0X_ERROR_NONE ) {
break;
}
loop_nb = loop_nb + 1;
- vl53l0x_polling_delay(dev);
- } while (loop_nb < VL53L0X_DEFAULT_MAX_LOOP);
-
- if (loop_nb >= VL53L0X_DEFAULT_MAX_LOOP) {
+ vl53l0x_polling_delay( dev );
+ } while ( loop_nb < VL53L0X_DEFAULT_MAX_LOOP );
+
+ if ( loop_nb >= VL53L0X_DEFAULT_MAX_LOOP ) {
status = VL53L0X_ERROR_TIME_OUT;
}
@@ -5121,7 +5145,7 @@
}
-int VL53L0X::init_sensor(uint8_t new_addr)
+int VL53L0X::init_sensor( uint8_t new_addr )
{
int status;
@@ -5131,67 +5155,67 @@
// status=VL53L0X_WaitDeviceBooted(Device);
// if(status)
// printf("WaitDeviceBooted fail\n\r");
- status=is_present();
- if(!status) {
- status=init(&_my_device);
- if(status != VL53L0X_ERROR_NONE) {
- printf("Failed to init VL53L0X sensor!\n\r");
+ status = is_present();
+ if( !status ) {
+ status = init( &_my_device );
+ if( status != VL53L0X_ERROR_NONE ) {
+ printf( "Failed to init VL53L0X sensor!\n\r" );
return status;
}
// deduce silicon version
- status = vl53l0x_get_device_info(&_my_device, &_device_info);
-
-
- status=prepare();
- if(status != VL53L0X_ERROR_NONE) {
- printf("Failed to prepare VL53L0X!\n\r");
+ status = vl53l0x_get_device_info( &_my_device, &_device_info );
+
+
+ status = prepare();
+ if( status != VL53L0X_ERROR_NONE ) {
+ printf( "Failed to prepare VL53L0X!\n\r" );
return status;
}
- if(new_addr!=DEFAULT_DEVICE_ADDRESS) {
- status=set_device_address(new_addr);
- if(status) {
- printf("Failed to change I2C address!\n\r");
+ if( new_addr != DEFAULT_DEVICE_ADDRESS ) {
+ status = set_device_address( new_addr );
+ if( status ) {
+ printf( "Failed to change I2C address!\n\r" );
return status;
}
} else {
- printf("Invalid new address!\n\r");
+ printf( "Invalid new address!\n\r" );
return VL53L0X_ERROR_INVALID_PARAMS;
}
}
return status;
}
-int VL53L0X::range_meas_int_continuous_mode(void (*fptr)(void))
+int VL53L0X::range_meas_int_continuous_mode( void ( *fptr )( void ) )
{
int status, clr_status;
- status = vl53l0x_stop_measurement(_device); // it is safer to do this while sensor is stopped
+ status = vl53l0x_stop_measurement( _device ); // it is safer to do this while sensor is stopped
// status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
- status = vl53l0x_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
- VL53L0X_INTERRUPTPOLARITY_HIGH);
-
- if (!status) {
- attach_interrupt_measure_detection_irq(fptr);
+ status = vl53l0x_set_gpio_config( _device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
+ VL53L0X_INTERRUPTPOLARITY_HIGH );
+
+ if ( !status ) {
+ attach_interrupt_measure_detection_irq( fptr );
enable_interrupt_measure_detection_irq();
}
- clr_status=clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS|VL53L0X_REG_RESULT_RANGE_STATUS);
- if(clr_status)
- VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
-
- if(!status) {
- status=range_start_continuous_mode();
- }
- return status;
-}
-
-
-int VL53L0X::start_measurement(OperatingMode operating_mode, void (*fptr)(void))
+ clr_status = clear_interrupt( VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS );
+ if( clr_status )
+ VL53L0X_ErrLog( "VL53L0X_ClearErrorInterrupt fail\r\n" );
+
+ if( !status ) {
+ status = range_start_continuous_mode();
+ }
+ return status;
+}
+
+
+int VL53L0X::start_measurement( OperatingMode operating_mode, void ( *fptr )( void ) )
{
int Status = VL53L0X_ERROR_NONE;
int ClrStatus;
@@ -5200,59 +5224,59 @@
uint8_t PhaseCal;
// *** from mass market cube expansion v1.1, ranging with satellites.
// default settings, for normal range.
- FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
- FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
+ FixPoint1616_t signalLimit = ( FixPoint1616_t )( 0.25 * 65536 );
+ FixPoint1616_t sigmaLimit = ( FixPoint1616_t )( 18 * 65536 );
uint32_t timingBudget = 33000;
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
- if (operating_mode == range_continuous_interrupt) {
- if (gpio1Int==NULL) {
- printf ("GPIO1 Error\r\n");
+ if ( operating_mode == range_continuous_interrupt ) {
+ if ( gpio1Int == NULL ) {
+ printf ( "GPIO1 Error\r\n" );
return 1;
}
- Status = vl53l0x_stop_measurement(_device); // it is safer to do this while sensor is stopped
+ Status = vl53l0x_stop_measurement( _device ); // it is safer to do this while sensor is stopped
// Status = VL53L0X_SetInterruptThresholds(Device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING, 0, 300);
- Status = vl53l0x_set_gpio_config(_device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
- VL53L0X_INTERRUPTPOLARITY_HIGH);
-
- if (Status == VL53L0X_ERROR_NONE) {
- attach_interrupt_measure_detection_irq(fptr);
+ Status = vl53l0x_set_gpio_config( _device, 0, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING,
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
+ VL53L0X_INTERRUPTPOLARITY_HIGH );
+
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ attach_interrupt_measure_detection_irq( fptr );
enable_interrupt_measure_detection_irq();
}
- ClrStatus=clear_interrupt(VL53L0X_REG_RESULT_INTERRUPT_STATUS|VL53L0X_REG_RESULT_RANGE_STATUS);
- if(ClrStatus)
- VL53L0X_ErrLog("VL53L0X_ClearErrorInterrupt fail\r\n");
-
- if(Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
+ ClrStatus = clear_interrupt( VL53L0X_REG_RESULT_INTERRUPT_STATUS | VL53L0X_REG_RESULT_RANGE_STATUS );
+ if( ClrStatus )
+ VL53L0X_ErrLog( "VL53L0X_ClearErrorInterrupt fail\r\n" );
+
+ if( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ); // Setup in continuous ranging mode
}
- if(Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_start_measurement(_device);
+ if( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_start_measurement( _device );
}
}
- if (operating_mode == range_single_shot_polling) {
+ if ( operating_mode == range_single_shot_polling ) {
// singelshot, polled ranging
- if(Status == VL53L0X_ERROR_NONE) {
+ if( Status == VL53L0X_ERROR_NONE ) {
// no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
- Status = vl53l0x_set_device_mode(_device, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
+ Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_SINGLE_RANGING ); // Setup in single ranging mode
}
// Enable/Disable Sigma and Signal check
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_limit_check_enable(_device,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_limit_check_enable( _device,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1 );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_limit_check_enable(_device,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_limit_check_enable( _device,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1 );
}
// *** from mass market cube expansion v1.1, ranging with satellites.
@@ -5260,8 +5284,8 @@
//*
// switch(rangingConfig) {
// case LONG_RANGE:
- signalLimit = (FixPoint1616_t)(0.1*65536);
- sigmaLimit = (FixPoint1616_t)(60*65536);
+ signalLimit = ( FixPoint1616_t )( 0.1 * 65536 );
+ sigmaLimit = ( FixPoint1616_t )( 60 * 65536 );
timingBudget = 33000;
preRangeVcselPeriod = 18;
finalRangeVcselPeriod = 14;
@@ -5285,45 +5309,45 @@
}
*/
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_limit_check_value(_device,
- VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_limit_check_value( _device,
+ VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_limit_check_value(_device,
- VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_limit_check_value( _device,
+ VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_measurement_timing_budget_micro_seconds(_device, timingBudget);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_measurement_timing_budget_micro_seconds( _device, timingBudget );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_vcsel_pulse_period(_device,
- VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_vcsel_pulse_period( _device,
+ VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_set_vcsel_pulse_period(_device,
- VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_set_vcsel_pulse_period( _device,
+ VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod );
}
- if (Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_perform_ref_calibration(_device, &VhvSettings, &PhaseCal);
+ if ( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_perform_ref_calibration( _device, &VhvSettings, &PhaseCal );
}
}
- if (operating_mode == range_continuous_polling) {
- if(Status == VL53L0X_ERROR_NONE) {
- printf ("Call of VL53L0X_SetDeviceMode\n");
- Status = vl53l0x_set_device_mode(_device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in continuous ranging mode
+ if ( operating_mode == range_continuous_polling ) {
+ if( Status == VL53L0X_ERROR_NONE ) {
+ printf ( "Call of VL53L0X_SetDeviceMode\n" );
+ Status = vl53l0x_set_device_mode( _device, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ); // Setup in continuous ranging mode
}
- if(Status == VL53L0X_ERROR_NONE) {
- printf ("Call of VL53L0X_StartMeasurement\n");
- Status = vl53l0x_start_measurement(_device);
+ if( Status == VL53L0X_ERROR_NONE ) {
+ printf ( "Call of VL53L0X_StartMeasurement\n" );
+ Status = vl53l0x_start_measurement( _device );
}
}
@@ -5331,80 +5355,2097 @@
}
-int VL53L0X::get_measurement(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data)
+int VL53L0X::get_measurement( OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *p_data )
{
int Status = VL53L0X_ERROR_NONE;
- if (operating_mode == range_single_shot_polling) {
- Status = vl53l0x_perform_single_ranging_measurement(_device, p_data);
- }
-
- if (operating_mode == range_continuous_polling) {
- if (Status == VL53L0X_ERROR_NONE)
- Status = vl53l0x_measurement_poll_for_completion(_device);
-
- if(Status == VL53L0X_ERROR_NONE) {
- Status = vl53l0x_get_ranging_measurement_data(_device, p_data);
+ if ( operating_mode == range_single_shot_polling ) {
+ Status = vl53l0x_perform_single_ranging_measurement( _device, p_data );
+ }
+
+ if ( operating_mode == range_continuous_polling ) {
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_measurement_poll_for_completion( _device );
+
+ if( Status == VL53L0X_ERROR_NONE ) {
+ Status = vl53l0x_get_ranging_measurement_data( _device, p_data );
// Clear the interrupt
- vl53l0x_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
- vl53l0x_polling_delay(_device);
+ vl53l0x_clear_interrupt_mask( _device, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY );
+ vl53l0x_polling_delay( _device );
}
}
- if (operating_mode == range_continuous_interrupt) {
- Status = vl53l0x_get_ranging_measurement_data(_device, p_data);
- vl53l0x_clear_interrupt_mask(_device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS);
+ if ( operating_mode == range_continuous_interrupt ) {
+ Status = vl53l0x_get_ranging_measurement_data( _device, p_data );
+ vl53l0x_clear_interrupt_mask( _device, VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR | VL53L0X_REG_RESULT_INTERRUPT_STATUS );
}
return Status;
}
-int VL53L0X::stop_measurement(OperatingMode operating_mode)
+int VL53L0X::stop_measurement( OperatingMode operating_mode )
{
int status = VL53L0X_ERROR_NONE;
// don't need to stop for a singleshot range!
- if (operating_mode==range_single_shot_polling) {
- }
-
- if (operating_mode==range_continuous_interrupt || operating_mode==range_continuous_polling) {
+ if ( operating_mode == range_single_shot_polling ) {
+ }
+
+ if ( operating_mode == range_continuous_interrupt || operating_mode == range_continuous_polling ) {
// continuous mode
- if(status == VL53L0X_ERROR_NONE) {
- printf ("Call of VL53L0X_StopMeasurement\n");
- status = vl53l0x_stop_measurement(_device);
+ if( status == VL53L0X_ERROR_NONE ) {
+ printf ( "Call of VL53L0X_StopMeasurement\n" );
+ status = vl53l0x_stop_measurement( _device );
}
- if(status == VL53L0X_ERROR_NONE) {
- printf ("Wait Stop to be competed\n");
- status = wait_stop_completed(_device);
+ if( status == VL53L0X_ERROR_NONE ) {
+ printf ( "Wait Stop to be competed\n" );
+ status = wait_stop_completed( _device );
}
- if(status == VL53L0X_ERROR_NONE)
- status = vl53l0x_clear_interrupt_mask(_device,
- VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
- }
-
- return status;
-}
-
-
-int VL53L0X::handle_irq(OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data)
+ if( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_clear_interrupt_mask( _device,
+ VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY );
+ }
+
+ return status;
+}
+
+
+int VL53L0X::handle_irq( OperatingMode operating_mode, VL53L0X_RangingMeasurementData_t *data )
{
int status;
- status = get_measurement(operating_mode, data);
+ status = get_measurement( operating_mode, data );
enable_interrupt_measure_detection_irq();
return status;
}
-
-
+int32_t VL53L0X::vl53l0x_cycle_power( void )
+{
+ int32_t status = STATUS_OK;
+#ifdef VL53L0X_LOG_ENABLE
+ trace_i2c( "// cycle sensor power\n" );
+#endif
+ return status;
+}
+
+uint8_t VL53L0X::vl53l0x_encode_vcsel_period( uint8_t vcsel_period_pclks )
+{
+ /*!
+ * Converts the encoded VCSEL period register value into the real period
+ * in PLL clocks
+ */
+
+ uint8_t vcsel_period_reg = 0;
+
+ vcsel_period_reg = ( vcsel_period_pclks >> 1 ) - 1;
+
+ return vcsel_period_reg;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_device_error_string( VL53L0X_DeviceError error_code,
+ char *p_device_error_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ switch ( error_code ) {
+ case VL53L0X_DEVICEERROR_NONE:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_NONE );
+ break;
+ case VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE );
+ break;
+ case VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE );
+ break;
+ case VL53L0X_DEVICEERROR_NOVHVVALUEFOUND:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND );
+ break;
+ case VL53L0X_DEVICEERROR_MSRCNOTARGET:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET );
+ break;
+ case VL53L0X_DEVICEERROR_SNRCHECK:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_SNRCHECK );
+ break;
+ case VL53L0X_DEVICEERROR_RANGEPHASECHECK:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK );
+ break;
+ case VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK );
+ break;
+ case VL53L0X_DEVICEERROR_TCC:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_TCC );
+ break;
+ case VL53L0X_DEVICEERROR_PHASECONSISTENCY:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY );
+ break;
+ case VL53L0X_DEVICEERROR_MINCLIP:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_MINCLIP );
+ break;
+ case VL53L0X_DEVICEERROR_RANGECOMPLETE:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE );
+ break;
+ case VL53L0X_DEVICEERROR_ALGOUNDERFLOW:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW );
+ break;
+ case VL53L0X_DEVICEERROR_ALGOOVERFLOW:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW );
+ break;
+ case VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD );
+ break;
+
+ default:
+ VL53L0X_COPYSTRING( p_device_error_string,
+ VL53L0X_STRING_UNKNOW_ERROR_CODE );
+
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_limit_check_info( VL53L0X_DEV dev, uint16_t limit_check_id,
+ char *p_limit_check_string )
+{
+
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ switch ( limit_check_id ) {
+ case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE );
+ break;
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE );
+ break;
+ case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP );
+ break;
+ case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD );
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC );
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE );
+ break;
+
+ default:
+ VL53L0X_COPYSTRING( p_limit_check_string,
+ VL53L0X_STRING_UNKNOW_ERROR_CODE );
+
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_pal_error_string( VL53L0X_Error pal_error_code,
+ char *p_pal_error_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ switch ( pal_error_code ) {
+ case VL53L0X_ERROR_NONE:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_NONE );
+ break;
+ case VL53L0X_ERROR_CALIBRATION_WARNING:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_CALIBRATION_WARNING );
+ break;
+ case VL53L0X_ERROR_MIN_CLIPPED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_MIN_CLIPPED );
+ break;
+ case VL53L0X_ERROR_UNDEFINED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_UNDEFINED );
+ break;
+ case VL53L0X_ERROR_INVALID_PARAMS:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_INVALID_PARAMS );
+ break;
+ case VL53L0X_ERROR_NOT_SUPPORTED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_NOT_SUPPORTED );
+ break;
+ case VL53L0X_ERROR_INTERRUPT_NOT_CLEARED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED );
+ break;
+ case VL53L0X_ERROR_RANGE_ERROR:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_RANGE_ERROR );
+ break;
+ case VL53L0X_ERROR_TIME_OUT:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_TIME_OUT );
+ break;
+ case VL53L0X_ERROR_MODE_NOT_SUPPORTED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED );
+ break;
+ case VL53L0X_ERROR_BUFFER_TOO_SMALL:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL );
+ break;
+ case VL53L0X_ERROR_GPIO_NOT_EXISTING:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING );
+ break;
+ case VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED );
+ break;
+ case VL53L0X_ERROR_CONTROL_INTERFACE:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_CONTROL_INTERFACE );
+ break;
+ case VL53L0X_ERROR_INVALID_COMMAND:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_INVALID_COMMAND );
+ break;
+ case VL53L0X_ERROR_DIVISION_BY_ZERO:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_DIVISION_BY_ZERO );
+ break;
+ case VL53L0X_ERROR_REF_SPAD_INIT:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_REF_SPAD_INIT );
+ break;
+ case VL53L0X_ERROR_NOT_IMPLEMENTED:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_ERROR_NOT_IMPLEMENTED );
+ break;
+
+ default:
+ VL53L0X_COPYSTRING( p_pal_error_string,
+ VL53L0X_STRING_UNKNOW_ERROR_CODE );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_pal_state_string( VL53L0X_State pal_state_code,
+ char *p_pal_state_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ switch ( pal_state_code ) {
+ case VL53L0X_STATE_POWERDOWN:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_POWERDOWN );
+ break;
+ case VL53L0X_STATE_WAIT_STATICINIT:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_WAIT_STATICINIT );
+ break;
+ case VL53L0X_STATE_STANDBY:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_STANDBY );
+ break;
+ case VL53L0X_STATE_IDLE:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_IDLE );
+ break;
+ case VL53L0X_STATE_RUNNING:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_RUNNING );
+ break;
+ case VL53L0X_STATE_UNKNOWN:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_UNKNOWN );
+ break;
+ case VL53L0X_STATE_ERROR:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_ERROR );
+ break;
+
+ default:
+ VL53L0X_COPYSTRING( p_pal_state_string,
+ VL53L0X_STRING_STATE_UNKNOWN );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_range_status_string( uint8_t range_status,
+ char *p_range_status_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ switch ( range_status ) {
+ case 0:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_RANGEVALID );
+ break;
+ case 1:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_SIGMA );
+ break;
+ case 2:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_SIGNAL );
+ break;
+ case 3:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_MINRANGE );
+ break;
+ case 4:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_PHASE );
+ break;
+ case 5:
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_HW );
+ break;
+
+ default: /**/
+ VL53L0X_COPYSTRING( p_range_status_string,
+ VL53L0X_STRING_RANGESTATUS_NONE );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_ref_calibration( VL53L0X_DEV dev,
+ uint8_t *p_vhv_settings, uint8_t *p_phase_cal )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t vhv_settings = 0;
+ uint8_t phase_cal = 0;
+
+ status = vl53l0x_ref_calibration_io( dev, 1,
+ vhv_settings, phase_cal,
+ p_vhv_settings, p_phase_cal,
+ 1, 1 );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::count_enabled_spads( uint8_t spad_array[],
+ uint32_t byte_count, uint32_t max_spads,
+ uint32_t *p_total_spads_enabled, uint8_t *p_is_aperture )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint32_t c_spads_per_byte = 8;
+ uint32_t last_byte;
+ uint32_t last_bit;
+ uint32_t byte_index = 0;
+ uint32_t bit_index = 0;
+ uint8_t temp_byte;
+ uint8_t spad_type_identified = 0;
+
+ /* The entire array will not be used for spads, therefore the last
+ * byte and last bit is determined from the max spads value.
+ */
+
+ last_byte = max_spads / c_spads_per_byte;
+ last_bit = max_spads % c_spads_per_byte;
+
+ /* Check that the max spads value does not exceed the array bounds. */
+ if ( last_byte >= byte_count )
+ status = VL53L0X_ERROR_REF_SPAD_INIT;
+
+ *p_total_spads_enabled = 0;
+
+ /* Count the bits enabled in the whole bytes */
+ for ( byte_index = 0; byte_index <= ( last_byte - 1 ); byte_index++ ) {
+ temp_byte = spad_array[byte_index];
+
+ for ( bit_index = 0; bit_index <= c_spads_per_byte; bit_index++ ) {
+ if ( ( temp_byte & 0x01 ) == 1 ) {
+ ( *p_total_spads_enabled )++;
+
+ if ( !spad_type_identified ) {
+ *p_is_aperture = 1;
+ if ( ( byte_index < 2 ) && ( bit_index < 4 ) )
+ *p_is_aperture = 0;
+ spad_type_identified = 1;
+ }
+ }
+ temp_byte >>= 1;
+ }
+ }
+
+ /* Count the number of bits enabled in the last byte accounting
+ * for the fact that not all bits in the byte may be used.
+ */
+ temp_byte = spad_array[last_byte];
+
+ for ( bit_index = 0; bit_index <= last_bit; bit_index++ ) {
+ if ( ( temp_byte & 0x01 ) == 1 )
+ ( *p_total_spads_enabled )++;
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_reference_spads( VL53L0X_DEV dev,
+ uint32_t *p_spad_count, uint8_t *p_is_aperture_spads )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t ref_spads_initialised;
+ uint8_t ref_spad_array[6];
+ uint32_t c_max_spad_count = 44;
+ uint32_t c_spad_array_size = 6;
+ uint32_t spads_enabled;
+ uint8_t is_aperture_spads = 0;
+
+ ref_spads_initialised = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ RefSpadsInitialised );
+
+ if ( ref_spads_initialised == 1 ) {
+
+ *p_spad_count = ( uint32_t )VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount );
+ *p_is_aperture_spads = VL53L0X_GETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType );
+ } else {
+
+ /* obtain spad info from device.*/
+ status = get_ref_spad_map( dev, ref_spad_array );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ /* count enabled spads within spad map array and
+ * determine if Aperture or Non-Aperture.
+ */
+ status = count_enabled_spads( ref_spad_array,
+ c_spad_array_size,
+ c_max_spad_count,
+ &spads_enabled,
+ &is_aperture_spads );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+
+ *p_spad_count = spads_enabled;
+ *p_is_aperture_spads = is_aperture_spads;
+
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ RefSpadsInitialised, 1 );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadCount,
+ ( uint8_t )spads_enabled );
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ ReferenceSpadType, is_aperture_spads );
+ }
+ }
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_get_sequence_steps_info(
+ VL53L0X_SequenceStepId sequence_step_id,
+ char *p_sequence_steps_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ switch ( sequence_step_id ) {
+ case VL53L0X_SEQUENCESTEP_TCC:
+ VL53L0X_COPYSTRING( p_sequence_steps_string,
+ VL53L0X_STRING_SEQUENCESTEP_TCC );
+ break;
+ case VL53L0X_SEQUENCESTEP_DSS:
+ VL53L0X_COPYSTRING( p_sequence_steps_string,
+ VL53L0X_STRING_SEQUENCESTEP_DSS );
+ break;
+ case VL53L0X_SEQUENCESTEP_MSRC:
+ VL53L0X_COPYSTRING( p_sequence_steps_string,
+ VL53L0X_STRING_SEQUENCESTEP_MSRC );
+ break;
+ case VL53L0X_SEQUENCESTEP_PRE_RANGE:
+ VL53L0X_COPYSTRING( p_sequence_steps_string,
+ VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE );
+ break;
+ case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
+ VL53L0X_COPYSTRING( p_sequence_steps_string,
+ VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE );
+ break;
+
+ default:
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ }
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_sequence_steps_info( VL53L0X_SequenceStepId sequence_step_id,
+ char *p_sequence_steps_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_sequence_steps_info(
+ sequence_step_id,
+ p_sequence_steps_string );
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+int32_t VL53L0X::vl53l0x_get_timer_frequency( int32_t *p_timer_freq_hz )
+{
+ *p_timer_freq_hz = 0;
+ return STATUS_FAIL;
+}
+
+int32_t VL53L0X::vl53l0x_get_timer_value( int32_t *p_timer_count )
+{
+ *p_timer_count = 0;
+ return STATUS_FAIL;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_enable_interrupt_mask( VL53L0X_DEV dev, uint32_t interrupt_mask )
+{
+ VL53L0X_Error Status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented for VL53L0X */
+
+ LOG_FUNCTION_END( Status );
+ return Status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_device_error_status( VL53L0X_DEV dev,
+ VL53L0X_DeviceError *p_device_error_status )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t range_status;
+
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_RdByte( dev, VL53L0X_REG_RESULT_RANGE_STATUS,
+ &range_status );
+
+ *p_device_error_status = ( VL53L0X_DeviceError )( ( range_status & 0x78 ) >> 3 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_get_device_error_string( VL53L0X_DeviceError error_code,
+ char *p_device_error_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_device_error_string( error_code, p_device_error_string );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_dmax_cal_parameters( VL53L0X_DEV dev,
+ uint16_t *p_range_milli_meter, FixPoint1616_t *p_signal_rate_rtn_mega_cps )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ *p_range_milli_meter = PALDevDataGet( dev, DmaxCalRangeMilliMeter );
+ *p_signal_rate_rtn_mega_cps = PALDevDataGet( dev,
+ DmaxCalSignalRateRtnMegaCps );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_dmax_cal_parameters( VL53L0X_DEV dev,
+ uint16_t range_milli_meter, FixPoint1616_t signal_rate_rtn_mega_cps )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ FixPoint1616_t signal_rate_rtn_mega_cps_temp = 0;
+
+ LOG_FUNCTION_START( "" );
+
+ /* Check if one of input parameter is zero, in that case the
+ * value are get from NVM */
+ if ( ( range_milli_meter == 0 ) || ( signal_rate_rtn_mega_cps == 0 ) ) {
+ /* NVM parameters */
+ /* Run VL53L0X_get_info_from_device wit option 4 to get
+ * signal rate at 400 mm if the value have been already
+ * get this function will return with no access to device */
+ vl53l0x_get_info_from_device( dev, 4 );
+
+ signal_rate_rtn_mega_cps_temp = VL53L0X_GETDEVICESPECIFICPARAMETER(
+ dev, SignalRateMeasFixed400mm );
+
+ PALDevDataSet( dev, DmaxCalRangeMilliMeter, 400 );
+ PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
+ signal_rate_rtn_mega_cps_temp );
+ } else {
+ /* User parameters */
+ PALDevDataSet( dev, DmaxCalRangeMilliMeter, range_milli_meter );
+ PALDevDataSet( dev, DmaxCalSignalRateRtnMegaCps,
+ signal_rate_rtn_mega_cps );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53L0x_set_gpio_config( VL53L0X_DEV dev, uint8_t pin,
+ VL53L0X_DeviceModes device_mode, VL53L0X_GpioFunctionality functionality,
+ VL53L0X_InterruptPolarity polarity )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t data;
+
+ LOG_FUNCTION_START( "" );
+
+ if ( pin != 0 ) {
+ status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
+ } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_DRIVE ) {
+ if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
+ data = 0x10;
+ else
+ data = 1;
+
+ status = VL53L0X_WrByte( dev,
+ VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, data );
+
+ } else if ( device_mode == VL53L0X_DEVICEMODE_GPIO_OSC ) {
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x01 );
+ status |= VL53L0X_WrByte( dev, 0x00, 0x00 );
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x00 );
+ status |= VL53L0X_WrByte( dev, 0x80, 0x01 );
+ status |= VL53L0X_WrByte( dev, 0x85, 0x02 );
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x04 );
+ status |= VL53L0X_WrByte( dev, 0xcd, 0x00 );
+ status |= VL53L0X_WrByte( dev, 0xcc, 0x11 );
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x07 );
+ status |= VL53L0X_WrByte( dev, 0xbe, 0x00 );
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x06 );
+ status |= VL53L0X_WrByte( dev, 0xcc, 0x09 );
+
+ status |= VL53L0X_WrByte( dev, 0xff, 0x00 );
+ status |= VL53L0X_WrByte( dev, 0xff, 0x01 );
+ status |= VL53L0X_WrByte( dev, 0x00, 0x00 );
+
+ } else {
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ switch ( functionality ) {
+ case VL53L0X_GPIOFUNCTIONALITY_OFF:
+ data = 0x00;
+ break;
+ case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW:
+ data = 0x01;
+ break;
+ case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH:
+ data = 0x02;
+ break;
+ case VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT:
+ data = 0x03;
+ break;
+ case VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY:
+ data = 0x04;
+ break;
+ default:
+ status =
+ VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
+ }
+ }
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = VL53L0X_WrByte( dev,
+ VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, data );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( polarity == VL53L0X_INTERRUPTPOLARITY_LOW )
+ data = 0;
+ else
+ data = ( uint8_t )( 1 << 4 );
+
+ status = VL53L0X_UpdateByte( dev,
+ VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH, 0xEF, data );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE )
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev,
+ Pin0GpioFunctionality, functionality );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_clear_interrupt_mask( dev, 0 );
+
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_get_gpio_config( VL53L0X_DEV dev, uint8_t pin,
+ VL53L0X_DeviceModes *p_device_mode,
+ VL53L0X_GpioFunctionality *p_functionality,
+ VL53L0X_InterruptPolarity *p_polarity )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ VL53L0X_GpioFunctionality gpio_functionality;
+ uint8_t data;
+
+ LOG_FUNCTION_START( "" );
+
+ /* pDeviceMode not managed by Ewok it return the current mode */
+
+ status = vl53l0x_get_device_mode( dev, p_device_mode );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( pin != 0 ) {
+ status = VL53L0X_ERROR_GPIO_NOT_EXISTING;
+ } else {
+ status = VL53L0X_RdByte( dev,
+ VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO, &data );
+ }
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ switch ( data & 0x07 ) {
+ case 0x00:
+ gpio_functionality = VL53L0X_GPIOFUNCTIONALITY_OFF;
+ break;
+ case 0x01:
+ gpio_functionality =
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW;
+ break;
+ case 0x02:
+ gpio_functionality =
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH;
+ break;
+ case 0x03:
+ gpio_functionality =
+ VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT;
+ break;
+ case 0x04:
+ gpio_functionality =
+ VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY;
+ break;
+ default:
+ status = VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED;
+ }
+ }
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = VL53L0X_RdByte( dev, VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH,
+ &data );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( ( data & ( uint8_t )( 1 << 4 ) ) == 0 )
+ *p_polarity = VL53L0X_INTERRUPTPOLARITY_LOW;
+ else
+ *p_polarity = VL53L0X_INTERRUPTPOLARITY_HIGH;
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ *p_functionality = gpio_functionality;
+ VL53L0X_SETDEVICESPECIFICPARAMETER( dev, Pin0GpioFunctionality,
+ gpio_functionality );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_histogram_measurement_data( VL53L0X_DEV dev,
+ VL53L0X_HistogramMeasurementData_t *p_histogram_measurement_data )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_histogram_mode( VL53L0X_DEV dev,
+ VL53L0X_HistogramModes *p_histogram_mode )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_histogram_mode( VL53L0X_DEV dev,
+ VL53L0X_HistogramModes histogram_mode )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( Status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_current( VL53L0X_DEV dev, uint16_t limit_check_id,
+ FixPoint1616_t *p_limit_check_current )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ VL53L0X_RangingMeasurementData_t last_range_data_buffer;
+
+ LOG_FUNCTION_START( "" );
+
+ if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ } else {
+ switch ( limit_check_id ) {
+ case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
+ /* Need to run a ranging to have the latest values */
+ *p_limit_check_current = PALDevDataGet( dev, SigmaEstimate );
+
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
+ /* Need to run a ranging to have the latest values */
+ last_range_data_buffer = PALDevDataGet( dev,
+ LastRangeMeasure );
+ *p_limit_check_current =
+ last_range_data_buffer.SignalRateRtnMegaCps;
+
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
+ /* Need to run a ranging to have the latest values */
+ *p_limit_check_current = PALDevDataGet( dev,
+ LastSignalRefMcps );
+
+ break;
+
+ case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
+ /* Need to run a ranging to have the latest values */
+ last_range_data_buffer = PALDevDataGet( dev,
+ LastRangeMeasure );
+ *p_limit_check_current =
+ last_range_data_buffer.SignalRateRtnMegaCps;
+
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
+ /* Need to run a ranging to have the latest values */
+ last_range_data_buffer = PALDevDataGet( dev,
+ LastRangeMeasure );
+ *p_limit_check_current =
+ last_range_data_buffer.SignalRateRtnMegaCps;
+
+ break;
+
+ case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
+ /* Need to run a ranging to have the latest values */
+ last_range_data_buffer = PALDevDataGet( dev,
+ LastRangeMeasure );
+ *p_limit_check_current =
+ last_range_data_buffer.SignalRateRtnMegaCps;
+
+ break;
+
+ default:
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_info( VL53L0X_DEV dev, uint16_t limit_check_id,
+ char *p_limit_check_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_limit_check_info( dev, limit_check_id,
+ p_limit_check_string );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_limit_check_status( VL53L0X_DEV dev, uint16_t limit_check_id,
+ uint8_t *p_limit_check_status )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t temp8;
+
+ LOG_FUNCTION_START( "" );
+
+ if ( limit_check_id >= VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS ) {
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ } else {
+
+ VL53L0X_GETARRAYPARAMETERFIELD( dev, LimitChecksStatus,
+ limit_check_id, temp8 );
+
+ *p_limit_check_status = temp8;
+
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_linearity_corrective_gain( VL53L0X_DEV dev,
+ int16_t linearity_corrective_gain )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ if ( ( linearity_corrective_gain < 0 ) || ( linearity_corrective_gain > 1000 ) )
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ else {
+ PALDevDataSet( dev, LinearityCorrectiveGain,
+ linearity_corrective_gain );
+
+ if ( linearity_corrective_gain != 1000 ) {
+ /* Disable FW Xtalk */
+ status = VL53L0X_WrWord( dev,
+ VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, 0 );
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_linearity_corrective_gain( VL53L0X_DEV dev,
+ uint16_t *p_linearity_corrective_gain )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ *p_linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_max_number_of_roi_zones( VL53L0X_DEV dev,
+ uint8_t *p_max_number_of_roi_zones )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ *p_max_number_of_roi_zones = 1;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_measurement_ref_signal( VL53L0X_DEV dev,
+ FixPoint1616_t *p_measurement_ref_signal )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t signal_ref_clip_limit_check_enable = 0;
+ LOG_FUNCTION_START( "" );
+
+ status = vl53l0x_get_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP,
+ &signal_ref_clip_limit_check_enable );
+ if ( signal_ref_clip_limit_check_enable != 0 ) {
+ *p_measurement_ref_signal = PALDevDataGet( dev, LastSignalRefMcps );
+ } else {
+ status = VL53L0X_ERROR_INVALID_COMMAND;
+ }
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_get_number_of_limit_check( uint16_t *p_number_of_limit_check )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ *p_number_of_limit_check = VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_number_of_roi_zones( VL53L0X_DEV dev,
+ uint8_t *p_number_of_roi_zones )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ *p_number_of_roi_zones = 1;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_number_of_roi_zones( VL53L0X_DEV dev,
+ uint8_t number_of_roi_zones )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ if ( number_of_roi_zones != 1 )
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_number_of_sequence_steps( VL53L0X_DEV dev,
+ uint8_t *p_number_of_sequence_steps )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ *p_number_of_sequence_steps = VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_pal_error_string( VL53L0X_Error pal_error_code,
+ char *p_pal_error_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_pal_error_string( pal_error_code, p_pal_error_string );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_pal_spec_version( VL53L0X_Version_t *p_pal_spec_version )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ p_pal_spec_version->major = VL53L0X_SPECIFICATION_VER_MAJOR;
+ p_pal_spec_version->minor = VL53L0X_SPECIFICATION_VER_MINOR;
+ p_pal_spec_version->build = VL53L0X_SPECIFICATION_VER_SUB;
+
+ p_pal_spec_version->revision = VL53L0X_SPECIFICATION_VER_REVISION;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_pal_state( VL53L0X_DEV dev, VL53L0X_State *p_pal_state )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ *p_pal_state = PALDevDataGet( dev, PalState );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_pal_state_string( VL53L0X_State pal_state_code,
+ char *p_pal_state_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_pal_state_string( pal_state_code, p_pal_state_string );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_power_mode( VL53L0X_DEV dev, VL53L0X_PowerModes *p_power_mode )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t byte;
+ LOG_FUNCTION_START( "" );
+
+ /* Only level1 of Power mode exists */
+ status = VL53L0X_RdByte( dev, 0x80, &byte );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( byte == 1 ) {
+ PALDevDataSet( dev, PowerMode,
+ VL53L0X_POWERMODE_IDLE_LEVEL1 );
+ } else {
+ PALDevDataSet( dev, PowerMode,
+ VL53L0X_POWERMODE_STANDBY_LEVEL1 );
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_power_mode( VL53L0X_DEV dev, VL53L0X_PowerModes power_mode )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ /* Only level1 of Power mode exists */
+ if ( ( power_mode != VL53L0X_POWERMODE_STANDBY_LEVEL1 )
+ && ( power_mode != VL53L0X_POWERMODE_IDLE_LEVEL1 ) ) {
+ status = VL53L0X_ERROR_MODE_NOT_SUPPORTED;
+ } else if ( power_mode == VL53L0X_POWERMODE_STANDBY_LEVEL1 ) {
+ /* set the standby level1 of power mode */
+ status = VL53L0X_WrByte( dev, 0x80, 0x00 );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ /* Set PAL State to standby */
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_STANDBY );
+ PALDevDataSet( dev, PowerMode,
+ VL53L0X_POWERMODE_STANDBY_LEVEL1 );
+ }
+
+ } else {
+ /* VL53L0X_POWERMODE_IDLE_LEVEL1 */
+ status = VL53L0X_WrByte( dev, 0x80, 0x00 );
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_static_init( dev );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, PowerMode,
+ VL53L0X_POWERMODE_IDLE_LEVEL1 );
+
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_product_revision( VL53L0X_DEV dev,
+ uint8_t *p_product_revision_major, uint8_t *p_product_revision_minor )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t revision_id;
+
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_RdByte( dev, VL53L0X_REG_IDENTIFICATION_REVISION_ID,
+ &revision_id );
+ *p_product_revision_major = 1;
+ *p_product_revision_minor = ( revision_id & 0xF0 ) >> 4;
+
+ LOG_FUNCTION_END( status );
+ return status;
+
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_range_status_string( uint8_t range_status,
+ char *p_range_status_string )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_range_status_string( range_status,
+ p_range_status_string );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_ref_calibration( VL53L0X_DEV dev, uint8_t *p_vhv_settings,
+ uint8_t *p_phase_cal )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_ref_calibration( dev, p_vhv_settings, p_phase_cal );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_enable( VL53L0X_DEV dev,
+ VL53L0X_SequenceStepId sequence_step_id, uint8_t *p_sequence_step_enabled )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t SequenceConfig = 0;
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_RdByte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG,
+ &SequenceConfig );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = sequence_step_enabled( dev, sequence_step_id,
+ SequenceConfig, p_sequence_step_enabled );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_sequence_step_timeout( VL53L0X_DEV dev,
+ VL53L0X_SequenceStepId sequence_step_id, FixPoint1616_t time_out_milli_secs )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ VL53L0X_Error status1 = VL53L0X_ERROR_NONE;
+ uint32_t TimeoutMicroSeconds = ( ( time_out_milli_secs * 1000 ) + 0x8000 )
+ >> 16;
+ uint32_t MeasurementTimingBudgetMicroSeconds;
+ FixPoint1616_t OldTimeOutMicroSeconds;
+
+ LOG_FUNCTION_START( "" );
+
+ /* Read back the current value in case we need to revert back to this.
+ */
+ status = get_sequence_step_timeout( dev, sequence_step_id,
+ &OldTimeOutMicroSeconds );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = set_sequence_step_timeout( dev, sequence_step_id,
+ TimeoutMicroSeconds );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_GETPARAMETERFIELD( dev,
+ MeasurementTimingBudgetMicroSeconds,
+ MeasurementTimingBudgetMicroSeconds );
+
+ /* At this point we don't know if the requested value is valid,
+ therefore proceed to update the entire timing budget and
+ if this fails, revert back to the previous value.
+ */
+ status = vl53l0x_set_measurement_timing_budget_micro_seconds( dev,
+ MeasurementTimingBudgetMicroSeconds );
+
+ if ( status != VL53L0X_ERROR_NONE ) {
+ status1 = set_sequence_step_timeout( dev, sequence_step_id,
+ OldTimeOutMicroSeconds );
+
+ if ( status1 == VL53L0X_ERROR_NONE ) {
+ status1 =
+ vl53l0x_set_measurement_timing_budget_micro_seconds(
+ dev,
+ MeasurementTimingBudgetMicroSeconds );
+ }
+
+ status = status1;
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_sequence_step_timeout( VL53L0X_DEV dev,
+ VL53L0X_SequenceStepId sequence_step_id, FixPoint1616_t *p_time_out_milli_secs )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint32_t timeout_micro_seconds;
+ LOG_FUNCTION_START( "" );
+
+ status = get_sequence_step_timeout( dev, sequence_step_id,
+ &timeout_micro_seconds );
+ if ( status == VL53L0X_ERROR_NONE ) {
+ timeout_micro_seconds <<= 8;
+ *p_time_out_milli_secs = ( timeout_micro_seconds + 500 ) / 1000;
+ *p_time_out_milli_secs <<= 8;
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_spad_ambient_damper_factor( VL53L0X_DEV dev,
+ uint16_t spad_ambient_damper_factor )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t byte;
+ LOG_FUNCTION_START( "" );
+
+ byte = ( uint8_t )( spad_ambient_damper_factor & 0x00FF );
+
+ status = VL53L0X_WrByte( dev, 0xFF, 0x01 );
+ status |= VL53L0X_WrByte( dev, 0x42, byte );
+ status |= VL53L0X_WrByte( dev, 0xFF, 0x00 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_get_spad_ambient_damper_factor( VL53L0X_DEV dev,
+ uint16_t *p_spad_ambient_damper_factor )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t Byte;
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_WrByte( dev, 0xFF, 0x01 );
+ status |= VL53L0X_RdByte( dev, 0x42, &Byte );
+ status |= VL53L0X_WrByte( dev, 0xFF, 0x00 );
+ *p_spad_ambient_damper_factor = ( uint16_t )Byte;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_spad_ambient_damper_threshold( VL53L0X_DEV dev,
+ uint16_t spad_ambient_damper_threshold )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_WrByte( dev, 0xFF, 0x01 );
+ status |= VL53L0X_WrWord( dev, 0x40, spad_ambient_damper_threshold );
+ status |= VL53L0X_WrByte( dev, 0xFF, 0x00 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_spad_ambient_damper_threshold( VL53L0X_DEV dev,
+ uint16_t *p_spad_ambient_damper_threshold )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_WrByte( dev, 0xFF, 0x01 );
+ status |= VL53L0X_RdWord( dev, 0x40, p_spad_ambient_damper_threshold );
+ status |= VL53L0X_WrByte( dev, 0xFF, 0x00 );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_upper_limit_milli_meter( VL53L0X_DEV dev,
+ uint16_t *p_upper_limit_milli_meter )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_tuning_setting_buffer( VL53L0X_DEV dev,
+ uint8_t *p_tuning_setting_buffer, uint8_t use_internal_tuning_settings )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ if ( use_internal_tuning_settings == 1 ) {
+ /* Force use internal settings */
+ PALDevDataSet( dev, UseInternalTuningSettings, 1 );
+ } else {
+
+ /* check that the first byte is not 0 */
+ if ( *p_tuning_setting_buffer != 0 ) {
+ PALDevDataSet( dev, pTuningSettingsPointer,
+ p_tuning_setting_buffer );
+ PALDevDataSet( dev, UseInternalTuningSettings, 0 );
+
+ } else {
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_tuning_setting_buffer( VL53L0X_DEV dev,
+ uint8_t **pp_tuning_setting_buffer, uint8_t *p_use_internal_tuning_settings )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "" );
+
+ *pp_tuning_setting_buffer = PALDevDataGet( dev, pTuningSettingsPointer );
+ *p_use_internal_tuning_settings = PALDevDataGet( dev,
+ UseInternalTuningSettings );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_version( VL53L0X_Version_t *p_version )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ p_version->major = VL53L0X_IMPLEMENTATION_VER_MAJOR;
+ p_version->minor = VL53L0X_IMPLEMENTATION_VER_MINOR;
+ p_version->build = VL53L0X_IMPLEMENTATION_VER_SUB;
+
+ p_version->revision = VL53L0X_IMPLEMENTATION_VER_REVISION;
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_lock_sequence_access( VL53L0X_DEV dev )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_unlock_sequence_access( VL53L0X_DEV dev )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_offset_calibration( VL53L0X_DEV dev,
+ FixPoint1616_t cal_distance_milli_meter,
+ int32_t *p_offset_micro_meter )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint16_t sum_ranging = 0;
+ FixPoint1616_t total_count = 0;
+ VL53L0X_RangingMeasurementData_t ranging_measurement_data;
+ FixPoint1616_t stored_mean_range;
+ uint32_t stored_mean_range_as_int;
+ uint32_t cal_distance_as_int_mm;
+ uint8_t sequence_step_enabled;
+ int meas = 0;
+
+ if ( cal_distance_milli_meter <= 0 )
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+
+ if ( status == VL53L0X_ERROR_NONE )
+ status = wrapped_vl53l0x_set_offset_calibration_data_micro_meter( dev, 0 );
+
+
+ /* Get the value of the TCC */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_get_sequence_step_enable( dev,
+ VL53L0X_SEQUENCESTEP_TCC, &sequence_step_enabled );
+
+
+ /* Disable the TCC */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_sequence_step_enable( dev,
+ VL53L0X_SEQUENCESTEP_TCC, 0 );
+
+
+ /* Disable the RIT */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
+
+ /* Perform 50 measurements and compute the averages */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ sum_ranging = 0;
+ total_count = 0;
+ for ( meas = 0; meas < 50; meas++ ) {
+ status = vl53l0x_perform_single_ranging_measurement( dev,
+ &ranging_measurement_data );
+
+ if ( status != VL53L0X_ERROR_NONE )
+ break;
+
+ /* The range is valid when RangeStatus = 0 */
+ if ( ranging_measurement_data.RangeStatus == 0 ) {
+ sum_ranging = sum_ranging +
+ ranging_measurement_data.RangeMilliMeter;
+ total_count = total_count + 1;
+ }
+ }
+
+ /* no valid values found */
+ if ( total_count == 0 )
+ status = VL53L0X_ERROR_RANGE_ERROR;
+ }
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ /* FixPoint1616_t / uint16_t = FixPoint1616_t */
+ stored_mean_range = ( FixPoint1616_t )( ( uint32_t )( sum_ranging << 16 )
+ / total_count );
+
+ stored_mean_range_as_int = ( stored_mean_range + 0x8000 ) >> 16;
+
+ /* Round Cal Distance to Whole Number.
+ * Note that the cal distance is in mm, therefore no resolution
+ * is lost.*/
+ cal_distance_as_int_mm = ( cal_distance_milli_meter + 0x8000 ) >> 16;
+
+ *p_offset_micro_meter = ( cal_distance_as_int_mm -
+ stored_mean_range_as_int ) * 1000;
+
+ /* Apply the calculated offset */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev, RangeOffsetMicroMeters,
+ *p_offset_micro_meter );
+ status = vl53l0x_set_offset_calibration_data_micro_meter( dev,
+ *p_offset_micro_meter );
+ }
+
+ }
+
+ /* Restore the TCC */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( sequence_step_enabled != 0 )
+ status = vl53l0x_set_sequence_step_enable( dev,
+ VL53L0X_SEQUENCESTEP_TCC, 1 );
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_x_talk_compensation_enable( VL53L0X_DEV dev,
+ uint8_t x_talk_compensation_enable )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ FixPoint1616_t temp_fix1616;
+ uint16_t linearity_corrective_gain;
+
+ LOG_FUNCTION_START( "" );
+
+ linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
+
+ if ( ( x_talk_compensation_enable == 0 )
+ || ( linearity_corrective_gain != 1000 ) ) {
+ temp_fix1616 = 0;
+ } else {
+ VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
+ temp_fix1616 );
+ }
+
+ /* the following register has a format 3.13 */
+ status = VL53L0X_WrWord( dev,
+ VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS,
+ VL53L0X_FIXPOINT1616TOFIXPOINT313( temp_fix1616 ) );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( x_talk_compensation_enable == 0 ) {
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
+ 0 );
+ } else {
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationEnable,
+ 1 );
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_xtalk_calibration( VL53L0X_DEV dev,
+ FixPoint1616_t xtalk_cal_distance,
+ FixPoint1616_t *p_xtalk_compensation_rate_mega_cps )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint16_t sum_ranging = 0;
+ uint16_t sum_spads = 0;
+ FixPoint1616_t sum_signal_rate = 0;
+ FixPoint1616_t total_count = 0;
+ uint8_t xtalk_meas = 0;
+ VL53L0X_RangingMeasurementData_t ranging_measurement_data;
+ FixPoint1616_t x_talk_stored_mean_signal_rate;
+ FixPoint1616_t x_talk_stored_mean_range;
+ FixPoint1616_t x_talk_stored_mean_rtn_spads;
+ uint32_t signal_x_talk_total_per_spad;
+ uint32_t x_talk_stored_mean_rtn_spads_as_int;
+ uint32_t x_talk_cal_distance_as_int;
+ FixPoint1616_t x_talk_compensation_rate_mega_cps;
+
+ if ( xtalk_cal_distance <= 0 )
+ status = VL53L0X_ERROR_INVALID_PARAMS;
+
+ /* Disable the XTalk compensation */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_x_talk_compensation_enable( dev, 0 );
+
+ /* Disable the RIT */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ status = vl53l0x_set_limit_check_enable( dev,
+ VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 0 );
+ }
+
+ /* Perform 50 measurements and compute the averages */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ sum_ranging = 0;
+ sum_spads = 0;
+ sum_signal_rate = 0;
+ total_count = 0;
+ for ( xtalk_meas = 0; xtalk_meas < 50; xtalk_meas++ ) {
+ status = vl53l0x_perform_single_ranging_measurement( dev,
+ &ranging_measurement_data );
+
+ if ( status != VL53L0X_ERROR_NONE )
+ break;
+
+ /* The range is valid when RangeStatus = 0 */
+ if ( ranging_measurement_data.RangeStatus == 0 ) {
+ sum_ranging = sum_ranging +
+ ranging_measurement_data.RangeMilliMeter;
+ sum_signal_rate = sum_signal_rate +
+ ranging_measurement_data.SignalRateRtnMegaCps;
+ sum_spads = sum_spads +
+ ranging_measurement_data.EffectiveSpadRtnCount
+ / 256;
+ total_count = total_count + 1;
+ }
+ }
+
+ /* no valid values found */
+ if ( total_count == 0 )
+ status = VL53L0X_ERROR_RANGE_ERROR;
+
+ }
+
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ /* FixPoint1616_t / uint16_t = FixPoint1616_t */
+ x_talk_stored_mean_signal_rate = sum_signal_rate / total_count;
+ x_talk_stored_mean_range = ( FixPoint1616_t )( ( uint32_t )(
+ sum_ranging << 16 ) / total_count );
+ x_talk_stored_mean_rtn_spads = ( FixPoint1616_t )( ( uint32_t )(
+ sum_spads << 16 ) / total_count );
+
+ /* Round Mean Spads to Whole Number.
+ * Typically the calculated mean SPAD count is a whole number
+ * or very close to a whole
+ * number, therefore any truncation will not result in a
+ * significant loss in accuracy.
+ * Also, for a grey target at a typical distance of around
+ * 400mm, around 220 SPADs will
+ * be enabled, therefore, any truncation will result in a loss
+ * of accuracy of less than
+ * 0.5%.
+ */
+ x_talk_stored_mean_rtn_spads_as_int = ( x_talk_stored_mean_rtn_spads +
+ 0x8000 ) >> 16;
+
+ /* Round Cal Distance to Whole Number.
+ * Note that the cal distance is in mm, therefore no resolution
+ * is lost.*/
+ x_talk_cal_distance_as_int = ( xtalk_cal_distance + 0x8000 ) >> 16;
+
+ if ( x_talk_stored_mean_rtn_spads_as_int == 0 ||
+ x_talk_cal_distance_as_int == 0 ||
+ x_talk_stored_mean_range >= xtalk_cal_distance ) {
+ x_talk_compensation_rate_mega_cps = 0;
+ } else {
+ /* Round Cal Distance to Whole Number.
+ Note that the cal distance is in mm, therefore no
+ resolution is lost.*/
+ x_talk_cal_distance_as_int = ( xtalk_cal_distance +
+ 0x8000 ) >> 16;
+
+ /* Apply division by mean spad count early in the
+ * calculation to keep the numbers small.
+ * This ensures we can maintain a 32bit calculation.
+ * Fixed1616 / int := Fixed1616 */
+ signal_x_talk_total_per_spad = ( x_talk_stored_mean_signal_rate ) /
+ x_talk_stored_mean_rtn_spads_as_int;
+
+ /* Complete the calculation for total Signal XTalk per
+ * SPAD
+ * Fixed1616 * (Fixed1616 - Fixed1616/int) :=
+ * (2^16 * Fixed1616)
+ */
+ signal_x_talk_total_per_spad *= ( ( 1 << 16 ) -
+ ( x_talk_stored_mean_range / x_talk_cal_distance_as_int ) );
+
+ /* Round from 2^16 * Fixed1616, to Fixed1616. */
+ x_talk_compensation_rate_mega_cps = ( signal_x_talk_total_per_spad
+ + 0x8000 ) >> 16;
+ }
+
+ *p_xtalk_compensation_rate_mega_cps = x_talk_compensation_rate_mega_cps;
+
+ /* Enable the XTalk compensation */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_x_talk_compensation_enable( dev, 1 );
+
+ /* Enable the XTalk compensation */
+ if ( status == VL53L0X_ERROR_NONE )
+ status = vl53l0x_set_x_talk_compensation_rate_mega_cps( dev,
+ x_talk_compensation_rate_mega_cps );
+
+ }
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_x_talk_compensation_rate_mega_cps( VL53L0X_DEV dev,
+ FixPoint1616_t x_talk_compensation_rate_mega_cps )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t temp8;
+ uint16_t linearity_corrective_gain;
+ uint16_t data;
+ LOG_FUNCTION_START( "" );
+
+ VL53L0X_GETPARAMETERFIELD( dev, XTalkCompensationEnable, temp8 );
+ linearity_corrective_gain = PALDevDataGet( dev, LinearityCorrectiveGain );
+
+ if ( temp8 == 0 ) { /* disabled write only internal value */
+ VL53L0X_SETPARAMETERFIELD( dev, XTalkCompensationRateMegaCps,
+ x_talk_compensation_rate_mega_cps );
+ } else {
+ /* the following register has a format 3.13 */
+ if ( linearity_corrective_gain == 1000 ) {
+ data = VL53L0X_FIXPOINT1616TOFIXPOINT313(
+ x_talk_compensation_rate_mega_cps );
+ } else {
+ data = 0;
+ }
+
+ status = VL53L0X_WrWord( dev,
+ VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS, data );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev,
+ XTalkCompensationRateMegaCps,
+ x_talk_compensation_rate_mega_cps );
+ }
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_perform_xtalk_measurement( VL53L0X_DEV dev,
+ uint32_t timeout_ms, FixPoint1616_t *p_xtalk_per_spad,
+ uint8_t *p_ambient_too_high )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_reset_device( VL53L0X_DEV dev )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t byte;
+ LOG_FUNCTION_START( "" );
+
+ /* Set reset bit */
+ status = VL53L0X_WrByte( dev, VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N,
+ 0x00 );
+
+ /* Wait for some time */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ do {
+ status = VL53L0X_RdByte( dev,
+ VL53L0X_REG_IDENTIFICATION_MODEL_ID, &byte );
+ } while ( byte != 0x00 );
+ }
+
+ VL53L0X_PollingDelay( dev );
+
+ /* Release reset */
+ status = VL53L0X_WrByte( dev, VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N,
+ 0x01 );
+
+ /* Wait until correct boot-up of the device */
+ if ( status == VL53L0X_ERROR_NONE ) {
+ do {
+ status = VL53L0X_RdByte( dev,
+ VL53L0X_REG_IDENTIFICATION_MODEL_ID, &byte );
+ } while ( byte == 0x00 );
+ }
+
+ VL53L0X_PollingDelay( dev );
+
+ /* Set PAL State to VL53L0X_STATE_POWERDOWN */
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, PalState, VL53L0X_STATE_POWERDOWN );
+
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::VL53L0X_reverse_bytes( uint8_t *data, uint32_t size )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t temp_data;
+ uint32_t mirror_index;
+ uint32_t middle = size / 2;
+ uint32_t index;
+
+ for ( index = 0; index < middle; index++ ) {
+ mirror_index = size - index - 1;
+ temp_data = data[index];
+ data[index] = data[mirror_index];
+ data[mirror_index] = temp_data;
+ }
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_ref_calibration( VL53L0X_DEV dev,
+ uint8_t vhv_settings, uint8_t phase_cal )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t p_vhv_settings;
+ uint8_t p_phase_cal;
+
+ status = vl53l0x_ref_calibration_io( dev, 0,
+ vhv_settings, phase_cal,
+ &p_vhv_settings, &p_phase_cal,
+ 1, 1 );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::VL53L0X_SetDeviceParameters( VL53L0X_DEV Dev,
+ const VL53L0X_DeviceParameters_t *pDeviceParameters )
+{
+ VL53L0X_Error Status = VL53L0X_ERROR_NONE;
+ int i;
+ LOG_FUNCTION_START( "" );
+ Status = vl53l0x_set_device_mode( Dev, pDeviceParameters->DeviceMode );
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_set_inter_measurement_period_milli_seconds( Dev,
+ pDeviceParameters->InterMeasurementPeriodMilliSeconds );
+
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_set_x_talk_compensation_rate_mega_cps( Dev,
+ pDeviceParameters->XTalkCompensationRateMegaCps );
+
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_set_offset_calibration_data_micro_meter( Dev,
+ pDeviceParameters->RangeOffsetMicroMeters );
+
+
+ for ( i = 0; i < VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS; i++ ) {
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status |= vl53l0x_set_limit_check_enable( Dev, i,
+ pDeviceParameters->LimitChecksEnable[i] );
+ else
+ break;
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status |= vl53l0x_set_limit_check_value( Dev, i,
+ pDeviceParameters->LimitChecksValue[i] );
+ else
+ break;
+
+ }
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_set_wrap_around_check_enable( Dev,
+ pDeviceParameters->WrapAroundCheckEnable );
+
+ if ( Status == VL53L0X_ERROR_NONE )
+ Status = vl53l0x_set_measurement_timing_budget_micro_seconds( Dev,
+ pDeviceParameters->MeasurementTimingBudgetMicroSeconds );
+
+
+ LOG_FUNCTION_END( Status );
+ return Status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_inter_measurement_period_milli_seconds( VL53L0X_DEV dev,
+ uint32_t inter_measurement_period_milli_seconds )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint16_t osc_calibrate_val;
+ uint32_t im_period_milli_seconds;
+
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_RdWord( dev, VL53L0X_REG_OSC_CALIBRATE_VAL,
+ &osc_calibrate_val );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ if ( osc_calibrate_val != 0 ) {
+ im_period_milli_seconds =
+ inter_measurement_period_milli_seconds
+ * osc_calibrate_val;
+ } else {
+ im_period_milli_seconds =
+ inter_measurement_period_milli_seconds;
+ }
+ status = VL53L0X_WrDWord( dev,
+ VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD,
+ im_period_milli_seconds );
+ }
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ VL53L0X_SETPARAMETERFIELD( dev,
+ InterMeasurementPeriodMilliSeconds,
+ inter_measurement_period_milli_seconds );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_wrap_around_check_enable( VL53L0X_DEV dev,
+ uint8_t wrap_around_check_enable )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ uint8_t data;
+ uint8_t wrap_around_check_enable_int;
+
+ LOG_FUNCTION_START( "" );
+
+ status = VL53L0X_RdByte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, &data );
+ if ( wrap_around_check_enable == 0 ) {
+ /* Disable wraparound */
+ data = data & 0x7F;
+ wrap_around_check_enable_int = 0;
+ } else {
+ /*Enable wraparound */
+ data = data | 0x80;
+ wrap_around_check_enable_int = 1;
+ }
+
+ status = VL53L0X_WrByte( dev, VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG, data );
+
+ if ( status == VL53L0X_ERROR_NONE ) {
+ PALDevDataSet( dev, SequenceConfig, data );
+ VL53L0X_SETPARAMETERFIELD( dev, WrapAroundCheckEnable,
+ wrap_around_check_enable_int );
+ }
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_group_param_hold( VL53L0X_DEV dev, uint8_t group_param_hold )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_range_fraction_enable( VL53L0X_DEV dev, uint8_t enable )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+
+ LOG_FUNCTION_START( "%d", ( int )Enable );
+
+ status = VL53L0X_WrByte( dev, VL53L0X_REG_SYSTEM_RANGE_CONFIG, enable );
+
+ if ( status == VL53L0X_ERROR_NONE )
+ PALDevDataSet( dev, RangeFractionalEnable, enable );
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_set_reference_spads( VL53L0X_DEV dev, uint32_t count,
+ uint8_t is_aperture_spads )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_set_reference_spads( dev, count, is_aperture_spads );
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_get_reference_spads( VL53L0X_DEV dev, uint32_t *p_spad_count,
+ uint8_t *p_is_aperture_spads )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NONE;
+ LOG_FUNCTION_START( "" );
+
+ status = wrapped_vl53l0x_get_reference_spads( dev, p_spad_count, p_is_aperture_spads );
+
+ LOG_FUNCTION_END( status );
+
+ return status;
+}
+
+VL53L0X_Error VL53L0X::wrapped_vl53l0x_wait_device_booted( VL53L0X_DEV dev )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented on VL53L0X */
+
+ LOG_FUNCTION_END( status );
+ return status;
+}
+
+VL53L0X_Error VL53L0X::vl53l0x_wait_device_ready_for_new_measurement( VL53L0X_DEV dev,
+ uint32_t max_loop )
+{
+ VL53L0X_Error status = VL53L0X_ERROR_NOT_IMPLEMENTED;
+ LOG_FUNCTION_START( "" );
+
+ /* not implemented for VL53L0X */
+
+ LOG_FUNCTION_END( satus );
+ return status;
+}
/******************************************************************************/
-
-
-
-
-
\ No newline at end of file