WALDO WiFi Autonomous LIDAR Distancing and Outlining tool

Mapping the physical to the virtual

With the ever growing desire for automation and the ever growing desire to move to the virtual, tools are needed to map the physical world. WALDO is a device that uses commercially available sensor and parts to create a tool that aims to make high definition, 3D mappings of physical spaces. These models have a wide range of applications ranging from store room inventory management and security to creation of virtual worlds in Web3 and the blockchain.

WALDO

WiFi Autonomous LiDaR Distancing and Outlining is a device that uses DIY LiDAR sensors with three degrees of freedom to provide ranging information in a half dome pattern. WALDO will scan 360 degrees on the XY plane and 45 degrees in YZ plane to create a 3D point cloud for the upper 1/2 dome of mapped area. The sample measurements are streamed over TCP from the PSoC6 to a python client.

Auto Alignment

In order to get detailed ranging information, it's important to get a good starting position for your scan pattern. WALDO uses the Infineon TLI43D-W2BW 3D magnetic sensor and 2 magnets (XY plane, and YZ plane) to achieve both XY and YZ alignment. The magnitudes are read periodically via I2C from the sensor as the stepper motors move the magnets in both planes. Generally, here are steps:

• Read magnetic sensor magnitudes and calculate the root sum squared (periodically @ ~ 200 Hz)
• Rotate XY plane 360 degrees in the clockwise and record largest magnitude
• Rotate XY plane anti-clockwise to 180 degrees beyond recorded largest magnitude (this will be the position of the LiDaR arm)
• Rotate YZ plane in decreasing step sizes clockwise and anti-closewise to maximize magnetic magnitude of the YZ magnet (producing a pointing of due North)

Measurement Data

WALDO sends packets of collected measurement data in the following format:

• xy step (uint) - Step index of XY plane
• yz step (uint) - Step index of XY plane
• x range (float) - X range in cm
• y range (float) - Y range in cm
• z range (float) - Z range in cm
• xy dir (bool) - Direction of XY motor
• yz dir (bool) - Direction of YZ motor

Assembling the Device

WALDO requires 3 mounts (and an optional 4th) that can be 3D printed using Cura and your favorite 3D printer. All of the 3D part schematics are included below.

WALDO Software

And now for the best part...Code!

ModusToolBox was a huge resource for getting a running start. From the ModusToolBox dashboard, you select to create a project for the Microsoft Visual Studio Code platform. Once the project creator is launched, simply select the Kit and project type and presto, you have a buildable runnable example.

FreeRTOS is used to power WALDO. Tasking and queueing are used to orchestrate the various software components (see Github repo for more info). When WALDO initializes, it will go into Auto Alignment to accurately align the LiDaR sensor to due North. Once the sensor is aligned, WALDO will start the step sequence to collect for the 3D 1/2 dome.

WALDO Library Sensors

Many different sensors and components by WALDO, and the ModusToolBox makes BYOS (Bring Your Own Sensors) relatively easy. Device code was written for communication, initialization and operation for:

Stepper Motor Driver

#ifndef STEPPER_MOTOR_ULN2003_H
#define STEPPER_MOTOR_ULN2003_H

#include "cybsp.h"

#define NUM_MOTOR_PINS 4
#define STEP_SEQ_LEN 8
#define XY_MOTOR_ID 0
#define YZ_MOTOR_ID 1

/*
* @brief Initialization of the Stepper Motors
*/
cy_rslt_t init_motors();

/*
* @brief Step the motor 1 time clockwise
* 
* @param step_idx Step index
* @param motor_id The id of the motor
*
* @return The next step index
*/
uint8_t step_clockwise(uint8_t step_idx, int motor_id);

/*
* @brief Step the motor 1 time anti-clockwise
* 
* @param step_idx Step index
* @param motor_id The id of the motor
*
* @return The next step index
*/
uint8_t step_anticlockwise(uint8_t step_idx, int motor_id);

