vl53l1_core_support.c

Go to the documentation of this file.

/*
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
*
* This file is part of VL53L1 Core and is dual licensed,
* either 'STMicroelectronics
* Proprietary license'
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
*
********************************************************************************
*
* 'STMicroelectronics Proprietary license'
*
********************************************************************************
*
* License terms: STMicroelectronics Proprietary in accordance with licensing
* terms at www.st.com/sla0081
*
* STMicroelectronics confidential
* Reproduction and Communication of this document is strictly prohibited unless
* specifically authorized in writing by STMicroelectronics.
*
*
********************************************************************************
*
* Alternatively, VL53L1 Core may be distributed under the terms of
* 'BSD 3-clause "New" or "Revised" License', in which case the following
* provisions apply instead of the ones mentioned above :
*
********************************************************************************
*
* License terms: BSD 3-clause "New" or "Revised" License.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
********************************************************************************
*
*/

#include "vl53l1_ll_def.h"
#include "vl53l1_ll_device.h"
#include "vl53l1_platform_log.h"
#include "vl53l1_core_support.h"
#include "vl53l1_platform_user_data.h"
#include "vl53l1_platform_user_defines.h"

#ifdef VL53L1_LOGGING
#include "vl53l1_debug.h"
#include "vl53l1_register_debug.h"
#endif

#define LOG_FUNCTION_START(fmt, ...) \
        _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
        _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
        _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
                status, fmt, ##__VA_ARGS__)

#define trace_print(level, ...) \
        _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
        level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)


uint32_t VL53L1_calc_pll_period_us(
        uint16_t  fast_osc_frequency)
{
        /*  Calculates PLL frequency using NVM fast_osc_frequency
         *  Fast osc frequency fixed point format = unsigned 4.12
         *
         *  PLL period fixed point format = unsigned 0.24
         *  Min input fast osc frequency  = 1 MHz
         *  PLL Multiplier = 64 (fixed)
         *  Min PLL freq = 64.0MHz
         *  -> max PLL period = 1/ 64
         *  ->  only the 18 LS bits are used
         *
         *  2^30 = (2^24) (1.0us) * 4096 (2^12) / 64 (PLL Multiplier)
         */

        uint32_t  pll_period_us        = 0;

        LOG_FUNCTION_START("");

        pll_period_us = (0x01 << 30) / fast_osc_frequency;

#ifdef VL53L1_LOGGING
        trace_print(VL53L1_TRACE_LEVEL_DEBUG,
                        "    %-48s : %10u\n", "pll_period_us",
                        pll_period_us);
#endif

        LOG_FUNCTION_END(0);

        return pll_period_us;
}


#ifdef PAL_EXTENDED
uint32_t  VL53L1_duration_maths(
        uint32_t  pll_period_us,
        uint32_t  vcsel_parm_pclks,
        uint32_t  window_vclks,
        uint32_t  elapsed_mclks)
{
        /*
         * Generates the ranging duration in us
         *
         * duration_us = elapsed_mclks * vcsel_perm_pclks *
         *                        window_vclks * pll_period_us
         *
         * returned value in [us] with no fraction bits
         */

        uint64_t  tmp_long_int = 0;
        uint32_t  duration_us  = 0;

        /* PLL period us =  0.24  18 LS bits used
         * window_vclks  =  12.0  (2304 max)
         * output 30b (6.24)
         */
        duration_us = window_vclks * pll_period_us;

        /* down shift by 12
         * output 18b (6.12)
         */
        duration_us = duration_us >> 12;

        /* Save first part of the calc (#1) */
        tmp_long_int = (uint64_t)duration_us;

        /* Multiply elapsed macro periods (22-bit)
         *      by VCSEL parameter 6.4  (max 63.9999)
         * output 32b (28.4)
         */
        duration_us = elapsed_mclks * vcsel_parm_pclks;

        /* down shift by 4 to remove fractional bits (#2)
         * output 28b (28.0)
         */
        duration_us = duration_us >> 4;

        /* Multiply #1 18b (6.12) by #2  28b (28.0)
         * output 46b (34.12)
         */
        tmp_long_int = tmp_long_int * (uint64_t)duration_us;

        /* Remove fractional part
         * output 34b (34.0)
         */
        tmp_long_int = tmp_long_int >> 12;

        /* Clip to 32-bits */
        if (tmp_long_int > 0xFFFFFFFF) {
                tmp_long_int = 0xFFFFFFFF;
        }

        duration_us  = (uint32_t)tmp_long_int;

        return duration_us;
}


uint32_t VL53L1_isqrt(uint32_t num)
{

        /*
         * Implements an integer square root
         *
         * From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
         */

        uint32_t  res = 0;
        uint32_t  bit = 1 << 30; /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */

        /* "bit" starts at the highest power of four <= the argument. */
        while (bit > num) {
                bit >>= 2;
        }

        while (bit != 0) {
                if (num >= res + bit)  {
                        num -= res + bit;
                        res = (res >> 1) + bit;
                } else {
                        res >>= 1;
                }
                bit >>= 2;
        }

        return res;
}


