123 lines
3.5 KiB
C
123 lines
3.5 KiB
C
/**
|
|
*
|
|
* Copyright (c) 2021 STMicroelectronics.
|
|
* All rights reserved.
|
|
*
|
|
* This software is licensed under terms that can be found in the LICENSE file
|
|
* in the root directory of this software component.
|
|
* If no LICENSE file comes with this software, it is provided AS-IS.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include "vl53l8cx_plugin_motion_indicator.h"
|
|
|
|
uint8_t vl53l8cx_motion_indicator_init(
|
|
VL53L8CX_Configuration *p_dev,
|
|
VL53L8CX_Motion_Configuration *p_motion_config,
|
|
uint8_t resolution)
|
|
{
|
|
uint8_t status = VL53L8CX_STATUS_OK;
|
|
|
|
(void)memset(p_motion_config, 0, sizeof(VL53L8CX_Motion_Configuration));
|
|
|
|
p_motion_config->ref_bin_offset = 13633;
|
|
p_motion_config->detection_threshold = 2883584;
|
|
p_motion_config->extra_noise_sigma = 0;
|
|
p_motion_config->null_den_clip_value = 0;
|
|
p_motion_config->mem_update_mode = 6;
|
|
p_motion_config->mem_update_choice = 2;
|
|
p_motion_config->sum_span = 4;
|
|
p_motion_config->feature_length = 9;
|
|
p_motion_config->nb_of_aggregates = 16;
|
|
p_motion_config->nb_of_temporal_accumulations = 16;
|
|
p_motion_config->min_nb_for_global_detection = 1;
|
|
p_motion_config->global_indicator_format_1 = 8;
|
|
p_motion_config->global_indicator_format_2 = 0;
|
|
p_motion_config->spare_1 = 0;
|
|
p_motion_config->spare_2 = 0;
|
|
p_motion_config->spare_3 = 0;
|
|
|
|
status |= vl53l8cx_motion_indicator_set_resolution(p_dev,
|
|
p_motion_config, resolution);
|
|
|
|
return status;
|
|
}
|
|
|
|
uint8_t vl53l8cx_motion_indicator_set_distance_motion(
|
|
VL53L8CX_Configuration *p_dev,
|
|
VL53L8CX_Motion_Configuration *p_motion_config,
|
|
uint16_t distance_min_mm,
|
|
uint16_t distance_max_mm)
|
|
{
|
|
uint8_t status = VL53L8CX_STATUS_OK;
|
|
float_t tmp;
|
|
|
|
if(((distance_max_mm - distance_min_mm) > (uint16_t)1500)
|
|
|| (distance_min_mm < (uint16_t)400)
|
|
|| (distance_max_mm > (uint16_t)4000))
|
|
{
|
|
status |= VL53L8CX_STATUS_INVALID_PARAM;
|
|
}
|
|
else
|
|
{
|
|
tmp = (float_t)((((float_t)distance_min_mm/(float_t)37.5348)
|
|
-(float_t)4.0)*(float_t)2048.5);
|
|
p_motion_config->ref_bin_offset = (int32_t)tmp;
|
|
|
|
tmp = (float_t)((((((float_t)distance_max_mm-
|
|
(float_t)distance_min_mm)/(float_t)10.0)+(float_t)30.02784)
|
|
/((float_t)15.01392))+(float_t)0.5);
|
|
p_motion_config->feature_length = (uint8_t)tmp;
|
|
|
|
status |= vl53l8cx_dci_write_data(p_dev,
|
|
(uint8_t*)(p_motion_config),
|
|
VL53L8CX_DCI_MOTION_DETECTOR_CFG,
|
|
(uint16_t)sizeof(*p_motion_config));
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
uint8_t vl53l8cx_motion_indicator_set_resolution(
|
|
VL53L8CX_Configuration *p_dev,
|
|
VL53L8CX_Motion_Configuration *p_motion_config,
|
|
uint8_t resolution)
|
|
{
|
|
uint8_t i, status = VL53L8CX_STATUS_OK;
|
|
|
|
switch(resolution)
|
|
{
|
|
case VL53L8CX_RESOLUTION_4X4:
|
|
for(i = 0; i < (uint8_t)VL53L8CX_RESOLUTION_4X4; i++)
|
|
{
|
|
p_motion_config->map_id[i] = (int8_t)i;
|
|
}
|
|
(void)memset(p_motion_config->map_id + 16, -1, 48);
|
|
break;
|
|
|
|
case VL53L8CX_RESOLUTION_8X8:
|
|
for(i = 0; i < (uint8_t)VL53L8CX_RESOLUTION_8X8; i++)
|
|
{
|
|
p_motion_config->map_id[i] = (int8_t)((((int8_t)
|
|
i % 8)/2) + (4*((int8_t)i/16)));
|
|
}
|
|
break;
|
|
|
|
default:
|
|
status |= VL53L8CX_STATUS_ERROR;
|
|
break;
|
|
}
|
|
|
|
if(status == VL53L8CX_STATUS_OK)
|
|
{
|
|
status |= vl53l8cx_dci_write_data(p_dev,
|
|
(uint8_t*)(p_motion_config),
|
|
VL53L8CX_DCI_MOTION_DETECTOR_CFG,
|
|
(uint16_t)sizeof(*p_motion_config));
|
|
}
|
|
|
|
return status;
|
|
}
|