From b1600ab1d60a7f97896b422a218cfd912f969e1b Mon Sep 17 00:00:00 2001 From: Mark van Renswoude Date: Thu, 30 Jan 2020 15:16:52 +0100 Subject: [PATCH] Migrated to Polulu VL53L0X library Saves around 7.5kb of precious program space --- platformio.ini | 11 + src/include/config.h | 4 + src/lib/motor.h | 1 + src/lib/motorstate.cpp | 80 +++ src/lib/motorstate.h | 19 + src/lib/screen/home.cpp | 36 +- src/lib/screen/home.h | 2 +- src/lib/screen/move.cpp | 3 +- src/lib/vl53l0x.cpp | 1059 ++++++++++++++++++++++++++++++++++++--- src/lib/vl53l0x.h | 216 ++++++-- src/main.cpp | 115 ++--- 11 files changed, 1361 insertions(+), 185 deletions(-) create mode 100644 src/lib/motorstate.cpp create mode 100644 src/lib/motorstate.h diff --git a/platformio.ini b/platformio.ini index 93c8591..77c38fa 100644 --- a/platformio.ini +++ b/platformio.ini @@ -38,3 +38,14 @@ upload_protocol = usbtiny upload_flags = -e upload_speed = 19200 + +; This environment is not actually one I use for the hardware, but +; PlatformIO's Home includes a very useful Inspect feature that +; requires a debug build. These builds are significantly larger and +; will cause the build to fail on the "checkprogsize" step. +; Targetting a microprocessor with more space allows the build to succeed. +[env:platformio-home-debug] +platform = atmelavr +board = atmega2560 +board_build.f_cpu = 16000000L +framework = arduino \ No newline at end of file diff --git a/src/include/config.h b/src/include/config.h index 4a28294..a43848a 100644 --- a/src/include/config.h +++ b/src/include/config.h @@ -52,8 +52,12 @@ class Config Settings for a VL53L0X time-of-flight sensor. */ + // Note that this must correspond to the default address of the module, + // the initialisation code skips reassigning the address to save calls. static const uint8_t HeightSensorI2CAddress = 0x29; + static const uint16_t HeightSensorBudget = 33000; + // Values above this will be disregarded as invalid static const uint16_t HeightMeasurementMax = 1500; diff --git a/src/lib/motor.h b/src/lib/motor.h index 9c275d6..a2de8d6 100644 --- a/src/lib/motor.h +++ b/src/lib/motor.h @@ -2,6 +2,7 @@ #define __motor +// Low-level functions to control the motor enum class MotorDirection { Up = 0, diff --git a/src/lib/motorstate.cpp b/src/lib/motorstate.cpp new file mode 100644 index 0000000..be35e02 --- /dev/null +++ b/src/lib/motorstate.cpp @@ -0,0 +1,80 @@ +#include "./motorstate.h" +#include "./motor.h" +#include "./state.h" +#include "./settings.h" +#include "include/config.h" + + +void motorStateMoveTo(uint16_t height) +{ + +} + + +bool motorStateCheckTargetReached() +{ + switch (State.MoveDirection) + { + case Direction::Up: + if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop) + { + if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget) + State.CurrentHeight = State.MoveTarget; + + motorStateStop(); + return true; + } + break; + + case Direction::Down: + if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop) + { + if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget) + State.CurrentHeight = State.MoveTarget; + + motorStateStop(); + return true; + } + break; + + default: + break; + } + + return false; +} + + +bool motorStateCheckOverCurrent() +{ + if (motorIsOverCurrent()) + { + motorStateStop(); + return true; + } + + return false; +} + + +void motorStateStop() +{ + motorStop(); + State.MoveDirection = Direction::None; +} + + +void getDisplayHeight(char* buffer, uint16_t value) +{ + uint8_t displayValue = (value + Settings.Height.Offset) / 10; + + if (displayValue > 99) + buffer[0] = '0' + ((displayValue / 100) % 10); + else + buffer[0] = '0'; + + buffer[1] = '.'; + buffer[2] = '0' + ((displayValue / 10) % 10); + buffer[3] = '0' + (displayValue % 10); + buffer[4] = 'm'; +} \ No newline at end of file diff --git a/src/lib/motorstate.h b/src/lib/motorstate.h new file mode 100644 index 0000000..5994226 --- /dev/null +++ b/src/lib/motorstate.h @@ -0,0 +1,19 @@ +#ifndef __motorstate +#define __motorstate + +#include + +// High-level functions to control the motor and update the global state +extern void motorStateMoveTo(uint16_t height); +extern bool motorStateCheckTargetReached(); +extern bool motorStateCheckOverCurrent(); +extern void motorStateStop(); + + +// Formats a height value as "0.00m" (always exactly 5 characters long). +// Buffer must be at least 5 bytes long. No null character is added. +// The value is the raw height sensor value, the offset is added by this function. +extern void getDisplayHeight(char* buffer, uint16_t value); + + +#endif \ No newline at end of file diff --git a/src/lib/screen/home.cpp b/src/lib/screen/home.cpp index 3948a60..5b3c374 100644 --- a/src/lib/screen/home.cpp +++ b/src/lib/screen/home.cpp @@ -1,7 +1,9 @@ #include "./home.h" +#include "./move.h" #include "fonts/FreeSansBold18pt7b.trimmed.h" #include "include/config.h" #include "lib/settings.h" +#include "lib/motorstate.h" #define HOME_FONT_BASELINE 25 @@ -16,22 +18,6 @@ #define PRESET_LINEHEIGHT (HOME_FONT_BASELINE + (2 * PRESET_MARGIN)) -// TODO move to shared file -// Formats a height value as "0.00m" (always exactly 5 characters long). -// Buffer must be at least 5 bytes long. No null character is added. -void getDisplayHeight(char* buffer, uint8_t value) -{ - if (value > 99) - buffer[0] = '0' + ((value / 100) % 10); - else - buffer[0] = '0'; - - buffer[1] = '.'; - buffer[2] = '0' + ((value / 10) % 10); - buffer[3] = '0' + (value % 10); - buffer[4] = 'm'; -} - void HomeScreen::onShow() { @@ -52,6 +38,22 @@ void HomeScreen::onShow() void HomeScreen::onButton(Button button) { + switch (button) + { + case Button::Up: + motorStateMoveTo(Settings.Height.Preset[0]); + this->getScreenManager()->show(); + break; + + case Button::Down: + motorStateMoveTo(Settings.Height.Preset[1]); + this->getScreenManager()->show(); + break; + + case Button::Menu: + // TODO show menu + break; + } } @@ -74,7 +76,7 @@ void HomeScreen::drawPreset2() } -void HomeScreen::drawPreset(uint8_t number, uint8_t y, uint8_t value) +void HomeScreen::drawPreset(uint8_t number, uint8_t y, uint16_t value) { auto display = this->getDisplay(); uint16_t numberColor; diff --git a/src/lib/screen/home.h b/src/lib/screen/home.h index d8c31e4..1cf3fd8 100644 --- a/src/lib/screen/home.h +++ b/src/lib/screen/home.h @@ -23,7 +23,7 @@ class HomeScreen : public BaseScreen void drawPreset1(); void drawPreset2(); - void drawPreset(uint8_t number, uint8_t y, uint8_t value); + void drawPreset(uint8_t number, uint8_t y, uint16_t value); void drawMenu(); }; diff --git a/src/lib/screen/move.cpp b/src/lib/screen/move.cpp index 8833c96..4fc16d4 100644 --- a/src/lib/screen/move.cpp +++ b/src/lib/screen/move.cpp @@ -3,6 +3,7 @@ #include "fonts/FreeSansBold18pt7b.trimmed.h" #include "include/config.h" #include "lib/settings.h" +#include "lib/motorstate.h" #define MOVE_FONT_BASELINE 25 @@ -24,7 +25,7 @@ void MoveScreen::onShow() void MoveScreen::onButton(Button button) { - // TODO stop the motor + motorStateStop(); this->getScreenManager()->show(); } diff --git a/src/lib/vl53l0x.cpp b/src/lib/vl53l0x.cpp index d1a7162..3aa438a 100644 --- a/src/lib/vl53l0x.cpp +++ b/src/lib/vl53l0x.cpp @@ -1,101 +1,1040 @@ -#include "vl53l0x.h" +// Slightly modified version of https://github.com/pololu/vl53l0x-arduino +// which returns an error code if initialization fails. +// Most of the functionality of this library is based on the VL53L0X API +// provided by ST (STSW-IMG005), and some of the explanatory comments are quoted +// or paraphrased from the API source code, API user manual (UM2039), and the +// VL53L0X datasheet. -bool checkResult(VL53L0X_Error error, VL53L0XResult* result, VL53L0XPosition position) +#include "VL53L0X.h" +#include + +// Defines ///////////////////////////////////////////////////////////////////// + +// The Arduino two-wire interface uses a 7-bit number for the address, +// and sets the last bit correctly based on reads and writes +#define ADDRESS_DEFAULT 0b0101001 + +// Record the current time to check an upcoming timeout against +#define startTimeout() (timeout_start_ms = millis()) + +// Check if timeout is enabled (set to nonzero value) and has expired +#define checkTimeoutExpired() (io_timeout > 0 && ((uint16_t)(millis() - timeout_start_ms) > io_timeout)) + +// Decode VCSEL (vertical cavity surface emitting laser) pulse period in PCLKs +// from register value +// based on VL53L0X_decode_vcsel_period() +#define decodeVcselPeriod(reg_val) (((reg_val) + 1) << 1) + +// Encode VCSEL pulse period register value from period in PCLKs +// based on VL53L0X_encode_vcsel_period() +#define encodeVcselPeriod(period_pclks) (((period_pclks) >> 1) - 1) + +// Calculate macro period in *nanoseconds* from VCSEL period in PCLKs +// based on VL53L0X_calc_macro_period_ps() +// PLL_period_ps = 1655; macro_period_vclks = 2304 +#define calcMacroPeriod(vcsel_period_pclks) ((((uint32_t)2304 * (vcsel_period_pclks) * 1655) + 500) / 1000) + +// Constructors //////////////////////////////////////////////////////////////// + +VL53L0X::VL53L0X(void) + : address(ADDRESS_DEFAULT) + , io_timeout(0) // no timeout + , did_timeout(false) { - if (error != VL53L0X_ERROR_NONE) +} + +// Public Methods ////////////////////////////////////////////////////////////// + +void VL53L0X::setAddress(uint8_t new_addr) +{ + writeReg(I2C_SLAVE_DEVICE_ADDRESS, new_addr & 0x7F); + address = new_addr; +} + +// Initialize sensor using sequence based on VL53L0X_DataInit(), +// VL53L0X_StaticInit(), and VL53L0X_PerformRefCalibration(). +// This function does not perform reference SPAD calibration +// (VL53L0X_PerformRefSpadManagement()), since the API user manual says that it +// is performed by ST on the bare modules; it seems like that should work well +// enough unless a cover glass is added. +// If io_2v8 (optional) is true or not given, the sensor is configured for 2V8 +// mode. +VL53L0XInitResult VL53L0X::init(bool io_2v8) +{ + // check model ID register (value specified in datasheet) + if (readReg(IDENTIFICATION_MODEL_ID) != 0xEE) { return VL53L0XInitResult::InvalidIdentification; } + + // VL53L0X_DataInit() begin + + // sensor uses 1V8 mode for I/O by default; switch to 2V8 mode if necessary + if (io_2v8) { - result->error = error; - result->position = position; + writeReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV, + readReg(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV) | 0x01); // set bit 0 + } + + // "Set I2C standard mode" + writeReg(0x88, 0x00); + + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + stop_variable = readReg(0x91); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks + writeReg(MSRC_CONFIG_CONTROL, readReg(MSRC_CONFIG_CONTROL) | 0x12); + + // set final range signal rate limit to 0.25 MCPS (million counts per second) + setSignalRateLimit(0.25); + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xFF); + + // VL53L0X_DataInit() end + + // VL53L0X_StaticInit() begin + + uint8_t spad_count; + bool spad_type_is_aperture; + if (!getSpadInfo(&spad_count, &spad_type_is_aperture)) { return VL53L0XInitResult::GetSpadInfoFailed; } + + // The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in + // the API, but the same data seems to be more easily readable from + // GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there + uint8_t ref_spad_map[6]; + readMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + + // -- VL53L0X_set_reference_spads() begin (assume NVM values are valid) + + writeReg(0xFF, 0x01); + writeReg(DYNAMIC_SPAD_REF_EN_START_OFFSET, 0x00); + writeReg(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD, 0x2C); + writeReg(0xFF, 0x00); + writeReg(GLOBAL_CONFIG_REF_EN_START_SELECT, 0xB4); + + uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; // 12 is the first aperture spad + uint8_t spads_enabled = 0; + + for (uint8_t i = 0; i < 48; i++) + { + if (i < first_spad_to_enable || spads_enabled == spad_count) + { + // This bit is lower than the first one that should be enabled, or + // (reference_spad_count) bits have already been enabled, so zero this bit + ref_spad_map[i / 8] &= ~(1 << (i % 8)); + } + else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) + { + spads_enabled++; + } + } + + writeMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6); + + // -- VL53L0X_set_reference_spads() end + + // -- VL53L0X_load_tuning_settings() begin + // DefaultTuningSettings from vl53l0x_tuning.h + + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + + writeReg(0xFF, 0x00); + writeReg(0x09, 0x00); + writeReg(0x10, 0x00); + writeReg(0x11, 0x00); + + writeReg(0x24, 0x01); + writeReg(0x25, 0xFF); + writeReg(0x75, 0x00); + + writeReg(0xFF, 0x01); + writeReg(0x4E, 0x2C); + writeReg(0x48, 0x00); + writeReg(0x30, 0x20); + + writeReg(0xFF, 0x00); + writeReg(0x30, 0x09); + writeReg(0x54, 0x00); + writeReg(0x31, 0x04); + writeReg(0x32, 0x03); + writeReg(0x40, 0x83); + writeReg(0x46, 0x25); + writeReg(0x60, 0x00); + writeReg(0x27, 0x00); + writeReg(0x50, 0x06); + writeReg(0x51, 0x00); + writeReg(0x52, 0x96); + writeReg(0x56, 0x08); + writeReg(0x57, 0x30); + writeReg(0x61, 0x00); + writeReg(0x62, 0x00); + writeReg(0x64, 0x00); + writeReg(0x65, 0x00); + writeReg(0x66, 0xA0); + + writeReg(0xFF, 0x01); + writeReg(0x22, 0x32); + writeReg(0x47, 0x14); + writeReg(0x49, 0xFF); + writeReg(0x4A, 0x00); + + writeReg(0xFF, 0x00); + writeReg(0x7A, 0x0A); + writeReg(0x7B, 0x00); + writeReg(0x78, 0x21); + + writeReg(0xFF, 0x01); + writeReg(0x23, 0x34); + writeReg(0x42, 0x00); + writeReg(0x44, 0xFF); + writeReg(0x45, 0x26); + writeReg(0x46, 0x05); + writeReg(0x40, 0x40); + writeReg(0x0E, 0x06); + writeReg(0x20, 0x1A); + writeReg(0x43, 0x40); + + writeReg(0xFF, 0x00); + writeReg(0x34, 0x03); + writeReg(0x35, 0x44); + + writeReg(0xFF, 0x01); + writeReg(0x31, 0x04); + writeReg(0x4B, 0x09); + writeReg(0x4C, 0x05); + writeReg(0x4D, 0x04); + + writeReg(0xFF, 0x00); + writeReg(0x44, 0x00); + writeReg(0x45, 0x20); + writeReg(0x47, 0x08); + writeReg(0x48, 0x28); + writeReg(0x67, 0x00); + writeReg(0x70, 0x04); + writeReg(0x71, 0x01); + writeReg(0x72, 0xFE); + writeReg(0x76, 0x00); + writeReg(0x77, 0x00); + + writeReg(0xFF, 0x01); + writeReg(0x0D, 0x01); + + writeReg(0xFF, 0x00); + writeReg(0x80, 0x01); + writeReg(0x01, 0xF8); + + writeReg(0xFF, 0x01); + writeReg(0x8E, 0x01); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + + // -- VL53L0X_load_tuning_settings() end + + // "Set interrupt config to new sample ready" + // -- VL53L0X_SetGpioConfig() begin + + writeReg(SYSTEM_INTERRUPT_CONFIG_GPIO, 0x04); + writeReg(GPIO_HV_MUX_ACTIVE_HIGH, readReg(GPIO_HV_MUX_ACTIVE_HIGH) & ~0x10); // active low + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); + + // -- VL53L0X_SetGpioConfig() end + + measurement_timing_budget_us = getMeasurementTimingBudget(); + + // "Disable MSRC and TCC by default" + // MSRC = Minimum Signal Rate Check + // TCC = Target CentreCheck + // -- VL53L0X_SetSequenceStepEnable() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + + // -- VL53L0X_SetSequenceStepEnable() end + + // "Recalculate timing budget" + setMeasurementTimingBudget(measurement_timing_budget_us); + + // VL53L0X_StaticInit() end + + // VL53L0X_PerformRefCalibration() begin (VL53L0X_perform_ref_calibration()) + + // -- VL53L0X_perform_vhv_calibration() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x01); + if (!performSingleRefCalibration(0x40)) { return VL53L0XInitResult::VHVCalibrationFailed; } + + // -- VL53L0X_perform_vhv_calibration() end + + // -- VL53L0X_perform_phase_calibration() begin + + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02); + if (!performSingleRefCalibration(0x00)) { return VL53L0XInitResult::PhaseCalibrationFailed; } + + // -- VL53L0X_perform_phase_calibration() end + + // "restore the previous Sequence Config" + writeReg(SYSTEM_SEQUENCE_CONFIG, 0xE8); + + // VL53L0X_PerformRefCalibration() end + + return VL53L0XInitResult::Success; +} + +// Write an 8-bit register +void VL53L0X::writeReg(uint8_t reg, uint8_t value) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.write(value); + last_status = Wire.endTransmission(); +} + +// Write a 16-bit register +void VL53L0X::writeReg16Bit(uint8_t reg, uint16_t value) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.write((value >> 8) & 0xFF); // value high byte + Wire.write( value & 0xFF); // value low byte + last_status = Wire.endTransmission(); +} + +// Write a 32-bit register +void VL53L0X::writeReg32Bit(uint8_t reg, uint32_t value) +{ + Wire.beginTransmission(address); + Wire.write(reg); + Wire.write((value >> 24) & 0xFF); // value highest byte + Wire.write((value >> 16) & 0xFF); + Wire.write((value >> 8) & 0xFF); + Wire.write( value & 0xFF); // value lowest byte + last_status = Wire.endTransmission(); +} + +// Read an 8-bit register +uint8_t VL53L0X::readReg(uint8_t reg) +{ + uint8_t value; + + Wire.beginTransmission(address); + Wire.write(reg); + last_status = Wire.endTransmission(); + + Wire.requestFrom(address, (uint8_t)1); + value = Wire.read(); + + return value; +} + +// Read a 16-bit register +uint16_t VL53L0X::readReg16Bit(uint8_t reg) +{ + uint16_t value; + + Wire.beginTransmission(address); + Wire.write(reg); + last_status = Wire.endTransmission(); + + Wire.requestFrom(address, (uint8_t)2); + value = (uint16_t)Wire.read() << 8; // value high byte + value |= Wire.read(); // value low byte + + return value; +} + +// Read a 32-bit register +uint32_t VL53L0X::readReg32Bit(uint8_t reg) +{ + uint32_t value; + + Wire.beginTransmission(address); + Wire.write(reg); + last_status = Wire.endTransmission(); + + Wire.requestFrom(address, (uint8_t)4); + value = (uint32_t)Wire.read() << 24; // value highest byte + value |= (uint32_t)Wire.read() << 16; + value |= (uint16_t)Wire.read() << 8; + value |= Wire.read(); // value lowest byte + + return value; +} + +// Write an arbitrary number of bytes from the given array to the sensor, +// starting at the given register +void VL53L0X::writeMulti(uint8_t reg, uint8_t const * src, uint8_t count) +{ + Wire.beginTransmission(address); + Wire.write(reg); + + while (count-- > 0) + { + Wire.write(*(src++)); + } + + last_status = Wire.endTransmission(); +} + +// Read an arbitrary number of bytes from the sensor, starting at the given +// register, into the given array +void VL53L0X::readMulti(uint8_t reg, uint8_t * dst, uint8_t count) +{ + Wire.beginTransmission(address); + Wire.write(reg); + last_status = Wire.endTransmission(); + + Wire.requestFrom(address, count); + + while (count-- > 0) + { + *(dst++) = Wire.read(); + } +} + +// Set the return signal rate limit check value in units of MCPS (mega counts +// per second). "This represents the amplitude of the signal reflected from the +// target and detected by the device"; setting this limit presumably determines +// the minimum measurement necessary for the sensor to report a valid reading. +// Setting a lower limit increases the potential range of the sensor but also +// seems to increase the likelihood of getting an inaccurate reading because of +// unwanted reflections from objects other than the intended target. +// Defaults to 0.25 MCPS as initialized by the ST API and this library. +bool VL53L0X::setSignalRateLimit(float limit_Mcps) +{ + if (limit_Mcps < 0 || limit_Mcps > 511.99) { return false; } + + // Q9.7 fixed point format (9 integer bits, 7 fractional bits) + writeReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT, limit_Mcps * (1 << 7)); + return true; +} + +// Get the return signal rate limit check value in MCPS +float VL53L0X::getSignalRateLimit(void) +{ + return (float)readReg16Bit(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7); +} + +// Set the measurement timing budget in microseconds, which is the time allowed +// for one measurement; the ST API and this library take care of splitting the +// timing budget among the sub-steps in the ranging sequence. A longer timing +// budget allows for more accurate measurements. Increasing the budget by a +// factor of N decreases the range measurement standard deviation by a factor of +// sqrt(N). Defaults to about 33 milliseconds; the minimum is 20 ms. +// based on VL53L0X_set_measurement_timing_budget_micro_seconds() +bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us) +{ + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + uint16_t const StartOverhead = 1910; + uint16_t const EndOverhead = 960; + uint16_t const MsrcOverhead = 660; + uint16_t const TccOverhead = 590; + uint16_t const DssOverhead = 690; + uint16_t const PreRangeOverhead = 660; + uint16_t const FinalRangeOverhead = 550; + + uint32_t const MinTimingBudget = 20000; + + if (budget_us < MinTimingBudget) { return false; } + + uint32_t used_budget_us = StartOverhead + EndOverhead; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + if (enables.tcc) + { + used_budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); + } + + if (enables.dss) + { + used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); + } + else if (enables.msrc) + { + used_budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); + } + + if (enables.pre_range) + { + used_budget_us += (timeouts.pre_range_us + PreRangeOverhead); + } + + if (enables.final_range) + { + used_budget_us += FinalRangeOverhead; + + // "Note that the final range timeout is determined by the timing + // budget and the sum of all other timeouts within the sequence. + // If there is no room for the final range timeout, then an error + // will be set. Otherwise the remaining time will be applied to + // the final range." + + if (used_budget_us > budget_us) + { + // "Requested timeout too big." + return false; + } + + uint32_t final_range_timeout_us = budget_us - used_budget_us; + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) + + // "For the final range timeout, the pre-range timeout + // must be added. To do this both final and pre-range + // timeouts must be expressed in macro periods MClks + // because they have different vcsel periods." + + uint32_t final_range_timeout_mclks = + timeoutMicrosecondsToMclks(final_range_timeout_us, + timeouts.final_range_vcsel_period_pclks); + + if (enables.pre_range) + { + final_range_timeout_mclks += timeouts.pre_range_mclks; + } + + writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(final_range_timeout_mclks)); + + // set_sequence_step_timeout() end + + measurement_timing_budget_us = budget_us; // store for internal reuse + } + return true; +} + +// Get the measurement timing budget in microseconds +// based on VL53L0X_get_measurement_timing_budget_micro_seconds() +// in us +uint32_t VL53L0X::getMeasurementTimingBudget(void) +{ + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + uint16_t const StartOverhead = 1910; + uint16_t const EndOverhead = 960; + uint16_t const MsrcOverhead = 660; + uint16_t const TccOverhead = 590; + uint16_t const DssOverhead = 690; + uint16_t const PreRangeOverhead = 660; + uint16_t const FinalRangeOverhead = 550; + + // "Start and end overhead times always present" + uint32_t budget_us = StartOverhead + EndOverhead; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + if (enables.tcc) + { + budget_us += (timeouts.msrc_dss_tcc_us + TccOverhead); + } + + if (enables.dss) + { + budget_us += 2 * (timeouts.msrc_dss_tcc_us + DssOverhead); + } + else if (enables.msrc) + { + budget_us += (timeouts.msrc_dss_tcc_us + MsrcOverhead); + } + + if (enables.pre_range) + { + budget_us += (timeouts.pre_range_us + PreRangeOverhead); + } + + if (enables.final_range) + { + budget_us += (timeouts.final_range_us + FinalRangeOverhead); + } + + measurement_timing_budget_us = budget_us; // store for internal reuse + return budget_us; +} + +// Set the VCSEL (vertical cavity surface emitting laser) pulse period for the +// given period type (pre-range or final range) to the given value in PCLKs. +// Longer periods seem to increase the potential range of the sensor. +// Valid values are (even numbers only): +// pre: 12 to 18 (initialized default: 14) +// final: 8 to 14 (initialized default: 10) +// based on VL53L0X_set_vcsel_pulse_period() +bool VL53L0X::setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks) +{ + uint8_t vcsel_period_reg = encodeVcselPeriod(period_pclks); + + SequenceStepEnables enables; + SequenceStepTimeouts timeouts; + + getSequenceStepEnables(&enables); + getSequenceStepTimeouts(&enables, &timeouts); + + // "Apply specific settings for the requested clock period" + // "Re-calculate and apply timeouts, in macro periods" + + // "When the VCSEL period for the pre or final range is changed, + // the corresponding timeout must be read from the device using + // the current VCSEL period, then the new VCSEL period can be + // applied. The timeout then must be written back to the device + // using the new VCSEL period. + // + // 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) + { + case 12: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18); + break; + + case 14: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30); + break; + + case 16: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40); + break; + + case 18: + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50); + break; + + default: + // invalid period + return false; + } + writeReg(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + + // apply new VCSEL period + writeReg(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); + + // update timeouts + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_PRE_RANGE) + + uint16_t new_pre_range_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.pre_range_us, period_pclks); + + writeReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(new_pre_range_timeout_mclks)); + + // set_sequence_step_timeout() end + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_MSRC) + + uint16_t new_msrc_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.msrc_dss_tcc_us, period_pclks); + + writeReg(MSRC_CONFIG_TIMEOUT_MACROP, + (new_msrc_timeout_mclks > 256) ? 255 : (new_msrc_timeout_mclks - 1)); + + // set_sequence_step_timeout() end + } + else if (type == VcselPeriodFinalRange) + { + switch (period_pclks) + { + case 8: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x30); + writeReg(0xFF, 0x00); + break; + + case 10: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + case 12: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + case 14: + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48); + writeReg(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08); + writeReg(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03); + writeReg(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07); + writeReg(0xFF, 0x01); + writeReg(ALGO_PHASECAL_LIM, 0x20); + writeReg(0xFF, 0x00); + break; + + default: + // invalid period + return false; + } + + // apply new VCSEL period + writeReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg); + + // update timeouts + + // set_sequence_step_timeout() begin + // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE) + + // "For the final range timeout, the pre-range timeout + // must be added. To do this both final and pre-range + // timeouts must be expressed in macro periods MClks + // because they have different vcsel periods." + + uint16_t new_final_range_timeout_mclks = + timeoutMicrosecondsToMclks(timeouts.final_range_us, period_pclks); + + if (enables.pre_range) + { + new_final_range_timeout_mclks += timeouts.pre_range_mclks; + } + + writeReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, + encodeTimeout(new_final_range_timeout_mclks)); + + // set_sequence_step_timeout end + } + else + { + // invalid type return false; } + // "Finally, the timing budget must be re-applied" + + setMeasurementTimingBudget(measurement_timing_budget_us); + + // "Perform the phase calibration. This is needed after changing on vcsel period." + // VL53L0X_perform_phase_calibration() begin + + uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG); + writeReg(SYSTEM_SEQUENCE_CONFIG, 0x02); + performSingleRefCalibration(0x0); + writeReg(SYSTEM_SEQUENCE_CONFIG, sequence_config); + + // VL53L0X_perform_phase_calibration() end + return true; } - -bool VL53L0X::init(uint8_t address, VL53L0XResult* result) +// Get the VCSEL pulse period in PCLKs for the given period type. +// based on VL53L0X_get_vcsel_pulse_period() +uint8_t VL53L0X::getVcselPulsePeriod(vcselPeriodType type) { - device.I2cDevAddr = 0x29; - device.comms_type = 1; - device.comms_speed_khz = 400; - device.i2c = &Wire; + if (type == VcselPeriodPreRange) + { + return decodeVcselPeriod(readReg(PRE_RANGE_CONFIG_VCSEL_PERIOD)); + } + else if (type == VcselPeriodFinalRange) + { + return decodeVcselPeriod(readReg(FINAL_RANGE_CONFIG_VCSEL_PERIOD)); + } + else { return 255; } +} - if (!checkResult(VL53L0X_DataInit(&device), result, VL53L0X_POSITION_DATAINIT)) - return false; +// Start continuous ranging measurements. If period_ms (optional) is 0 or not +// given, continuous back-to-back mode is used (the sensor takes measurements as +// often as possible); otherwise, continuous timed mode is used, with the given +// inter-measurement period in milliseconds determining how often the sensor +// takes a measurement. +// based on VL53L0X_StartMeasurement() +void VL53L0X::startContinuous(uint32_t period_ms) +{ + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, stop_variable); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); + if (period_ms != 0) + { + // continuous timed mode - auto setAddress = address & 0x7F; + // VL53L0X_SetInterMeasurementPeriodMilliSeconds() begin - // * 2 to convert from 7 to 8 bits - if (!checkResult(VL53L0X_SetDeviceAddress(&device, setAddress * 2), result, VL53L0X_POSITION_SETDEVICEADDRESS)) - return false; + uint16_t osc_calibrate_val = readReg16Bit(OSC_CALIBRATE_VAL); - delay(10); - device.I2cDevAddr = setAddress; + if (osc_calibrate_val != 0) + { + period_ms *= osc_calibrate_val; + } - VL53L0X_DeviceInfo_t deviceInfo; - if (!checkResult(VL53L0X_GetDeviceInfo(&device, &deviceInfo), result, VL53L0X_POSITION_GETDEVICEINFO)) - return false; + writeReg32Bit(SYSTEM_INTERMEASUREMENT_PERIOD, period_ms); - if (!checkResult(VL53L0X_StaticInit(&device), result, VL53L0X_POSITION_STATICINIT)) - return false; + // VL53L0X_SetInterMeasurementPeriodMilliSeconds() end - uint32_t refSpadCount; - uint8_t isApertureSpads; + writeReg(SYSRANGE_START, 0x04); // VL53L0X_REG_SYSRANGE_MODE_TIMED + } + else + { + // continuous back-to-back mode + writeReg(SYSRANGE_START, 0x02); // VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK + } +} - if (!checkResult(VL53L0X_PerformRefSpadManagement(&device, &refSpadCount, &isApertureSpads), result, VL53L0X_POSITION_REFSPAD)) - return false; +// Stop continuous measurements +// based on VL53L0X_StopMeasurement() +void VL53L0X::stopContinuous(void) +{ + writeReg(SYSRANGE_START, 0x01); // VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT - // TODO expose as API to be run before changing desk positions - uint8_t vhvSettings; - uint8_t phaseCal; + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, 0x00); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); +} - if (!checkResult(VL53L0X_PerformRefCalibration(&device, &vhvSettings, &phaseCal), result, VL53L0X_POSITION_REFCAL)) - return false; +// Returns a range reading in millimeters when continuous mode is active +// (readRangeSingleMillimeters() also calls this function after starting a +// single-shot range measurement) +uint16_t VL53L0X::readRangeContinuousMillimeters(void) +{ + startTimeout(); + while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) + { + if (checkTimeoutExpired()) + { + did_timeout = true; + return 65535; + } + } - if (!checkResult(VL53L0X_SetDeviceMode(&device, VL53L0X_DEVICEMODE_SINGLE_RANGING), result, VL53L0X_POSITION_DEVICEMODE)) - return false; + // assumptions: Linearity Corrective Gain is 1000 (default); + // fractional ranging is not enabled + uint16_t range = readReg16Bit(RESULT_RANGE_STATUS + 10); - if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRANGE)) - return false; + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); - if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRATE)) - return false; + return range; +} - if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1), result, VL53L0X_POSITION_TRESHOLD1)) - return false; +// Performs a single-shot range measurement and returns the reading in +// millimeters +// based on VL53L0X_PerformSingleRangingMeasurement() +uint16_t VL53L0X::readRangeSingleMillimeters(void) +{ + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + writeReg(0x91, stop_variable); + writeReg(0x00, 0x01); + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); - if (!checkResult(VL53L0X_SetLimitCheckValue(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, (FixPoint1616_t)( 1.5 * 0.023 * 65536)), result, VL53L0X_POSITION_TRESHOLD2)) - return false; + writeReg(SYSRANGE_START, 0x01); + + // "Wait until start bit has been cleared" + startTimeout(); + while (readReg(SYSRANGE_START) & 0x01) + { + if (checkTimeoutExpired()) + { + did_timeout = true; + return 65535; + } + } + + return readRangeContinuousMillimeters(); +} + +// Did a timeout occur in one of the read functions since the last call to +// timeoutOccurred()? +bool VL53L0X::timeoutOccurred() +{ + bool tmp = did_timeout; + did_timeout = false; + return tmp; +} + +// 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 VL53L0X::getSpadInfo(uint8_t * count, bool * type_is_aperture) +{ + uint8_t tmp; + + writeReg(0x80, 0x01); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x00); + + writeReg(0xFF, 0x06); + writeReg(0x83, readReg(0x83) | 0x04); + writeReg(0xFF, 0x07); + writeReg(0x81, 0x01); + + writeReg(0x80, 0x01); + + writeReg(0x94, 0x6b); + writeReg(0x83, 0x00); + startTimeout(); + while (readReg(0x83) == 0x00) + { + if (checkTimeoutExpired()) { return false; } + } + writeReg(0x83, 0x01); + tmp = readReg(0x92); + + *count = tmp & 0x7f; + *type_is_aperture = (tmp >> 7) & 0x01; + + writeReg(0x81, 0x00); + writeReg(0xFF, 0x06); + writeReg(0x83, readReg(0x83) & ~0x04); + writeReg(0xFF, 0x01); + writeReg(0x00, 0x01); + + writeReg(0xFF, 0x00); + writeReg(0x80, 0x00); - result->error = VL53L0X_ERROR_NONE; - result->position = VL53L0X_POSITION_UNKNOWN; return true; } - -bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error) +// Get sequence step enables +// based on VL53L0X_GetSequenceStepEnables() +void VL53L0X::getSequenceStepEnables(SequenceStepEnables * enables) { - *error = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&device, budget_us); - return (*error == VL53L0X_ERROR_NONE); + uint8_t sequence_config = readReg(SYSTEM_SEQUENCE_CONFIG); + + enables->tcc = (sequence_config >> 4) & 0x1; + enables->dss = (sequence_config >> 3) & 0x1; + enables->msrc = (sequence_config >> 2) & 0x1; + enables->pre_range = (sequence_config >> 6) & 0x1; + enables->final_range = (sequence_config >> 7) & 0x1; +} + +// Get sequence step timeouts +// based on get_sequence_step_timeout(), +// but gets all timeouts instead of just the requested one, and also stores +// intermediate values +void VL53L0X::getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts) +{ + timeouts->pre_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodPreRange); + + timeouts->msrc_dss_tcc_mclks = readReg(MSRC_CONFIG_TIMEOUT_MACROP) + 1; + timeouts->msrc_dss_tcc_us = + timeoutMclksToMicroseconds(timeouts->msrc_dss_tcc_mclks, + timeouts->pre_range_vcsel_period_pclks); + + timeouts->pre_range_mclks = + decodeTimeout(readReg16Bit(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)); + timeouts->pre_range_us = + timeoutMclksToMicroseconds(timeouts->pre_range_mclks, + timeouts->pre_range_vcsel_period_pclks); + + timeouts->final_range_vcsel_period_pclks = getVcselPulsePeriod(VcselPeriodFinalRange); + + timeouts->final_range_mclks = + decodeTimeout(readReg16Bit(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI)); + + if (enables->pre_range) + { + timeouts->final_range_mclks -= timeouts->pre_range_mclks; + } + + timeouts->final_range_us = + timeoutMclksToMicroseconds(timeouts->final_range_mclks, + timeouts->final_range_vcsel_period_pclks); +} + +// 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 VL53L0X::decodeTimeout(uint16_t reg_val) +{ + // format: "(LSByte * 2^MSByte) + 1" + return (uint16_t)((reg_val & 0x00FF) << + (uint16_t)((reg_val & 0xFF00) >> 8)) + 1; +} + +// Encode sequence step timeout register value from timeout in MCLKs +// based on VL53L0X_encode_timeout() +uint16_t VL53L0X::encodeTimeout(uint32_t timeout_mclks) +{ + // 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 MCLKs to microseconds with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_us() +uint32_t VL53L0X::timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return ((timeout_period_mclks * macro_period_ns) + 500) / 1000; +} + +// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs +// based on VL53L0X_calc_timeout_mclks() +uint32_t VL53L0X::timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) +{ + uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks); + + return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns); } -bool VL53L0X::getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t *data, VL53L0X_Error* error) +// based on VL53L0X_perform_single_ref_calibration() +bool VL53L0X::performSingleRefCalibration(uint8_t vhv_init_byte) { - *error = VL53L0X_PerformSingleRangingMeasurement(&device, data); - return (*error == VL53L0X_ERROR_NONE); -} + writeReg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + startTimeout(); + while ((readReg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) + { + if (checkTimeoutExpired()) { return false; } + } -bool VL53L0X::getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight) -{ - VL53L0X_RangingMeasurementData_t data; - auto result = this->getSingleRangingMeasurement(&data, error); + writeReg(SYSTEM_INTERRUPT_CLEAR, 0x01); - if (result) - *distanceMillimeters = data.RangeMilliMeter; + writeReg(SYSRANGE_START, 0x00); - return result && data.RangeMilliMeter <= maxValidHeight; + return true; } \ No newline at end of file diff --git a/src/lib/vl53l0x.h b/src/lib/vl53l0x.h index 77b71f0..57483de 100644 --- a/src/lib/vl53l0x.h +++ b/src/lib/vl53l0x.h @@ -1,53 +1,189 @@ -/* +// Slightly modified version of https://github.com/pololu/vl53l0x-arduino +// which returns an error code if initialization fails. - The Adafruit VL53L0X library is included because it's an easy way to get - access to the VL53L0X API headers. I will not be using the Adafruit library - however because it lacks a way to set the timing budget, which I found to be - very much required for accurate results. +#ifndef VL53L0X_h +#define VL53L0X_h -*/ +#include -#ifndef __vl53l0x -#define __vl53l0x -#include "Arduino.h" -#include "Wire.h" -#include "vl53l0x_api.h" - -typedef int8_t VL53L0XPosition; - -typedef struct +enum class VL53L0XInitResult { - VL53L0X_Error error; - VL53L0XPosition position; -} VL53L0XResult; + Success = 0, + InvalidIdentification = 1, + GetSpadInfoFailed = 2, + VHVCalibrationFailed = 3, + PhaseCalibrationFailed = 4, +}; -#define VL53L0X_POSITION_UNKNOWN ((VL53L0XPosition) 0) - -// Positions for the init method -#define VL53L0X_POSITION_DATAINIT ((VL53L0XPosition) 1) -#define VL53L0X_POSITION_SETDEVICEADDRESS ((VL53L0XPosition) 2) -#define VL53L0X_POSITION_GETDEVICEINFO ((VL53L0XPosition) 3) -#define VL53L0X_POSITION_STATICINIT ((VL53L0XPosition) 4) -#define VL53L0X_POSITION_REFSPAD ((VL53L0XPosition) 5) -#define VL53L0X_POSITION_REFCAL ((VL53L0XPosition) 6) -#define VL53L0X_POSITION_DEVICEMODE ((VL53L0XPosition) 7) -#define VL53L0X_POSITION_LIMITRANGE ((VL53L0XPosition) 8) -#define VL53L0X_POSITION_LIMITRATE ((VL53L0XPosition) 9) -#define VL53L0X_POSITION_TRESHOLD1 ((VL53L0XPosition) 10) -#define VL53L0X_POSITION_TRESHOLD2 ((VL53L0XPosition) 11) - class VL53L0X { public: - bool init(uint8_t address, VL53L0XResult* result); - bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error); - bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error); - bool getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight); + // register addresses from API vl53l0x_device.h (ordered as listed there) + enum regAddr + { + SYSRANGE_START = 0x00, - private: - VL53L0X_Dev_t device; + 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, + }; + + enum vcselPeriodType { VcselPeriodPreRange, VcselPeriodFinalRange }; + + uint8_t last_status; // status of last I2C transmission + + VL53L0X(void); + + void setAddress(uint8_t new_addr); + inline uint8_t getAddress(void) { return address; } + + VL53L0XInitResult init(bool io_2v8 = true); + + void writeReg(uint8_t reg, uint8_t value); + void writeReg16Bit(uint8_t reg, uint16_t value); + void writeReg32Bit(uint8_t reg, uint32_t value); + uint8_t readReg(uint8_t reg); + uint16_t readReg16Bit(uint8_t reg); + uint32_t readReg32Bit(uint8_t reg); + + void writeMulti(uint8_t reg, uint8_t const * src, uint8_t count); + void readMulti(uint8_t reg, uint8_t * dst, uint8_t count); + + bool setSignalRateLimit(float limit_Mcps); + float getSignalRateLimit(void); + + bool setMeasurementTimingBudget(uint32_t budget_us); + uint32_t getMeasurementTimingBudget(void); + + bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks); + uint8_t getVcselPulsePeriod(vcselPeriodType type); + + void startContinuous(uint32_t period_ms = 0); + void stopContinuous(void); + uint16_t readRangeContinuousMillimeters(void); + uint16_t readRangeSingleMillimeters(void); + + inline void setTimeout(uint16_t timeout) { io_timeout = timeout; } + inline uint16_t getTimeout(void) { return io_timeout; } + bool timeoutOccurred(void); + + private: + // TCC: Target CentreCheck + // MSRC: Minimum Signal Rate Check + // DSS: Dynamic Spad Selection + + struct SequenceStepEnables + { + boolean tcc, msrc, dss, pre_range, final_range; + }; + + struct SequenceStepTimeouts + { + 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; + }; + + uint8_t address; + uint16_t io_timeout; + bool did_timeout; + uint16_t timeout_start_ms; + + uint8_t stop_variable; // read by init and used when starting measurement; is StopVariable field of VL53L0X_DevData_t structure in API + uint32_t measurement_timing_budget_us; + + 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); + + static uint16_t decodeTimeout(uint16_t value); + static uint16_t encodeTimeout(uint32_t timeout_mclks); + static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks); + static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks); }; -#endif \ No newline at end of file +#endif + + diff --git a/src/main.cpp b/src/main.cpp index e1ed73e..277d12c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,7 @@ #include "./lib/vl53l0x.h" #include "./lib/state.h" #include "./lib/motor.h" +#include "./lib/motorstate.h" #include "./lib/screen/home.h" @@ -17,8 +18,7 @@ enum class InitSequenceStep { EEPROM = 0, HeightSensorInit = 1, - HeightSensorBudget = 2, - HeightSensorTest = 3, + HeightSensorTest = 2, Last = HeightSensorTest }; @@ -26,7 +26,7 @@ enum class InitSequenceStep // Forward declarations inline void setupHeightSensor(); -inline uint8_t testHeightSensor(); +inline uint16_t testHeightSensor(); void initSequenceStart(); void initSequenceSuccess(InitSequenceStep step); @@ -34,6 +34,7 @@ void initSequenceError(InitSequenceStep step); void initSequenceDisplayHeight(uint16_t measurement); void initSequenceEnd(); +bool heightSensorGetRange(uint16_t* measurement); auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST); @@ -88,45 +89,55 @@ inline void setupHeightSensor() { Wire.begin(); - VL53L0XResult result; - if (!heightSensor.init(Config::HeightSensorI2CAddress, &result)) + heightSensor.setTimeout(500); + auto error = heightSensor.init(); + + if (error != VL53L0XInitResult::Success) { initSequenceError(InitSequenceStep::HeightSensorInit); - display.print(result.error); - display.print(" @ "); - display.print(result.position); + switch (error) + { + case VL53L0XInitResult::InvalidIdentification: + display.print("Invalid identification"); + break; + + case VL53L0XInitResult::GetSpadInfoFailed: + display.print("GetSpadInfo failed"); + break; + + case VL53L0XInitResult::VHVCalibrationFailed: + display.print("VHV calibration failed"); + break; + + case VL53L0XInitResult::PhaseCalibrationFailed: + display.print("Phase calibration failed"); + break; + + default: + display.print("Unknown error"); + break; + } while(1); } + + heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget); + initSequenceSuccess(InitSequenceStep::HeightSensorInit); - - - VL53L0X_Error error; - if (!heightSensor.setMeasurementTimingBudget(33000, &error)) - { - initSequenceError(InitSequenceStep::HeightSensorBudget); - - display.print(result.error); - - while(1); - } - initSequenceSuccess(InitSequenceStep::HeightSensorBudget); } -inline uint8_t testHeightSensor() +inline uint16_t testHeightSensor() { - VL53L0X_Error error; uint16_t reference = 0; uint8_t closeCount = 0; + uint16_t measurement; while (closeCount < 3) { - uint16_t measurement; - - if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax)) + if (heightSensorGetRange(&measurement)) { initSequenceDisplayHeight(measurement); @@ -150,7 +161,7 @@ inline uint8_t testHeightSensor() } initSequenceSuccess(InitSequenceStep::HeightSensorTest); - return reference / 10; + return reference; } @@ -171,11 +182,8 @@ void loop() if (State.MoveDirection != Direction::None) { - if (motorIsOverCurrent()) + if (motorStateCheckOverCurrent()) { - motorStop(); - State.MoveDirection = Direction::None; - // TODO go to overcurrent screen } else @@ -191,54 +199,32 @@ uint32_t lastValidMeasurement; void updateHeight() { - VL53L0X_Error error; uint16_t measurement; - if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax)) + if (heightSensorGetRange(&measurement)) { State.CurrentHeight = measurement; lastValidMeasurement = State.CurrentTime; - - // Check if we've reached the target - switch (State.MoveDirection) - { - case Direction::Up: - if (measurement >= State.MoveTarget - Config::HeightMeasurementDeltaStop) - { - if (measurement - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget) - State.CurrentHeight = State.MoveTarget; - - motorStop(); - State.MoveDirection = Direction::None; - } - break; - - case Direction::Down: - if (measurement <= State.MoveTarget + Config::HeightMeasurementDeltaStop) - { - if (State.MoveTarget - measurement <= Config::HeightMeasurementDeltaOnTarget) - State.CurrentHeight = State.MoveTarget; - - motorStop(); - State.MoveDirection = Direction::None; - } - break; - - default: - break; - } + if (motorStateCheckTargetReached()) + screenManager.show(); } else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout) { - motorStop(); - State.MoveDirection = Direction::None; + motorStateStop(); // TODO go to height sensor error screen } } +bool heightSensorGetRange(uint16_t* measurement) +{ + *measurement = heightSensor.readRangeSingleMillimeters(); + return !heightSensor.timeoutOccurred() && *measurement <= Config::HeightMeasurementMax; +} + + /* For display sleep: @@ -283,9 +269,6 @@ void initSequenceStart() display.print(" height sensor"); display.setCursor(0, initSequenceTextY(2)); - display.print(" timing budget"); - - display.setCursor(0, initSequenceTextY(3)); display.print(" sensor test"); }