Cons-n-Stuff/geki_pico
1
0
Fork 0
mirror of https://github.com/whowechina/geki_pico.git synced 2025-02-17 18:59:14 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity

Refactor of vl53l0x

Browse Source
This commit is contained in:
whowechina 2024-09-21 20:13:07 +08:00
parent 7fd745a5a0
commit 226d517cc3
2 changed files with 120 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
firmware/src/vl53l0x.c
View File

@ -16,6 +16,92 @@
#define VL53L0X_DEF_ADDR 0x29
// register addresses from API vl53l0x_device.h (ordered as listed there)
enum regAddr
{
SYSRANGE_START = 0x00,
SYSTEM_THRESH_HIGH = 0x0C,
SYSTEM_THRESH_LOW = 0x0E,
SYSTEM_SEQUENCE_CONFIG = 0x01,
SYSTEM_RANGE_CONFIG = 0x09,
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
SYSTEM_INTERRUPT_CLEAR = 0x0B,
RESULT_INTERRUPT_STATUS = 0x13,
RESULT_RANGE_STATUS = 0x14,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
MSRC_CONFIG_CONTROL = 0x60,
PRE_RANGE_CONFIG_MIN_SNR = 0x27,
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
SYSTEM_HISTOGRAM_BIN = 0x81,
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
IDENTIFICATION_MODEL_ID = 0xC0,
IDENTIFICATION_REVISION_ID = 0xC2,
OSC_CALIBRATE_VAL = 0xF8,
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
ALGO_PHASECAL_LIM = 0x30,
ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
};
#define IO_TIMEOUT_US 1000
#define TOF_WAIT_US 200000
@ -29,6 +115,26 @@
// PLL_period_ps = 1655; macro_period_vclks = 2304
#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
// TCC: Target CentreCheck
// MSRC: Minimum Signal Rate Check
// DSS: Dynamic Spad Selection
typedef struct {
bool tcc, msrc, dss, pre_range, final_range;
} SequenceStepEnables;
typedef struct {
uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks;
uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks;
uint32_t msrc_dss_tcc_us, pre_range_us, final_range_us;
} SequenceStepTimeouts;
static bool getSpadInfo(uint8_t *count, bool *type_is_aperture);
static void getSequenceStepEnables(SequenceStepEnables *enables);
static void getSequenceStepTimeouts(SequenceStepEnables const *enables, SequenceStepTimeouts *timeouts);
static bool performSingleRefCalibration(uint8_t vhv_init_byte);
static struct {
i2c_inst_t *port;
uint8_t addr;
@ -43,15 +149,13 @@ static int current_instance = 0;
#define I2C_PORT instances[current_instance].port
#define I2C_ADDR instances[current_instance].addr
// Write an 8-bit register
void write_reg(uint8_t reg, uint8_t value)
static void write_reg(uint8_t reg, uint8_t value)
{
uint8_t data[2] = { reg, value };
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, data, 2, false, time_us_64() + IO_TIMEOUT_US);
}
// Write a 16-bit register
void write_reg16(uint8_t reg, uint16_t value)
static void write_reg16(uint8_t reg, uint16_t value)
{
uint8_t data[3] = { reg, value >> 8, value & 0xff };
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, data, 3, false, time_us_64() + IO_TIMEOUT_US);
@ -65,8 +169,7 @@ static void write_reg_list(const uint16_t *list)
}
}
// Read an 8-bit register
uint8_t read_reg(uint8_t reg)
static uint8_t read_reg(uint8_t reg)
{
uint8_t value;
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
@ -74,8 +177,7 @@ uint8_t read_reg(uint8_t reg)
return value;
}
// Read a 16-bit register
uint16_t read_reg16(uint8_t reg)
static uint16_t read_reg16(uint8_t reg)
{
uint8_t value[2];
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
@ -83,23 +185,18 @@ uint16_t read_reg16(uint8_t reg)
return (value[0] << 8) | value[1];
}
// Write an arbitrary number of bytes from the given array to the sensor,
// starting at the given register
void write_many(uint8_t reg, const uint8_t *src, uint8_t len)
static void write_many(uint8_t reg, const uint8_t *src, uint8_t len)
{
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, src, len, false, time_us_64() + IO_TIMEOUT_US);
}
// Read an arbitrary number of bytes from the sensor, starting at the given
// register, into the given array
void read_many(uint8_t reg, uint8_t *dst, uint8_t len)
static void read_many(uint8_t reg, uint8_t *dst, uint8_t len)
{
i2c_write_blocking_until(I2C_PORT, I2C_ADDR, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
i2c_read_blocking_until(I2C_PORT, I2C_ADDR, dst, len, false, time_us_64() + IO_TIMEOUT_US * len);
}
const uint16_t reg_mode1[] = { 4, 0x8800, 0x8001, 0xff01, 0x0000 };
const uint16_t reg_mode2[] = { 3, 0x0001, 0xff00, 0x8000 };
const uint16_t reg_spad0[] = { 4, 0x8001, 0xff01, 0x0000, 0xff06 };
@ -212,17 +309,11 @@ bool vl53l0x_init_tof()
return true;
}
// Get the return signal rate limit check value in MCPS
float getSignalRateLimit()
{
return (float)read_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
}
// Decode sequence step timeout in MCLKs from register value
// based on VL53L0X_decode_timeout()
// Note: the original function returned a uint32_t, but the return value is
// always stored in a uint16_t.
uint16_t decodeTimeout(uint16_t reg_val)
static uint16_t decodeTimeout(uint16_t reg_val)
{
// format: "(LSByte * 2^MSByte) + 1"
return (uint16_t)((reg_val & 0x00FF) <<
@ -430,7 +521,6 @@ bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks)
// For the MSRC timeout, the same applies - this timeout being
// dependant on the pre-range vcsel period."
if (type == VcselPeriodPreRange) {
// "Set phase check limits"
switch (period_pclks) {
@ -637,49 +727,10 @@ uint16_t readRangeContinuousMillimeters()
return instances[current_instance].range;
}
#if 0
// Performs a single-shot range measurement and returns the reading in
// millimeters
// based on VL53L0X_PerformSingleRangingMeasurement()
uint16_t readRangeSingleMillimeters()
{
static uint16_t range = 65535;
static bool reading = false;
static uint64_t start_time = 0;
uint64_t now = time_us_64();
if (now - start_time > TOF_WAIT_US) {
reading = false;
}
if (reading) {
if ((read_reg(SYSRANGE_START) & 0x01) == 0) {
range = readRangeContinuousMillimeters(index);
reading = false;
}
} else {
write_reg(0x80, 0x01);
write_reg(0xFF, 0x01);
write_reg(0x00, 0x00);
write_reg(0x91, instances[index].stop_variable);
write_reg(0x00, 0x01);
write_reg(0xFF, 0x00);
write_reg(0x80, 0x00);
write_reg(SYSRANGE_START, 0x01);
start_time = now;
reading = true;
}
return range;
}
#endif
// Private Methods /////////////////////////////////////////////////////////////
// Get reference SPAD (single photon avalanche diode) count and type
// based on VL53L0X_get_info_from_device(),
// but only gets reference SPAD count and type
bool getSpadInfo(uint8_t *count, bool *type_is_aperture)
static bool getSpadInfo(uint8_t *count, bool *type_is_aperture)
{
write_reg_list(reg_spad0);
write_reg(0x83, read_reg(0x83) | 0x04);
@ -711,7 +762,7 @@ bool getSpadInfo(uint8_t *count, bool *type_is_aperture)
// Get sequence step enables
// based on VL53L0X_GetSequenceStepEnables()
void getSequenceStepEnables(SequenceStepEnables * enables)
static void getSequenceStepEnables(SequenceStepEnables * enables)
{
uint8_t seq_cfg = read_reg(SYSTEM_SEQUENCE_CONFIG);
@ -726,7 +777,7 @@ void getSequenceStepEnables(SequenceStepEnables * enables)
// based on get_sequence_step_timeout(),
// but gets all timeouts instead of just the requested one, and also stores
// intermediate values
void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts)
static void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts *timeouts)
{
timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange);

119
firmware/src/vl53l0x.h
View File

@ -13,107 +13,18 @@
#include "hardware/i2c.h"
// register addresses from API vl53l0x_device.h (ordered as listed there)
enum regAddr
{
SYSRANGE_START = 0x00,
SYSTEM_THRESH_HIGH = 0x0C,
SYSTEM_THRESH_LOW = 0x0E,
SYSTEM_SEQUENCE_CONFIG = 0x01,
SYSTEM_RANGE_CONFIG = 0x09,
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
SYSTEM_INTERRUPT_CLEAR = 0x0B,
RESULT_INTERRUPT_STATUS = 0x13,
RESULT_RANGE_STATUS = 0x14,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
MSRC_CONFIG_CONTROL = 0x60,
PRE_RANGE_CONFIG_MIN_SNR = 0x27,
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
SYSTEM_HISTOGRAM_BIN = 0x81,
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
IDENTIFICATION_MODEL_ID = 0xC0,
IDENTIFICATION_REVISION_ID = 0xC2,
OSC_CALIBRATE_VAL = 0xF8,
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
ALGO_PHASECAL_LIM = 0x30,
ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
};
typedef enum {
VcselPeriodPreRange, VcselPeriodFinalRange
} vcselPeriodType;
void vl53l0x_init(unsigned index, i2c_inst_t *i2c_port, uint8_t i2c_addr);
void vl53l0x_use(unsigned index);
bool vl53l0x_is_present();
bool vl53l0x_init_tof();
bool setSignalRateLimit(float limit_Mcps);
float getSignalRateLimit();
bool setMeasurementTimingBudget(uint32_t budget_us);
uint32_t getMeasurementTimingBudget();
typedef enum {
VcselPeriodPreRange, VcselPeriodFinalRange
} vcselPeriodType;
bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks);
uint8_t getVcselPulsePeriod(vcselPeriodType type);
@ -123,26 +34,4 @@ void vl53l0x_stop_continuous();
uint16_t readRangeContinuousMillimeters();
uint16_t readRangeSingleMillimeters();
// TCC: Target CentreCheck
// MSRC: Minimum Signal Rate Check
// DSS: Dynamic Spad Selection
typedef struct {
bool tcc, msrc, dss, pre_range, final_range;
} SequenceStepEnables;
typedef struct {
uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks;
uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks;
uint32_t msrc_dss_tcc_us, pre_range_us, final_range_us;
} SequenceStepTimeouts;
bool getSpadInfo(uint8_t * count, bool * type_is_aperture);
void getSequenceStepEnables(SequenceStepEnables * enables);
void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts);
bool performSingleRefCalibration(uint8_t vhv_init_byte);
#endif