uint16_t VL53L1_rate_maths(
        int32_t   events,
        uint32_t  time_us)
{
        /*
         * Converts events into count rate
         *
         * Max events = 512 Mcps * 1sec
         *            = 512,000,000 events
         *            = 29b
         *
         * If events >  2^24 use  3-bit fractional bits is used internally
         * otherwise  7-bit fractional bits are used
         */

        uint32_t  tmp_int   = 0;
        uint32_t  frac_bits = 7;
        uint16_t  rate_mcps = 0; /* 9.7 format */

        /*
         *  Clip input event range
         */

        if (events > VL53L1_SPAD_TOTAL_COUNT_MAX) {
                tmp_int = VL53L1_SPAD_TOTAL_COUNT_MAX;
        } else if (events > 0) {
                tmp_int = (uint32_t)events;
        }

        /*
         * if events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES use 3 rather
         *  than 7 fractional bits internal to function
         */

        if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
                frac_bits = 3;
        } else {
                frac_bits = 7;
        }

        /*
         * Create 3 or 7 fractional bits
         * output 32b (29.3 or 25.7)
         * Divide by range duration in [us] - no fractional bits
         */
        if (time_us > 0) {
                tmp_int = ((tmp_int << frac_bits) + (time_us / 2)) / time_us;
        }

        /*
         * Re align if reduced resolution
         */
        if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
                tmp_int = tmp_int << 4;
        }

        /*
         * Firmware internal count is 17.7 (24b) but it this
         * case clip to 16-bit value for reporting
         */

        if (tmp_int > 0xFFFF) {
                tmp_int = 0xFFFF;
        }

        rate_mcps =  (uint16_t)tmp_int;

        return rate_mcps;
}

uint16_t VL53L1_rate_per_spad_maths(
        uint32_t  frac_bits,
        uint32_t  peak_count_rate,
        uint16_t  num_spads,
        uint32_t  max_output_value)
{

        uint32_t  tmp_int   = 0;

        /* rate_per_spad Format varies with prog frac_bits */
        uint16_t  rate_per_spad = 0;

        /* Calculate rate per spad with variable fractional bits */

        /* Frac_bits should be programmed as final frac_bits - 7 as
         * the pk_rate contains an inherent 7 bit resolution
         */

        if (num_spads > 0) {
                tmp_int = (peak_count_rate << 8) << frac_bits;
                tmp_int = (tmp_int + ((uint32_t)num_spads / 2)) / (uint32_t)num_spads;
        } else {
                tmp_int = ((peak_count_rate) << frac_bits);
        }

        /* Clip in case of overwrap - special code */

        if (tmp_int > max_output_value) {
                tmp_int = max_output_value;
        }

        rate_per_spad = (uint16_t)tmp_int;

        return rate_per_spad;
}

int32_t VL53L1_range_maths(
        uint16_t  fast_osc_frequency,
        uint16_t  phase,
        uint16_t  zero_distance_phase,
        uint8_t   fractional_bits,
        int32_t   gain_factor,
        int32_t   range_offset_mm)
{
        /*
         * Converts phase information into distance in [mm]
         */

        uint32_t    pll_period_us = 0; /* 0.24 format */
        int64_t     tmp_long_int  = 0;
        int32_t     range_mm      = 0;

        /* Calculate PLL period in [ps] */

        pll_period_us  = VL53L1_calc_pll_period_us(fast_osc_frequency);

        /* Raw range in [mm]
         *
         * calculate the phase difference between return and reference phases
         *
         * phases 16b (5.11)
         * output 17b including sign bit
         */

        tmp_long_int = (int64_t)phase - (int64_t)zero_distance_phase;

        /*
         * multiply by the PLL period
         *
         * PLL period 24bit (0.24) but only 18 LS bits used
         *
         * Output  35b (0.35) (17b + 18b)
         */

        tmp_long_int =  tmp_long_int * (int64_t)pll_period_us;

        /*
         * Down shift by 9 - Output 26b (0.26)
         */

        tmp_long_int =  tmp_long_int / (0x01 << 9);

        /*
         *  multiply by speed of light in air divided by 8
         *  Factor of 8 includes 2 for the round trip and 4 scaling
         *
         *  VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8 = 16b (16.2)
         *
         *  Output 42b (18.24) (16b + 26b)
         */

        tmp_long_int =  tmp_long_int * VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8;

        /*
         * Down shift by 22 - Output 20b (18.2)
         */

        tmp_long_int =  tmp_long_int / (0x01 << 22);

        /* Add range offset */
        range_mm  = (int32_t)tmp_long_int + range_offset_mm;

        /* apply correction gain */
        range_mm *= gain_factor;
        range_mm += 0x0400;
        range_mm /= 0x0800;

        /* Remove fractional bits */
        if (fractional_bits == 0)
                range_mm = range_mm / (0x01 << 2);
        else if (fractional_bits == 1)
                range_mm = range_mm / (0x01 << 1);

        return range_mm;
}
#endif

uint8_t VL53L1_decode_vcsel_period(uint8_t vcsel_period_reg)
{
        /*
         * Converts the encoded VCSEL period register value into
         * the real period in PLL clocks
         */

        uint8_t vcsel_period_pclks = 0;

        vcsel_period_pclks = (vcsel_period_reg + 1) << 1;

        return vcsel_period_pclks;
}


void VL53L1_decode_row_col(
        uint8_t  spad_number,
        uint8_t  *prow,
        uint8_t  *pcol)
{

        if (spad_number > 127) {
                *prow = 8 + ((255-spad_number) & 0x07);
                *pcol = (spad_number-128) >> 3;
        } else {
                *prow = spad_number & 0x07;
                *pcol = (127-spad_number) >> 3;
        }
}