Skip to content

Commit

Permalink
Merge pull request #61 from TableUV/user/jack/tof-integration-hacky-demo
Browse files Browse the repository at this point in the history 
User/jack/tof integration hacky demo
  • Loading branch information
@jaku-jaku
jaku-jaku authored Mar 28, 2021
2 parents c76151b + 11e8ebc commit 1716937
Show file tree
Hide file tree
Showing 19 changed files with 296 additions and 129 deletions.
8 changes: 4 additions & 4 deletions Firmware/ESP32/Arduino_platform/include/avr_driver_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,10 +116,10 @@ typedef enum motor_command_direction{
// tof_sensor_config
// XSHUT is active low, pulling low shutsdown the sensor
typedef enum tof_sensor_config{
TOF_SENSOR_CONFIG_DISABLE_ALL, //pull XSHUT low, sensor shutdown (active low )
TOF_SENSOR_CONFIG_ENABLE_1,
TOF_SENSOR_CONFIG_ENABLE_2,
TOF_SENSOR_CONFIG_ENABLE_3,
TOF_SENSOR_CONFIG_ENABLE_ALL = 0U, //pull XSHUT high, sensor shutdown (active high)
TOF_SENSOR_CONFIG_ENABLE_TWO = 1U,
TOF_SENSOR_CONFIG_ENABLE_ONE = 2U,
TOF_SENSOR_CONFIG_DISABLE_ALL = 3U,
TOF_SENSOR_CONFIG_COUNT,
TOF_SENSOR_CONFIG_UNDEFINED
} tof_sensor_config_E;
Expand Down
38 changes: 23 additions & 15 deletions Firmware/ESP32/Arduino_platform/include/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,13 +72,17 @@ extern "C"{
# define MOCK ( ENABLE)

/***** FEATURE ENABLES ****/
# define FEATURE_SLAM (DISABLE) // APP SLAM
# define FEATURE_LIDAR (DISABLE)
# define FEATURE_DEMO_TOF_OBSTACLE ( ENABLE) // DEV avr driver: motor, mist, encoder feedback
# define FEATURE_DEV_DRIVER ( ENABLE) // DEV avr driver: motor, mist, encoder feedback
# define FEATURE_SLAM ( ENABLE) // APP SLAM
# define FEATURE_LIDAR ( ENABLE)
# define FEATURE_LIDAR_CALIBRATION_MODE ( TODO) // TODO: implement calibration strategy
# define FEATURE_SUPER_USE_PROFILED_MOTIONS ( TODO) // TODO: implement calibration strategy
# define FEATURE_SUPER_USE_HARDCODE_CHORE ( ENABLE)
# define FEATURE_SUPER_CMD_DEV_DRIVER ( ENABLE) // Super command on actuators
# define FEATURE_PERIPHERALS ( ENABLE)
# define FEATURE_UV ( DISABLE)
# define FEATURE_FAST_MODE (DISABLE) // Wacky shit | Attempt to use 50Hz, currently it is safe to use 20Hz, to increase, we need to find a way to increase ToF to 400kHz.
# define FEATURE_UV ((DISABLE) && (! FEATURE_FAST_MODE))
# define FEATURE_IMU (DISABLE)
# define FEATURE_SENSOR_AVR ( ENABLE)
# define FEATURE_AVR_DRIVER_ALL ( ENABLE) //
Expand All @@ -94,19 +98,23 @@ extern "C"{
/***** DEBUG PRINT FLAGS ****/
# define DEBUG_FPRINT ( ENABLE)
# ifdef DEBUG_FPRINT
# define DEBUG_FPRINT_FEATURE_LIDAR (DISABLE) // Live feed of sensor readings
# define DEBUG_FPRINT_APP_SUPERVISOR (DISABLE) // Live feed of collision status
# define DEBUG_FPRINT_FEATURE_MAP (DISABLE) // Live feed of global map
# define DEBUG_FPRINT_FEATURE_MAP_CENTERED ( TRUE) // Map Feeding Mode: true->memory_map, false->center_map
# define DEBUG_FPRINT_FEATURE_OBSTACLES (DISABLE) // Live feed of obstacle detection
# define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY (DISABLE) // Live feed of choreography
# define DEBUG_FPRINT_FEATURE_LIDAR ( ENABLE) // Live feed of tof sensor readings
# define DEBUG_FPRINT_APP_SUPER_STATE ( ENABLE) // Live feed of supervisor state
# define DEBUG_FPRINT_APP_SUPER_AVR_SENSOR ( ENABLE) // Live feed of collision status
# define DEBUG_FPRINT_APP_SUPER_CHOREOGRAPHY ( ENABLE) // Live feed of choreography status
# define DEBUG_FPRINT_FEATURE_MAP (DISABLE) // Live feed of global map
# define DEBUG_FPRINT_FEATURE_MAP_CENTERED ( TRUE) // Map Feeding Mode: true->memory_map, false->center_map
# define DEBUG_FPRINT_FEATURE_OBSTACLES (DISABLE) // Live feed of obstacle detection
# define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY (DISABLE) // Live feed of choreography
# else
# define DEBUG_FPRINT_FEATURE_LIDAR (DISABLE)
# define DEBUG_FPRINT_APP_SUPERVISOR (DISABLE)
# define DEBUG_FPRINT_FEATURE_MAP (DISABLE)
# define DEBUG_FPRINT_FEATURE_MAP_CENTERED (DISABLE)
# define DEBUG_FPRINT_FEATURE_OBSTACLES (DISABLE)
# define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY (DISABLE)
# define DEBUG_FPRINT_FEATURE_LIDAR (DISABLE)
# define DEBUG_FPRINT_APP_SUPER_STATE (DISABLE)
# define DEBUG_FPRINT_APP_SUPER_AVR_SENSOR (DISABLE)
# define DEBUG_FPRINT_APP_SUPER_CHOREOGRAPHY (DISABLE)
# define DEBUG_FPRINT_FEATURE_MAP (DISABLE)
# define DEBUG_FPRINT_FEATURE_MAP_CENTERED (DISABLE)
# define DEBUG_FPRINT_FEATURE_OBSTACLES (DISABLE)
# define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY (DISABLE)
# endif

/***********************************
Expand Down
82 changes: 48 additions & 34 deletions Firmware/ESP32/Arduino_platform/lib/DEV/dev_ToF_Lidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
// TableUV Lib
#include "../IO/io_ping_map.h"
#include "../../include/common.h"
#include "dev_avr_driver.h"

// Arduino Lib
#include <SparkFun_VL53L1X.h>
Expand All @@ -30,14 +31,15 @@
#define TOF_WIDTH_OF_SPADS_PER_ZONE (5U) // MIN: 4 pix
#define TOF_HEIGHT_OF_SPADS_PER_ZONE (5U) // MIN: 4 pix
#define TOF_MAX_DIST_MM (1300U) // 1.3 [m] in short range mode
#define TOF_INTERMEDIATE_SETTING_DELAY_MS (20U)
#define TOF_INTERMEDIATE_SETTING_DELAY_MS (10U)
#define MP_MUTEX_BLOCK_TIME_MS ((1U)/portTICK_PERIOD_MS)
#define TOF_SENSOR_COUNT (1U) // (DEV_TOF_LIDAR_COUNT)
#define TOF_SENSOR_COUNT (DEV_TOF_LIDAR_COUNT)

typedef struct{
TwoWire I2C;
SFEVL53L1X tofs[DEV_TOF_LIDAR_COUNT];
const uint8_t address[DEV_TOF_LIDAR_COUNT];
const tof_sensor_config_E avr_config[DEV_TOF_LIDAR_COUNT];
const uint8_t firing_sequence[DEV_TOF_FIRING_KEYFRAME_COUNT];
const uint8_t firing_sequence_label[DEV_TOF_LIDAR_COUNT][DEV_TOF_FIRING_KEYFRAME_COUNT];
uint8_t prev_firingframe[DEV_TOF_LIDAR_COUNT];
Expand All @@ -57,14 +59,19 @@ void dev_ToF_reset_all_sensors(void);
static dev_tof_lidar_data_S lidar_data = {
.I2C = TwoWire(1),
.tofs = {
SFEVL53L1X(lidar_data.I2C, 21, TOF_INT_1),//TOF_SHUT, TOF_INT_1),
SFEVL53L1X(lidar_data.I2C, 22, TOF_INT_2),//TOF_SHUT, TOF_INT_2),
SFEVL53L1X(lidar_data.I2C, 23, TOF_INT_3),//TOF_SHUT, TOF_INT_3)
/* DEV_TOF_LIDAR_C */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_2),//TOF_SHUT, TOF_INT_1),
/* DEV_TOF_LIDAR_L */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_1),//TOF_SHUT, TOF_INT_2),
/* DEV_TOF_LIDAR_R */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_3),//TOF_SHUT, TOF_INT_3)
},
.address = {
0x62,
0x64,
0x66 // Arbitrary Address (non-default)
[DEV_TOF_LIDAR_C] = 0x62,
[DEV_TOF_LIDAR_L] = 0x64,
[DEV_TOF_LIDAR_R] = 0x66, // Arbitrary Address (non-default)
},
.avr_config = { // MUST BE IN THIS ORDER:
[DEV_TOF_LIDAR_C] = TOF_SENSOR_CONFIG_ENABLE_ONE,
[DEV_TOF_LIDAR_L] = TOF_SENSOR_CONFIG_ENABLE_TWO,
[DEV_TOF_LIDAR_R] = TOF_SENSOR_CONFIG_ENABLE_ALL,
},
/**Table of Optical Centers**
*
Expand Down Expand Up @@ -95,14 +102,14 @@ static dev_tof_lidar_data_S lidar_data = {
//Firing Order: Left <- [2] - [4] - [1] - [5] - [3] -> Right
},
.firing_sequence_label = {
DEV_TOF_FIRING_GEOMETRICAL_3, DEV_TOF_FIRING_GEOMETRICAL_1, DEV_TOF_FIRING_GEOMETRICAL_5, DEV_TOF_FIRING_GEOMETRICAL_2, DEV_TOF_FIRING_GEOMETRICAL_4,
DEV_TOF_FIRING_GEOMETRICAL_8, DEV_TOF_FIRING_GEOMETRICAL_6, DEV_TOF_FIRING_GEOMETRICAL_10, DEV_TOF_FIRING_GEOMETRICAL_7, DEV_TOF_FIRING_GEOMETRICAL_9,
DEV_TOF_FIRING_GEOMETRICAL_13, DEV_TOF_FIRING_GEOMETRICAL_11, DEV_TOF_FIRING_GEOMETRICAL_15, DEV_TOF_FIRING_GEOMETRICAL_12, DEV_TOF_FIRING_GEOMETRICAL_14,
/* DEV_TOF_LIDAR_C */ DEV_TOF_FIRING_GEOMETRICAL_8, DEV_TOF_FIRING_GEOMETRICAL_6, DEV_TOF_FIRING_GEOMETRICAL_10, DEV_TOF_FIRING_GEOMETRICAL_7, DEV_TOF_FIRING_GEOMETRICAL_9,
/* DEV_TOF_LIDAR_L */ DEV_TOF_FIRING_GEOMETRICAL_13, DEV_TOF_FIRING_GEOMETRICAL_11, DEV_TOF_FIRING_GEOMETRICAL_15, DEV_TOF_FIRING_GEOMETRICAL_12, DEV_TOF_FIRING_GEOMETRICAL_14,
/* DEV_TOF_LIDAR_R */ DEV_TOF_FIRING_GEOMETRICAL_3, DEV_TOF_FIRING_GEOMETRICAL_1, DEV_TOF_FIRING_GEOMETRICAL_5, DEV_TOF_FIRING_GEOMETRICAL_2, DEV_TOF_FIRING_GEOMETRICAL_4,
},
.prev_firingframe = {
DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
/* DEV_TOF_LIDAR_C */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
/* DEV_TOF_LIDAR_L */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
/* DEV_TOF_LIDAR_R */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
},
.mp_mutex = xSemaphoreCreateBinary(),
.mp_data ={
Expand Down Expand Up @@ -141,40 +148,43 @@ void dev_ToF_reset_all_sensors(void)
// int16_t offset;

// start ToFs
for (int sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
for (int sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
{
sensor = & (lidar_data.tofs[sensor_id]);
if (sensor->begin() == true)
const int8_t begin_status = sensor->begin();
if (begin_status == VL53L1X_ERROR_NONE)
{
PRINTF("%d Sensor online!\n", sensor_id);
PRINTF("[ DEV:ToF ] Sensor[%d] online! [Code:%d]\n", sensor_id, begin_status);
}
else
{
PRINTF("%d Sensor offline!\n", sensor_id);
PRINTF("[ DEV:ToF ] Sensor[%d] offline! [Code:%d]\n", sensor_id, begin_status);
}

// Take down all sensors
sensor->sensorOff();
}

// Take down all sensors : DEFAULT: off on AVR
dev_avr_driver_set_req_Tof_config(TOF_SENSOR_CONFIG_DISABLE_ALL);
dev_driver_avr_update20ms(); // Force update
delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);

// read sensor initial values & set address & activate sensor
for (int sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
for (int sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
{
sensor = & (lidar_data.tofs[sensor_id]);

// turn on sensor
sensor->sensorOn();
int boot = sensor->checkBootState();
// turn on sensor | WARNING: Do not call: sensor->sensorOn();
dev_avr_driver_set_req_Tof_config(lidar_data.avr_config[sensor_id]);
dev_driver_avr_update20ms(); // Force update
delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);

const int8_t boot = sensor->checkBootState();

// change address
sensor->setI2CAddress(lidar_data.address[sensor_id]);

PRINTF("%d [#%d] Sensor Boot Code: %d \n", sensor_id, lidar_data.address[sensor_id], boot);
PRINTF("[ DEV:ToF ] Sensor[%d] [#%d] Boot Code: %d \n", sensor_id, lidar_data.address[sensor_id], boot);

// activate sensor
sensor->init();
const int8_t status = sensor->init(); // TODO: get if successful
PRINTF("[ DEV:ToF ] Sensor[%d] [#%d] Init Code: %d \n", sensor_id, lidar_data.address[sensor_id], status);

// set initial values
sensor->setDistanceModeShort();
Expand All @@ -188,6 +198,7 @@ void dev_ToF_reset_all_sensors(void)
// begin firing
sensor->startRanging();
delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);
sensor->clearInterrupt();
}
}

Expand All @@ -196,8 +207,11 @@ void dev_ToF_reset_all_sensors(void)
///////////////////////////////////////
void dev_ToF_Lidar_init(void)
{
// release semaphore
xSemaphoreGive(lidar_data.mp_mutex);

// start tof I2C
lidar_data.I2C.begin(TOF_I2C_SDA, TOF_I2C_SCL);
lidar_data.I2C.begin(TOF_I2C_SDA, TOF_I2C_SCL, TOF_I2C_FREQ);

// reset ToF
dev_ToF_reset_all_sensors();
Expand All @@ -216,9 +230,9 @@ void dev_ToF_Lidar_update20ms(void)
uint8_t firing_frame;
uint8_t firing_frame_new;
uint8_t geo_label;

// firing
for (uint8_t sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
for (uint8_t sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
{
sensor = & (lidar_data.tofs[sensor_id]);
// if data ready
Expand All @@ -237,7 +251,7 @@ void dev_ToF_Lidar_update20ms(void)
}

# if (DEBUG_FPRINT_FEATURE_LIDAR)
PRINTF("%d Sensor: %3d [mm] F:[%d] Label:(%2d) Status:(%d) \n", sensor_id, dist_mm, firing_frame, lidar_data.firing_sequence_label[sensor_id][firing_frame], error);
PRINTF("[ DEV:TOF ] Sensor[%d]: %3d [mm] F:[%d] Label:(%2d) Status:(%d) \n", sensor_id, dist_mm, firing_frame, lidar_data.firing_sequence_label[sensor_id][firing_frame], error);
# endif
// update new firing pattern
if (sensor->setCenter(lidar_data.firing_sequence[firing_frame_new]) == VL53L1_ERROR_NONE)
Expand All @@ -247,7 +261,7 @@ void dev_ToF_Lidar_update20ms(void)
sensor->clearInterrupt();

// store data
if (error == VL53L1_ERROR_NONE)
if ((error == DEV_TOF_RANGE_STATUS_SIGNAL_FAILURE) || (error == DEV_TOF_RANGE_STATUS_NO_ERROR))
{
geo_label = lidar_data.firing_sequence_label[sensor_id][firing_frame];
if (xSemaphoreTake(lidar_data.mp_mutex, MP_MUTEX_BLOCK_TIME_MS) == pdTRUE) {
Expand Down
31 changes: 29 additions & 2 deletions Firmware/ESP32/Arduino_platform/lib/DEV/dev_ToF_Lidar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,9 @@ typedef enum{
} DEV_TOF_FIRING_KEYFRAME_E;

typedef enum{
DEV_TOF_LIDAR_R = (0U),
DEV_TOF_LIDAR_C,
DEV_TOF_LIDAR_C = (0U),
DEV_TOF_LIDAR_L,
DEV_TOF_LIDAR_R,
DEV_TOF_LIDAR_COUNT,
DEV_TOF_LIDAR_UNKNOWN
} DEV_TOF_LIDAR_E;
Expand All @@ -57,6 +57,33 @@ typedef enum{ // Geometrical Region 1 -> 15
DEV_TOF_FIRING_GEOMETRICAL_UNKNOWN,
} DEV_TOF_FIRING_GEOMETRICAL_REGION_E;

/**
* @brief There are five range statuses: 0, 1, 2, 4, and 7.
* Range status is 0 => no error.
* Range status 1 and 2 => error warnings
* Range status 4 and 7 => errors.
* @details Range status:
* 1: (sigma failure) => repeatability or standard deviation of the measurement is bad due to a decreasing signal noise ratio.
* Increasing the timing budget can improve the standard deviation and avoid a range status 1.
* 2: (signal failure)=> This means that the return signal is too week to return a good answer.
* The reason is because the target is too far, or the target is not reflective enough, or the target is too small.
* Increasing the timing budget might help, but there may simply be no target available.
* 4: (out of bounds)=> This means that the sensor is ranging in a “nonappropriated” zone and the measured result may be inconsistent.
* This status is considered as a warning but, in general, it happens when a target is at the maximum distance possible from the sensor, i.e. around 5 m.
* However, this is only for very bright targets.
* 7: (wraparound)=> This situation may occur when the target is very reflective and the distance to the target/sensor is longer than the physical limited distance measurable by the sensor.
* Such distances include approximately 5 m when the senor is in Long distance mode and approximately 1.3 m when the sensor is in Short distance mode.
* Example: a traffic sign located at 6 m can be seen by the sensor and returns a range of 1 m. This is due to “radar aliasing”:
* if only an approximate distance is required, we may add 6 m to the distance returned. However, that is a very approximate estimation.
*/

typedef enum{
DEV_TOF_RANGE_STATUS_NO_ERROR = 0,
DEV_TOF_RANGE_STATUS_SIGMA_FAILURE = 1,
DEV_TOF_RANGE_STATUS_SIGNAL_FAILURE = 2,
DEV_TOF_RANGE_STATUS_OUT_OF_BOUNDS_FAILURE = 4,
DEV_TOF_RANGE_STATUS_WRAPAROUND_FAILURE = 7,
} DEV_TOF_RANGE_STATUS_E;

#define DEV_TOF_TOTAL_POINTS_PER_SCAN (DEV_TOF_LIDAR_COUNT * DEV_TOF_FIRING_KEYFRAME_COUNT)
#define DEV_TOF_BUFFER_SCALE (2U) // 2x buffer shall be enough to cover
Expand Down
Loading

0 comments on commit 1716937

Please sign in to comment.