/*
* @brief Step the motor many times
* 
* @param step_idx Step index
* @param motor_id The id of the motor
* @param clockwise The motor direction
* @param num The num steps
*
* @return The next step index
*/
uint8_t step(uint8_t step_idx, int motor_id, bool clockwise, int num);

#endif

LiDaR Sensor

#ifndef TF_LUNA_H
#define TF_LUNA_H

#include "FreeRTOS.h"
#include "cyhal.h"
#include "cybsp.h"
#include "semphr.h"


/*
* @brief Get distancing info
* 
* @param i2c The i2c handle
* @param val The ranging value
*
* @return the result
*/
cy_rslt_t get_dist(cyhal_i2c_t* i2c,float* val);

#endif

3D Magnet Sensor

#ifndef TLI493_W2BW_H
#define TLI493_W2BW_H

#include "cyhal.h"
#include "cybsp.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cybsp.h"
#include "semphr.h"


/*
* @brief Initialize the magentic sensor 
* 
* @param i2c The i2c handle
* @param cb The callback for new measurements
*
* @return the result
*/
cy_rslt_t init_sensor(cyhal_i2c_t *i2c, cyhal_gpio_event_callback_t cb);

/*
* @brief Update the mag values
* 
* @param i2c The i2c handle
*/
void update_mag_vals(cyhal_i2c_t *i2c);

/*
* @brief Get X magnitude
* 
* @return X magnitude
*/
float get_X();
/*
* @brief Get X magnitude
* 
* @return X magnitude
*/
float get_Y();
/*
* @brief Get Y magnitude
* 
* @return Y magnitude
*/
float get_Z();
/*
* @brief Get Z magnitude
* 
* @return Z magnitude
*/
float get_R();
/*
* @brief Get R magnitude
* 
* @return R magnitude
*/
float get_T();
/*
* @brief Get T temperature
* 
* @return T 
*/
float get_N();
/*
* @brief Get azimuth
* 
* @return amazimuth
*/
float get_azimuth();
/*
* @brief Get polar
* 
* @return polar
*/
float get_polar();

#endif

Measurement Sample Struct

typedef struct {
    uint32_t xy_step;
    uint32_t yz_step;
    float x;
    float y;
    float z;
    uint16_t xy_dir;
    uint16_t yz_dir;
}measurement;

Measurement Resolution

The measurement resolution can be configured using defines in the code. In scanner_task.c, you'll find:

#define YZ_RUN true
#define YZ_DIR true
#define YZ_STEP_PERIOD 1
#define YZ_PERIOD 1024

#define XY_RUN true
#define XY_DIR true
// #define XY_STEP_PERIOD YZ_PERIOD * 2    // 2 full cycle 2 channels (up/down)
#define XY_STEP_PERIOD 512
#define XY_PERIOD XY_STEP_PERIOD * 4096 // full 360
#define TOTAL_STEPS XY_PERIOD           // run 1 full xy period

WALDO Library Tasks

Three separate tasks are required for the operation of WALDO:

Magnitude Monitor - Uses I2C to communicate with the Magnetic Sensor and calculate the magnitudes

#ifndef MAG_TASK_H_
#define MAG_TASK_H_
#include "FreeRTOS.h"
#include "task.h"
#include "cybsp.h"
#include "semphr.h"
/*******************************************************************************
* Macros
********************************************************************************/
/* Task priority and stack size for the Motion sensor task */
#define TASK_MAG_PRIORITY     (configMAX_PRIORITIES - 1)
#define TASK_MAG_STACK_SIZE   (512u)
/*******************************************************************************
* Function Prototypes
********************************************************************************/
cy_rslt_t create_mag_task(cyhal_i2c_t* i2c,SemaphoreHandle_t* i2c_semaphore);
#endif

Sample Collection - Uses I2C to communicate with the LiDaR Sensor to get ranging information and calculate cartesian coordinates

