AMiRo-OS

/*

* 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.

*

*

********************************************************************************

*

*/

/**

 * @file  vl53l1_core_support.c

 *

 * @brief EwokPlus25 core function definition

 */

#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)

{

        /**

         *  Decodes the array (row,col) location from

         *  the input SPAD number

         */

        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;

        }

}