Pico driver for vl53l1x Time of Flight breakout
This commit is contained in:
Simon Reap
Phil Howard
parent
53f3539c3a
commit
0166208a52
|
|
@ -1,3 +1,4 @@
|
|||
add_subdirectory(st7789)
|
||||
add_subdirectory(msa301)
|
||||
add_subdirectory(rv3028)
|
||||
add_subdirectory(vl53l1x)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,10 @@
|
|||
add_library(vl53l1x INTERFACE)
|
||||
|
||||
target_sources(vl53l1x INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/vl53l1x.cpp
|
||||
)
|
||||
|
||||
target_include_directories(vl53l1x INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(vl53l1x INTERFACE pico_stdlib hardware_i2c)
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
Most of the functionality of this library is based on the VL53L1X API provided
|
||||
provided by ST (STSW-IMG007), and some of the explanatory comments are quoted
|
||||
or paraphrased from the API source code, API user manual (UM2356), and VL53L1X
|
||||
datasheet. Therefore, the license terms for the API source code (BSD 3-clause
|
||||
"New" or "Revised" License) also apply to this derivative work, as specified
|
||||
below.
|
||||
|
||||
For more information, see
|
||||
|
||||
https://www.pololu.com/
|
||||
https://forum.pololu.com/
|
||||
|
||||
--------------------------------------------------------------------------------
|
||||
|
||||
Copyright (c) 2017, STMicroelectronics
|
||||
Copyright (c) 2018-2020, Pololu Corporation
|
||||
All Rights Reserved
|
||||
|
||||
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.
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
add_library(vl53l1x INTERFACE)
|
||||
|
||||
target_sources(vl53l1x INTERFACE
|
||||
${CMAKE_CURRENT_LIST_DIR}/vl53l1x.cpp
|
||||
)
|
||||
|
||||
target_include_directories(vl53l1x INTERFACE ${CMAKE_CURRENT_LIST_DIR})
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(vl53l1x INTERFACE pico_stdlib hardware_i2c)
|
||||
|
|
@ -0,0 +1,811 @@
|
|||
// Most of the functionality of this library is based on the VL53L1X API
|
||||
// provided by ST (STSW-IMG007), and some of the explanatory comments are quoted
|
||||
// or paraphrased from the API source code, API user manual (UM2356), and
|
||||
// VL53L1X datasheet. Therefore, the license terms for the API source code
|
||||
// (BSD 3-clause "New" or "Revised" License) also apply to this derivative work.
|
||||
// Based on the code in https://github.com/pololu/vl53l1x-arduino
|
||||
// Modified by https://github.com/simon3270/driver-vl53l1x
|
||||
|
||||
#include "vl53l1x.hpp"
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
|
||||
namespace pimoroni {
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
// Initialize sensor using settings taken mostly from VL53L1_DataInit() and
|
||||
// VL53L1_StaticInit().
|
||||
// We are running a breakout, so will definitely configure the sensor for 2V8 mode
|
||||
uint16_t VL53L1X::getid() {
|
||||
return readReg16Bit(IDENTIFICATION__MODEL_ID);
|
||||
}
|
||||
uint16_t VL53L1X::getosc() {
|
||||
return readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY);
|
||||
}
|
||||
void VL53L1X::setosc(uint16_t value) {
|
||||
writeReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY, value);
|
||||
}
|
||||
|
||||
bool VL53L1X::init(bool io_2v8)
|
||||
{
|
||||
// Set some defaults
|
||||
setTimeout(0);
|
||||
did_timeout = false;
|
||||
calibrated = true;
|
||||
saved_vhv_init = 0;
|
||||
saved_vhv_timeout = 0;
|
||||
// distance_mode = 1;
|
||||
|
||||
// Initialise I2C connection
|
||||
i2c_init(i2c, 400000);
|
||||
|
||||
gpio_set_function(sda, GPIO_FUNC_I2C); gpio_pull_up(sda);
|
||||
gpio_set_function(scl, GPIO_FUNC_I2C); gpio_pull_up(scl);
|
||||
|
||||
last_status = 0;
|
||||
|
||||
// check model ID and module type registers (values specified in datasheet)
|
||||
if (readReg16Bit(IDENTIFICATION__MODEL_ID) != 0xEACC) { return false; }
|
||||
|
||||
// VL53L1_software_reset() begin
|
||||
|
||||
writeReg(SOFT_RESET, 0x00);
|
||||
sleep_us(100);
|
||||
writeReg(SOFT_RESET, 0x01);
|
||||
|
||||
// give it some time to boot; otherwise the sensor NACKs during the readReg()
|
||||
// call below and the Arduino 101 doesn't seem to handle that well
|
||||
sleep_ms(1000);
|
||||
|
||||
// VL53L1_poll_for_boot_completion() begin
|
||||
startTimeout();
|
||||
|
||||
// check last_status in case we still get a NACK to try to deal with it correctly
|
||||
while ((readReg(FIRMWARE__SYSTEM_STATUS) & 0x01) == 0 || last_status != 0)
|
||||
{
|
||||
if (checkTimeoutExpired())
|
||||
{
|
||||
did_timeout = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// VL53L1_poll_for_boot_completion() end
|
||||
|
||||
// VL53L1_software_reset() end
|
||||
|
||||
// VL53L1_DataInit() begin
|
||||
|
||||
// sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary
|
||||
if (io_2v8)
|
||||
{
|
||||
writeReg(PAD_I2C_HV__EXTSUP_CONFIG,
|
||||
readReg(PAD_I2C_HV__EXTSUP_CONFIG) | 0x01);
|
||||
}
|
||||
|
||||
// store oscillator info for later use
|
||||
fast_osc_frequency = readReg16Bit(OSC_MEASURED__FAST_OSC__FREQUENCY);
|
||||
osc_calibrate_val = readReg16Bit(RESULT__OSC_CALIBRATE_VAL);
|
||||
|
||||
// VL53L1_DataInit() end
|
||||
|
||||
// VL53L1_StaticInit() begin
|
||||
|
||||
// Note that the API does not actually apply the configuration settings below
|
||||
// when VL53L1_StaticInit() is called: it keeps a copy of the sensor's
|
||||
// register contents in memory and doesn't actually write them until a
|
||||
// measurement is started. Writing the configuration here means we don't have
|
||||
// to keep it all in memory and avoids a lot of redundant writes later.
|
||||
|
||||
// the API sets the preset mode to LOWPOWER_AUTONOMOUS here:
|
||||
// VL53L1_set_preset_mode() begin
|
||||
|
||||
// VL53L1_preset_mode_standard_ranging() begin
|
||||
|
||||
// values labeled "tuning parm default" are from vl53l1_tuning_parm_defaults.h
|
||||
// (API uses these in VL53L1_init_tuning_parm_storage_struct())
|
||||
|
||||
// static config
|
||||
// API resets PAD_I2C_HV__EXTSUP_CONFIG here, but maybe we don't want to do
|
||||
// that? (seems like it would disable 2V8 mode)
|
||||
writeReg16Bit(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate); // should already be this value after reset
|
||||
writeReg(GPIO__TIO_HV_STATUS, 0x02);
|
||||
writeReg(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8); // tuning parm default
|
||||
writeReg(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16); // tuning parm default
|
||||
writeReg(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01);
|
||||
writeReg(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF);
|
||||
writeReg(ALGO__RANGE_MIN_CLIP, 0); // tuning parm default
|
||||
writeReg(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2); // tuning parm default
|
||||
|
||||
// general config
|
||||
writeReg16Bit(SYSTEM__THRESH_RATE_HIGH, 0x0000);
|
||||
writeReg16Bit(SYSTEM__THRESH_RATE_LOW, 0x0000);
|
||||
writeReg(DSS_CONFIG__APERTURE_ATTENUATION, 0x38);
|
||||
|
||||
// timing config
|
||||
// most of these settings will be determined later by distance and timing
|
||||
// budget configuration
|
||||
writeReg16Bit(RANGE_CONFIG__SIGMA_THRESH, 360); // tuning parm default
|
||||
writeReg16Bit(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192); // tuning parm default
|
||||
|
||||
// dynamic config
|
||||
|
||||
writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01);
|
||||
writeReg(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01);
|
||||
writeReg(SD_CONFIG__QUANTIFIER, 2); // tuning parm default
|
||||
|
||||
// VL53L1_preset_mode_standard_ranging() end
|
||||
|
||||
// from VL53L1_preset_mode_timed_ranging_*
|
||||
// GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1,
|
||||
// and things don't seem to work if we don't set GPH back to 0 (which the API
|
||||
// does here).
|
||||
writeReg(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00);
|
||||
writeReg(SYSTEM__SEED_CONFIG, 1); // tuning parm default
|
||||
|
||||
// from VL53L1_config_low_power_auto_mode
|
||||
writeReg(SYSTEM__SEQUENCE_CONFIG, 0x8B); // VHV, PHASECAL, DSS1, RANGE
|
||||
writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8);
|
||||
writeReg(DSS_CONFIG__ROI_MODE_CONTROL, 2); // REQUESTED_EFFFECTIVE_SPADS
|
||||
|
||||
// VL53L1_set_preset_mode() end
|
||||
|
||||
// default to long range, 50 ms timing budget
|
||||
// note that this is different than what the API defaults to
|
||||
setDistanceMode(Long);
|
||||
setMeasurementTimingBudget(50000);
|
||||
|
||||
// VL53L1_StaticInit() end
|
||||
|
||||
// the API triggers this change in VL53L1_init_and_start_range() once a
|
||||
// measurement is started; assumes MM1 and MM2 are disabled
|
||||
writeReg16Bit(ALGO__PART_TO_PART_RANGE_OFFSET_MM,
|
||||
readReg16Bit(MM_CONFIG__OUTER_OFFSET_MM) * 4);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Write an 8-bit register
|
||||
void VL53L1X::writeReg(uint16_t reg, uint8_t value)
|
||||
{
|
||||
uint8_t buffer[3] = {(reg >> 8) & 0xFF, reg & 0xFF, value};
|
||||
i2c_write_blocking(i2c, address, buffer, 3, false);
|
||||
}
|
||||
|
||||
// Write a 16-bit register
|
||||
void VL53L1X::writeReg16Bit(uint16_t reg, uint16_t value)
|
||||
{
|
||||
uint8_t buffer[4] = {(reg >> 8) & 0xFF, reg & 0xFF, (value >> 8) & 0xFF, value & 0xFF};
|
||||
i2c_write_blocking(i2c, address, buffer, 4, false);
|
||||
}
|
||||
|
||||
// Write a 32-bit register
|
||||
void VL53L1X::writeReg32Bit(uint16_t reg, uint32_t value)
|
||||
{
|
||||
uint8_t buffer[6] = {(reg >> 8) & 0xFF, reg & 0xFF,
|
||||
(value >> 24) & 0xFF, (value >> 16) & 0xFF, (value >> 8) & 0xFF, value & 0xFF};
|
||||
i2c_write_blocking(i2c, address, buffer, 6, false);
|
||||
}
|
||||
|
||||
// Read an 8-bit register
|
||||
uint8_t VL53L1X::readReg(regAddr reg)
|
||||
{
|
||||
uint8_t regbuf[2] = {((uint8_t)reg >> 8) & 0xFF, (uint8_t)reg & 0xFF};
|
||||
uint8_t buffer[1];
|
||||
uint8_t value;
|
||||
i2c_write_blocking(i2c, address, regbuf, 2, true);
|
||||
i2c_read_blocking(i2c, address, buffer, 1, false);
|
||||
value = buffer[0];
|
||||
return value;
|
||||
}
|
||||
|
||||
// Read a 16-bit register
|
||||
uint16_t VL53L1X::readReg16Bit(uint16_t reg)
|
||||
{
|
||||
uint8_t regbuf[2] = {(reg >> 8) & 0xFF, reg & 0xFF};
|
||||
uint8_t buffer[2];
|
||||
uint16_t value;
|
||||
reg= (reg << 8) + (reg >> 8);
|
||||
i2c_write_blocking(i2c, address, regbuf, 2, true);
|
||||
i2c_read_blocking(i2c, address, buffer, 2, false);
|
||||
value= (buffer[0] << 8) + buffer[1];
|
||||
return value;
|
||||
}
|
||||
|
||||
// Read a 32-bit register
|
||||
uint32_t VL53L1X::readReg32Bit(uint16_t reg)
|
||||
{
|
||||
uint8_t regbuf[2] = {(reg >> 8) & 0xFF, reg & 0xFF};
|
||||
uint8_t buffer[4];
|
||||
uint32_t value;
|
||||
reg= (reg << 8) + (reg >> 8);
|
||||
i2c_write_blocking(i2c, address, regbuf, 2, true);
|
||||
i2c_read_blocking(i2c, address, buffer, 4, false);
|
||||
value= (buffer[0] << 24) + (buffer[1] << 16) + (buffer[2] << 8) + buffer[3];
|
||||
return value;
|
||||
}
|
||||
|
||||
// set distance mode to Short, Medium, or Long
|
||||
// based on VL53L1_SetDistanceMode()
|
||||
bool VL53L1X::setDistanceMode(DistanceMode mode)
|
||||
{
|
||||
// save existing timing budget
|
||||
uint32_t budget_us = getMeasurementTimingBudget();
|
||||
|
||||
switch (mode)
|
||||
{
|
||||
case Short:
|
||||
// from VL53L1_preset_mode_standard_ranging_short_range()
|
||||
|
||||
// timing config
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07);
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05);
|
||||
writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38);
|
||||
|
||||
// dynamic config
|
||||
writeReg(SD_CONFIG__WOI_SD0, 0x07);
|
||||
writeReg(SD_CONFIG__WOI_SD1, 0x05);
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 6); // tuning parm default
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 6); // tuning parm default
|
||||
|
||||
break;
|
||||
|
||||
case Medium:
|
||||
// from VL53L1_preset_mode_standard_ranging()
|
||||
|
||||
// timing config
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B);
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09);
|
||||
writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78);
|
||||
|
||||
// dynamic config
|
||||
writeReg(SD_CONFIG__WOI_SD0, 0x0B);
|
||||
writeReg(SD_CONFIG__WOI_SD1, 0x09);
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 10); // tuning parm default
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 10); // tuning parm default
|
||||
|
||||
break;
|
||||
|
||||
case Long: // long
|
||||
// from VL53L1_preset_mode_standard_ranging_long_range()
|
||||
|
||||
// timing config
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F);
|
||||
writeReg(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D);
|
||||
writeReg(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8);
|
||||
|
||||
// dynamic config
|
||||
writeReg(SD_CONFIG__WOI_SD0, 0x0F);
|
||||
writeReg(SD_CONFIG__WOI_SD1, 0x0D);
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD0, 14); // tuning parm default
|
||||
writeReg(SD_CONFIG__INITIAL_PHASE_SD1, 14); // tuning parm default
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
// unrecognized mode - do nothing
|
||||
return false;
|
||||
}
|
||||
|
||||
// reapply timing budget
|
||||
setMeasurementTimingBudget(budget_us);
|
||||
|
||||
// save mode so it can be returned by getDistanceMode()
|
||||
distance_mode = mode;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VL53L1X::setDistanceModeInt(uint8_t mode)
|
||||
{
|
||||
// Map the mode here to the internal Enum - must be a better way!
|
||||
switch (mode)
|
||||
{
|
||||
case 0:
|
||||
// Do nothing
|
||||
break;
|
||||
case 1:
|
||||
setDistanceMode(Short);
|
||||
break;
|
||||
case 2:
|
||||
setDistanceMode(Medium);
|
||||
break;
|
||||
case 3:
|
||||
setDistanceMode(Long);
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Set the measurement timing budget in microseconds, which is the time allowed
|
||||
// for one measurement. A longer timing budget allows for more accurate
|
||||
// measurements.
|
||||
// based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
|
||||
bool VL53L1X::setMeasurementTimingBudget(uint32_t budget_us)
|
||||
{
|
||||
// assumes PresetMode is LOWPOWER_AUTONOMOUS
|
||||
|
||||
if (budget_us <= TimingGuard) { return false; }
|
||||
|
||||
uint32_t range_config_timeout_us = budget_us -= TimingGuard;
|
||||
if (range_config_timeout_us > 1100000) { return false; } // FDA_MAX_TIMING_BUDGET_US * 2
|
||||
|
||||
range_config_timeout_us /= 2;
|
||||
|
||||
// VL53L1_calc_timeout_register_values() begin
|
||||
|
||||
uint32_t macro_period_us;
|
||||
|
||||
// "Update Macro Period for Range A VCSEL Period"
|
||||
macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
|
||||
|
||||
// "Update Phase timeout - uses Timing A"
|
||||
// Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT)
|
||||
// via VL53L1_get_preset_mode_timing_cfg().
|
||||
uint32_t phasecal_timeout_mclks = timeoutMicrosecondsToMclks(1000, macro_period_us);
|
||||
if (phasecal_timeout_mclks > 0xFF) { phasecal_timeout_mclks = 0xFF; }
|
||||
writeReg(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks);
|
||||
|
||||
// "Update MM Timing A timeout"
|
||||
// Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT)
|
||||
// via VL53L1_get_preset_mode_timing_cfg(). With the API, the register
|
||||
// actually ends up with a slightly different value because it gets assigned,
|
||||
// retrieved, recalculated with a different macro period, and reassigned,
|
||||
// but it probably doesn't matter because it seems like the MM ("mode
|
||||
// mitigation"?) sequence steps are disabled in low power auto mode anyway.
|
||||
writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
|
||||
timeoutMicrosecondsToMclks(1, macro_period_us)));
|
||||
|
||||
// "Update Range Timing A timeout"
|
||||
writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout(
|
||||
timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
|
||||
|
||||
// "Update Macro Period for Range B VCSEL Period"
|
||||
macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_B));
|
||||
|
||||
// "Update MM Timing B timeout"
|
||||
// (See earlier comment about MM Timing A timeout.)
|
||||
writeReg16Bit(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
|
||||
timeoutMicrosecondsToMclks(1, macro_period_us)));
|
||||
|
||||
// "Update Range Timing B timeout"
|
||||
writeReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout(
|
||||
timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us)));
|
||||
|
||||
// VL53L1_calc_timeout_register_values() end
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Get the measurement timing budget in microseconds
|
||||
// based on VL53L1_SetMeasurementTimingBudgetMicroSeconds()
|
||||
uint32_t VL53L1X::getMeasurementTimingBudget()
|
||||
{
|
||||
// assumes PresetMode is LOWPOWER_AUTONOMOUS and these sequence steps are
|
||||
// enabled: VHV, PHASECAL, DSS1, RANGE
|
||||
|
||||
// VL53L1_get_timeouts_us() begin
|
||||
|
||||
// "Update Macro Period for Range A VCSEL Period"
|
||||
uint32_t macro_period_us = calcMacroPeriod(readReg(RANGE_CONFIG__VCSEL_PERIOD_A));
|
||||
|
||||
// "Get Range Timing A timeout"
|
||||
|
||||
uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout(
|
||||
readReg16Bit(RANGE_CONFIG__TIMEOUT_MACROP_A)), macro_period_us);
|
||||
|
||||
// VL53L1_get_timeouts_us() end
|
||||
|
||||
return 2 * range_config_timeout_us + TimingGuard;
|
||||
}
|
||||
|
||||
// Start continuous ranging measurements, with the given inter-measurement
|
||||
// period in milliseconds determining how often the sensor takes a measurement.
|
||||
void VL53L1X::startContinuous(uint32_t period_ms)
|
||||
{
|
||||
// from VL53L1_set_inter_measurement_period_ms()
|
||||
writeReg32Bit(SYSTEM__INTERMEASUREMENT_PERIOD, period_ms * osc_calibrate_val);
|
||||
|
||||
writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
|
||||
writeReg(SYSTEM__MODE_START, 0x40); // mode_range__timed
|
||||
}
|
||||
|
||||
// Stop continuous measurements
|
||||
// based on VL53L1_stop_range()
|
||||
void VL53L1X::stopContinuous()
|
||||
{
|
||||
writeReg(SYSTEM__MODE_START, 0x80); // mode_range__abort
|
||||
|
||||
// VL53L1_low_power_auto_data_stop_range() begin
|
||||
|
||||
calibrated = false;
|
||||
|
||||
// "restore vhv configs"
|
||||
if (saved_vhv_init != 0)
|
||||
{
|
||||
writeReg(VHV_CONFIG__INIT, saved_vhv_init);
|
||||
}
|
||||
if (saved_vhv_timeout != 0)
|
||||
{
|
||||
writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, saved_vhv_timeout);
|
||||
}
|
||||
|
||||
// "remove phasecal override"
|
||||
writeReg(PHASECAL_CONFIG__OVERRIDE, 0x00);
|
||||
|
||||
// VL53L1_low_power_auto_data_stop_range() end
|
||||
}
|
||||
|
||||
// Returns a range reading in millimeters when continuous mode is active. If
|
||||
// blocking is true (the default), this function waits for a new measurement to
|
||||
// be available. If blocking is false, it will try to return data immediately.
|
||||
// (readSingle() also calls this function after starting a single-shot range
|
||||
// measurement)
|
||||
uint16_t VL53L1X::read(bool blocking)
|
||||
{
|
||||
if (blocking)
|
||||
{
|
||||
startTimeout();
|
||||
while (!dataReady())
|
||||
{
|
||||
if (checkTimeoutExpired())
|
||||
{
|
||||
did_timeout = true;
|
||||
return 0;
|
||||
}
|
||||
sleep_us(100);
|
||||
}
|
||||
}
|
||||
|
||||
readResults();
|
||||
|
||||
if (!calibrated)
|
||||
{
|
||||
setupManualCalibration();
|
||||
calibrated = true;
|
||||
}
|
||||
|
||||
updateDSS();
|
||||
|
||||
getRangingData();
|
||||
|
||||
writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
|
||||
|
||||
return ranging_data.range_mm;
|
||||
}
|
||||
|
||||
// Starts a single-shot range measurement. If blocking is true (the default),
|
||||
// this function waits for the measurement to finish and returns the reading.
|
||||
// Otherwise, it returns 0 immediately.
|
||||
uint16_t VL53L1X::readSingle(bool blocking)
|
||||
{
|
||||
writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range
|
||||
writeReg(SYSTEM__MODE_START, 0x10); // mode_range__single_shot
|
||||
|
||||
if (blocking)
|
||||
{
|
||||
return read(true);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// convert a RangeStatus to a readable string
|
||||
// Note that on an AVR, these strings are stored in RAM (dynamic memory), which
|
||||
// makes working with them easier but uses up 200+ bytes of RAM (many AVR-based
|
||||
// Arduinos only have about 2000 bytes of RAM). You can avoid this memory usage
|
||||
// if you do not call this function in your sketch.
|
||||
const char * VL53L1X::rangeStatusToString(RangeStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case RangeValid:
|
||||
return "range valid";
|
||||
|
||||
case SigmaFail:
|
||||
return "sigma fail";
|
||||
|
||||
case SignalFail:
|
||||
return "signal fail";
|
||||
|
||||
case RangeValidMinRangeClipped:
|
||||
return "range valid, min range clipped";
|
||||
|
||||
case OutOfBoundsFail:
|
||||
return "out of bounds fail";
|
||||
|
||||
case HardwareFail:
|
||||
return "hardware fail";
|
||||
|
||||
case RangeValidNoWrapCheckFail:
|
||||
return "range valid, no wrap check fail";
|
||||
|
||||
case WrapTargetFail:
|
||||
return "wrap target fail";
|
||||
|
||||
case XtalkSignalFail:
|
||||
return "xtalk signal fail";
|
||||
|
||||
case SynchronizationInt:
|
||||
return "synchronization int";
|
||||
|
||||
case MinRangeFail:
|
||||
return "min range fail";
|
||||
|
||||
case None:
|
||||
return "no update";
|
||||
|
||||
default:
|
||||
return "unknown status";
|
||||
}
|
||||
}
|
||||
|
||||
// Did a timeout occur in one of the read functions since the last call to
|
||||
// timeoutOccurred()?
|
||||
bool VL53L1X::timeoutOccurred()
|
||||
{
|
||||
bool tmp = did_timeout;
|
||||
did_timeout = false;
|
||||
return tmp;
|
||||
}
|
||||
|
||||
// Private Methods /////////////////////////////////////////////////////////////
|
||||
|
||||
// "Setup ranges after the first one in low power auto mode by turning off
|
||||
// FW calibration steps and programming static values"
|
||||
// based on VL53L1_low_power_auto_setup_manual_calibration()
|
||||
void VL53L1X::setupManualCalibration()
|
||||
{
|
||||
// "save original vhv configs"
|
||||
saved_vhv_init = readReg(VHV_CONFIG__INIT);
|
||||
saved_vhv_timeout = readReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND);
|
||||
|
||||
// "disable VHV init"
|
||||
writeReg(VHV_CONFIG__INIT, saved_vhv_init & 0x7F);
|
||||
|
||||
// "set loop bound to tuning param"
|
||||
writeReg(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,
|
||||
(saved_vhv_timeout & 0x03) + (3 << 2)); // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT)
|
||||
|
||||
// "override phasecal"
|
||||
writeReg(PHASECAL_CONFIG__OVERRIDE, 0x01);
|
||||
writeReg(CAL_CONFIG__VCSEL_START, readReg(PHASECAL_RESULT__VCSEL_START));
|
||||
}
|
||||
|
||||
// read measurement results into buffer
|
||||
void VL53L1X::readResults()
|
||||
{
|
||||
uint16_t reg = RESULT__RANGE_STATUS;
|
||||
uint8_t regbuf[2] = {(reg >> 8) & 0xFF, reg & 0xFF};
|
||||
uint8_t buffer[17];
|
||||
i2c_write_blocking(i2c, address, regbuf, 2, true);
|
||||
i2c_read_blocking(i2c, address, buffer, 17, false);
|
||||
|
||||
results.range_status = buffer[0];
|
||||
|
||||
// bus->read(); // report_status: not used
|
||||
|
||||
results.stream_count = buffer[2];
|
||||
|
||||
results.dss_actual_effective_spads_sd0 = (uint16_t)buffer[3] << 8; // high byte
|
||||
results.dss_actual_effective_spads_sd0 |= buffer[4]; // low byte
|
||||
|
||||
// bus->read(); // peak_signal_count_rate_mcps_sd0: not used
|
||||
// bus->read();
|
||||
|
||||
results.ambient_count_rate_mcps_sd0 = (uint16_t)buffer[7] << 8; // high byte
|
||||
results.ambient_count_rate_mcps_sd0 |= buffer[8]; // low byte
|
||||
|
||||
// bus->read(); // sigma_sd0: not used
|
||||
// bus->read();
|
||||
|
||||
// bus->read(); // phase_sd0: not used
|
||||
// bus->read();
|
||||
|
||||
results.final_crosstalk_corrected_range_mm_sd0 = (uint16_t)buffer[13] << 8; // high byte
|
||||
results.final_crosstalk_corrected_range_mm_sd0 |= buffer[14]; // low byte
|
||||
|
||||
results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 = (uint16_t)buffer[15] << 8; // high byte
|
||||
results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 |= buffer[16]; // low byte
|
||||
}
|
||||
|
||||
// perform Dynamic SPAD Selection calculation/update
|
||||
// based on VL53L1_low_power_auto_update_DSS()
|
||||
void VL53L1X::updateDSS()
|
||||
{
|
||||
uint16_t spadCount = results.dss_actual_effective_spads_sd0;
|
||||
|
||||
if (spadCount != 0)
|
||||
{
|
||||
// "Calc total rate per spad"
|
||||
|
||||
uint32_t totalRatePerSpad =
|
||||
(uint32_t)results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0 +
|
||||
results.ambient_count_rate_mcps_sd0;
|
||||
|
||||
// "clip to 16 bits"
|
||||
if (totalRatePerSpad > 0xFFFF) { totalRatePerSpad = 0xFFFF; }
|
||||
|
||||
// "shift up to take advantage of 32 bits"
|
||||
totalRatePerSpad <<= 16;
|
||||
|
||||
totalRatePerSpad /= spadCount;
|
||||
|
||||
if (totalRatePerSpad != 0)
|
||||
{
|
||||
// "get the target rate and shift up by 16"
|
||||
uint32_t requiredSpads = ((uint32_t)TargetRate << 16) / totalRatePerSpad;
|
||||
|
||||
// "clip to 16 bit"
|
||||
if (requiredSpads > 0xFFFF) { requiredSpads = 0xFFFF; }
|
||||
|
||||
// "override DSS config"
|
||||
writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, requiredSpads);
|
||||
// DSS_CONFIG__ROI_MODE_CONTROL should already be set to REQUESTED_EFFFECTIVE_SPADS
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// If we reached this point, it means something above would have resulted in a
|
||||
// divide by zero.
|
||||
// "We want to gracefully set a spad target, not just exit with an error"
|
||||
|
||||
// "set target to mid point"
|
||||
writeReg16Bit(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 0x8000);
|
||||
}
|
||||
|
||||
// get range, status, rates from results buffer
|
||||
// based on VL53L1_GetRangingMeasurementData()
|
||||
void VL53L1X::getRangingData()
|
||||
{
|
||||
// VL53L1_copy_sys_and_core_results_to_range_results() begin
|
||||
|
||||
uint16_t range = results.final_crosstalk_corrected_range_mm_sd0;
|
||||
|
||||
// "apply correction gain"
|
||||
// gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT)
|
||||
// Basically, this appears to scale the result by 2011/2048, or about 98%
|
||||
// (with the 1024 added for proper rounding).
|
||||
ranging_data.range_mm = ((uint32_t)range * 2011 + 0x0400) / 0x0800;
|
||||
|
||||
// VL53L1_copy_sys_and_core_results_to_range_results() end
|
||||
|
||||
// set range_status in ranging_data based on value of RESULT__RANGE_STATUS register
|
||||
// mostly based on ConvertStatusLite()
|
||||
switch(results.range_status)
|
||||
{
|
||||
case 17: // MULTCLIPFAIL
|
||||
case 2: // VCSELWATCHDOGTESTFAILURE
|
||||
case 1: // VCSELCONTINUITYTESTFAILURE
|
||||
case 3: // NOVHVVALUEFOUND
|
||||
// from SetSimpleData()
|
||||
ranging_data.range_status = HardwareFail;
|
||||
break;
|
||||
|
||||
case 13: // USERROICLIP
|
||||
// from SetSimpleData()
|
||||
ranging_data.range_status = MinRangeFail;
|
||||
break;
|
||||
|
||||
case 18: // GPHSTREAMCOUNT0READY
|
||||
ranging_data.range_status = SynchronizationInt;
|
||||
break;
|
||||
|
||||
case 5: // RANGEPHASECHECK
|
||||
ranging_data.range_status = OutOfBoundsFail;
|
||||
break;
|
||||
|
||||
case 4: // MSRCNOTARGET
|
||||
ranging_data.range_status = SignalFail;
|
||||
break;
|
||||
|
||||
case 6: // SIGMATHRESHOLDCHECK
|
||||
ranging_data.range_status = SigmaFail;
|
||||
break;
|
||||
|
||||
case 7: // PHASECONSISTENCY
|
||||
ranging_data.range_status = WrapTargetFail;
|
||||
break;
|
||||
|
||||
case 12: // RANGEIGNORETHRESHOLD
|
||||
ranging_data.range_status = XtalkSignalFail;
|
||||
break;
|
||||
|
||||
case 8: // MINCLIP
|
||||
ranging_data.range_status = RangeValidMinRangeClipped;
|
||||
break;
|
||||
|
||||
case 9: // RANGECOMPLETE
|
||||
// from VL53L1_copy_sys_and_core_results_to_range_results()
|
||||
if (results.stream_count == 0)
|
||||
{
|
||||
ranging_data.range_status = RangeValidNoWrapCheckFail;
|
||||
}
|
||||
else
|
||||
{
|
||||
ranging_data.range_status = RangeValid;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
ranging_data.range_status = None;
|
||||
}
|
||||
|
||||
// from SetSimpleData()
|
||||
ranging_data.peak_signal_count_rate_MCPS =
|
||||
countRateFixedToFloat(results.peak_signal_count_rate_crosstalk_corrected_mcps_sd0);
|
||||
ranging_data.ambient_count_rate_MCPS =
|
||||
countRateFixedToFloat(results.ambient_count_rate_mcps_sd0);
|
||||
}
|
||||
|
||||
// Decode sequence step timeout in MCLKs from register value
|
||||
// based on VL53L1_decode_timeout()
|
||||
uint32_t VL53L1X::decodeTimeout(uint16_t reg_val)
|
||||
{
|
||||
return ((uint32_t)(reg_val & 0xFF) << (reg_val >> 8)) + 1;
|
||||
}
|
||||
|
||||
// Encode sequence step timeout register value from timeout in MCLKs
|
||||
// based on VL53L1_encode_timeout()
|
||||
uint16_t VL53L1X::encodeTimeout(uint32_t timeout_mclks)
|
||||
{
|
||||
// encoded format: "(LSByte * 2^MSByte) + 1"
|
||||
|
||||
uint32_t ls_byte = 0;
|
||||
uint16_t ms_byte = 0;
|
||||
|
||||
if (timeout_mclks > 0)
|
||||
{
|
||||
ls_byte = timeout_mclks - 1;
|
||||
|
||||
while ((ls_byte & 0xFFFFFF00) > 0)
|
||||
{
|
||||
ls_byte >>= 1;
|
||||
ms_byte++;
|
||||
}
|
||||
|
||||
return (ms_byte << 8) | (ls_byte & 0xFF);
|
||||
}
|
||||
else { return 0; }
|
||||
}
|
||||
|
||||
// Convert sequence step timeout from macro periods to microseconds with given
|
||||
// macro period in microseconds (12.12 format)
|
||||
// based on VL53L1_calc_timeout_us()
|
||||
uint32_t VL53L1X::timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us)
|
||||
{
|
||||
return ((uint64_t)timeout_mclks * macro_period_us + 0x800) >> 12;
|
||||
}
|
||||
|
||||
// Convert sequence step timeout from microseconds to macro periods with given
|
||||
// macro period in microseconds (12.12 format)
|
||||
// based on VL53L1_calc_timeout_mclks()
|
||||
uint32_t VL53L1X::timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us)
|
||||
{
|
||||
return (((uint32_t)timeout_us << 12) + (macro_period_us >> 1)) / macro_period_us;
|
||||
}
|
||||
|
||||
// Calculate macro period in microseconds (12.12 format) with given VCSEL period
|
||||
// assumes fast_osc_frequency has been read and stored
|
||||
// based on VL53L1_calc_macro_period_us()
|
||||
uint32_t VL53L1X::calcMacroPeriod(uint8_t vcsel_period)
|
||||
{
|
||||
// from VL53L1_calc_pll_period_us()
|
||||
// fast osc frequency in 4.12 format; PLL period in 0.24 format
|
||||
uint32_t pll_period_us = ((uint32_t)0x01 << 30) / fast_osc_frequency;
|
||||
|
||||
// from VL53L1_decode_vcsel_period()
|
||||
uint8_t vcsel_period_pclks = (vcsel_period + 1) << 1;
|
||||
|
||||
// VL53L1_MACRO_PERIOD_VCSEL_PERIODS = 2304
|
||||
uint32_t macro_period_us = (uint32_t)2304 * pll_period_us;
|
||||
macro_period_us >>= 6;
|
||||
macro_period_us *= vcsel_period_pclks;
|
||||
macro_period_us >>= 6;
|
||||
|
||||
return macro_period_us;
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -5,3 +5,4 @@ add_subdirectory(pico_scroll)
|
|||
add_subdirectory(pico_explorer)
|
||||
add_subdirectory(pico_rgb_keypad)
|
||||
add_subdirectory(pico_rtc_display)
|
||||
add_subdirectory(pico_tof_display)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,14 @@
|
|||
add_executable(
|
||||
tof_display
|
||||
demo.cpp
|
||||
)
|
||||
|
||||
# Pull in pico libraries that we need
|
||||
target_link_libraries(tof_display pico_stdlib pico_explorer pico_display vl53l1x)
|
||||
|
||||
pico_enable_stdio_uart(tof_display 1)
|
||||
|
||||
# create map/bin/hex file etc.
|
||||
pico_add_extra_outputs(tof_display)
|
||||
|
||||
#example_auto_set_url(tof_display)
|
||||
|
|
@ -0,0 +1,242 @@
|
|||
#include <string.h>
|
||||
|
||||
// **************************************************************************
|
||||
// Demonstrate the Pimoroni ToF module (VL53L1X)
|
||||
// Assumes that a Pico Display Pack (a 1.14inch IPS LCD screen with four
|
||||
// useful buttons) is installed, and that the VL53L1X I2C module has
|
||||
// sda, scl and int on GPIO 20, 21 and 22
|
||||
// Displays the current distance (in mm) and mode
|
||||
// Button B holds the reading until released
|
||||
// Button X toggles metric/imperial display
|
||||
// Button Y increments the mode (Auto, Short, Medium, Long)
|
||||
// (There are on-screen reminders of the active buttons)
|
||||
// **************************************************************************
|
||||
|
||||
// To use PicoExplorer rather than PicoDisplay, uncomment the following line
|
||||
// #define USE_PICO_EXPLORER 1
|
||||
// This:
|
||||
// - Includes pico_explorer.hpp rather than pico_display.hpp
|
||||
// - Replaces all PicoDisplay references with PicoExplorer
|
||||
// - Leaves out the .set_led() calls in flash_led()
|
||||
#ifdef USE_PICO_EXPLORER
|
||||
#include "pico_explorer.hpp"
|
||||
#else
|
||||
#include "pico_display.hpp"
|
||||
#endif
|
||||
#include "vl53l1x.hpp"
|
||||
|
||||
using namespace pimoroni;
|
||||
|
||||
#ifdef USE_PICO_EXPLORER
|
||||
uint16_t buffer[PicoExplorer::WIDTH * PicoExplorer::HEIGHT];
|
||||
PicoExplorer pico_display(buffer);
|
||||
uint16_t screen_width = PicoExplorer::WIDTH;
|
||||
uint16_t screen_height = PicoExplorer::HEIGHT;
|
||||
uint16_t disptext_reminder_size = 2;
|
||||
uint16_t disptext_b_reminder_xoff = 5;
|
||||
uint16_t disptext_b_reminder_yoff = 210;
|
||||
uint16_t disptext_x_reminder_xoff = 165;
|
||||
uint16_t disptext_x_reminder_yoff = 2;
|
||||
uint16_t disptext_y_reminder_xoff = 165;
|
||||
uint16_t disptext_y_reminder_yoff = 210;
|
||||
uint16_t disptext_mode_xoff = 10;
|
||||
uint16_t disptext_mode_yoff = 30;
|
||||
uint16_t disptext_mode_size = 3;
|
||||
uint16_t disptext_dist_xoff = 10;
|
||||
uint16_t disptext_dist_yoff = 90;
|
||||
uint16_t disptext_dist_size = 6;
|
||||
#else
|
||||
uint16_t buffer[PicoDisplay::WIDTH * PicoDisplay::HEIGHT];
|
||||
PicoDisplay pico_display(buffer);
|
||||
uint16_t screen_width = PicoDisplay::WIDTH;
|
||||
uint16_t screen_height = PicoDisplay::HEIGHT;
|
||||
uint16_t disptext_reminder_size = 2;
|
||||
uint16_t disptext_b_reminder_xoff = 2;
|
||||
uint16_t disptext_b_reminder_yoff = 110;
|
||||
uint16_t disptext_x_reminder_xoff = 175;
|
||||
uint16_t disptext_x_reminder_yoff = 2;
|
||||
uint16_t disptext_y_reminder_xoff = 175;
|
||||
uint16_t disptext_y_reminder_yoff = 110;
|
||||
uint16_t disptext_mode_xoff = 12;
|
||||
uint16_t disptext_mode_yoff = 15;
|
||||
uint16_t disptext_mode_size = 3;
|
||||
uint16_t disptext_dist_xoff = 10;
|
||||
uint16_t disptext_dist_yoff = 45;
|
||||
uint16_t disptext_dist_size = 4;
|
||||
#endif
|
||||
|
||||
#define MM_TO_INCH 25.4
|
||||
|
||||
VL53L1X vl53l1x;
|
||||
|
||||
char * mode_to_text[4];
|
||||
|
||||
#define LOW_COUNT_MOD 40
|
||||
#define HIGH_COUNT_MOD 20
|
||||
bool repeat_count_reached(uint16_t curr_count) {
|
||||
// Check whether the current counter means that a key has repeated
|
||||
if (curr_count <= 10*LOW_COUNT_MOD) {
|
||||
return (0 == (curr_count % LOW_COUNT_MOD));
|
||||
} else {
|
||||
return (0 == (curr_count % HIGH_COUNT_MOD));
|
||||
}
|
||||
}
|
||||
|
||||
#define FLASH_MOD 20
|
||||
void flash_led(uint32_t curr_count) {
|
||||
// Flash the LED based on the current loop counter
|
||||
// curr_count=0 will turn LED off
|
||||
#ifndef USE_PICO_EXPLORER
|
||||
if ((curr_count % FLASH_MOD) < (FLASH_MOD / 2)) {
|
||||
// value less than half modded number - LED off
|
||||
pico_display.set_led(0, 0, 0);
|
||||
} else {
|
||||
// value more than half modded number - LED on
|
||||
pico_display.set_led(128, 128, 128);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
int main() {
|
||||
bool vl53_present = false;
|
||||
uint16_t vl53_mode = 1;
|
||||
|
||||
pico_display.init();
|
||||
|
||||
vl53_present = vl53l1x.init();
|
||||
|
||||
// Use these variables to make the buttons single-shot
|
||||
// Counts number of loops pressed, 0 if not pressed
|
||||
// Only for x and y - a and b are single-shot
|
||||
uint16_t a_pressed = 0;
|
||||
uint16_t b_pressed = 0;
|
||||
uint16_t x_pressed = 0;
|
||||
uint16_t y_pressed = 0;
|
||||
|
||||
mode_to_text[0] = "Auto";
|
||||
mode_to_text[1] = "Short";
|
||||
mode_to_text[2] = "Medium";
|
||||
mode_to_text[3] = "Long";
|
||||
|
||||
struct pt {
|
||||
float x;
|
||||
float y;
|
||||
uint8_t r;
|
||||
float dx;
|
||||
float dy;
|
||||
uint16_t pen;
|
||||
};
|
||||
|
||||
uint32_t i = 0;
|
||||
char buf[256];
|
||||
if (vl53_present) {
|
||||
vl53l1x.startContinuous(1000);
|
||||
vl53l1x.setMeasurementTimingBudget(50000);
|
||||
vl53l1x.setDistanceModeInt(vl53_mode);
|
||||
}
|
||||
|
||||
// The distance (in millimetres)
|
||||
uint16_t dist = 0;
|
||||
|
||||
// True if units in metric, false for Imperial (feet and inches)
|
||||
bool units_metric = true;
|
||||
|
||||
// Whether the display is being held
|
||||
bool dist_held = false;
|
||||
|
||||
while(true) {
|
||||
|
||||
if (a_pressed == 0 && pico_display.is_pressed(pico_display.A)) {
|
||||
a_pressed = 1;
|
||||
} else if (a_pressed >= 1 && !pico_display.is_pressed(pico_display.A)) {
|
||||
a_pressed = 0;
|
||||
}
|
||||
|
||||
if (b_pressed == 0 && pico_display.is_pressed(pico_display.B)) {
|
||||
b_pressed = 1;
|
||||
dist_held = true;
|
||||
} else if (b_pressed >= 1 && !pico_display.is_pressed(pico_display.B)) {
|
||||
b_pressed = 0;
|
||||
dist_held = false;
|
||||
}
|
||||
|
||||
if (x_pressed == 0 && pico_display.is_pressed(pico_display.X)) {
|
||||
x_pressed = 1;
|
||||
units_metric = !units_metric;
|
||||
} else if (x_pressed >= 1 && pico_display.is_pressed(pico_display.X)) {
|
||||
// Button still pressed - check if has reached repeat count
|
||||
if (repeat_count_reached(x_pressed++)) {
|
||||
// Do nothing for now - may need this later!
|
||||
}
|
||||
} else if (x_pressed >= 1 && !pico_display.is_pressed(pico_display.X)) {
|
||||
x_pressed = 0;
|
||||
}
|
||||
|
||||
if (y_pressed == 0 && pico_display.is_pressed(pico_display.Y)) {
|
||||
y_pressed = 1;
|
||||
if (vl53_present) {
|
||||
vl53_mode++;
|
||||
if (vl53_mode > 3) vl53_mode = 1;
|
||||
vl53l1x.setDistanceModeInt(vl53_mode);
|
||||
}
|
||||
} else if (y_pressed >= 1 && pico_display.is_pressed(pico_display.Y)) {
|
||||
// Button still pressed - check if has reached repeat count
|
||||
if (repeat_count_reached(y_pressed++)) {
|
||||
// Do nothing for now - may need this later!
|
||||
}
|
||||
} else if (y_pressed >= 1 && !pico_display.is_pressed(pico_display.Y)) {
|
||||
y_pressed = 0;
|
||||
}
|
||||
|
||||
Rect text_box(5, 5, screen_width-10, screen_height-10);
|
||||
pico_display.set_pen(55, 65, 75);
|
||||
pico_display.rectangle(text_box);
|
||||
// text_box.deflate(10);
|
||||
pico_display.set_clip(text_box);
|
||||
pico_display.set_pen(255, 255, 255);
|
||||
// Show the current distance
|
||||
flash_led(0);
|
||||
if (vl53_present) {
|
||||
pico_display.text("Units",
|
||||
Point(text_box.x+disptext_x_reminder_xoff,
|
||||
text_box.y+disptext_x_reminder_yoff), 230, disptext_reminder_size);
|
||||
pico_display.text("+Mode",
|
||||
Point(text_box.x+disptext_y_reminder_xoff,
|
||||
text_box.y+disptext_y_reminder_yoff), 230, disptext_reminder_size);
|
||||
pico_display.text("Hold",
|
||||
Point(text_box.x+disptext_b_reminder_xoff,
|
||||
text_box.y+disptext_b_reminder_yoff), 230, disptext_reminder_size);
|
||||
|
||||
sprintf(buf, "Mode: %s", mode_to_text[vl53_mode]);
|
||||
pico_display.text(buf,
|
||||
Point(text_box.x+disptext_mode_xoff,
|
||||
text_box.y+disptext_mode_yoff), 230, disptext_mode_size);
|
||||
|
||||
// Get the distance (use previous distance if number is held)
|
||||
if (!dist_held) dist = vl53l1x.read();
|
||||
if (units_metric) {
|
||||
sprintf(buf, "%dmm", dist);
|
||||
} else {
|
||||
uint16_t ft = ((uint16_t)(dist/MM_TO_INCH))/12;
|
||||
sprintf(buf, "%dft %.1fin", ft,
|
||||
((float)dist/MM_TO_INCH)-ft*12.0);
|
||||
}
|
||||
pico_display.text(buf,
|
||||
Point(text_box.x+disptext_dist_xoff,
|
||||
text_box.y+disptext_dist_yoff), 120, disptext_dist_size);
|
||||
} else {
|
||||
pico_display.text("VL53L1X Missing",
|
||||
Point(text_box.x+disptext_dist_xoff,
|
||||
text_box.y+disptext_dist_yoff), 230, disptext_dist_size);
|
||||
}
|
||||
|
||||
pico_display.remove_clip();
|
||||
|
||||
// update screen
|
||||
pico_display.update();
|
||||
|
||||
i++;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue