Migrated to Polulu VL53L0X library

Saves around 7.5kb of precious program space

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

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;

src/lib/motor.h

@@ -2,6 +2,7 @@
 #define __motor
 
 
+// Low-level functions to control the motor
 enum class MotorDirection
 {
   Up = 0,

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';
+}

src/lib/motorstate.h

@@ -0,0 +1,19 @@
+#ifndef __motorstate
+#define __motorstate
+
+#include <stdint.h>
+
+// 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

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<MoveScreen>();
+      break;
+
+    case Button::Down:
+      motorStateMoveTo(Settings.Height.Preset[1]);
+      this->getScreenManager()->show<MoveScreen>();
+      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;

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();
 };
 

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<HomeScreen>();
 }
 

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
+#ifndef VL53L0X_h
-  access to the VL53L0X API headers. I will not be using the Adafruit library
+#define VL53L0X_h
-  however because it lacks a way to set the timing budget, which I found to be
-  very much required for accurate results.
 
-*/
+#include <Arduino.h>
 
-#ifndef __vl53l0x
-#define __vl53l0x
 
-#include "Arduino.h"
+enum class VL53L0XInitResult
-#include "Wire.h"
-#include "vl53l0x_api.h"
-
-typedef int8_t VL53L0XPosition;
-
-typedef struct
 {
-  VL53L0X_Error error;
+  Success = 0,
-  VL53L0XPosition position;
+  InvalidIdentification = 1,
-} VL53L0XResult;
+  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);
+    // register addresses from API vl53l0x_device.h (ordered as listed there)
-    bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error);
+    enum regAddr
-    bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error);
+    {
-    bool getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight);
+      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,
+    };
+
+    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:
-    VL53L0X_Dev_t device;
+    // 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
+
+

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 = 2,
-  HeightSensorTest = 3,
 
   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;
+  heightSensor.setTimeout(500);
-  if (!heightSensor.init(Config::HeightSensorI2CAddress, &result))
+  auto error = heightSensor.init();
+
+  if (error != VL53L0XInitResult::Success)
   {
     initSequenceError(InitSequenceStep::HeightSensorInit);
 
-    display.print(result.error);
+    switch (error)
-    display.print(" @ ");
+    {
-    display.print(result.position);
+      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 (heightSensorGetRange(&measurement))
-
-    if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax))
     {
       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;
 
-
+    if (motorStateCheckTargetReached())
-    // Check if we've reached the target
+      screenManager.show<HomeScreen>();
-    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;
-    }
   }
   else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
   {
-    motorStop();
+    motorStateStop();
-    State.MoveDirection = Direction::None;
 
     // 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");
 }