#ifndef SCANNER_TASK_H_
#define SCANNER_TASK_H_
#include "FreeRTOS.h"
#include "task.h"
#include "cybsp.h"
#include "semphr.h"
/*******************************************************************************
* Macros
********************************************************************************/
/* Task priority and stack size for the Motion sensor task */
#define SCAN_TASK_PRIORITY (configMAX_PRIORITIES - 1)
#define SCAN_TASK_STACK_SIZE (512u)
/*******************************************************************************
* Function Prototypes
********************************************************************************/
cy_rslt_t create_scanner_task(cyhal_i2c_t *i2c, SemaphoreHandle_t *i2c_semaphore);
/*
* @brief Autonomous alignmen
*
* @param xy_step_idx Starting xy step index
* @param yz_step_idx Starting yz step index
*/
void auto_align(uint8_t *xy_step_idx, uint8_t *yz_step_idx);
/*
* @brief Main run loop for motor activity and scan pattern
*
* @param i2c i2c handle
* @param starting_xy_step_idx Starting xy step index
* @param starting_yz_step_idx Starting yz step index
* @param total_steps Total number of motor steps
* @param xy_run XY run
* @param yz_run YZ run
* @param xy_step_period Number of steps between direction switch
* @param yz_step_period Number of steps between direction switch
* @param xy_direction initial direction of xy
* @param yz_direction initial direction of yz`
* @param step_sleep sleep between steps
*/
void run_motors(
cyhal_i2c_t *i2c,
uint8_t starting_xy_step_idx,
uint8_t starting_yz_step_idx,
uint32_t total_steps,
bool xy_run,
bool yz_run,
uint32_t xy_period,
uint32_t yz_period,
uint32_t xy_step_period,
uint32_t yz_step_period,
bool xy_direction,
bool yz_direction,
uint16_t step_sleep);
/*
* @brief Process lidar data for step
*
* @param period_idx The period index
* @param period The current period
* @param step_period The current step period
* @param motor_id Motor identifier
* @param step_idx current step_index
* @param stepped true if stepped
*/
void process_step(
uint32_t period_idx,
uint32_t period,
uint32_t step_period,
int motor_id,
bool *direction,
uint8_t *step_idx,
bool *stepped);
#endif /* SCANNER_TASK_H_ */

Sample Publish - Uses the network stack to send collected measurement information to clients

#ifndef SAMPLE_PUBLISHER_TASK_H_
#define SAMPLE_PUBLISHER_TASK_H_
#include "cyhal.h"
#include "cybsp.h"
#include "FreeRTOS.h"
#define BATCH_SIZE 32
typedef struct
{
uint32_t xy_step;
uint32_t yz_step;
float x;
float y;
float z;
uint16_t xy_dir;
uint16_t yz_dir;
} measurement;
typedef measurement measurement_batch[BATCH_SIZE];
/*******************************************************************************
* Function Prototypes
********************************************************************************/
void sample_publisher_task(void *arg);
/*
* @brief Init the wifi
*
* @return the result
*/
cy_rslt_t init_wifi();
/*
* @brief Create publisher task
*
* @return the result
*/
cy_rslt_t create_sample_publisher_task();
#endif

These values can be used to configure the desired resolution.

Scripts

Much of the processing is done directly on the PSoC, but there is some client code that was more easily written in Python. You'll find scripts to connect to the measurement stream, and post-process the samples to XYZ format.

Rendering Results

WALDO only produces raw measurements. If you want point clouds, jpg or even Blender models, you'll need to use 3rd part software. The following are useful:

• Blender
• XYZ Render
• Point Cloud to Mesh

Conclusion

If you made it this far, thank you!!! WALDO was a lot of fun to work on, but there's more todo. One of the challenges was wire management. I'd like to enhance the mount models to better house all of the wiring required. The ModusToolbox also has some great support for the PSoC widgets (slider/buttons/etc). I'd like to incorporate some features (such as resolution selection and re-auto align) via the buttons. Also, I think some post-processing software to generate actual meshes/etc. would be useful. I would love to hear your thoughts, questions and concerns.