导航部分效果最好的版本，主分支
This commit is contained in:
483
App/Can/snc_can_app.c
Normal file
483
App/Can/snc_can_app.c
Normal file
@@ -0,0 +1,483 @@
|
||||
#include "snc_can_app.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
|
||||
/* ========================= 外部句柄 ========================= */
|
||||
extern FDCAN_HandleTypeDef hfdcan1;
|
||||
|
||||
/* ========================= 在线判定参数 ========================= */
|
||||
#define SNC_STATUS_TIMEOUT_MS 200U
|
||||
#define SNC_ACTUAL_RPM_TIMEOUT_MS 200U
|
||||
#define SNC_TARGET_RPM_TIMEOUT_MS 200U
|
||||
#define SNC_COMM_DIAG_TIMEOUT_MS 300U
|
||||
|
||||
/* ========================= FDCAN 过滤器索引 ========================= */
|
||||
#define SNC_FILTER_INDEX_STATUS 0U
|
||||
#define SNC_FILTER_INDEX_ACTUAL_RPM 1U
|
||||
#define SNC_FILTER_INDEX_TARGET_RPM 2U
|
||||
#define SNC_FILTER_INDEX_COMM_DIAG 3U
|
||||
#define SNC_FILTER_INDEX_ODOM 4U
|
||||
|
||||
SNC_CAN_AppContext_t g_snc_can_app;
|
||||
|
||||
/* ========================= CRC8-SAE J1850 查表 ========================= */
|
||||
static const uint8_t s_crc8_j1850_table[256] =
|
||||
{
|
||||
0x00U, 0x1DU, 0x3AU, 0x27U, 0x74U, 0x69U, 0x4EU, 0x53U, 0xE8U, 0xF5U, 0xD2U, 0xCFU, 0x9CU, 0x81U, 0xA6U, 0xBBU,
|
||||
0xCDU, 0xD0U, 0xF7U, 0xEAU, 0xB9U, 0xA4U, 0x83U, 0x9EU, 0x25U, 0x38U, 0x1FU, 0x02U, 0x51U, 0x4CU, 0x6BU, 0x76U,
|
||||
0x87U, 0x9AU, 0xBDU, 0xA0U, 0xF3U, 0xEEU, 0xC9U, 0xD4U, 0x6FU, 0x72U, 0x55U, 0x48U, 0x1BU, 0x06U, 0x21U, 0x3CU,
|
||||
0x4AU, 0x57U, 0x70U, 0x6DU, 0x3EU, 0x23U, 0x04U, 0x19U, 0xA2U, 0xBFU, 0x98U, 0x85U, 0xD6U, 0xCBU, 0xECU, 0xF1U,
|
||||
0x13U, 0x0EU, 0x29U, 0x34U, 0x67U, 0x7AU, 0x5DU, 0x40U, 0xFBU, 0xE6U, 0xC1U, 0xDCU, 0x8FU, 0x92U, 0xB5U, 0xA8U,
|
||||
0xDEU, 0xC3U, 0xE4U, 0xF9U, 0xAAU, 0xB7U, 0x90U, 0x8DU, 0x36U, 0x2BU, 0x0CU, 0x11U, 0x42U, 0x5FU, 0x78U, 0x65U,
|
||||
0x94U, 0x89U, 0xAEU, 0xB3U, 0xE0U, 0xFDU, 0xDAU, 0xC7U, 0x7CU, 0x61U, 0x46U, 0x5BU, 0x08U, 0x15U, 0x32U, 0x2FU,
|
||||
0x59U, 0x44U, 0x63U, 0x7EU, 0x2DU, 0x30U, 0x17U, 0x0AU, 0xB1U, 0xACU, 0x8BU, 0x96U, 0xC5U, 0xD8U, 0xFFU, 0xE2U,
|
||||
0x26U, 0x3BU, 0x1CU, 0x01U, 0x52U, 0x4FU, 0x68U, 0x75U, 0xCEU, 0xD3U, 0xF4U, 0xE9U, 0xBAU, 0xA7U, 0x80U, 0x9DU,
|
||||
0xEBU, 0xF6U, 0xD1U, 0xCCU, 0x9FU, 0x82U, 0xA5U, 0xB8U, 0x03U, 0x1EU, 0x39U, 0x24U, 0x77U, 0x6AU, 0x4DU, 0x50U,
|
||||
0xA1U, 0xBCU, 0x9BU, 0x86U, 0xD5U, 0xC8U, 0xEFU, 0xF2U, 0x49U, 0x54U, 0x73U, 0x6EU, 0x3DU, 0x20U, 0x07U, 0x1AU,
|
||||
0x6CU, 0x71U, 0x56U, 0x4BU, 0x18U, 0x05U, 0x22U, 0x3FU, 0x84U, 0x99U, 0xBEU, 0xA3U, 0xF0U, 0xEDU, 0xCAU, 0xD7U,
|
||||
0x35U, 0x28U, 0x0FU, 0x12U, 0x41U, 0x5CU, 0x7BU, 0x66U, 0xDDU, 0xC0U, 0xE7U, 0xFAU, 0xA9U, 0xB4U, 0x93U, 0x8EU,
|
||||
0xF8U, 0xE5U, 0xC2U, 0xDFU, 0x8CU, 0x91U, 0xB6U, 0xABU, 0x10U, 0x0DU, 0x2AU, 0x37U, 0x64U, 0x79U, 0x5EU, 0x43U,
|
||||
0xB2U, 0xAFU, 0x88U, 0x95U, 0xC6U, 0xDBU, 0xFCU, 0xE1U, 0x5AU, 0x47U, 0x60U, 0x7DU, 0x2EU, 0x33U, 0x14U, 0x09U,
|
||||
0x7FU, 0x62U, 0x45U, 0x58U, 0x0BU, 0x16U, 0x31U, 0x2CU, 0x97U, 0x8AU, 0xADU, 0xB0U, 0xE3U, 0xFEU, 0xD9U, 0xC4U
|
||||
};
|
||||
|
||||
static inline int16_t snc_read_i16_le(uint8_t lo, uint8_t hi)
|
||||
{
|
||||
return (int16_t)((uint16_t)lo | ((uint16_t)hi << 8));
|
||||
}
|
||||
|
||||
static inline void snc_write_i16_le(uint8_t *dst, int16_t val)
|
||||
{
|
||||
dst[0] = (uint8_t)(val & 0xFF);
|
||||
dst[1] = (uint8_t)((uint16_t)val >> 8);
|
||||
}
|
||||
|
||||
static int16_t snc_saturate_float_to_i16(float x)
|
||||
{
|
||||
if (x > 32767.0f) return 32767;
|
||||
if (x < -32768.0f) return -32768;
|
||||
|
||||
// 手动实现四舍五入，避免调用 lroundf 库函数
|
||||
return (int16_t)(x >= 0.0f ? (x + 0.5f) : (x - 0.5f));
|
||||
}
|
||||
|
||||
uint8_t SNC_CAN_Crc8J1850(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
uint8_t crc = 0xFFU;
|
||||
|
||||
while (len-- > 0U)
|
||||
{
|
||||
crc = s_crc8_j1850_table[(uint8_t)(crc ^ *data++)];
|
||||
}
|
||||
|
||||
return (uint8_t)(crc ^ 0xFFU);
|
||||
}
|
||||
|
||||
static uint32_t snc_fdcan_dlc_from_bytes(uint8_t dlc_bytes)
|
||||
{
|
||||
switch (dlc_bytes)
|
||||
{
|
||||
case 0: return FDCAN_DLC_BYTES_0;
|
||||
case 1: return FDCAN_DLC_BYTES_1;
|
||||
case 2: return FDCAN_DLC_BYTES_2;
|
||||
case 3: return FDCAN_DLC_BYTES_3;
|
||||
case 4: return FDCAN_DLC_BYTES_4;
|
||||
case 5: return FDCAN_DLC_BYTES_5;
|
||||
case 6: return FDCAN_DLC_BYTES_6;
|
||||
case 7: return FDCAN_DLC_BYTES_7;
|
||||
case 8: return FDCAN_DLC_BYTES_8;
|
||||
default: return FDCAN_DLC_BYTES_8;
|
||||
}
|
||||
}
|
||||
|
||||
static HAL_StatusTypeDef snc_fdcan_add_tx_std(uint16_t std_id, const uint8_t *data, uint8_t dlc_bytes)
|
||||
{
|
||||
FDCAN_TxHeaderTypeDef txHeader;
|
||||
|
||||
memset(&txHeader, 0, sizeof(txHeader));
|
||||
|
||||
txHeader.Identifier = std_id;
|
||||
txHeader.IdType = FDCAN_STANDARD_ID;
|
||||
txHeader.TxFrameType = FDCAN_DATA_FRAME;
|
||||
txHeader.DataLength = snc_fdcan_dlc_from_bytes(dlc_bytes);
|
||||
txHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
|
||||
txHeader.BitRateSwitch = FDCAN_BRS_OFF;
|
||||
txHeader.FDFormat = FDCAN_CLASSIC_CAN;
|
||||
txHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
|
||||
txHeader.MessageMarker = 0U;
|
||||
|
||||
/* 先判断 TX FIFO/Queue 是否有空位，避免异常状态下继续硬塞 */
|
||||
if (HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) == 0U)
|
||||
{
|
||||
return HAL_BUSY;
|
||||
}
|
||||
|
||||
return HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &txHeader, (uint8_t *)data);
|
||||
}
|
||||
|
||||
static void snc_fdcan_config_filter(uint32_t index, uint16_t std_id)
|
||||
{
|
||||
FDCAN_FilterTypeDef sFilter;
|
||||
|
||||
sFilter.IdType = FDCAN_STANDARD_ID;
|
||||
sFilter.FilterIndex = index;
|
||||
sFilter.FilterType = FDCAN_FILTER_MASK;
|
||||
sFilter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
|
||||
sFilter.FilterID1 = std_id;
|
||||
sFilter.FilterID2 = 0x7FFU; /* 完全匹配 11-bit ID */
|
||||
|
||||
if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilter) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
}
|
||||
|
||||
static void snc_parse_status(const uint8_t *d)
|
||||
{
|
||||
g_snc_can_app.status.state = d[0];
|
||||
g_snc_can_app.status.health = d[1];
|
||||
g_snc_can_app.status.diag_bits =
|
||||
((uint32_t)d[2]) |
|
||||
((uint32_t)d[3] << 8) |
|
||||
((uint32_t)d[4] << 16) |
|
||||
((uint32_t)d[5] << 24);
|
||||
g_snc_can_app.status.cmd_age_10ms = d[6];
|
||||
g_snc_can_app.status.status_counter = d[7];
|
||||
g_snc_can_app.status.last_update_ms = HAL_GetTick();
|
||||
g_snc_can_app.status.online = true;
|
||||
}
|
||||
|
||||
static void snc_parse_actual_rpm(const uint8_t *d)
|
||||
{
|
||||
g_snc_can_app.actual_rpm.fl = snc_read_i16_le(d[0], d[1]);
|
||||
g_snc_can_app.actual_rpm.rl = snc_read_i16_le(d[2], d[3]);
|
||||
g_snc_can_app.actual_rpm.fr = snc_read_i16_le(d[4], d[5]);
|
||||
g_snc_can_app.actual_rpm.rr = snc_read_i16_le(d[6], d[7]);
|
||||
g_snc_can_app.actual_rpm.last_update_ms = HAL_GetTick();
|
||||
}
|
||||
|
||||
static void snc_parse_target_rpm(const uint8_t *d)
|
||||
{
|
||||
g_snc_can_app.target_rpm.fl = snc_read_i16_le(d[0], d[1]);
|
||||
g_snc_can_app.target_rpm.rl = snc_read_i16_le(d[2], d[3]);
|
||||
g_snc_can_app.target_rpm.fr = snc_read_i16_le(d[4], d[5]);
|
||||
g_snc_can_app.target_rpm.rr = snc_read_i16_le(d[6], d[7]);
|
||||
g_snc_can_app.target_rpm.last_update_ms = HAL_GetTick();
|
||||
}
|
||||
|
||||
static void snc_parse_comm_diag(const uint8_t *d)
|
||||
{
|
||||
g_snc_can_app.comm_diag.valid_cmd_total_lsb = d[0];
|
||||
g_snc_can_app.comm_diag.crc_error_total_lsb = d[1];
|
||||
g_snc_can_app.comm_diag.counter_reject_total_lsb = d[2];
|
||||
g_snc_can_app.comm_diag.can_tx_drop_total_lsb = d[3];
|
||||
g_snc_can_app.comm_diag.busoff_total_lsb = d[4];
|
||||
g_snc_can_app.comm_diag.rx_overrun_total_lsb = d[5];
|
||||
g_snc_can_app.comm_diag.last_accepted_counter = d[6];
|
||||
g_snc_can_app.comm_diag.consecutive_counter_errors = (uint8_t)((d[7] >> 4) & 0x0F);
|
||||
g_snc_can_app.comm_diag.consecutive_crc_errors = (uint8_t)(d[7] & 0x0F);
|
||||
g_snc_can_app.comm_diag.last_update_ms = HAL_GetTick();
|
||||
}
|
||||
|
||||
static void snc_parse_odom_delta(const uint8_t *d)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
uint32_t prev_update_ms = g_snc_can_app.odom_delta.last_update_ms;
|
||||
|
||||
/* 保留最近一帧快照（兼容性，调试用） */
|
||||
g_snc_can_app.odom_delta.fl_delta = snc_read_i16_le(d[0], d[1]);
|
||||
g_snc_can_app.odom_delta.rl_delta = snc_read_i16_le(d[2], d[3]);
|
||||
g_snc_can_app.odom_delta.fr_delta = snc_read_i16_le(d[4], d[5]);
|
||||
g_snc_can_app.odom_delta.rr_delta = snc_read_i16_le(d[6], d[7]);
|
||||
g_snc_can_app.odom_delta.last_update_ms = now;
|
||||
|
||||
/* ---- 累加到待消费缓冲区，解决漏积分问题 ---- */
|
||||
SNC_OdomDeltaAccum_t *acc = &g_snc_can_app.odom_accum;
|
||||
|
||||
acc->fl_accum += (int32_t)snc_read_i16_le(d[0], d[1]);
|
||||
acc->rl_accum += (int32_t)snc_read_i16_le(d[2], d[3]);
|
||||
acc->fr_accum += (int32_t)snc_read_i16_le(d[4], d[5]);
|
||||
acc->rr_accum += (int32_t)snc_read_i16_le(d[6], d[7]);
|
||||
|
||||
if (acc->frame_count == 0U) {
|
||||
acc->first_frame_ms = now;
|
||||
}
|
||||
acc->last_frame_ms = now;
|
||||
|
||||
/*
|
||||
* 累加增量真实覆盖的时间窗。
|
||||
* 每个 0x200 帧表示“自上一帧以来”的编码器增量，
|
||||
* 因此应累加相邻帧之间的时间差，而不是简单用 last-first。
|
||||
*/
|
||||
if (prev_update_ms != 0U) {
|
||||
uint32_t frame_dt_ms = now - prev_update_ms;
|
||||
if (frame_dt_ms <= 1000U) {
|
||||
acc->span_ms += frame_dt_ms;
|
||||
}
|
||||
}
|
||||
|
||||
if (acc->frame_count < 255U) {
|
||||
acc->frame_count++;
|
||||
}
|
||||
}
|
||||
|
||||
void SNC_CAN_AppInit(void)
|
||||
{
|
||||
memset(&g_snc_can_app, 0, sizeof(g_snc_can_app));
|
||||
|
||||
/* 配置 5 个标准滤波器，只接收协议相关 ID */
|
||||
snc_fdcan_config_filter(SNC_FILTER_INDEX_STATUS, SNC_CAN_ID_STATUS);
|
||||
snc_fdcan_config_filter(SNC_FILTER_INDEX_ACTUAL_RPM, SNC_CAN_ID_ACTUAL_RPM);
|
||||
snc_fdcan_config_filter(SNC_FILTER_INDEX_TARGET_RPM, SNC_CAN_ID_TARGET_RPM);
|
||||
snc_fdcan_config_filter(SNC_FILTER_INDEX_COMM_DIAG, SNC_CAN_ID_COMM_DIAG);
|
||||
snc_fdcan_config_filter(SNC_FILTER_INDEX_ODOM, SNC_CAN_ID_ODOM);
|
||||
|
||||
/* 其余标准帧/扩展帧全部拒收 */
|
||||
if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,
|
||||
FDCAN_REJECT,
|
||||
FDCAN_REJECT,
|
||||
FDCAN_REJECT_REMOTE,
|
||||
FDCAN_REJECT_REMOTE) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
|
||||
if (HAL_FDCAN_ActivateNotification(&hfdcan1,
|
||||
FDCAN_IT_RX_FIFO0_NEW_MESSAGE,
|
||||
0U) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void)
|
||||
{
|
||||
uint8_t data[0];
|
||||
HAL_StatusTypeDef ret = snc_fdcan_add_tx_std(SNC_CAN_ID_HEARTBEAT, data, 0U);
|
||||
|
||||
if (ret == HAL_OK)
|
||||
{
|
||||
g_snc_can_app.tx_total++;
|
||||
}
|
||||
else
|
||||
{
|
||||
g_snc_can_app.tx_fail_total++;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef SNC_CAN_SendCmdVel(float vx_mps, float wz_radps, uint8_t ctrl_flags)
|
||||
{
|
||||
uint8_t data[8];
|
||||
int16_t vx_i16 = snc_saturate_float_to_i16(vx_mps * 1000.0f);
|
||||
int16_t wz_i16 = snc_saturate_float_to_i16(wz_radps * 1000.0f);
|
||||
HAL_StatusTypeDef ret;
|
||||
|
||||
g_snc_can_app.tx_counter = (uint8_t)(g_snc_can_app.tx_counter + 1U);
|
||||
g_snc_can_app.tx_ctrl_flags = ctrl_flags;
|
||||
|
||||
snc_write_i16_le(&data[0], vx_i16);
|
||||
snc_write_i16_le(&data[2], wz_i16);
|
||||
data[4] = ctrl_flags;
|
||||
data[5] = 0U;
|
||||
data[6] = g_snc_can_app.tx_counter;
|
||||
data[7] = SNC_CAN_Crc8J1850(data, 7U);
|
||||
|
||||
ret = snc_fdcan_add_tx_std(SNC_CAN_ID_CMD_VEL, data, 8U);
|
||||
|
||||
if (ret == HAL_OK)
|
||||
{
|
||||
g_snc_can_app.tx_total++;
|
||||
}
|
||||
else
|
||||
{
|
||||
g_snc_can_app.tx_fail_total++;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void SNC_CAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rxFifo0ITs)
|
||||
{
|
||||
FDCAN_RxHeaderTypeDef rxHeader;
|
||||
uint8_t data[8];
|
||||
|
||||
if (hfdcan != &hfdcan1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if ((rxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0U)
|
||||
{
|
||||
if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &rxHeader, data) != HAL_OK)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if ((rxHeader.IdType != FDCAN_STANDARD_ID) ||
|
||||
(rxHeader.RxFrameType != FDCAN_DATA_FRAME))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
g_snc_can_app.rx_total++;
|
||||
|
||||
switch (rxHeader.Identifier)
|
||||
{
|
||||
case SNC_CAN_ID_STATUS:
|
||||
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
|
||||
{
|
||||
snc_parse_status(data);
|
||||
}
|
||||
break;
|
||||
|
||||
case SNC_CAN_ID_ACTUAL_RPM:
|
||||
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
|
||||
{
|
||||
snc_parse_actual_rpm(data);
|
||||
}
|
||||
break;
|
||||
|
||||
case SNC_CAN_ID_TARGET_RPM:
|
||||
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
|
||||
{
|
||||
snc_parse_target_rpm(data);
|
||||
}
|
||||
break;
|
||||
|
||||
case SNC_CAN_ID_COMM_DIAG:
|
||||
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
|
||||
{
|
||||
snc_parse_comm_diag(data);
|
||||
}
|
||||
break;
|
||||
|
||||
case SNC_CAN_ID_ODOM:
|
||||
if (rxHeader.DataLength == FDCAN_DLC_BYTES_8)
|
||||
{
|
||||
snc_parse_odom_delta(data);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SNC_CAN_20msTask(void)
|
||||
{
|
||||
/* 这里默认发心跳。
|
||||
速度命令建议由你的上层控制逻辑每 20ms 调 SNC_CAN_SendCmdVel()。 */
|
||||
(void)SNC_CAN_SendHeartbeat();
|
||||
}
|
||||
|
||||
void SNC_CAN_100msTask(void)
|
||||
{
|
||||
/* 使用静态变量记录上一次的接收总数 */
|
||||
static uint32_t last_rx_total = 0;
|
||||
|
||||
/* 如果当前总数和上次记录的总数不一致，说明这 100ms 内成功收到了新消息 */
|
||||
if (g_snc_can_app.rx_total != last_rx_total)
|
||||
{
|
||||
last_rx_total = g_snc_can_app.rx_total;
|
||||
|
||||
// 翻转 PE2 电平，产生肉眼可见的闪烁效果
|
||||
HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_3);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 如果 100ms 内没有收到任何新消息（CAN 断开或下位机死机）
|
||||
强制关闭 LED。
|
||||
注意：这里假设高电平 (SET) 是熄灭。如果是低电平熄灭，请改为 GPIO_PIN_RESET */
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_3, GPIO_PIN_SET);
|
||||
}
|
||||
}
|
||||
|
||||
void SNC_CAN_PollOnlineState(uint32_t now_ms)
|
||||
{
|
||||
if ((now_ms - g_snc_can_app.status.last_update_ms) > SNC_STATUS_TIMEOUT_MS)
|
||||
{
|
||||
g_snc_can_app.status.online = false;
|
||||
}
|
||||
}
|
||||
|
||||
const SNC_CAN_AppContext_t *SNC_CAN_GetContext(void)
|
||||
{
|
||||
return &g_snc_can_app;
|
||||
}
|
||||
|
||||
uint8_t SNC_CAN_ConsumeOdomDelta(int16_t *fl, int16_t *rl,
|
||||
int16_t *fr, int16_t *rr,
|
||||
uint32_t *dt_ms)
|
||||
{
|
||||
/*
|
||||
* 原子取走累加器内容并清零。
|
||||
*
|
||||
* 本函数由 monitorTask (普通任务上下文) 调用，
|
||||
* 而写端 snc_parse_odom_delta() 运行在 CAN Rx ISR 中。
|
||||
*
|
||||
* 用 taskENTER_CRITICAL / taskEXIT_CRITICAL 保证取走 + 清零
|
||||
* 的原子性：临界区会关中断，防止 ISR 在取走过程中写入新增量。
|
||||
*/
|
||||
SNC_OdomDeltaAccum_t snapshot;
|
||||
uint8_t count;
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
{
|
||||
snapshot = g_snc_can_app.odom_accum; /* 结构体拷贝 */
|
||||
/* 清零累加器，等待下一批帧 */
|
||||
g_snc_can_app.odom_accum.fl_accum = 0;
|
||||
g_snc_can_app.odom_accum.rl_accum = 0;
|
||||
g_snc_can_app.odom_accum.fr_accum = 0;
|
||||
g_snc_can_app.odom_accum.rr_accum = 0;
|
||||
g_snc_can_app.odom_accum.first_frame_ms = 0U;
|
||||
g_snc_can_app.odom_accum.last_frame_ms = 0U;
|
||||
g_snc_can_app.odom_accum.span_ms = 0U;
|
||||
g_snc_can_app.odom_accum.frame_count = 0U;
|
||||
}
|
||||
taskEXIT_CRITICAL();
|
||||
|
||||
count = snapshot.frame_count;
|
||||
|
||||
if (count == 0U) {
|
||||
/* 期间没有新帧到达 */
|
||||
*fl = 0; *rl = 0; *fr = 0; *rr = 0;
|
||||
*dt_ms = 0U;
|
||||
return 0U;
|
||||
}
|
||||
|
||||
/* int32 → int16 饱和截断（正常工况下不会溢出） */
|
||||
*fl = (int16_t)((snapshot.fl_accum > 32767) ? 32767 :
|
||||
(snapshot.fl_accum < -32768) ? -32768 : snapshot.fl_accum);
|
||||
*rl = (int16_t)((snapshot.rl_accum > 32767) ? 32767 :
|
||||
(snapshot.rl_accum < -32768) ? -32768 : snapshot.rl_accum);
|
||||
*fr = (int16_t)((snapshot.fr_accum > 32767) ? 32767 :
|
||||
(snapshot.fr_accum < -32768) ? -32768 : snapshot.fr_accum);
|
||||
*rr = (int16_t)((snapshot.rr_accum > 32767) ? 32767 :
|
||||
(snapshot.rr_accum < -32768) ? -32768 : snapshot.rr_accum);
|
||||
|
||||
/* 实际累计时间窗 */
|
||||
if (snapshot.span_ms == 0U) {
|
||||
/* 首帧或刚恢复时无法可靠计算，返回 0 让调用方使用保底值 */
|
||||
*dt_ms = 0U;
|
||||
} else {
|
||||
*dt_ms = snapshot.span_ms;
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
183
App/Can/snc_can_app.h
Normal file
183
App/Can/snc_can_app.h
Normal file
@@ -0,0 +1,183 @@
|
||||
#ifndef __SNC_CAN_APP_H
|
||||
#define __SNC_CAN_APP_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "fdcan.h"
|
||||
|
||||
#define SNC_ODOM_TIMEOUT_MS 300U
|
||||
|
||||
/* ========================= 协议 CAN ID ========================= */
|
||||
#define SNC_CAN_ID_HEARTBEAT 0x080U
|
||||
#define SNC_CAN_ID_CMD_VEL 0x100U
|
||||
|
||||
#define SNC_CAN_ID_STATUS 0x181U
|
||||
#define SNC_CAN_ID_ACTUAL_RPM 0x182U
|
||||
#define SNC_CAN_ID_TARGET_RPM 0x183U
|
||||
#define SNC_CAN_ID_COMM_DIAG 0x184U
|
||||
#define SNC_CAN_ID_ODOM 0x200U
|
||||
|
||||
/* ========================= 系统状态定义 ========================= */
|
||||
typedef enum
|
||||
{
|
||||
SNC_SYSTEM_BOOTING = 0,
|
||||
SNC_SYSTEM_OPERATIONAL = 1,
|
||||
SNC_SYSTEM_SAFE_FAULT = 2
|
||||
} SNC_SystemState_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SNC_SYSTEM_HEALTH_OK = 0,
|
||||
SNC_SYSTEM_HEALTH_WARNING = 1,
|
||||
SNC_SYSTEM_HEALTH_FAULT = 2
|
||||
} SNC_SystemHealth_t;
|
||||
|
||||
/* ========================= 诊断位定义 ========================= */
|
||||
typedef enum
|
||||
{
|
||||
SNC_DIAG_COMM_TIMEOUT = (1UL << 0),
|
||||
SNC_DIAG_CAN_BUS_OFF = (1UL << 1),
|
||||
SNC_DIAG_CMD_CRC_STORM = (1UL << 2),
|
||||
SNC_DIAG_CMD_CNT_STORM = (1UL << 3),
|
||||
SNC_DIAG_MOTOR_FL_STALL = (1UL << 4),
|
||||
SNC_DIAG_MOTOR_RL_STALL = (1UL << 5),
|
||||
SNC_DIAG_MOTOR_FR_STALL = (1UL << 6),
|
||||
SNC_DIAG_MOTOR_RR_STALL = (1UL << 7),
|
||||
SNC_DIAG_CONTROL_SATURATION = (1UL << 8)
|
||||
} SNC_DiagBits_t;
|
||||
|
||||
/* ========================= 下位机状态快照 ========================= */
|
||||
typedef struct
|
||||
{
|
||||
uint8_t state;
|
||||
uint8_t health;
|
||||
uint32_t diag_bits;
|
||||
uint8_t cmd_age_10ms;
|
||||
uint8_t status_counter;
|
||||
uint32_t last_update_ms;
|
||||
bool online;
|
||||
} SNC_ChassisStatus_t;
|
||||
|
||||
/* ========================= 轮速数据 ========================= */
|
||||
typedef struct
|
||||
{
|
||||
int16_t fl;
|
||||
int16_t rl;
|
||||
int16_t fr;
|
||||
int16_t rr;
|
||||
uint32_t last_update_ms;
|
||||
} SNC_WheelRpmFrame_t;
|
||||
|
||||
/* ========================= 里程增量 ========================= */
|
||||
typedef struct
|
||||
{
|
||||
int16_t fl_delta;
|
||||
int16_t rl_delta;
|
||||
int16_t fr_delta;
|
||||
int16_t rr_delta;
|
||||
uint32_t last_update_ms;
|
||||
} SNC_OdomDeltaFrame_t;
|
||||
|
||||
/**
|
||||
* @brief 里程增量累加器
|
||||
*
|
||||
* 解决 0x200 增量帧的"漏积分/重复积分"问题：
|
||||
* - ISR 收到每帧 0x200 时，将增量**累加**到此结构体（不覆盖）
|
||||
* - 消费者调用 SNC_CAN_ConsumeOdomDelta() 原子取走累计值并清零
|
||||
*
|
||||
* 这样即使消费者频率 (100ms) 低于帧到达频率 (~60ms)，
|
||||
* 也不会丢失任何增量；也不会重复消费同一份增量。
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
int32_t fl_accum; /**< 左前轮累计增量 ticks (int32 防溢出) */
|
||||
int32_t rl_accum; /**< 左后轮累计增量 ticks */
|
||||
int32_t fr_accum; /**< 右前轮累计增量 ticks */
|
||||
int32_t rr_accum; /**< 右后轮累计增量 ticks */
|
||||
uint32_t first_frame_ms; /**< 本次累加窗口内第一帧的时间戳 [ms] */
|
||||
uint32_t last_frame_ms; /**< 本次累加窗口内最后一帧的时间戳 [ms] */
|
||||
uint32_t span_ms; /**< 本次累计增量实际覆盖的时间窗 [ms] */
|
||||
uint8_t frame_count; /**< 本次累加窗口内收到的帧数 */
|
||||
} SNC_OdomDeltaAccum_t;
|
||||
|
||||
/* ========================= 通信诊断 ========================= */
|
||||
typedef struct
|
||||
{
|
||||
uint8_t valid_cmd_total_lsb;
|
||||
uint8_t crc_error_total_lsb;
|
||||
uint8_t counter_reject_total_lsb;
|
||||
uint8_t can_tx_drop_total_lsb;
|
||||
uint8_t busoff_total_lsb;
|
||||
uint8_t rx_overrun_total_lsb;
|
||||
uint8_t last_accepted_counter;
|
||||
uint8_t consecutive_counter_errors;
|
||||
uint8_t consecutive_crc_errors;
|
||||
uint32_t last_update_ms;
|
||||
} SNC_CommDiagFrame_t;
|
||||
|
||||
/* ========================= 上位机应用层总状态 ========================= */
|
||||
typedef struct
|
||||
{
|
||||
SNC_ChassisStatus_t status;
|
||||
SNC_WheelRpmFrame_t actual_rpm;
|
||||
SNC_WheelRpmFrame_t target_rpm;
|
||||
SNC_OdomDeltaFrame_t odom_delta; /**< 最近一帧快照 (保留兼容性, 勿用于积分) */
|
||||
SNC_OdomDeltaAccum_t odom_accum; /**< 里程增量累加器 (正确消费入口) */
|
||||
SNC_CommDiagFrame_t comm_diag;
|
||||
|
||||
uint8_t tx_counter;
|
||||
uint8_t tx_ctrl_flags;
|
||||
uint32_t tx_total;
|
||||
uint32_t tx_fail_total;
|
||||
uint32_t rx_total;
|
||||
} SNC_CAN_AppContext_t;
|
||||
|
||||
/* ========================= 全局上下文 ========================= */
|
||||
extern SNC_CAN_AppContext_t g_snc_can_app;
|
||||
|
||||
/* ========================= 初始化与任务 ========================= */
|
||||
void SNC_CAN_AppInit(void);
|
||||
void SNC_CAN_20msTask(void);
|
||||
void SNC_CAN_100msTask(void);
|
||||
void SNC_CAN_PollOnlineState(uint32_t now_ms);
|
||||
|
||||
/* ========================= 发送接口 ========================= */
|
||||
HAL_StatusTypeDef SNC_CAN_SendHeartbeat(void);
|
||||
HAL_StatusTypeDef SNC_CAN_SendCmdVel(float vx_mps, float wz_radps, uint8_t ctrl_flags);
|
||||
|
||||
/* ========================= 接收回调 ========================= */
|
||||
void SNC_CAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t rxFifo0ITs);
|
||||
|
||||
/* ========================= 数据读取接口 ========================= */
|
||||
const SNC_CAN_AppContext_t *SNC_CAN_GetContext(void);
|
||||
|
||||
/**
|
||||
* @brief 原子取走并清零里程增量累加器
|
||||
*
|
||||
* 消费者（monitorTask）每周期调用一次，取走自上次消费以来
|
||||
* ISR 累积的全部增量帧总和。返回后累加器归零，等待下一批帧。
|
||||
*
|
||||
* @param[out] fl 左前轮累计增量 ticks (截断为 int16)
|
||||
* @param[out] rl 左后轮累计增量 ticks
|
||||
* @param[out] fr 右前轮累计增量 ticks
|
||||
* @param[out] rr 右后轮累计增量 ticks
|
||||
* @param[out] dt_ms 累加增量实际覆盖的时间窗 [ms]
|
||||
* 若无法可靠计算（如系统首帧）则为 0，可用默认帧间隔替代
|
||||
* @return 本次取走的帧数；0 表示期间没有新帧到达
|
||||
*/
|
||||
uint8_t SNC_CAN_ConsumeOdomDelta(int16_t *fl, int16_t *rl,
|
||||
int16_t *fr, int16_t *rr,
|
||||
uint32_t *dt_ms);
|
||||
|
||||
/* ========================= 工具接口 ========================= */
|
||||
uint8_t SNC_CAN_Crc8J1850(const uint8_t *data, uint16_t len);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __SNC_CAN_APP_H */
|
||||
68
App/Contract/chassis_can_msg.h
Normal file
68
App/Contract/chassis_can_msg.h
Normal file
@@ -0,0 +1,68 @@
|
||||
#ifndef CHASSIS_CAN_MSG_H
|
||||
#define CHASSIS_CAN_MSG_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* 强制 1 字节对齐，完美匹配物理总线上的 8 字节数据帧 */
|
||||
#pragma pack(push, 1)
|
||||
|
||||
/* =========================================================
|
||||
* TX: 上位机 (H743) -> 底盘 (F407)
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 0x100 速度控制帧 (建议 20ms 发送周期) [cite: 608-612, 624]
|
||||
*/
|
||||
typedef struct {
|
||||
int16_t vx_x1000; // Byte 0-1: 线速度 m/s * 1000 (小端) [cite: 612]
|
||||
int16_t wz_x1000; // Byte 2-3: 角速度 rad/s * 1000 (小端) [cite: 612]
|
||||
uint8_t ctrl_flags; // Byte 4: 控制标志位 [cite: 612]
|
||||
uint8_t reserved; // Byte 5: 预留，固定填 0 [cite: 612]
|
||||
uint8_t rolling_counter; // Byte 6: 滚动计数器 (新counter相对上一帧差值必须在1..3之间) [cite: 612, 622]
|
||||
uint8_t crc8; // Byte 7: 对 Byte0~6 做的 CRC8-SAE J1850 [cite: 612]
|
||||
} CanMsg_CmdVel_0x100_t;
|
||||
|
||||
|
||||
/* =========================================================
|
||||
* RX: 底盘 (F407) -> 上位机 (H743)
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 0x181 底盘状态帧 (20ms 发送周期) [cite: 633-637]
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t system_state; // Byte 0: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 636, 665]
|
||||
uint8_t system_health; // Byte 1: 0=OK, 1=WARNING, 2=FAULT [cite: 636, 667]
|
||||
uint32_t diag_bits; // Byte 2-5: 诊断位图 (小端) [cite: 637, 669]
|
||||
uint8_t cmd_age_10ms; // Byte 6: 距最近一次合法 0x100 过去多少个 10ms [cite: 637]
|
||||
uint8_t status_counter; // Byte 7: 状态帧发送计数器 [cite: 637]
|
||||
} CanMsg_Status_0x181_t;
|
||||
|
||||
/**
|
||||
* @brief 0x200 里程增量帧 (约 60ms 轮询周期) [cite: 655-658]
|
||||
* @note 这是时间窗内的脉冲增量，上位机需自行累加积分以推算里程 [cite: 660-662]
|
||||
*/
|
||||
typedef struct {
|
||||
int16_t fl_delta_ticks; // Byte 0-1: 左前轮增量 [cite: 658]
|
||||
int16_t rl_delta_ticks; // Byte 2-3: 左后轮增量 [cite: 658]
|
||||
int16_t fr_delta_ticks; // Byte 4-5: 右前轮增量 [cite: 658]
|
||||
int16_t rr_delta_ticks; // Byte 6-7: 右后轮增量 [cite: 658]
|
||||
} CanMsg_OdomDelta_0x200_t;
|
||||
|
||||
/**
|
||||
* @brief 0x184 通信诊断帧 (100ms 发送周期) [cite: 651-653]
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t valid_cmd_total; // Byte 0: 合法命令累计 [cite: 653]
|
||||
uint8_t crc_error_total; // Byte 1: CRC 错误累计 [cite: 653]
|
||||
uint8_t counter_reject_total; // Byte 2: Counter 拒收累计 [cite: 653]
|
||||
uint8_t can_tx_drop_total; // Byte 3: CAN 发送丢帧累计 [cite: 653]
|
||||
uint8_t busoff_total; // Byte 4: Bus-Off 累计 [cite: 653]
|
||||
uint8_t rx_overrun_total; // Byte 5: FIFO overrun 累计 [cite: 653]
|
||||
uint8_t last_accepted_counter; // Byte 6: 最近一次接受的 counter [cite: 653]
|
||||
uint8_t err_nibbles; // Byte 7: 高4位=连续counter错，低4位=连续CRC错 [cite: 653]
|
||||
} CanMsg_CommDiag_0x184_t;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
#endif // CHASSIS_CAN_MSG_H
|
||||
111
App/Contract/robot_blackboard.c
Normal file
111
App/Contract/robot_blackboard.c
Normal file
@@ -0,0 +1,111 @@
|
||||
#include "robot_blackboard.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include <string.h>
|
||||
|
||||
/* 全局唯一的黑板实例 */
|
||||
static RobotBlackboard_t g_blackboard = {0};
|
||||
|
||||
/* =========================================================
|
||||
* 生产者 API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data) {
|
||||
if (imu_data == NULL) return;
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
// 更新原始航向角 ([-180, +180) 度)
|
||||
g_blackboard.imu_yaw.value = imu_data->yaw;
|
||||
g_blackboard.imu_yaw.timestamp_ms = imu_data->last_update;
|
||||
g_blackboard.imu_yaw.is_valid = (imu_data->online != 0);
|
||||
|
||||
// 更新 unwrap 后的连续航向角 (无跳变, 度)
|
||||
g_blackboard.imu_yaw_continuous.value = imu_data->yaw_continuous;
|
||||
g_blackboard.imu_yaw_continuous.timestamp_ms = imu_data->last_update;
|
||||
g_blackboard.imu_yaw_continuous.is_valid = (imu_data->online != 0);
|
||||
|
||||
// 更新角速度 (deg/s)
|
||||
g_blackboard.imu_wz.value = imu_data->wz;
|
||||
g_blackboard.imu_wz.timestamp_ms = imu_data->last_update;
|
||||
g_blackboard.imu_wz.is_valid = (imu_data->online != 0);
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data) {
|
||||
if (vl_data == NULL) return;
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
// 约定: [0]左前, [1]左后, [2]右前, [3]右后
|
||||
g_blackboard.dist_left_f.value = vl_data->range_mm_filtered[0];
|
||||
g_blackboard.dist_left_f.is_valid = (vl_data->valid_mask & (1 << 0)) != 0;
|
||||
g_blackboard.dist_left_f.timestamp_ms = vl_data->tick_ms;
|
||||
|
||||
g_blackboard.dist_left_r.value = vl_data->range_mm_filtered[1];
|
||||
g_blackboard.dist_left_r.is_valid = (vl_data->valid_mask & (1 << 1)) != 0;
|
||||
g_blackboard.dist_left_r.timestamp_ms = vl_data->tick_ms;
|
||||
|
||||
g_blackboard.dist_right_f.value = vl_data->range_mm_filtered[2];
|
||||
g_blackboard.dist_right_f.is_valid = (vl_data->valid_mask & (1 << 2)) != 0;
|
||||
g_blackboard.dist_right_f.timestamp_ms = vl_data->tick_ms;
|
||||
|
||||
g_blackboard.dist_right_r.value = vl_data->range_mm_filtered[3];
|
||||
g_blackboard.dist_right_r.is_valid = (vl_data->valid_mask & (1 << 3)) != 0;
|
||||
g_blackboard.dist_right_r.timestamp_ms = vl_data->tick_ms;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data) {
|
||||
if (ls_data == NULL) return;
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
|
||||
// 1. 前方 STP
|
||||
g_blackboard.dist_front_stp.value = ls_data->ch[LASER_CH_FRONT_STP].distance_mm;
|
||||
g_blackboard.dist_front_stp.is_valid = ls_data->ch[LASER_CH_FRONT_STP].valid;
|
||||
g_blackboard.dist_front_stp.timestamp_ms = ls_data->ch[LASER_CH_FRONT_STP].update_tick_ms;
|
||||
|
||||
// 2. 前方 ATK
|
||||
g_blackboard.dist_front_atk.value = ls_data->ch[LASER_CH_FRONT_ATK].distance_mm;
|
||||
g_blackboard.dist_front_atk.is_valid = ls_data->ch[LASER_CH_FRONT_ATK].valid;
|
||||
g_blackboard.dist_front_atk.timestamp_ms = ls_data->ch[LASER_CH_FRONT_ATK].update_tick_ms;
|
||||
|
||||
// 3. 后方 STP
|
||||
g_blackboard.dist_rear_stp.value = ls_data->ch[LASER_CH_REAR_STP].distance_mm;
|
||||
g_blackboard.dist_rear_stp.is_valid = ls_data->ch[LASER_CH_REAR_STP].valid;
|
||||
g_blackboard.dist_rear_stp.timestamp_ms = ls_data->ch[LASER_CH_REAR_STP].update_tick_ms;
|
||||
|
||||
// 4. 后方 ATK
|
||||
g_blackboard.dist_rear_atk.value = ls_data->ch[LASER_CH_REAR_ATK].distance_mm;
|
||||
g_blackboard.dist_rear_atk.is_valid = ls_data->ch[LASER_CH_REAR_ATK].valid;
|
||||
g_blackboard.dist_rear_atk.timestamp_ms = ls_data->ch[LASER_CH_REAR_ATK].update_tick_ms;
|
||||
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
void Blackboard_UpdateOdom(float vx, float wz) {
|
||||
taskENTER_CRITICAL();
|
||||
g_blackboard.odom_vx = vx;
|
||||
g_blackboard.odom_wz = wz;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online) {
|
||||
taskENTER_CRITICAL();
|
||||
g_blackboard.chassis_state = state;
|
||||
g_blackboard.chassis_diag = diag;
|
||||
g_blackboard.chassis_online = online;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 消费者 API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot) {
|
||||
if (out_snapshot == NULL) return;
|
||||
|
||||
// 关中断拷贝整个结构体，保证上层算法拿到的数据处于同一个时间切片
|
||||
taskENTER_CRITICAL();
|
||||
*out_snapshot = g_blackboard;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
74
App/Contract/robot_blackboard.h
Normal file
74
App/Contract/robot_blackboard.h
Normal file
@@ -0,0 +1,74 @@
|
||||
#ifndef ROBOT_BLACKBOARD_H
|
||||
#define ROBOT_BLACKBOARD_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "IMU/hwt101.h" // 引用 IMU 接口
|
||||
#include "laser/laser_manager.h" // 引用前后雷达接口
|
||||
#include "vl53_board.h" // 引用侧向雷达接口
|
||||
|
||||
/**
|
||||
* @brief 标准原子数据，自带时间戳，专治“传感器偷偷掉线”
|
||||
*/
|
||||
typedef struct {
|
||||
float value; // 数据本体 (距离、角度、速度等)
|
||||
uint32_t timestamp_ms; // 数据更新时的系统时间 (HAL_GetTick)
|
||||
bool is_valid; // 驱动层判定的有效性 (如是否超量程、是否报警)
|
||||
} SensorItem_t;
|
||||
|
||||
/**
|
||||
* @brief 机器人全局情报中心 (System Context)
|
||||
* @note 对齐抽象后的数据，算法层不需要再关心这是 1 号雷达还是 2 号雷达
|
||||
*/
|
||||
typedef struct {
|
||||
/* --- 1. 空间姿态 (整合自 HWT101) --- */
|
||||
SensorItem_t imu_yaw; // 原始航向角 (deg)，范围 [-180, +180)，跨界时跳变
|
||||
SensorItem_t imu_yaw_continuous; // unwrap 后的连续航向角 (deg)，无跳变，可直接做差计算转角
|
||||
SensorItem_t imu_wz; // Z轴角速度 (deg/s，上层使用前需转 rad/s)
|
||||
|
||||
/* --- 2. 侧向走廊观测 (整合自 Vl53Board) --- */
|
||||
SensorItem_t dist_left_f; // 左前侧向距离 (mm)
|
||||
SensorItem_t dist_left_r; // 左后侧向距离 (mm)
|
||||
SensorItem_t dist_right_f; // 右前侧向距离 (mm)
|
||||
SensorItem_t dist_right_r; // 右后侧向距离 (mm)
|
||||
|
||||
/* --- 3. 前后到端/防撞观测 (整合自 Laser Manager) --- */
|
||||
SensorItem_t dist_front_stp; // 前方 STP 激光测距 (mm)
|
||||
SensorItem_t dist_front_atk; // 前方 ATK 激光测距 (mm)
|
||||
SensorItem_t dist_rear_stp; // 后方 STP 激光测距 (mm)
|
||||
SensorItem_t dist_rear_atk; // 后方 ATK 激光测距 (mm)
|
||||
|
||||
/* --- 4. 底盘反馈 (整合自 CAN 0x181 & 0x200) --- */
|
||||
float odom_vx; // 基于 ticks 积分推算的当前线速度 (m/s)
|
||||
float odom_wz; // 基于 ticks 积分推算的当前角速度 (rad/s)
|
||||
uint32_t chassis_diag; // 底盘硬诊断位图 [cite: 669]
|
||||
uint8_t chassis_state; // 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT [cite: 665]
|
||||
bool chassis_online; // 底盘 CAN 通讯是否正常
|
||||
} RobotBlackboard_t;
|
||||
|
||||
|
||||
/* =========================================================
|
||||
* 生产者 API (各种中断/驱动采集任务调用)
|
||||
* ========================================================= */
|
||||
void Blackboard_UpdateIMU(const HWT101_Data_t *imu_data);
|
||||
void Blackboard_UpdateVl53(const Vl53BoardSnapshot_t *vl_data);
|
||||
void Blackboard_UpdateLaser(const laser_simple_snapshot_t *ls_data);
|
||||
|
||||
// 由你的里程计解算函数调用 (传入算好的实时速度)
|
||||
void Blackboard_UpdateOdom(float vx, float wz);
|
||||
|
||||
// 由 CAN 0x181 接收回调调用
|
||||
void Blackboard_UpdateChassisState(uint8_t state, uint32_t diag, bool online);
|
||||
|
||||
|
||||
/* =========================================================
|
||||
* 消费者 API (算法/控制任务调用)
|
||||
* ========================================================= */
|
||||
/**
|
||||
* @brief 拍一张极速快照，获取全部对齐的传感器数据
|
||||
* @note 内部自带临界区保护，绝对防撕裂
|
||||
*/
|
||||
void Blackboard_GetSnapshot(RobotBlackboard_t *out_snapshot);
|
||||
|
||||
#endif // ROBOT_BLACKBOARD_H
|
||||
44
App/Contract/robot_cmd_slot.c
Normal file
44
App/Contract/robot_cmd_slot.c
Normal file
@@ -0,0 +1,44 @@
|
||||
#include "robot_cmd_slot.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "main.h" // 提供 HAL_GetTick()
|
||||
|
||||
/* 全局唯一的指令槽实例 */
|
||||
static RobotTargetCmd_t g_cmd_slot = {0};
|
||||
|
||||
void CmdSlot_Push(float vx, float wz, uint8_t flags)
|
||||
{
|
||||
// 1. 获取当前系统时间作为时间戳
|
||||
uint32_t now_ms = HAL_GetTick();
|
||||
|
||||
// 2. 进入临界区，关中断，防止写入一半时被高优先级任务或中断打断（防数据撕裂）
|
||||
taskENTER_CRITICAL();
|
||||
g_cmd_slot.target_vx = vx;
|
||||
g_cmd_slot.target_wz = wz;
|
||||
g_cmd_slot.ctrl_flags = flags;
|
||||
g_cmd_slot.gen_tick_ms = now_ms;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms)
|
||||
{
|
||||
if (out_cmd == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 1. 进入临界区，安全地拷贝走整个结构体
|
||||
taskENTER_CRITICAL();
|
||||
*out_cmd = g_cmd_slot;
|
||||
taskEXIT_CRITICAL();
|
||||
|
||||
// 2. 计算指令老化时间 (利用无符号整型减法自然处理溢出回绕)
|
||||
uint32_t now_ms = HAL_GetTick();
|
||||
uint32_t age_ms = now_ms - out_cmd->gen_tick_ms;
|
||||
|
||||
// 3. 判定是否超时
|
||||
if (age_ms > timeout_ms) {
|
||||
return false; // 🚨 指令太老了，算法层可能死机了！
|
||||
}
|
||||
|
||||
return true; // ✅ 指令新鲜，可以发送
|
||||
}
|
||||
37
App/Contract/robot_cmd_slot.h
Normal file
37
App/Contract/robot_cmd_slot.h
Normal file
@@ -0,0 +1,37 @@
|
||||
#ifndef ROBOT_CMD_SLOT_H
|
||||
#define ROBOT_CMD_SLOT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/**
|
||||
* @brief 算法层生成的抽象目标指令
|
||||
*/
|
||||
typedef struct {
|
||||
float target_vx; // 目标线速度 (m/s)
|
||||
float target_wz; // 目标角速度 (rad/s)
|
||||
uint8_t ctrl_flags; // 控制模式/标志
|
||||
uint32_t gen_tick_ms; // 指令生成时的系统时间 (用于底层判断算法是否卡死)
|
||||
} RobotTargetCmd_t;
|
||||
|
||||
/* =========================================================
|
||||
* API 接口
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 写入接口：由主导航/控制任务调用
|
||||
* @param vx 期望线速度 (m/s)
|
||||
* @param wz 期望角速度 (rad/s)
|
||||
* @param flags 扩展控制标志
|
||||
*/
|
||||
void CmdSlot_Push(float vx, float wz, uint8_t flags);
|
||||
|
||||
/**
|
||||
* @brief 读取接口：由 20ms 的 CAN Tx 任务专属调用
|
||||
* @param out_cmd 获取到的指令拷贝
|
||||
* @param timeout_ms 容忍算法不更新的最大时间 (推荐 100ms)
|
||||
* @return true: 指令新鲜可用; false: 算法超时太久，建议强制发 0 停车保护
|
||||
*/
|
||||
bool CmdSlot_Pop(RobotTargetCmd_t *out_cmd, uint32_t timeout_ms);
|
||||
|
||||
#endif // ROBOT_CMD_SLOT_H
|
||||
287
App/Contract/robot_odom.c
Normal file
287
App/Contract/robot_odom.c
Normal file
@@ -0,0 +1,287 @@
|
||||
/**
|
||||
* @file robot_odom.c
|
||||
* @brief 里程计积分实现：消费 CAN 0x200 轮增量帧，
|
||||
* 经差分驱动运动学转换为机体速度并推送黑板。
|
||||
*
|
||||
* 调用链（monitorTask 100ms 周期）:
|
||||
* CAN Rx ISR ──> snc_parse_odom_delta() [snc_can_app.c]
|
||||
* │ (每帧累加到 odom_accum，不覆盖)
|
||||
* v
|
||||
* monitorTask ──> SNC_CAN_ConsumeOdomDelta() [snc_can_app.c]
|
||||
* │ (原子取走累计增量并清零)
|
||||
* v
|
||||
* Odom_Update() [robot_odom.c]
|
||||
* │ (仅在有新帧时调用，杜绝重复积分)
|
||||
* v
|
||||
* Blackboard_UpdateOdom(vx, wz) [robot_blackboard.c]
|
||||
* │
|
||||
* v
|
||||
* navTask ──> Blackboard_GetSnapshot() ──> CorridorFilter_Update()
|
||||
*
|
||||
* 运动学公式:
|
||||
* v_left = (fl_delta + rl_delta) / 2 * TICKS_TO_M / dt
|
||||
* v_right = (fr_delta + rr_delta) / 2 * TICKS_TO_M / dt
|
||||
* vx = (v_left + v_right) / 2 [m/s]
|
||||
* wz = (v_right - v_left) / track [rad/s]
|
||||
*
|
||||
* 里程累加（用于沿程 s 估计）:
|
||||
* s += |vx| * dt [m]
|
||||
*/
|
||||
|
||||
#include "robot_odom.h"
|
||||
#include "robot_blackboard.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
/* =========================================================
|
||||
* 内部静态状态
|
||||
* ========================================================= */
|
||||
|
||||
/** @brief 里程计运行时数据（全部由临界区保护） */
|
||||
static struct {
|
||||
float vx; /**< 机体线速度 [m/s] */
|
||||
float wz; /**< 机体角速度 [rad/s] */
|
||||
float distance; /**< 累计行驶距离 [m]（从 init 复位后积分） */
|
||||
bool online; /**< 里程计有效标志 */
|
||||
uint32_t last_update_ms; /**< 上次有效更新时的系统时间 [ms] */
|
||||
uint16_t zero_cnt; /**< 连续收到零增量的帧数 */
|
||||
bool initialized; /**< 是否已完成初始化 */
|
||||
} s_odom = {
|
||||
.vx = 0.0f,
|
||||
.wz = 0.0f,
|
||||
.distance = 0.0f,
|
||||
.online = false,
|
||||
.last_update_ms = 0U,
|
||||
.zero_cnt = 0U,
|
||||
.initialized = false,
|
||||
};
|
||||
|
||||
/* =========================================================
|
||||
* 内部辅助函数
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 内部临界区读写接口（使用 taskENTER/EXIT_CRITICAL）
|
||||
*/
|
||||
static inline void lock_odom(void)
|
||||
{
|
||||
taskENTER_CRITICAL();
|
||||
}
|
||||
|
||||
static inline void unlock_odom(void)
|
||||
{
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 差分驱动运动学核心
|
||||
*
|
||||
* @param fl 左前轮增量 ticks
|
||||
* @param rl 左后轮增量 ticks
|
||||
* @param fr 右前轮增量 ticks
|
||||
* @param rr 右后轮增量 ticks
|
||||
* @param dt 采样周期 [s]
|
||||
* @param out_vx 输出线速度 [m/s]
|
||||
* @param out_wz 输出角速度 [rad/s]
|
||||
*
|
||||
* @note 内部使用均值滤波：同侧前后轮求平均后再做差速，
|
||||
* 这样可以抑制单轮打滑对整体估计的影响。
|
||||
*/
|
||||
static void differential_kinematics(int16_t fl, int16_t rl,
|
||||
int16_t fr, int16_t rr,
|
||||
float dt,
|
||||
float *out_vx, float *out_wz)
|
||||
{
|
||||
/* 1. 同侧均值滤波（抵消前后轮差分） */
|
||||
float v_left_ticks = 0.5f * ((float)fl + (float)rl); /* [ticks/period] */
|
||||
float v_right_ticks = 0.5f * ((float)fr + (float)rr); /* [ticks/period] */
|
||||
|
||||
/* 2. tick -> m/s */
|
||||
float v_left = v_left_ticks * ODOM_TICKS_TO_M / dt; /* [m/s] */
|
||||
float v_right = v_right_ticks * ODOM_TICKS_TO_M / dt; /* [m/s] */
|
||||
|
||||
/* 3. 差分驱动运动学逆解 */
|
||||
/* vx = (vL + vR) / 2 */
|
||||
/* wz = (vR - vL) / track */
|
||||
*out_vx = 0.5f * (v_left + v_right);
|
||||
*out_wz = (v_right - v_left) * ODOM_WHEEL_TRACK_INV;
|
||||
|
||||
/* 4. 合理性检查：物理约束限幅
|
||||
* H743 主频 480MHz，dt 最小约 1ms，
|
||||
* 若单帧 tick 绝对值超过 65535/dt，说明增量帧发生异常合并/撕裂。 */
|
||||
const float MAX_VX_PERIOD = 5.0f; /* 允许的最大瞬时线速度 [m/s]（车辆标称最高 1m/s） */
|
||||
const float MAX_WZ_PERIOD = 20.0f; /* 允许的最大瞬时角速度 [rad/s]（极端原地转向） */
|
||||
|
||||
if (fabsf(*out_vx) > MAX_VX_PERIOD) {
|
||||
*out_vx = (*out_vx > 0.0f) ? MAX_VX_PERIOD : -MAX_VX_PERIOD;
|
||||
}
|
||||
if (fabsf(*out_wz) > MAX_WZ_PERIOD) {
|
||||
*out_wz = (*out_wz > 0.0f) ? MAX_WZ_PERIOD : -MAX_WZ_PERIOD;
|
||||
}
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 公共 API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void Odom_Init(void)
|
||||
{
|
||||
lock_odom();
|
||||
s_odom.vx = 0.0f;
|
||||
s_odom.wz = 0.0f;
|
||||
s_odom.distance = 0.0f;
|
||||
s_odom.online = false;
|
||||
s_odom.last_update_ms = 0U;
|
||||
s_odom.zero_cnt = 0U;
|
||||
s_odom.initialized = true;
|
||||
unlock_odom();
|
||||
}
|
||||
|
||||
void Odom_Update(uint32_t now_ms,
|
||||
int16_t fl_delta, int16_t rl_delta,
|
||||
int16_t fr_delta, int16_t rr_delta,
|
||||
uint32_t odom_span_ms)
|
||||
{
|
||||
if (!s_odom.initialized) {
|
||||
Odom_Init();
|
||||
}
|
||||
|
||||
/* 计算采样周期 dt
|
||||
*
|
||||
* 优先使用 odom_span_ms（CAN 0x200 帧的真实时间跨度），
|
||||
* 这是编码器增量实际覆盖的时间窗，不受 monitorTask 调度抖动影响。
|
||||
*
|
||||
* 退化策略：
|
||||
* - odom_span_ms > 0 → 直接使用（最精确）
|
||||
* - odom_span_ms == 0 → 仅收到一帧，无法算帧间 dt，
|
||||
* 退化为 now_ms - last_update_ms（兼容旧行为）
|
||||
* - 首帧（last_update_ms == 0 且 odom_span_ms == 0）→ 默认 60ms
|
||||
* （0x200 帧典型间隔）
|
||||
*/
|
||||
float dt;
|
||||
if (odom_span_ms > 0U && odom_span_ms <= 1000U) {
|
||||
/* 有真实帧时间跨度，优先使用 */
|
||||
dt = (float)odom_span_ms / 1000.0f;
|
||||
} else if (s_odom.last_update_ms == 0U || !s_odom.online) {
|
||||
/*
|
||||
* 系统首帧，或 odom 刚从断流中恢复但当前只有一帧时，
|
||||
* 无法可靠反推出这份增量覆盖的真实时间窗，退化为典型帧间隔。
|
||||
*/
|
||||
dt = 0.06f;
|
||||
} else {
|
||||
uint32_t dt_ms = now_ms - s_odom.last_update_ms;
|
||||
if (dt_ms == 0U || dt_ms > 1000U) {
|
||||
/* 超时（超过 1 秒没有新数据）视为里程计离线 */
|
||||
dt = 0.0f;
|
||||
} else {
|
||||
dt = (float)dt_ms / 1000.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* 检测零增量：四轮全 0 可能是底盘未动或通信丢失 */
|
||||
bool is_zero_delta = (fl_delta == 0 && rl_delta == 0 &&
|
||||
fr_delta == 0 && rr_delta == 0);
|
||||
|
||||
if (dt <= 0.0f) {
|
||||
/* 非法 dt，强制离线 */
|
||||
lock_odom();
|
||||
s_odom.online = false;
|
||||
unlock_odom();
|
||||
return;
|
||||
}
|
||||
|
||||
/* 计算差分运动学 */
|
||||
float vx_local, wz_local;
|
||||
differential_kinematics(fl_delta, rl_delta, fr_delta, rr_delta,
|
||||
dt, &vx_local, &wz_local);
|
||||
|
||||
/* 零速度保护：连续多帧零增量后，强制归零（防止漂移） */
|
||||
lock_odom();
|
||||
if (is_zero_delta) {
|
||||
s_odom.zero_cnt++;
|
||||
if (s_odom.zero_cnt >= 3U) {
|
||||
/* 连续 3 帧零增量（~180ms @ 60ms 帧率），认为机器人静止 */
|
||||
s_odom.vx = 0.0f;
|
||||
s_odom.wz = 0.0f;
|
||||
s_odom.online = true;
|
||||
}
|
||||
} else {
|
||||
s_odom.zero_cnt = 0U;
|
||||
s_odom.vx = vx_local;
|
||||
s_odom.wz = wz_local;
|
||||
s_odom.online = true;
|
||||
|
||||
/* 积分累积里程（取线速度的绝对值，保证后退也累加） */
|
||||
s_odom.distance += fabsf(s_odom.vx) * dt;
|
||||
}
|
||||
|
||||
s_odom.last_update_ms = now_ms;
|
||||
unlock_odom();
|
||||
|
||||
/* 推送黑板（黑板内部自带临界区保护） */
|
||||
Blackboard_UpdateOdom(s_odom.vx, s_odom.wz);
|
||||
}
|
||||
|
||||
void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms)
|
||||
{
|
||||
bool timed_out = false;
|
||||
|
||||
if (!s_odom.initialized || timeout_ms == 0U) {
|
||||
return;
|
||||
}
|
||||
|
||||
lock_odom();
|
||||
if (s_odom.online &&
|
||||
s_odom.last_update_ms != 0U &&
|
||||
(now_ms - s_odom.last_update_ms) > timeout_ms) {
|
||||
s_odom.vx = 0.0f;
|
||||
s_odom.wz = 0.0f;
|
||||
s_odom.online = false;
|
||||
s_odom.zero_cnt = 0U;
|
||||
timed_out = true;
|
||||
}
|
||||
unlock_odom();
|
||||
|
||||
if (timed_out) {
|
||||
Blackboard_UpdateOdom(0.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status)
|
||||
{
|
||||
if (out_vx != NULL) *out_vx = s_odom.vx;
|
||||
if (out_wz != NULL) *out_wz = s_odom.wz;
|
||||
|
||||
if (out_status != NULL) {
|
||||
lock_odom();
|
||||
out_status->online = s_odom.online;
|
||||
out_status->zero_delta_count = s_odom.zero_cnt;
|
||||
out_status->last_update_ms = s_odom.last_update_ms;
|
||||
unlock_odom();
|
||||
}
|
||||
}
|
||||
|
||||
float Odom_GetDistance(void)
|
||||
{
|
||||
float d;
|
||||
lock_odom();
|
||||
d = s_odom.distance;
|
||||
unlock_odom();
|
||||
return d;
|
||||
}
|
||||
|
||||
void Odom_Reset(void)
|
||||
{
|
||||
lock_odom();
|
||||
s_odom.vx = 0.0f;
|
||||
s_odom.wz = 0.0f;
|
||||
s_odom.distance = 0.0f;
|
||||
s_odom.online = false;
|
||||
s_odom.last_update_ms = 0U;
|
||||
s_odom.zero_cnt = 0U;
|
||||
unlock_odom();
|
||||
|
||||
Blackboard_UpdateOdom(0.0f, 0.0f);
|
||||
}
|
||||
115
App/Contract/robot_odom.h
Normal file
115
App/Contract/robot_odom.h
Normal file
@@ -0,0 +1,115 @@
|
||||
/**
|
||||
* @file robot_odom.h
|
||||
* @brief 里程计积分模块：从 CAN 0x200 轮增量帧累加积分，
|
||||
* 经差分驱动运动学转换为机体线速度 vx 与角速度 wz，
|
||||
* 最终通过 Blackboard_UpdateOdom() 推送给导航流水线。
|
||||
*
|
||||
* @note 本模块不依赖任何传感器，只消费 CAN 协议层已有的 odom_delta 数据。
|
||||
* 所有标定参数统一在 robot_params.h 中管理。
|
||||
*
|
||||
* 运动学模型（差分驱动）:
|
||||
* v_left = (fl_delta + rl_delta) / 2
|
||||
* v_right = (fr_delta + rr_delta) / 2
|
||||
* vx = (v_left + v_right) / 2 [m/s]
|
||||
* wz = (v_right - v_left) / wheel_track [rad/s]
|
||||
*/
|
||||
|
||||
#ifndef ROBOT_ODOM_H
|
||||
#define ROBOT_ODOM_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "robot_params.h" /* 统一参数头文件 */
|
||||
|
||||
/* =========================================================
|
||||
* 标定参数（从 robot_params.h 引入，勿在此文件重复定义）
|
||||
* ========================================================= */
|
||||
#define ODOM_TICKS_PER_REV PARAM_ENCODER_CPR
|
||||
#define ODOM_WHEEL_DIAMETER PARAM_WHEEL_DIAMETER
|
||||
#define ODOM_WHEEL_TRACK PARAM_WHEEL_TRACK
|
||||
|
||||
/* =========================================================
|
||||
* 预计算常数（自动生成，勿手动修改）
|
||||
* ========================================================= */
|
||||
#define ODOM_TICKS_TO_M (3.14159265358979f * (ODOM_WHEEL_DIAMETER) / (ODOM_TICKS_PER_REV))
|
||||
/**< @brief 每 tick 对应的轮缘弧长 [m]，即 PI*D / CPR */
|
||||
#define ODOM_WHEEL_TRACK_INV (1.0f / (ODOM_WHEEL_TRACK))
|
||||
/**< @brief 轮距倒数 [1/m]，用于快速计算角速度 */
|
||||
|
||||
/* =========================================================
|
||||
* 配置结构
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
/** @brief 里程计有效标志，false 时 vx/wz 强制为 0 */
|
||||
bool online;
|
||||
/** @brief 里程计在线但 Tick 增量为 0 的连续帧数（用于检测卡死） */
|
||||
uint16_t zero_delta_count;
|
||||
/** @brief 上次更新时的系统时间 [ms] */
|
||||
uint32_t last_update_ms;
|
||||
} OdomStatus_t;
|
||||
|
||||
/* =========================================================
|
||||
* API 接口
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 里程计模块初始化
|
||||
* @note 重置内部累加器，建议在系统启动阶段调用一次
|
||||
*/
|
||||
void Odom_Init(void);
|
||||
|
||||
/**
|
||||
* @brief 里程计更新主函数
|
||||
* @param now_ms 当前系统时间 [ms]，由 HAL_GetTick() 提供
|
||||
* @param fl_delta 左前轮增量 ticks（本周期内编码器增量，可为负）
|
||||
* @param rl_delta 左后轮增量 ticks
|
||||
* @param fr_delta 右前轮增量 ticks
|
||||
* @param rr_delta 右后轮增量 ticks
|
||||
* @param odom_span_ms 本批次增量帧的真实时间跨度 [ms]，由
|
||||
* SNC_CAN_ConsumeOdomDelta() 返回。
|
||||
* 传 0 表示无法计算（如仅一帧），此时退化为
|
||||
* now_ms - last_update_ms。
|
||||
*
|
||||
* @note 由调用方（本例中 monitorTask 或 navTask）定期轮询 CAN 上下文
|
||||
* 中的 odom_delta 数据后主动调用，推荐调用周期 20~100ms。
|
||||
* 函数内部完成：均值滤波 -> 差分运动学 -> Blackboard 更新。
|
||||
*/
|
||||
void Odom_Update(uint32_t now_ms,
|
||||
int16_t fl_delta, int16_t rl_delta,
|
||||
int16_t fr_delta, int16_t rr_delta,
|
||||
uint32_t odom_span_ms);
|
||||
|
||||
/**
|
||||
* @brief odom 断流超时处理
|
||||
* @param now_ms 当前系统时间 [ms]
|
||||
* @param timeout_ms 允许的最大无更新时长 [ms]
|
||||
*
|
||||
* @note 超时后会将 vx/wz 清零、标记离线，并同步刷新黑板，
|
||||
* 防止导航层继续使用最后一次有效速度。
|
||||
*/
|
||||
void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms);
|
||||
|
||||
/**
|
||||
* @brief 读取当前里程计状态快照（供外部任务消费）
|
||||
* @param out_status 输出状态结构体
|
||||
* @param out_vx 输出线速度 [m/s]
|
||||
* @param out_wz 输出角速度 [rad/s]
|
||||
*
|
||||
* @note 内部使用临界区保护，调用方可在任何上下文安全使用。
|
||||
*/
|
||||
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status);
|
||||
|
||||
/**
|
||||
* @brief 获取累积里程（沿程 s）
|
||||
* @return 累计行驶距离 [m]（从 Odom_Init 复位后开始积分）
|
||||
*
|
||||
* @note 由走廊状态机用于段落终止判断。也可用于比赛计时参考。
|
||||
*/
|
||||
float Odom_GetDistance(void);
|
||||
|
||||
/**
|
||||
* @brief 复位里程计（清零累积距离和状态）
|
||||
*/
|
||||
void Odom_Reset(void);
|
||||
|
||||
#endif /* ROBOT_ODOM_H */
|
||||
183
App/IMU/hwt101.c
Normal file
183
App/IMU/hwt101.c
Normal file
@@ -0,0 +1,183 @@
|
||||
#include "hwt101.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "cmsis_os2.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "stm32h7xx_hal_uart.h"
|
||||
#include "task.h"
|
||||
|
||||
/* 寄存器地址 */
|
||||
#define REG_SAVE 0x00
|
||||
#define REG_RRATE 0x03
|
||||
#define REG_BAUD 0x04
|
||||
#define REG_KEY 0x69
|
||||
#define REG_CALIYAW 0x76
|
||||
|
||||
/* D2 域特区分配 */
|
||||
__attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
|
||||
static uint8_t s_dma_buf[HWT101_DMA_BUF_SIZE];
|
||||
|
||||
/* 内部静态变量 */
|
||||
static UART_HandleTypeDef *s_huart = NULL;
|
||||
static HWT101_Data_t s_data = {0};
|
||||
static uint16_t s_read_ptr = 0;
|
||||
static uint8_t s_frame[11];
|
||||
static uint8_t s_frame_idx = 0;
|
||||
|
||||
/* yaw unwrap 状态：将 [-180, +180) 的原始 yaw 转为连续角度 */
|
||||
static float s_yaw_prev = 0.0f; // 上一次原始 yaw (deg)
|
||||
static float s_yaw_continuous = 0.0f; // 累计连续角度 (deg)
|
||||
static bool s_yaw_initialized = false; // 首次标记
|
||||
|
||||
void HWT101_Init(UART_HandleTypeDef *huart) {
|
||||
if (huart == NULL) return;
|
||||
s_huart = huart;
|
||||
s_read_ptr = 0;
|
||||
|
||||
// 【终极修复 1】：强制使用 32 位指针，将这块 (NOLOAD) 内存全部写 0。
|
||||
// 必须是 32 位写入！这会彻底激活 RAM_D2 的硬件 ECC，消灭上电时的随机乱码，防止 DMA 单字节写入引发总线错误。
|
||||
uint32_t *p32 = (uint32_t *)s_dma_buf;
|
||||
for (uint32_t i = 0; i < (HWT101_DMA_BUF_SIZE / 4); i++) {
|
||||
p32[i] = 0;
|
||||
}
|
||||
|
||||
// ⚠️ 【终极修复 2】：因为 .dma_buffer 所在的 0x30000000 已被 MPU 配置为 Device/Non-Cacheable 内存，
|
||||
// 绝对不能调用 SCB_InvalidateDCache_by_Addr，否则 Cortex-M7 会判定越权操作直接进入 HardFault 死机！
|
||||
// (已删除原有的 SCB_InvalidateDCache_by_Addr 代码)
|
||||
|
||||
HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
|
||||
}
|
||||
|
||||
void HWT101_Process(void) {
|
||||
if (s_huart == NULL) return;
|
||||
|
||||
// 获取当前 DMA 写指针
|
||||
uint16_t write_ptr = HWT101_DMA_BUF_SIZE - (uint16_t)__HAL_DMA_GET_COUNTER(s_huart->hdmarx);
|
||||
|
||||
// ⚠️ 【终极修复 3】：删除原有的刷 Cache 代码。
|
||||
// 依靠硬件 MPU 免 Cache 机制，CPU 读到的必定是 DMA 搬运过来的最新数据，不会有数据一致性问题。
|
||||
|
||||
while (s_read_ptr != write_ptr) {
|
||||
uint8_t byte = s_dma_buf[s_read_ptr];
|
||||
s_read_ptr = (s_read_ptr + 1) % HWT101_DMA_BUF_SIZE;
|
||||
|
||||
// 状态机加速：如果不是帧头，直接跳过，不用进入后续逻辑
|
||||
if (s_frame_idx == 0 && byte != 0x55) continue;
|
||||
|
||||
s_frame[s_frame_idx++] = byte;
|
||||
if (s_frame_idx >= 11) {
|
||||
uint8_t sum = 0;
|
||||
for (int i = 0; i < 10; i++) sum += s_frame[i];
|
||||
|
||||
if (sum == s_frame[10]) {
|
||||
// 更新全局变量时使用临界区保护，防止数据撕裂
|
||||
taskENTER_CRITICAL();
|
||||
if (s_frame[1] == 0x52) {
|
||||
int16_t wz_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
|
||||
s_data.wz = (float)wz_raw / 32768.0f * 2000.0f;
|
||||
} else if (s_frame[1] == 0x53) {
|
||||
int16_t yaw_raw = (int16_t)((s_frame[7] << 8) | s_frame[6]);
|
||||
float yaw_deg = (float)yaw_raw / 32768.0f * 180.0f;
|
||||
|
||||
/* 保存原始 yaw ([-180, +180) 度) */
|
||||
s_data.yaw = yaw_deg;
|
||||
|
||||
/* ---- yaw unwrap: 消除 ±180° 跳变，生成连续角度 ---- */
|
||||
if (!s_yaw_initialized) {
|
||||
s_yaw_continuous = yaw_deg;
|
||||
s_yaw_prev = yaw_deg;
|
||||
s_yaw_initialized = true;
|
||||
} else {
|
||||
float diff = yaw_deg - s_yaw_prev;
|
||||
if (diff > 180.0f) diff -= 360.0f;
|
||||
if (diff < -180.0f) diff += 360.0f;
|
||||
s_yaw_continuous += diff;
|
||||
s_yaw_prev = yaw_deg;
|
||||
}
|
||||
s_data.yaw_continuous = s_yaw_continuous;
|
||||
|
||||
s_data.last_update = HAL_GetTick();
|
||||
s_data.online = 1;
|
||||
}
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
s_frame_idx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 超时检测
|
||||
if (HAL_GetTick() - s_data.last_update > 200) {
|
||||
s_data.online = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 安全的数据获取接口
|
||||
void HWT101_GetData(HWT101_Data_t *out_data) {
|
||||
if (out_data == NULL) return;
|
||||
// 关中断拷贝，绝对防止浮点数撕裂
|
||||
taskENTER_CRITICAL();
|
||||
*out_data = s_data;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
/* ==========================================
|
||||
* 传感器配置函数 (与原版保持一致)
|
||||
* ========================================== */
|
||||
static HAL_StatusTypeDef HWT101_Unlock(void) {
|
||||
uint8_t cmd[5] = {0xFF, 0xAA, REG_KEY, 0x88, 0xB5};
|
||||
return HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||||
}
|
||||
|
||||
static HAL_StatusTypeDef HWT101_Save(void) {
|
||||
uint8_t cmd[5] = {0xFF, 0xAA, REG_SAVE, 0x00, 0x00};
|
||||
HAL_StatusTypeDef res = HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||||
osDelay(200);
|
||||
return res;
|
||||
}
|
||||
|
||||
static HAL_StatusTypeDef HWT101_WriteReg(uint8_t reg, int16_t value) {
|
||||
if (HWT101_Unlock() != HAL_OK) return HAL_ERROR;
|
||||
osDelay(20);
|
||||
uint8_t cmd[5] = {0xFF, 0xAA, reg, (uint8_t)(value & 0xFF), (uint8_t)(value >> 8)};
|
||||
return HAL_UART_Transmit(s_huart, cmd, 5, 20);
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef HWT101_Config(HWT101_Rate_t rate, HWT101_Baud_t baud) {
|
||||
if (HWT101_WriteReg(REG_RRATE, (int16_t)rate) != HAL_OK) return HAL_ERROR;
|
||||
osDelay(100);
|
||||
if (HWT101_WriteReg(REG_BAUD, (int16_t)baud) != HAL_OK) return HAL_ERROR;
|
||||
osDelay(100);
|
||||
return HWT101_Save();
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef HWT101_ZeroYaw(void) {
|
||||
if (HWT101_WriteReg(REG_CALIYAW, 0x0000) != HAL_OK) return HAL_ERROR;
|
||||
osDelay(500);
|
||||
|
||||
/* 重置 unwrap 状态，下次收到帧时重新初始化连续角度 */
|
||||
taskENTER_CRITICAL();
|
||||
s_yaw_initialized = false;
|
||||
s_yaw_continuous = 0.0f;
|
||||
s_yaw_prev = 0.0f;
|
||||
s_data.yaw_continuous = 0.0f;
|
||||
taskEXIT_CRITICAL();
|
||||
|
||||
return HWT101_Save();
|
||||
}
|
||||
|
||||
void HWT101_ErrorRecovery(UART_HandleTypeDef *huart) {
|
||||
if (s_huart != NULL && huart == s_huart) {
|
||||
HAL_UART_DMAStop(s_huart);
|
||||
|
||||
// 暴力清除 ORE/NE/FE/PE 错误标志，防止无限死循环中断风暴
|
||||
__HAL_UART_CLEAR_FLAG(s_huart, UART_CLEAR_OREF | UART_CLEAR_NEF | UART_CLEAR_PEF | UART_CLEAR_FEF);
|
||||
// 读取一次 RDR 寄存器，把里面卡住的垃圾字节倒掉
|
||||
volatile uint32_t dump = s_huart->Instance->RDR;
|
||||
(void)dump;
|
||||
|
||||
// 干净地重新启动 DMA
|
||||
HAL_UART_Receive_DMA(s_huart, s_dma_buf, HWT101_DMA_BUF_SIZE);
|
||||
s_read_ptr = 0;
|
||||
s_frame_idx = 0;
|
||||
}
|
||||
}
|
||||
41
App/IMU/hwt101.h
Normal file
41
App/IMU/hwt101.h
Normal file
@@ -0,0 +1,41 @@
|
||||
#ifndef HWT101_H
|
||||
#define HWT101_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "main.h"
|
||||
|
||||
#define HWT101_DMA_BUF_SIZE 256 // 必须是 32 的整数倍
|
||||
|
||||
typedef enum {
|
||||
HWT101_RATE_10HZ = 0x06,
|
||||
HWT101_RATE_50HZ = 0x08,
|
||||
HWT101_RATE_100HZ = 0x09, // 推荐：100Hz
|
||||
HWT101_RATE_200HZ = 0x0B
|
||||
} HWT101_Rate_t;
|
||||
|
||||
typedef enum {
|
||||
HWT101_BAUD_9600 = 0x02,
|
||||
HWT101_BAUD_115200 = 0x06,
|
||||
HWT101_BAUD_230400 = 0x07
|
||||
} HWT101_Baud_t;
|
||||
|
||||
typedef struct {
|
||||
float yaw; // 原始偏航角 (deg)，范围 [-180, +180)，跨界时跳变
|
||||
float yaw_continuous; // unwrap 后的连续偏航角 (deg)，无跳变，可直接做差计算转角
|
||||
float wz; // Z 轴角速度 (deg/s)
|
||||
uint32_t last_update;
|
||||
uint8_t online;
|
||||
} HWT101_Data_t;
|
||||
|
||||
/* API 函数声明 */
|
||||
void HWT101_Init(UART_HandleTypeDef *huart);
|
||||
void HWT101_Process(void);
|
||||
|
||||
// 【升级 2】改为传入指针，安全拷贝数据，防止数据撕裂
|
||||
void HWT101_GetData(HWT101_Data_t *out_data);
|
||||
// 在末尾 API 声明处加上这句
|
||||
void HWT101_ErrorRecovery(UART_HandleTypeDef *huart);
|
||||
HAL_StatusTypeDef HWT101_Config(HWT101_Rate_t rate, HWT101_Baud_t baud);
|
||||
HAL_StatusTypeDef HWT101_ZeroYaw(void);
|
||||
|
||||
#endif /* HWT101_H */
|
||||
143
App/VL53L0X_API/MIGRATION_NOTES.md
Normal file
143
App/VL53L0X_API/MIGRATION_NOTES.md
Normal file
@@ -0,0 +1,143 @@
|
||||
# VL53L0X_API Notes
|
||||
|
||||
本文档只说明 `VL53L0X_API` 目录里我们自己写的兼容层分别负责什么，方便后续在旧工程里继续集成。
|
||||
|
||||
## 目录分层
|
||||
|
||||
### `core/`
|
||||
|
||||
这是 ST 官方 API 核心层，主要负责：
|
||||
|
||||
- 设备初始化
|
||||
- 寄存器访问逻辑
|
||||
- 测距流程
|
||||
- 官方校准函数
|
||||
- `PerformRefSpadManagement`
|
||||
- `PerformRefCalibration`
|
||||
- `PerformOffsetCalibration`
|
||||
- `PerformXTalkCalibration`
|
||||
|
||||
这部分一般不要随便改，除非明确要修改 ST 官方行为。
|
||||
|
||||
### `platform/`
|
||||
|
||||
这是我们自己写的兼容层，负责把 ST API 接到 STM32 工程里。
|
||||
|
||||
## 我们自己写的兼容层文件说明
|
||||
|
||||
### `platform/vl53l0x_platform.h`
|
||||
|
||||
平台适配层头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 声明和 STM32 平台相关的接口
|
||||
- 给 ST API 提供底层读写函数声明
|
||||
- 提供总线、GPIO、延时、地址切换相关接口声明
|
||||
|
||||
### `platform/vl53l0x_platform.c`
|
||||
|
||||
平台适配层实现。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 对接 STM32 的 `HAL I2C`
|
||||
- 实现 VL53L0X 所需的寄存器读写
|
||||
- 绑定每颗传感器的 I2C 句柄和地址
|
||||
- 控制 `XSHUT`
|
||||
- 支持运行时改 I2C 地址
|
||||
|
||||
简单说，这一层负责把 ST API 的“平台无关接口”落到我们实际板子上。
|
||||
|
||||
### `platform/vl53_board.h`
|
||||
|
||||
板级管理层头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 定义多颗 VL53 共用的管理结构体
|
||||
- 定义快照结构 `Vl53BoardSnapshot_t`
|
||||
- 定义硬件配置结构 `Vl53BoardHwCfg_t`
|
||||
- 对外暴露板级接口，例如：
|
||||
- `Vl53Board_Init`
|
||||
- `Vl53Board_StartContinuous`
|
||||
- `Vl53Board_StopContinuous`
|
||||
- `Vl53Board_ReadAll`
|
||||
- 当前版本还扩展了校准相关数据结构和接口
|
||||
|
||||
这一层是“上层任务代码直接调用的 API”。
|
||||
|
||||
### `platform/vl53_board.c`
|
||||
|
||||
板级管理层实现。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 管理一组 VL53 传感器
|
||||
- 完成多颗设备的上电、拉低/拉高 `XSHUT`、分配运行地址
|
||||
- 调 ST 官方初始化流程
|
||||
- 启动/停止连续测量
|
||||
- 轮询读取测距结果
|
||||
- 组织成统一快照给上层使用
|
||||
- 对每颗测距值做简单卡尔曼滤波
|
||||
|
||||
如果说 `vl53l0x_platform.c` 是“底层总线适配”，那 `vl53_board.c` 就是“更贴近业务的板级封装”。
|
||||
|
||||
### `platform/vl53_calibration_config.h`
|
||||
|
||||
这是我们自己加的运行时校准配置头。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 集中保存已经确认过的校准值
|
||||
- 让上层在启动后把这些值写回 VL53
|
||||
|
||||
当前主要用于保存：
|
||||
|
||||
- `offset_micro_meters`
|
||||
- `xtalk_compensation_rate_mcps`
|
||||
- 对应的 `*_calibrated` 标志
|
||||
|
||||
这不是 ST 官方自带文件，是我们为了工程集成方便单独加的配置层。
|
||||
|
||||
### `platform/vl53l0x_types.h`
|
||||
|
||||
类型辅助头文件。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 补充平台侧需要的公共类型定义
|
||||
- 给 `platform` 层做类型适配
|
||||
|
||||
### `platform/vl53l0x_platform_log.h`
|
||||
|
||||
日志相关兼容头。
|
||||
|
||||
主要作用：
|
||||
|
||||
- 兼容 ST API 的日志宏
|
||||
- 当前工程里基本属于轻量占位层
|
||||
|
||||
## 推荐修改边界
|
||||
|
||||
如果后续要继续改 VL53 集成，优先修改：
|
||||
|
||||
- `platform/vl53_board.c/.h`
|
||||
- `platform/vl53_calibration_config.h`
|
||||
|
||||
如果只是换板子、换 I2C、换 XSHUT 引脚，优先修改：
|
||||
|
||||
- `platform/vl53l0x_platform.c/.h`
|
||||
|
||||
如果不是明确要动 ST 官方行为，尽量不要去改：
|
||||
|
||||
- `core/`
|
||||
|
||||
## 一句话总结
|
||||
|
||||
我们自己写的兼容层分两层：
|
||||
|
||||
- `vl53l0x_platform.*`：把 ST API 接到 STM32 HAL
|
||||
- `vl53_board.*`：把多颗 VL53 封装成上层任务可直接使用的板级接口
|
||||
|
||||
而 `vl53_calibration_config.h` 是我们额外加的“运行时固定标定值配置文件”。
|
||||
BIN
App/VL53L0X_API/VL53L0X.pdf
Normal file
BIN
App/VL53L0X_API/VL53L0X.pdf
Normal file
Binary file not shown.
1926
App/VL53L0X_API/core/inc/vl53l0x_api.h
Normal file
1926
App/VL53L0X_API/core/inc/vl53l0x_api.h
Normal file
File diff suppressed because it is too large
Load Diff
85
App/VL53L0X_API/core/inc/vl53l0x_api_calibration.h
Normal file
85
App/VL53L0X_API/core/inc/vl53l0x_api_calibration.h
Normal file
@@ -0,0 +1,85 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef _VL53L0X_API_CALIBRATION_H_
|
||||
#define _VL53L0X_API_CALIBRATION_H_
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
#include "vl53l0x_platform.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
VL53L0X_Error VL53L0X_perform_xtalk_calibration(VL53L0X_DEV Dev,
|
||||
FixPoint1616_t XTalkCalDistance,
|
||||
FixPoint1616_t *pXTalkCompensationRateMegaCps);
|
||||
|
||||
VL53L0X_Error VL53L0X_perform_offset_calibration(VL53L0X_DEV Dev,
|
||||
FixPoint1616_t CalDistanceMilliMeter,
|
||||
int32_t *pOffsetMicroMeter);
|
||||
|
||||
VL53L0X_Error VL53L0X_set_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
|
||||
int32_t OffsetCalibrationDataMicroMeter);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_offset_calibration_data_micro_meter(VL53L0X_DEV Dev,
|
||||
int32_t *pOffsetCalibrationDataMicroMeter);
|
||||
|
||||
VL53L0X_Error VL53L0X_apply_offset_adjustment(VL53L0X_DEV Dev);
|
||||
|
||||
VL53L0X_Error VL53L0X_perform_ref_spad_management(VL53L0X_DEV Dev,
|
||||
uint32_t *refSpadCount, uint8_t *isApertureSpads);
|
||||
|
||||
VL53L0X_Error VL53L0X_set_reference_spads(VL53L0X_DEV Dev,
|
||||
uint32_t count, uint8_t isApertureSpads);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_reference_spads(VL53L0X_DEV Dev,
|
||||
uint32_t *pSpadCount, uint8_t *pIsApertureSpads);
|
||||
|
||||
VL53L0X_Error VL53L0X_perform_phase_calibration(VL53L0X_DEV Dev,
|
||||
uint8_t *pPhaseCal, const uint8_t get_data_enable,
|
||||
const uint8_t restore_config);
|
||||
|
||||
VL53L0X_Error VL53L0X_perform_ref_calibration(VL53L0X_DEV Dev,
|
||||
uint8_t *pVhvSettings, uint8_t *pPhaseCal, uint8_t get_data_enable);
|
||||
|
||||
VL53L0X_Error VL53L0X_set_ref_calibration(VL53L0X_DEV Dev,
|
||||
uint8_t VhvSettings, uint8_t PhaseCal);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_ref_calibration(VL53L0X_DEV Dev,
|
||||
uint8_t *pVhvSettings, uint8_t *pPhaseCal);
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L0X_API_CALIBRATION_H_ */
|
||||
113
App/VL53L0X_API/core/inc/vl53l0x_api_core.h
Normal file
113
App/VL53L0X_API/core/inc/vl53l0x_api_core.h
Normal file
@@ -0,0 +1,113 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef _VL53L0X_API_CORE_H_
|
||||
#define _VL53L0X_API_CORE_H_
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
#include "vl53l0x_platform.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_reverse_bytes(uint8_t *data, uint32_t size);
|
||||
|
||||
VL53L0X_Error VL53L0X_measurement_poll_for_completion(VL53L0X_DEV Dev);
|
||||
|
||||
uint8_t VL53L0X_encode_vcsel_period(uint8_t vcsel_period_pclks);
|
||||
|
||||
uint8_t VL53L0X_decode_vcsel_period(uint8_t vcsel_period_reg);
|
||||
|
||||
uint32_t VL53L0X_isqrt(uint32_t num);
|
||||
|
||||
uint32_t VL53L0X_quadrature_sum(uint32_t a, uint32_t b);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_info_from_device(VL53L0X_DEV Dev, uint8_t option);
|
||||
|
||||
VL53L0X_Error VL53L0X_set_vcsel_pulse_period(VL53L0X_DEV Dev,
|
||||
VL53L0X_VcselPeriod VcselPeriodType, uint8_t VCSELPulsePeriodPCLK);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_vcsel_pulse_period(VL53L0X_DEV Dev,
|
||||
VL53L0X_VcselPeriod VcselPeriodType, uint8_t *pVCSELPulsePeriodPCLK);
|
||||
|
||||
uint32_t VL53L0X_decode_timeout(uint16_t encoded_timeout);
|
||||
|
||||
VL53L0X_Error get_sequence_step_timeout(VL53L0X_DEV Dev,
|
||||
VL53L0X_SequenceStepId SequenceStepId,
|
||||
uint32_t *pTimeOutMicroSecs);
|
||||
|
||||
VL53L0X_Error set_sequence_step_timeout(VL53L0X_DEV Dev,
|
||||
VL53L0X_SequenceStepId SequenceStepId,
|
||||
uint32_t TimeOutMicroSecs);
|
||||
|
||||
VL53L0X_Error VL53L0X_set_measurement_timing_budget_micro_seconds(
|
||||
VL53L0X_DEV Dev,
|
||||
uint32_t MeasurementTimingBudgetMicroSeconds);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_measurement_timing_budget_micro_seconds(
|
||||
VL53L0X_DEV Dev,
|
||||
uint32_t *pMeasurementTimingBudgetMicroSeconds);
|
||||
|
||||
VL53L0X_Error VL53L0X_load_tuning_settings(VL53L0X_DEV Dev,
|
||||
uint8_t *pTuningSettingBuffer);
|
||||
|
||||
VL53L0X_Error VL53L0X_calc_sigma_estimate(VL53L0X_DEV Dev,
|
||||
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||
FixPoint1616_t *pSigmaEstimate);
|
||||
|
||||
VL53L0X_Error VL53L0X_calc_dmax(
|
||||
VL53L0X_DEV Dev, FixPoint1616_t ambRateMeas, uint32_t *pdmax_mm);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_total_xtalk_rate(VL53L0X_DEV Dev,
|
||||
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||
FixPoint1616_t *ptotal_xtalk_rate_mcps);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_total_signal_rate(VL53L0X_DEV Dev,
|
||||
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||
FixPoint1616_t *ptotal_signal_rate_mcps);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_range_status(VL53L0X_DEV Dev,
|
||||
uint8_t DeviceRangeStatus,
|
||||
FixPoint1616_t SignalRate,
|
||||
uint16_t EffectiveSpadRtnCount,
|
||||
VL53L0X_RangingMeasurementData_t *pRangingMeasurementData,
|
||||
uint8_t *pPalRangeStatus);
|
||||
|
||||
uint32_t VL53L0X_calc_timeout_mclks(VL53L0X_DEV Dev,
|
||||
uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
|
||||
|
||||
uint16_t VL53L0X_encode_timeout(uint32_t timeout_macro_clks);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L0X_API_CORE_H_ */
|
||||
47
App/VL53L0X_API/core/inc/vl53l0x_api_ranging.h
Normal file
47
App/VL53L0X_API/core/inc/vl53l0x_api_ranging.h
Normal file
@@ -0,0 +1,47 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef _VL53L0X_API_RANGING_H_
|
||||
#define _VL53L0X_API_RANGING_H_
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
#include "vl53l0x_platform.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L0X_API_RANGING_H_ */
|
||||
278
App/VL53L0X_API/core/inc/vl53l0x_api_strings.h
Normal file
278
App/VL53L0X_API/core/inc/vl53l0x_api_strings.h
Normal file
@@ -0,0 +1,278 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef VL53L0X_API_STRINGS_H_
|
||||
#define VL53L0X_API_STRINGS_H_
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
#include "vl53l0x_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev,
|
||||
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode,
|
||||
char *pDeviceErrorString);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus,
|
||||
char *pRangeStatusString);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode,
|
||||
char *pPalErrorString);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode,
|
||||
char *pPalStateString);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_sequence_steps_info(
|
||||
VL53L0X_SequenceStepId SequenceStepId,
|
||||
char *pSequenceStepsString);
|
||||
|
||||
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev,
|
||||
uint16_t LimitCheckId,
|
||||
char *pLimitCheckString);
|
||||
|
||||
|
||||
#ifdef USE_EMPTY_STRING
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME ""
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS0 ""
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS1 ""
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS2 ""
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_ES1 ""
|
||||
#define VL53L0X_STRING_DEVICE_INFO_TYPE ""
|
||||
|
||||
/* PAL ERROR strings */
|
||||
#define VL53L0X_STRING_ERROR_NONE ""
|
||||
#define VL53L0X_STRING_ERROR_CALIBRATION_WARNING ""
|
||||
#define VL53L0X_STRING_ERROR_MIN_CLIPPED ""
|
||||
#define VL53L0X_STRING_ERROR_UNDEFINED ""
|
||||
#define VL53L0X_STRING_ERROR_INVALID_PARAMS ""
|
||||
#define VL53L0X_STRING_ERROR_NOT_SUPPORTED ""
|
||||
#define VL53L0X_STRING_ERROR_RANGE_ERROR ""
|
||||
#define VL53L0X_STRING_ERROR_TIME_OUT ""
|
||||
#define VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED ""
|
||||
#define VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL ""
|
||||
#define VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING ""
|
||||
#define VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ""
|
||||
#define VL53L0X_STRING_ERROR_CONTROL_INTERFACE ""
|
||||
#define VL53L0X_STRING_ERROR_INVALID_COMMAND ""
|
||||
#define VL53L0X_STRING_ERROR_DIVISION_BY_ZERO ""
|
||||
#define VL53L0X_STRING_ERROR_REF_SPAD_INIT ""
|
||||
#define VL53L0X_STRING_ERROR_NOT_IMPLEMENTED ""
|
||||
|
||||
#define VL53L0X_STRING_UNKNOW_ERROR_CODE ""
|
||||
|
||||
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L0X_STRING_RANGESTATUS_NONE ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_RANGEVALID ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_SIGMA ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_SIGNAL ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_MINRANGE ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_PHASE ""
|
||||
#define VL53L0X_STRING_RANGESTATUS_HW ""
|
||||
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L0X_STRING_STATE_POWERDOWN ""
|
||||
#define VL53L0X_STRING_STATE_WAIT_STATICINIT ""
|
||||
#define VL53L0X_STRING_STATE_STANDBY ""
|
||||
#define VL53L0X_STRING_STATE_IDLE ""
|
||||
#define VL53L0X_STRING_STATE_RUNNING ""
|
||||
#define VL53L0X_STRING_STATE_UNKNOWN ""
|
||||
#define VL53L0X_STRING_STATE_ERROR ""
|
||||
|
||||
|
||||
/* Device Specific */
|
||||
#define VL53L0X_STRING_DEVICEERROR_NONE ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_SNRCHECK ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_TCC ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_MINCLIP ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD ""
|
||||
#define VL53L0X_STRING_DEVICEERROR_UNKNOWN ""
|
||||
|
||||
/* Check Enable */
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE ""
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE ""
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP ""
|
||||
#define VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD ""
|
||||
|
||||
/* Sequence Step */
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_TCC ""
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_DSS ""
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_MSRC ""
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE ""
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE ""
|
||||
#else
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME "VL53L0X cut1.0"
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS0 "VL53L0X TS0"
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS1 "VL53L0X TS1"
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_TS2 "VL53L0X TS2"
|
||||
#define VL53L0X_STRING_DEVICE_INFO_NAME_ES1 "VL53L0X ES1 or later"
|
||||
#define VL53L0X_STRING_DEVICE_INFO_TYPE "VL53L0X"
|
||||
|
||||
/* PAL ERROR strings */
|
||||
#define VL53L0X_STRING_ERROR_NONE \
|
||||
"No Error"
|
||||
#define VL53L0X_STRING_ERROR_CALIBRATION_WARNING \
|
||||
"Calibration Warning Error"
|
||||
#define VL53L0X_STRING_ERROR_MIN_CLIPPED \
|
||||
"Min clipped error"
|
||||
#define VL53L0X_STRING_ERROR_UNDEFINED \
|
||||
"Undefined error"
|
||||
#define VL53L0X_STRING_ERROR_INVALID_PARAMS \
|
||||
"Invalid parameters error"
|
||||
#define VL53L0X_STRING_ERROR_NOT_SUPPORTED \
|
||||
"Not supported error"
|
||||
#define VL53L0X_STRING_ERROR_RANGE_ERROR \
|
||||
"Range error"
|
||||
#define VL53L0X_STRING_ERROR_TIME_OUT \
|
||||
"Time out error"
|
||||
#define VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED \
|
||||
"Mode not supported error"
|
||||
#define VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL \
|
||||
"Buffer too small"
|
||||
#define VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING \
|
||||
"GPIO not existing"
|
||||
#define VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED \
|
||||
"GPIO funct not supported"
|
||||
#define VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED \
|
||||
"Interrupt not Cleared"
|
||||
#define VL53L0X_STRING_ERROR_CONTROL_INTERFACE \
|
||||
"Control Interface Error"
|
||||
#define VL53L0X_STRING_ERROR_INVALID_COMMAND \
|
||||
"Invalid Command Error"
|
||||
#define VL53L0X_STRING_ERROR_DIVISION_BY_ZERO \
|
||||
"Division by zero Error"
|
||||
#define VL53L0X_STRING_ERROR_REF_SPAD_INIT \
|
||||
"Reference Spad Init Error"
|
||||
#define VL53L0X_STRING_ERROR_NOT_IMPLEMENTED \
|
||||
"Not implemented error"
|
||||
|
||||
#define VL53L0X_STRING_UNKNOW_ERROR_CODE \
|
||||
"Unknown Error Code"
|
||||
|
||||
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L0X_STRING_RANGESTATUS_NONE "No Update"
|
||||
#define VL53L0X_STRING_RANGESTATUS_RANGEVALID "Range Valid"
|
||||
#define VL53L0X_STRING_RANGESTATUS_SIGMA "Sigma Fail"
|
||||
#define VL53L0X_STRING_RANGESTATUS_SIGNAL "Signal Fail"
|
||||
#define VL53L0X_STRING_RANGESTATUS_MINRANGE "Min Range Fail"
|
||||
#define VL53L0X_STRING_RANGESTATUS_PHASE "Phase Fail"
|
||||
#define VL53L0X_STRING_RANGESTATUS_HW "Hardware Fail"
|
||||
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L0X_STRING_STATE_POWERDOWN "POWERDOWN State"
|
||||
#define VL53L0X_STRING_STATE_WAIT_STATICINIT \
|
||||
"Wait for staticinit State"
|
||||
#define VL53L0X_STRING_STATE_STANDBY "STANDBY State"
|
||||
#define VL53L0X_STRING_STATE_IDLE "IDLE State"
|
||||
#define VL53L0X_STRING_STATE_RUNNING "RUNNING State"
|
||||
#define VL53L0X_STRING_STATE_UNKNOWN "UNKNOWN State"
|
||||
#define VL53L0X_STRING_STATE_ERROR "ERROR State"
|
||||
|
||||
|
||||
/* Device Specific */
|
||||
#define VL53L0X_STRING_DEVICEERROR_NONE "No Update"
|
||||
#define VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE \
|
||||
"VCSEL Continuity Test Failure"
|
||||
#define VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE \
|
||||
"VCSEL Watchdog Test Failure"
|
||||
#define VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND \
|
||||
"No VHV Value found"
|
||||
#define VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET \
|
||||
"MSRC No Target Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_SNRCHECK \
|
||||
"SNR Check Exit"
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK \
|
||||
"Range Phase Check Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK \
|
||||
"Sigma Threshold Check Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_TCC \
|
||||
"TCC Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY \
|
||||
"Phase Consistency Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_MINCLIP \
|
||||
"Min Clip Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE \
|
||||
"Range Complete"
|
||||
#define VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW \
|
||||
"Range Algo Underflow Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW \
|
||||
"Range Algo Overlow Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD \
|
||||
"Range Ignore Threshold Error"
|
||||
#define VL53L0X_STRING_DEVICEERROR_UNKNOWN \
|
||||
"Unknown error code"
|
||||
|
||||
/* Check Enable */
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE \
|
||||
"SIGMA FINAL RANGE"
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE \
|
||||
"SIGNAL RATE FINAL RANGE"
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP \
|
||||
"SIGNAL REF CLIP"
|
||||
#define VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD \
|
||||
"RANGE IGNORE THRESHOLD"
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC \
|
||||
"SIGNAL RATE MSRC"
|
||||
#define VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE \
|
||||
"SIGNAL RATE PRE RANGE"
|
||||
|
||||
/* Sequence Step */
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_TCC "TCC"
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_DSS "DSS"
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_MSRC "MSRC"
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE "PRE RANGE"
|
||||
#define VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE "FINAL RANGE"
|
||||
#endif /* USE_EMPTY_STRING */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
666
App/VL53L0X_API/core/inc/vl53l0x_def.h
Normal file
666
App/VL53L0X_API/core/inc/vl53l0x_def.h
Normal file
@@ -0,0 +1,666 @@
|
||||
/*******************************************************************************
|
||||
* Copyright <20> 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/**
|
||||
* @file VL53L0X_def.h
|
||||
*
|
||||
* @brief Type definitions for VL53L0X API.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L0X_DEF_H_
|
||||
#define _VL53L0X_DEF_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @defgroup VL53L0X_globaldefine_group VL53L0X Defines
|
||||
* @brief VL53L0X Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** PAL SPECIFICATION major version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_MAJOR 1
|
||||
/** PAL SPECIFICATION minor version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_MINOR 2
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_SUB 7
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X10_SPECIFICATION_VER_REVISION 1440
|
||||
|
||||
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_MAJOR 1
|
||||
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_MINOR 0
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_SUB 9
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X10_IMPLEMENTATION_VER_REVISION 3673
|
||||
|
||||
/** PAL SPECIFICATION major version */
|
||||
#define VL53L0X_SPECIFICATION_VER_MAJOR 1
|
||||
/** PAL SPECIFICATION minor version */
|
||||
#define VL53L0X_SPECIFICATION_VER_MINOR 2
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X_SPECIFICATION_VER_SUB 7
|
||||
/** PAL SPECIFICATION sub version */
|
||||
#define VL53L0X_SPECIFICATION_VER_REVISION 1440
|
||||
|
||||
/** VL53L0X PAL IMPLEMENTATION major version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_MAJOR 1
|
||||
/** VL53L0X PAL IMPLEMENTATION minor version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_MINOR 0
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_SUB 4
|
||||
/** VL53L0X PAL IMPLEMENTATION sub version */
|
||||
#define VL53L0X_IMPLEMENTATION_VER_REVISION 4960
|
||||
#define VL53L0X_DEFAULT_MAX_LOOP 2000
|
||||
#define VL53L0X_MAX_STRING_LENGTH 32
|
||||
|
||||
|
||||
#include "vl53l0x_device.h"
|
||||
#include "vl53l0x_types.h"
|
||||
|
||||
|
||||
/****************************************
|
||||
* PRIVATE define do not edit
|
||||
****************************************/
|
||||
|
||||
/** @brief Defines the parameters of the Get Version Functions
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t revision; /*!< revision number */
|
||||
uint8_t major; /*!< major number */
|
||||
uint8_t minor; /*!< minor number */
|
||||
uint8_t build; /*!< build number */
|
||||
} VL53L0X_Version_t;
|
||||
|
||||
|
||||
/** @brief Defines the parameters of the Get Device Info Functions
|
||||
*/
|
||||
typedef struct {
|
||||
char Name[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Name of the Device e.g. Left_Distance */
|
||||
char Type[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Type of the Device e.g VL53L0X */
|
||||
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||
/*!< Product Identifier String */
|
||||
uint8_t ProductType;
|
||||
/*!< Product Type, VL53L0X = 1, VL53L1 = 2 */
|
||||
uint8_t ProductRevisionMajor;
|
||||
/*!< Product revision major */
|
||||
uint8_t ProductRevisionMinor;
|
||||
/*!< Product revision minor */
|
||||
} VL53L0X_DeviceInfo_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_Error_group Error and Warning code returned by API
|
||||
* The following DEFINE are used to identify the PAL ERROR
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef int8_t VL53L0X_Error;
|
||||
|
||||
#define VL53L0X_ERROR_NONE ((VL53L0X_Error) 0)
|
||||
#define VL53L0X_ERROR_CALIBRATION_WARNING ((VL53L0X_Error) - 1)
|
||||
/*!< Warning invalid calibration data may be in used
|
||||
* \a VL53L0X_InitData()
|
||||
* \a VL53L0X_GetOffsetCalibrationData
|
||||
* \a VL53L0X_SetOffsetCalibrationData
|
||||
*/
|
||||
#define VL53L0X_ERROR_MIN_CLIPPED ((VL53L0X_Error) - 2)
|
||||
/*!< Warning parameter passed was clipped to min before to be applied */
|
||||
|
||||
#define VL53L0X_ERROR_UNDEFINED ((VL53L0X_Error) - 3)
|
||||
/*!< Unqualified error */
|
||||
#define VL53L0X_ERROR_INVALID_PARAMS ((VL53L0X_Error) - 4)
|
||||
/*!< Parameter passed is invalid or out of range */
|
||||
#define VL53L0X_ERROR_NOT_SUPPORTED ((VL53L0X_Error) - 5)
|
||||
/*!< Function is not supported in current mode or configuration */
|
||||
#define VL53L0X_ERROR_RANGE_ERROR ((VL53L0X_Error) - 6)
|
||||
/*!< Device report a ranging error interrupt status */
|
||||
#define VL53L0X_ERROR_TIME_OUT ((VL53L0X_Error) - 7)
|
||||
/*!< Aborted due to time out */
|
||||
#define VL53L0X_ERROR_MODE_NOT_SUPPORTED ((VL53L0X_Error) - 8)
|
||||
/*!< Asked mode is not supported by the device */
|
||||
#define VL53L0X_ERROR_BUFFER_TOO_SMALL ((VL53L0X_Error) - 9)
|
||||
/*!< ... */
|
||||
#define VL53L0X_ERROR_GPIO_NOT_EXISTING ((VL53L0X_Error) - 10)
|
||||
/*!< User tried to setup a non-existing GPIO pin */
|
||||
#define VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L0X_Error) - 11)
|
||||
/*!< unsupported GPIO functionality */
|
||||
#define VL53L0X_ERROR_INTERRUPT_NOT_CLEARED ((VL53L0X_Error) - 12)
|
||||
/*!< Error during interrupt clear */
|
||||
#define VL53L0X_ERROR_CONTROL_INTERFACE ((VL53L0X_Error) - 20)
|
||||
/*!< error reported from IO functions */
|
||||
#define VL53L0X_ERROR_INVALID_COMMAND ((VL53L0X_Error) - 30)
|
||||
/*!< The command is not allowed in the current device state
|
||||
* (power down)
|
||||
*/
|
||||
#define VL53L0X_ERROR_DIVISION_BY_ZERO ((VL53L0X_Error) - 40)
|
||||
/*!< In the function a division by zero occurs */
|
||||
#define VL53L0X_ERROR_REF_SPAD_INIT ((VL53L0X_Error) - 50)
|
||||
/*!< Error during reference SPAD initialization */
|
||||
#define VL53L0X_ERROR_NOT_IMPLEMENTED ((VL53L0X_Error) - 99)
|
||||
/*!< Tells requested functionality has not been implemented yet or
|
||||
* not compatible with the device
|
||||
*/
|
||||
/** @} VL53L0X_define_Error_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_DeviceModes_group Defines Device modes
|
||||
* Defines all possible modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_DeviceModes;
|
||||
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_RANGING ((VL53L0X_DeviceModes) 0)
|
||||
#define VL53L0X_DEVICEMODE_CONTINUOUS_RANGING ((VL53L0X_DeviceModes) 1)
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_HISTOGRAM ((VL53L0X_DeviceModes) 2)
|
||||
#define VL53L0X_DEVICEMODE_CONTINUOUS_TIMED_RANGING ((VL53L0X_DeviceModes) 3)
|
||||
#define VL53L0X_DEVICEMODE_SINGLE_ALS ((VL53L0X_DeviceModes) 10)
|
||||
#define VL53L0X_DEVICEMODE_GPIO_DRIVE ((VL53L0X_DeviceModes) 20)
|
||||
#define VL53L0X_DEVICEMODE_GPIO_OSC ((VL53L0X_DeviceModes) 21)
|
||||
/* ... Modes to be added depending on device */
|
||||
/** @} VL53L0X_define_DeviceModes_group */
|
||||
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_HistogramModes_group Defines Histogram modes
|
||||
* Defines all possible Histogram modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_HistogramModes;
|
||||
|
||||
#define VL53L0X_HISTOGRAMMODE_DISABLED ((VL53L0X_HistogramModes) 0)
|
||||
/*!< Histogram Disabled */
|
||||
#define VL53L0X_HISTOGRAMMODE_REFERENCE_ONLY ((VL53L0X_HistogramModes) 1)
|
||||
/*!< Histogram Reference array only */
|
||||
#define VL53L0X_HISTOGRAMMODE_RETURN_ONLY ((VL53L0X_HistogramModes) 2)
|
||||
/*!< Histogram Return array only */
|
||||
#define VL53L0X_HISTOGRAMMODE_BOTH ((VL53L0X_HistogramModes) 3)
|
||||
/*!< Histogram both Reference and Return Arrays */
|
||||
/* ... Modes to be added depending on device */
|
||||
/** @} VL53L0X_define_HistogramModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_PowerModes_group List of available Power Modes
|
||||
* List of available Power Modes
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L0X_PowerModes;
|
||||
|
||||
#define VL53L0X_POWERMODE_STANDBY_LEVEL1 ((VL53L0X_PowerModes) 0)
|
||||
/*!< Standby level 1 */
|
||||
#define VL53L0X_POWERMODE_STANDBY_LEVEL2 ((VL53L0X_PowerModes) 1)
|
||||
/*!< Standby level 2 */
|
||||
#define VL53L0X_POWERMODE_IDLE_LEVEL1 ((VL53L0X_PowerModes) 2)
|
||||
/*!< Idle level 1 */
|
||||
#define VL53L0X_POWERMODE_IDLE_LEVEL2 ((VL53L0X_PowerModes) 3)
|
||||
/*!< Idle level 2 */
|
||||
|
||||
/** @} VL53L0X_define_PowerModes_group */
|
||||
|
||||
|
||||
#define VL53L0X_DMAX_LUT_SIZE 7
|
||||
/*!< Defines the number of items in the DMAX lookup table */
|
||||
|
||||
/** @brief Structure defining data pair that makes up the DMAX Lookup table.
|
||||
*/
|
||||
typedef struct {
|
||||
FixPoint1616_t ambRate_mcps[VL53L0X_DMAX_LUT_SIZE];
|
||||
/*!< Ambient rate (mcps) */
|
||||
FixPoint1616_t dmax_mm[VL53L0X_DMAX_LUT_SIZE];
|
||||
/*!< DMAX Value (mm) */
|
||||
} VL53L0X_DMaxLUT_t;
|
||||
|
||||
/** @brief Defines all parameters for the device
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L0X_DeviceModes DeviceMode;
|
||||
/*!< Defines type of measurement to be done for the next measure */
|
||||
VL53L0X_HistogramModes HistogramMode;
|
||||
/*!< Defines type of histogram measurement to be done for the next
|
||||
* measure
|
||||
*/
|
||||
uint32_t MeasurementTimingBudgetMicroSeconds;
|
||||
/*!< Defines the allowed total time for a single measurement */
|
||||
uint32_t InterMeasurementPeriodMilliSeconds;
|
||||
/*!< Defines time between two consecutive measurements (between two
|
||||
* measurement starts). If set to 0 means back-to-back mode
|
||||
*/
|
||||
uint8_t XTalkCompensationEnable;
|
||||
/*!< Tells if Crosstalk compensation shall be enable or not */
|
||||
uint16_t XTalkCompensationRangeMilliMeter;
|
||||
/*!< CrossTalk compensation range in millimeter */
|
||||
FixPoint1616_t XTalkCompensationRateMegaCps;
|
||||
/*!< CrossTalk compensation rate in Mega counts per seconds.
|
||||
* Expressed in 16.16 fixed point format.
|
||||
*/
|
||||
int32_t RangeOffsetMicroMeters;
|
||||
/*!< Range offset adjustment (mm). */
|
||||
|
||||
uint8_t LimitChecksEnable[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Limit Check enable for this device. */
|
||||
uint8_t LimitChecksStatus[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Status of the check linked to last
|
||||
* measurement.
|
||||
*/
|
||||
FixPoint1616_t LimitChecksValue[VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Limit Check value for this device */
|
||||
|
||||
VL53L0X_DMaxLUT_t dmax_lut;
|
||||
/*!< Lookup table defining ambient rates and associated
|
||||
* dmax values.
|
||||
*/
|
||||
|
||||
uint8_t WrapAroundCheckEnable;
|
||||
/*!< Tells if Wrap Around Check shall be enable or not */
|
||||
} VL53L0X_DeviceParameters_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_State_group Defines the current status
|
||||
* of the device
|
||||
* Defines the current status of the device
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L0X_State;
|
||||
|
||||
#define VL53L0X_STATE_POWERDOWN ((VL53L0X_State) 0)
|
||||
/*!< Device is in HW reset */
|
||||
#define VL53L0X_STATE_WAIT_STATICINIT ((VL53L0X_State) 1)
|
||||
/*!< Device is initialized and wait for static initialization */
|
||||
#define VL53L0X_STATE_STANDBY ((VL53L0X_State) 2)
|
||||
/*!< Device is in Low power Standby mode */
|
||||
#define VL53L0X_STATE_IDLE ((VL53L0X_State) 3)
|
||||
/*!< Device has been initialized and ready to do measurements */
|
||||
#define VL53L0X_STATE_RUNNING ((VL53L0X_State) 4)
|
||||
/*!< Device is performing measurement */
|
||||
#define VL53L0X_STATE_UNKNOWN ((VL53L0X_State) 98)
|
||||
/*!< Device is in unknown state and need to be rebooted */
|
||||
#define VL53L0X_STATE_ERROR ((VL53L0X_State) 99)
|
||||
/*!< Device is in error state and need to be rebooted */
|
||||
|
||||
/** @} VL53L0X_define_State_group */
|
||||
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_RangeData_t
|
||||
* @brief Range measurement data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t TimeStamp; /*!< 32-bit time stamp. */
|
||||
uint32_t MeasurementTimeUsec;
|
||||
/*!< Give the Measurement time needed by the device to do the
|
||||
* measurement.
|
||||
*/
|
||||
|
||||
|
||||
uint16_t RangeMilliMeter; /*!< range distance in millimeter. */
|
||||
|
||||
uint16_t RangeDMaxMilliMeter;
|
||||
/*!< Tells what is the maximum detection distance of the device
|
||||
* in current setup and environment conditions (Filled when
|
||||
* applicable)
|
||||
*/
|
||||
|
||||
FixPoint1616_t SignalRateRtnMegaCps;
|
||||
/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of target
|
||||
* reflectance.
|
||||
*/
|
||||
FixPoint1616_t AmbientRateRtnMegaCps;
|
||||
/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of the ambien
|
||||
* t light.
|
||||
*/
|
||||
|
||||
uint16_t EffectiveSpadRtnCount;
|
||||
/*!< Return the effective SPAD count for the return signal.
|
||||
* To obtain Real value it should be divided by 256
|
||||
*/
|
||||
|
||||
uint8_t ZoneId;
|
||||
/*!< Denotes which zone and range scheduler stage the range
|
||||
* data relates to.
|
||||
*/
|
||||
uint8_t RangeFractionalPart;
|
||||
/*!< Fractional part of range distance. Final value is a
|
||||
* FixPoint168 value.
|
||||
*/
|
||||
uint8_t RangeStatus;
|
||||
/*!< Range Status for the current measurement. This is device
|
||||
* dependent. Value = 0 means value is valid.
|
||||
* See \ref RangeStatusPage
|
||||
*/
|
||||
} VL53L0X_RangingMeasurementData_t;
|
||||
|
||||
|
||||
#define VL53L0X_HISTOGRAM_BUFFER_SIZE 24
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_HistogramData_t
|
||||
* @brief Histogram measurement data.
|
||||
*/
|
||||
typedef struct {
|
||||
/* Histogram Measurement data */
|
||||
uint32_t HistogramData[VL53L0X_HISTOGRAM_BUFFER_SIZE];
|
||||
/*!< Histogram data */
|
||||
/*!< Indicate the types of histogram data :
|
||||
*Return only, Reference only, both Return and Reference
|
||||
*/
|
||||
uint8_t FirstBin; /*!< First Bin value */
|
||||
uint8_t BufferSize; /*!< Buffer Size - Set by the user.*/
|
||||
uint8_t NumberOfBins;
|
||||
/*!< Number of bins filled by the histogram measurement */
|
||||
|
||||
VL53L0X_DeviceError ErrorStatus;
|
||||
/*!< Error status of the current measurement. \n
|
||||
* see @a ::VL53L0X_DeviceError @a VL53L0X_GetStatusErrorString()
|
||||
*/
|
||||
} VL53L0X_HistogramMeasurementData_t;
|
||||
|
||||
#define VL53L0X_REF_SPAD_BUFFER_SIZE 6
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_SpadData_t
|
||||
* @brief Spad Configuration Data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t RefSpadEnables[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||
/*!< Reference Spad Enables */
|
||||
uint8_t RefGoodSpadMap[VL53L0X_REF_SPAD_BUFFER_SIZE];
|
||||
/*!< Reference Spad Good Spad Map */
|
||||
} VL53L0X_SpadData_t;
|
||||
|
||||
typedef struct {
|
||||
FixPoint1616_t OscFrequencyMHz; /* Frequency used */
|
||||
|
||||
uint16_t LastEncodedTimeout;
|
||||
/* last encoded Time out used for timing budget*/
|
||||
|
||||
VL53L0X_GpioFunctionality Pin0GpioFunctionality;
|
||||
/* store the functionality of the GPIO: pin0 */
|
||||
|
||||
uint32_t FinalRangeTimeoutMicroSecs;
|
||||
/*!< Execution time of the final range*/
|
||||
uint8_t FinalRangeVcselPulsePeriod;
|
||||
/*!< Vcsel pulse period (pll clocks) for the final range measurement*/
|
||||
uint32_t PreRangeTimeoutMicroSecs;
|
||||
/*!< Execution time of the final range*/
|
||||
uint8_t PreRangeVcselPulsePeriod;
|
||||
/*!< Vcsel pulse period (pll clocks) for the pre-range measurement*/
|
||||
|
||||
uint16_t SigmaEstRefArray;
|
||||
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||
uint16_t SigmaEstEffPulseWidth;
|
||||
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||
* of ns e.g. 900 = 9.0ns
|
||||
*/
|
||||
uint16_t SigmaEstEffAmbWidth;
|
||||
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||
* e.g. 500 = 5.0ns
|
||||
*/
|
||||
|
||||
|
||||
/* Indicate if read from device has been done (==1) or not (==0) */
|
||||
uint8_t ReadDataFromDeviceDone;
|
||||
uint8_t ModuleId; /* Module ID */
|
||||
uint8_t Revision; /* test Revision */
|
||||
char ProductId[VL53L0X_MAX_STRING_LENGTH];
|
||||
/* Product Identifier String */
|
||||
uint8_t ReferenceSpadCount; /* used for ref spad management */
|
||||
uint8_t ReferenceSpadType; /* used for ref spad management */
|
||||
uint8_t RefSpadsInitialised; /* reports if ref spads are initialised. */
|
||||
uint32_t PartUIDUpper; /*!< Unique Part ID Upper */
|
||||
uint32_t PartUIDLower; /*!< Unique Part ID Lower */
|
||||
/*!< Peek Signal rate at 400 mm*/
|
||||
FixPoint1616_t SignalRateMeasFixed400mm;
|
||||
|
||||
} VL53L0X_DeviceSpecificParameters_t;
|
||||
|
||||
/**
|
||||
* @struct VL53L0X_DevData_t
|
||||
*
|
||||
* @brief VL53L0X PAL device ST private data structure \n
|
||||
* End user should never access any of these field directly
|
||||
*
|
||||
* These must never access directly but only via macro
|
||||
*/
|
||||
typedef struct {
|
||||
int32_t Part2PartOffsetNVMMicroMeter;
|
||||
/*!< backed up NVM value */
|
||||
int32_t Part2PartOffsetAdjustmentNVMMicroMeter;
|
||||
/*!< backed up NVM value representing additional offset adjustment */
|
||||
VL53L0X_DeviceParameters_t CurrentParameters;
|
||||
/*!< Current Device Parameter */
|
||||
VL53L0X_RangingMeasurementData_t LastRangeMeasure;
|
||||
/*!< Ranging Data */
|
||||
VL53L0X_HistogramMeasurementData_t LastHistogramMeasure;
|
||||
/*!< Histogram Data */
|
||||
VL53L0X_DeviceSpecificParameters_t DeviceSpecificParameters;
|
||||
/*!< Parameters specific to the device */
|
||||
VL53L0X_SpadData_t SpadData;
|
||||
/*!< Spad Data */
|
||||
uint8_t SequenceConfig;
|
||||
/*!< Internal value for the sequence config */
|
||||
uint8_t RangeFractionalEnable;
|
||||
/*!< Enable/Disable fractional part of ranging data */
|
||||
VL53L0X_State PalState;
|
||||
/*!< Current state of the PAL for this device */
|
||||
VL53L0X_PowerModes PowerMode;
|
||||
/*!< Current Power Mode */
|
||||
uint16_t SigmaEstRefArray;
|
||||
/*!< Reference array sigma value in 1/100th of [mm] e.g. 100 = 1mm */
|
||||
uint16_t SigmaEstEffPulseWidth;
|
||||
/*!< Effective Pulse width for sigma estimate in 1/100th
|
||||
* of ns e.g. 900 = 9.0ns
|
||||
*/
|
||||
uint16_t SigmaEstEffAmbWidth;
|
||||
/*!< Effective Ambient width for sigma estimate in 1/100th of ns
|
||||
* e.g. 500 = 5.0ns
|
||||
*/
|
||||
uint8_t StopVariable;
|
||||
/*!< StopVariable used during the stop sequence */
|
||||
uint16_t targetRefRate;
|
||||
/*!< Target Ambient Rate for Ref spad management */
|
||||
FixPoint1616_t SigmaEstimate;
|
||||
/*!< Sigma Estimate - based on ambient & VCSEL rates and
|
||||
* signal_total_events
|
||||
*/
|
||||
FixPoint1616_t SignalEstimate;
|
||||
/*!< Signal Estimate - based on ambient & VCSEL rates and cross talk */
|
||||
FixPoint1616_t LastSignalRefMcps;
|
||||
/*!< Latest Signal ref in Mcps */
|
||||
uint8_t *pTuningSettingsPointer;
|
||||
/*!< Pointer for Tuning Settings table */
|
||||
uint8_t UseInternalTuningSettings;
|
||||
/*!< Indicate if we use Tuning Settings table */
|
||||
uint16_t LinearityCorrectiveGain;
|
||||
/*!< Linearity Corrective Gain value in x1000 */
|
||||
} VL53L0X_DevData_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_InterruptPolarity_group Defines the Polarity
|
||||
* of the Interrupt
|
||||
* Defines the Polarity of the Interrupt
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_InterruptPolarity;
|
||||
|
||||
#define VL53L0X_INTERRUPTPOLARITY_LOW ((VL53L0X_InterruptPolarity) 0)
|
||||
/*!< Set active low polarity best setup for falling edge. */
|
||||
#define VL53L0X_INTERRUPTPOLARITY_HIGH ((VL53L0X_InterruptPolarity) 1)
|
||||
/*!< Set active high polarity best setup for rising edge. */
|
||||
|
||||
/** @} VL53L0X_define_InterruptPolarity_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_define_VcselPeriod_group Vcsel Period Defines
|
||||
* Defines the range measurement for which to access the vcsel period.
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_VcselPeriod;
|
||||
|
||||
#define VL53L0X_VCSEL_PERIOD_PRE_RANGE ((VL53L0X_VcselPeriod) 0)
|
||||
/*!<Identifies the pre-range vcsel period. */
|
||||
#define VL53L0X_VCSEL_PERIOD_FINAL_RANGE ((VL53L0X_VcselPeriod) 1)
|
||||
/*!<Identifies the final range vcsel period. */
|
||||
|
||||
/** @} VL53L0X_define_VcselPeriod_group */
|
||||
|
||||
/** @defgroup VL53L0X_define_SchedulerSequence_group Defines the steps
|
||||
* carried out by the scheduler during a range measurement.
|
||||
* @{
|
||||
* Defines the states of all the steps in the scheduler
|
||||
* i.e. enabled/disabled.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t TccOn; /*!<Reports if Target Centre Check On */
|
||||
uint8_t MsrcOn; /*!<Reports if MSRC On */
|
||||
uint8_t DssOn; /*!<Reports if DSS On */
|
||||
uint8_t PreRangeOn; /*!<Reports if Pre-Range On */
|
||||
uint8_t FinalRangeOn; /*!<Reports if Final-Range On */
|
||||
} VL53L0X_SchedulerSequenceSteps_t;
|
||||
|
||||
/** @} VL53L0X_define_SchedulerSequence_group */
|
||||
|
||||
/** @defgroup VL53L0X_define_SequenceStepId_group Defines the Polarity
|
||||
* of the Interrupt
|
||||
* Defines the the sequence steps performed during ranging..
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_SequenceStepId;
|
||||
|
||||
#define VL53L0X_SEQUENCESTEP_TCC ((VL53L0X_VcselPeriod) 0)
|
||||
/*!<Target CentreCheck identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_DSS ((VL53L0X_VcselPeriod) 1)
|
||||
/*!<Dynamic Spad Selection function Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_MSRC ((VL53L0X_VcselPeriod) 2)
|
||||
/*!<Minimum Signal Rate Check function Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_PRE_RANGE ((VL53L0X_VcselPeriod) 3)
|
||||
/*!<Pre-Range check Identifier. */
|
||||
#define VL53L0X_SEQUENCESTEP_FINAL_RANGE ((VL53L0X_VcselPeriod) 4)
|
||||
/*!<Final Range Check Identifier. */
|
||||
|
||||
#define VL53L0X_SEQUENCESTEP_NUMBER_OF_CHECKS 5
|
||||
/*!<Number of Sequence Step Managed by the API. */
|
||||
|
||||
/** @} VL53L0X_define_SequenceStepId_group */
|
||||
|
||||
|
||||
/* MACRO Definitions */
|
||||
/** @defgroup VL53L0X_define_GeneralMacro_group General Macro Defines
|
||||
* General Macro Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Defines */
|
||||
#define VL53L0X_SETPARAMETERFIELD(Dev, field, value) \
|
||||
PALDevDataSet(Dev, CurrentParameters.field, value)
|
||||
|
||||
#define VL53L0X_GETPARAMETERFIELD(Dev, field, variable) \
|
||||
(variable = ((PALDevDataGet(Dev, CurrentParameters)).field))
|
||||
|
||||
|
||||
#define VL53L0X_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
|
||||
PALDevDataSet(Dev, CurrentParameters.field[index], value)
|
||||
|
||||
#define VL53L0X_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
|
||||
(variable = (PALDevDataGet(Dev, CurrentParameters)).field[index])
|
||||
|
||||
#define VL53L0X_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
|
||||
PALDevDataSet(Dev, DeviceSpecificParameters.field, value)
|
||||
|
||||
#define VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, field) \
|
||||
PALDevDataGet(Dev, DeviceSpecificParameters).field
|
||||
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT97(Value) \
|
||||
(uint16_t)((Value>>9)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT97TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<9)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT88(Value) \
|
||||
(uint16_t)((Value>>8)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT88TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<8)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT412(Value) \
|
||||
(uint16_t)((Value>>4)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT412TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<4)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT313(Value) \
|
||||
(uint16_t)((Value>>3)&0xFFFF)
|
||||
#define VL53L0X_FIXPOINT313TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<3)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT08(Value) \
|
||||
(uint8_t)((Value>>8)&0x00FF)
|
||||
#define VL53L0X_FIXPOINT08TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<8)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT53(Value) \
|
||||
(uint8_t)((Value>>13)&0x00FF)
|
||||
#define VL53L0X_FIXPOINT53TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<13)
|
||||
|
||||
#define VL53L0X_FIXPOINT1616TOFIXPOINT102(Value) \
|
||||
(uint16_t)((Value>>14)&0x0FFF)
|
||||
#define VL53L0X_FIXPOINT102TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)(Value<<12)
|
||||
|
||||
#define VL53L0X_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
|
||||
(uint16_t)lsb)
|
||||
|
||||
/** @} VL53L0X_define_GeneralMacro_group */
|
||||
|
||||
/** @} VL53L0X_globaldefine_group */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#define TRACE_MODULE_API 0
|
||||
#define _LOG_FUNCTION_START(...) (void)0
|
||||
#define _LOG_FUNCTION_END(...) (void)0
|
||||
#define _LOG_FUNCTION_END_FMT(...) (void)0
|
||||
#endif /* _VL53L0X_DEF_H_ */
|
||||
262
App/VL53L0X_API/core/inc/vl53l0x_device.h
Normal file
262
App/VL53L0X_API/core/inc/vl53l0x_device.h
Normal file
@@ -0,0 +1,262 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/**
|
||||
* Device specific defines. To be adapted by implementer for the targeted
|
||||
* device.
|
||||
*/
|
||||
|
||||
#ifndef _VL53L0X_DEVICE_H_
|
||||
#define _VL53L0X_DEVICE_H_
|
||||
|
||||
#include "vl53l0x_types.h"
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_DevSpecDefines_group VL53L0X cut1.1 Device
|
||||
* Specific Defines
|
||||
* @brief VL53L0X cut1.1 Device Specific Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_DeviceError_group Device Error
|
||||
* @brief Device Error code
|
||||
*
|
||||
* This enum is Device specific it should be updated in the implementation
|
||||
* Use @a VL53L0X_GetStatusErrorString() to get the string.
|
||||
* It is related to Status Register of the Device.
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_DeviceError;
|
||||
|
||||
#define VL53L0X_DEVICEERROR_NONE ((VL53L0X_DeviceError) 0)
|
||||
/*!< 0 NoError */
|
||||
#define VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L0X_DeviceError) 1)
|
||||
#define VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L0X_DeviceError) 2)
|
||||
#define VL53L0X_DEVICEERROR_NOVHVVALUEFOUND ((VL53L0X_DeviceError) 3)
|
||||
#define VL53L0X_DEVICEERROR_MSRCNOTARGET ((VL53L0X_DeviceError) 4)
|
||||
#define VL53L0X_DEVICEERROR_SNRCHECK ((VL53L0X_DeviceError) 5)
|
||||
#define VL53L0X_DEVICEERROR_RANGEPHASECHECK ((VL53L0X_DeviceError) 6)
|
||||
#define VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L0X_DeviceError) 7)
|
||||
#define VL53L0X_DEVICEERROR_TCC ((VL53L0X_DeviceError) 8)
|
||||
#define VL53L0X_DEVICEERROR_PHASECONSISTENCY ((VL53L0X_DeviceError) 9)
|
||||
#define VL53L0X_DEVICEERROR_MINCLIP ((VL53L0X_DeviceError) 10)
|
||||
#define VL53L0X_DEVICEERROR_RANGECOMPLETE ((VL53L0X_DeviceError) 11)
|
||||
#define VL53L0X_DEVICEERROR_ALGOUNDERFLOW ((VL53L0X_DeviceError) 12)
|
||||
#define VL53L0X_DEVICEERROR_ALGOOVERFLOW ((VL53L0X_DeviceError) 13)
|
||||
#define VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L0X_DeviceError) 14)
|
||||
|
||||
/** @} end of VL53L0X_DeviceError_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_CheckEnable_group Check Enable list
|
||||
* @brief Check Enable code
|
||||
*
|
||||
* Define used to specify the LimitCheckId.
|
||||
* Use @a VL53L0X_GetLimitCheckInfo() to get the string.
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE 0
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP 2
|
||||
#define VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD 3
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC 4
|
||||
#define VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE 5
|
||||
|
||||
#define VL53L0X_CHECKENABLE_NUMBER_OF_CHECKS 6
|
||||
|
||||
/** @} end of VL53L0X_CheckEnable_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L0X_GpioFunctionality_group Gpio Functionality
|
||||
* @brief Defines the different functionalities for the device GPIO(s)
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L0X_GpioFunctionality;
|
||||
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_OFF \
|
||||
((VL53L0X_GpioFunctionality) 0) /*!< NO Interrupt */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_LOW \
|
||||
((VL53L0X_GpioFunctionality) 1) /*!< Level Low (value < thresh_low) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_HIGH \
|
||||
((VL53L0X_GpioFunctionality) 2) /*!< Level High (value>thresh_high) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_THRESHOLD_CROSSED_OUT \
|
||||
((VL53L0X_GpioFunctionality) 3)
|
||||
/*!< Out Of Window (value < thresh_low OR value > thresh_high) */
|
||||
#define VL53L0X_GPIOFUNCTIONALITY_NEW_MEASURE_READY \
|
||||
((VL53L0X_GpioFunctionality) 4) /*!< New Sample Ready */
|
||||
|
||||
/** @} end of VL53L0X_GpioFunctionality_group */
|
||||
|
||||
|
||||
/* Device register map */
|
||||
|
||||
/** @defgroup VL53L0X_DefineRegisters_group Define Registers
|
||||
* @brief List of all the defined registers
|
||||
* @{
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_START 0x000
|
||||
/** mask existing bit in #VL53L0X_REG_SYSRANGE_START*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_MASK 0x0F
|
||||
/** bit 0 in #VL53L0X_REG_SYSRANGE_START write 1 toggle state in
|
||||
* continuous mode and arm next shot in single shot mode
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_START_STOP 0x01
|
||||
/** bit 1 write 0 in #VL53L0X_REG_SYSRANGE_START set single shot mode */
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_SINGLESHOT 0x00
|
||||
/** bit 1 write 1 in #VL53L0X_REG_SYSRANGE_START set back-to-back
|
||||
* operation mode
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_BACKTOBACK 0x02
|
||||
/** bit 2 write 1 in #VL53L0X_REG_SYSRANGE_START set timed operation
|
||||
* mode
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_TIMED 0x04
|
||||
/** bit 3 write 1 in #VL53L0X_REG_SYSRANGE_START set histogram operation
|
||||
* mode
|
||||
*/
|
||||
#define VL53L0X_REG_SYSRANGE_MODE_HISTOGRAM 0x08
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_THRESH_HIGH 0x000C
|
||||
#define VL53L0X_REG_SYSTEM_THRESH_LOW 0x000E
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_SEQUENCE_CONFIG 0x0001
|
||||
#define VL53L0X_REG_SYSTEM_RANGE_CONFIG 0x0009
|
||||
#define VL53L0X_REG_SYSTEM_INTERMEASUREMENT_PERIOD 0x0004
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_CONFIG_GPIO 0x000A
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_DISABLED 0x00
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_LOW 0x01
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH 0x02
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_OUT_OF_WINDOW 0x03
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY 0x04
|
||||
|
||||
#define VL53L0X_REG_GPIO_HV_MUX_ACTIVE_HIGH 0x0084
|
||||
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_INTERRUPT_CLEAR 0x000B
|
||||
|
||||
/* Result registers */
|
||||
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x0013
|
||||
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x0014
|
||||
|
||||
#define VL53L0X_REG_RESULT_CORE_PAGE 1
|
||||
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN 0x00BC
|
||||
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_RTN 0x00C0
|
||||
#define VL53L0X_REG_RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF 0x00D0
|
||||
#define VL53L0X_REG_RESULT_CORE_RANGING_TOTAL_EVENTS_REF 0x00D4
|
||||
#define VL53L0X_REG_RESULT_PEAK_SIGNAL_RATE_REF 0x00B6
|
||||
|
||||
/* Algo register */
|
||||
|
||||
#define VL53L0X_REG_ALGO_PART_TO_PART_RANGE_OFFSET_MM 0x0028
|
||||
|
||||
#define VL53L0X_REG_I2C_SLAVE_DEVICE_ADDRESS 0x008a
|
||||
|
||||
/* Check Limit registers */
|
||||
#define VL53L0X_REG_MSRC_CONFIG_CONTROL 0x0060
|
||||
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_MIN_SNR 0X0027
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_LOW 0x0056
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VALID_PHASE_HIGH 0x0057
|
||||
#define VL53L0X_REG_PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT 0x0064
|
||||
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_SNR 0X0067
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_LOW 0x0047
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VALID_PHASE_HIGH 0x0048
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT 0x0044
|
||||
|
||||
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_HI 0X0061
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_SIGMA_THRESH_LO 0X0062
|
||||
|
||||
/* PRE RANGE registers */
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x0050
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0051
|
||||
#define VL53L0X_REG_PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0052
|
||||
|
||||
#define VL53L0X_REG_SYSTEM_HISTOGRAM_BIN 0x0081
|
||||
#define VL53L0X_REG_HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT 0x0033
|
||||
#define VL53L0X_REG_HISTOGRAM_CONFIG_READOUT_CTRL 0x0055
|
||||
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x0070
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI 0x0071
|
||||
#define VL53L0X_REG_FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO 0x0072
|
||||
#define VL53L0X_REG_CROSSTALK_COMPENSATION_PEAK_RATE_MCPS 0x0020
|
||||
|
||||
#define VL53L0X_REG_MSRC_CONFIG_TIMEOUT_MACROP 0x0046
|
||||
|
||||
|
||||
#define VL53L0X_REG_SOFT_RESET_GO2_SOFT_RESET_N 0x00bf
|
||||
#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0x00c0
|
||||
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0x00c2
|
||||
|
||||
#define VL53L0X_REG_OSC_CALIBRATE_VAL 0x00f8
|
||||
|
||||
|
||||
#define VL53L0X_SIGMA_ESTIMATE_MAX_VALUE 65535
|
||||
/* equivalent to a range sigma of 655.35mm */
|
||||
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_VCSEL_WIDTH 0x032
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_0 0x0B0
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_1 0x0B1
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_2 0x0B2
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_3 0x0B3
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_4 0x0B4
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_SPAD_ENABLES_REF_5 0x0B5
|
||||
|
||||
#define VL53L0X_REG_GLOBAL_CONFIG_REF_EN_START_SELECT 0xB6
|
||||
#define VL53L0X_REG_DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD 0x4E /* 0x14E */
|
||||
#define VL53L0X_REG_DYNAMIC_SPAD_REF_EN_START_OFFSET 0x4F /* 0x14F */
|
||||
#define VL53L0X_REG_POWER_MANAGEMENT_GO1_POWER_FORCE 0x80
|
||||
|
||||
/*
|
||||
* Speed of light in um per 1E-10 Seconds
|
||||
*/
|
||||
|
||||
#define VL53L0X_SPEED_OF_LIGHT_IN_AIR 2997
|
||||
|
||||
#define VL53L0X_REG_VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV 0x0089
|
||||
|
||||
#define VL53L0X_REG_ALGO_PHASECAL_LIM 0x0030 /* 0x130 */
|
||||
#define VL53L0X_REG_ALGO_PHASECAL_CONFIG_TIMEOUT 0x0030
|
||||
|
||||
/** @} VL53L0X_DefineRegisters_group */
|
||||
|
||||
/** @} VL53L0X_DevSpecDefines_group */
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/* _VL53L0X_DEVICE_H_ */
|
||||
|
||||
|
||||
194
App/VL53L0X_API/core/inc/vl53l0x_interrupt_threshold_settings.h
Normal file
194
App/VL53L0X_API/core/inc/vl53l0x_interrupt_threshold_settings.h
Normal file
@@ -0,0 +1,194 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
|
||||
#define _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
uint8_t InterruptThresholdSettings[] = {
|
||||
|
||||
/* Start of Interrupt Threshold Settings */
|
||||
0x1, 0xff, 0x00,
|
||||
0x1, 0x80, 0x01,
|
||||
0x1, 0xff, 0x01,
|
||||
0x1, 0x00, 0x00,
|
||||
0x1, 0xff, 0x01,
|
||||
0x1, 0x4f, 0x02,
|
||||
0x1, 0xFF, 0x0E,
|
||||
0x1, 0x00, 0x03,
|
||||
0x1, 0x01, 0x84,
|
||||
0x1, 0x02, 0x0A,
|
||||
0x1, 0x03, 0x03,
|
||||
0x1, 0x04, 0x08,
|
||||
0x1, 0x05, 0xC8,
|
||||
0x1, 0x06, 0x03,
|
||||
0x1, 0x07, 0x8D,
|
||||
0x1, 0x08, 0x08,
|
||||
0x1, 0x09, 0xC6,
|
||||
0x1, 0x0A, 0x01,
|
||||
0x1, 0x0B, 0x02,
|
||||
0x1, 0x0C, 0x00,
|
||||
0x1, 0x0D, 0xD5,
|
||||
0x1, 0x0E, 0x18,
|
||||
0x1, 0x0F, 0x12,
|
||||
0x1, 0x10, 0x01,
|
||||
0x1, 0x11, 0x82,
|
||||
0x1, 0x12, 0x00,
|
||||
0x1, 0x13, 0xD5,
|
||||
0x1, 0x14, 0x18,
|
||||
0x1, 0x15, 0x13,
|
||||
0x1, 0x16, 0x03,
|
||||
0x1, 0x17, 0x86,
|
||||
0x1, 0x18, 0x0A,
|
||||
0x1, 0x19, 0x09,
|
||||
0x1, 0x1A, 0x08,
|
||||
0x1, 0x1B, 0xC2,
|
||||
0x1, 0x1C, 0x03,
|
||||
0x1, 0x1D, 0x8F,
|
||||
0x1, 0x1E, 0x0A,
|
||||
0x1, 0x1F, 0x06,
|
||||
0x1, 0x20, 0x01,
|
||||
0x1, 0x21, 0x02,
|
||||
0x1, 0x22, 0x00,
|
||||
0x1, 0x23, 0xD5,
|
||||
0x1, 0x24, 0x18,
|
||||
0x1, 0x25, 0x22,
|
||||
0x1, 0x26, 0x01,
|
||||
0x1, 0x27, 0x82,
|
||||
0x1, 0x28, 0x00,
|
||||
0x1, 0x29, 0xD5,
|
||||
0x1, 0x2A, 0x18,
|
||||
0x1, 0x2B, 0x0B,
|
||||
0x1, 0x2C, 0x28,
|
||||
0x1, 0x2D, 0x78,
|
||||
0x1, 0x2E, 0x28,
|
||||
0x1, 0x2F, 0x91,
|
||||
0x1, 0x30, 0x00,
|
||||
0x1, 0x31, 0x0B,
|
||||
0x1, 0x32, 0x00,
|
||||
0x1, 0x33, 0x0B,
|
||||
0x1, 0x34, 0x00,
|
||||
0x1, 0x35, 0xA1,
|
||||
0x1, 0x36, 0x00,
|
||||
0x1, 0x37, 0xA0,
|
||||
0x1, 0x38, 0x00,
|
||||
0x1, 0x39, 0x04,
|
||||
0x1, 0x3A, 0x28,
|
||||
0x1, 0x3B, 0x30,
|
||||
0x1, 0x3C, 0x0C,
|
||||
0x1, 0x3D, 0x04,
|
||||
0x1, 0x3E, 0x0F,
|
||||
0x1, 0x3F, 0x79,
|
||||
0x1, 0x40, 0x28,
|
||||
0x1, 0x41, 0x1E,
|
||||
0x1, 0x42, 0x2F,
|
||||
0x1, 0x43, 0x87,
|
||||
0x1, 0x44, 0x00,
|
||||
0x1, 0x45, 0x0B,
|
||||
0x1, 0x46, 0x00,
|
||||
0x1, 0x47, 0x0B,
|
||||
0x1, 0x48, 0x00,
|
||||
0x1, 0x49, 0xA7,
|
||||
0x1, 0x4A, 0x00,
|
||||
0x1, 0x4B, 0xA6,
|
||||
0x1, 0x4C, 0x00,
|
||||
0x1, 0x4D, 0x04,
|
||||
0x1, 0x4E, 0x01,
|
||||
0x1, 0x4F, 0x00,
|
||||
0x1, 0x50, 0x00,
|
||||
0x1, 0x51, 0x80,
|
||||
0x1, 0x52, 0x09,
|
||||
0x1, 0x53, 0x08,
|
||||
0x1, 0x54, 0x01,
|
||||
0x1, 0x55, 0x00,
|
||||
0x1, 0x56, 0x0F,
|
||||
0x1, 0x57, 0x79,
|
||||
0x1, 0x58, 0x09,
|
||||
0x1, 0x59, 0x05,
|
||||
0x1, 0x5A, 0x00,
|
||||
0x1, 0x5B, 0x60,
|
||||
0x1, 0x5C, 0x05,
|
||||
0x1, 0x5D, 0xD1,
|
||||
0x1, 0x5E, 0x0C,
|
||||
0x1, 0x5F, 0x3C,
|
||||
0x1, 0x60, 0x00,
|
||||
0x1, 0x61, 0xD0,
|
||||
0x1, 0x62, 0x0B,
|
||||
0x1, 0x63, 0x03,
|
||||
0x1, 0x64, 0x28,
|
||||
0x1, 0x65, 0x10,
|
||||
0x1, 0x66, 0x2A,
|
||||
0x1, 0x67, 0x39,
|
||||
0x1, 0x68, 0x0B,
|
||||
0x1, 0x69, 0x02,
|
||||
0x1, 0x6A, 0x28,
|
||||
0x1, 0x6B, 0x10,
|
||||
0x1, 0x6C, 0x2A,
|
||||
0x1, 0x6D, 0x61,
|
||||
0x1, 0x6E, 0x0C,
|
||||
0x1, 0x6F, 0x00,
|
||||
0x1, 0x70, 0x0F,
|
||||
0x1, 0x71, 0x79,
|
||||
0x1, 0x72, 0x00,
|
||||
0x1, 0x73, 0x0B,
|
||||
0x1, 0x74, 0x00,
|
||||
0x1, 0x75, 0x0B,
|
||||
0x1, 0x76, 0x00,
|
||||
0x1, 0x77, 0xA1,
|
||||
0x1, 0x78, 0x00,
|
||||
0x1, 0x79, 0xA0,
|
||||
0x1, 0x7A, 0x00,
|
||||
0x1, 0x7B, 0x04,
|
||||
0x1, 0xFF, 0x04,
|
||||
0x1, 0x79, 0x1D,
|
||||
0x1, 0x7B, 0x27,
|
||||
0x1, 0x96, 0x0E,
|
||||
0x1, 0x97, 0xFE,
|
||||
0x1, 0x98, 0x03,
|
||||
0x1, 0x99, 0xEF,
|
||||
0x1, 0x9A, 0x02,
|
||||
0x1, 0x9B, 0x44,
|
||||
0x1, 0x73, 0x07,
|
||||
0x1, 0x70, 0x01,
|
||||
0x1, 0xff, 0x01,
|
||||
0x1, 0x00, 0x01,
|
||||
0x1, 0xff, 0x00,
|
||||
0x00, 0x00, 0x00
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L0X_INTERRUPT_THRESHOLD_SETTINGS_H_ */
|
||||
146
App/VL53L0X_API/core/inc/vl53l0x_tuning.h
Normal file
146
App/VL53L0X_API/core/inc/vl53l0x_tuning.h
Normal file
@@ -0,0 +1,146 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L0X_TUNING_H_
|
||||
#define _VL53L0X_TUNING_H_
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
uint8_t DefaultTuningSettings[] = {
|
||||
|
||||
/* update 02/11/2015_v36 */
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x00, 0x00,
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x09, 0x00,
|
||||
0x01, 0x10, 0x00,
|
||||
0x01, 0x11, 0x00,
|
||||
|
||||
0x01, 0x24, 0x01,
|
||||
0x01, 0x25, 0xff,
|
||||
0x01, 0x75, 0x00,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x4e, 0x2c,
|
||||
0x01, 0x48, 0x00,
|
||||
0x01, 0x30, 0x20,
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x30, 0x09, /* mja changed from 0x64. */
|
||||
0x01, 0x54, 0x00,
|
||||
0x01, 0x31, 0x04,
|
||||
0x01, 0x32, 0x03,
|
||||
0x01, 0x40, 0x83,
|
||||
0x01, 0x46, 0x25,
|
||||
0x01, 0x60, 0x00,
|
||||
0x01, 0x27, 0x00,
|
||||
0x01, 0x50, 0x06,
|
||||
0x01, 0x51, 0x00,
|
||||
0x01, 0x52, 0x96,
|
||||
0x01, 0x56, 0x08,
|
||||
0x01, 0x57, 0x30,
|
||||
0x01, 0x61, 0x00,
|
||||
0x01, 0x62, 0x00,
|
||||
0x01, 0x64, 0x00,
|
||||
0x01, 0x65, 0x00,
|
||||
0x01, 0x66, 0xa0,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x22, 0x32,
|
||||
0x01, 0x47, 0x14,
|
||||
0x01, 0x49, 0xff,
|
||||
0x01, 0x4a, 0x00,
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x7a, 0x0a,
|
||||
0x01, 0x7b, 0x00,
|
||||
0x01, 0x78, 0x21,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x23, 0x34,
|
||||
0x01, 0x42, 0x00,
|
||||
0x01, 0x44, 0xff,
|
||||
0x01, 0x45, 0x26,
|
||||
0x01, 0x46, 0x05,
|
||||
0x01, 0x40, 0x40,
|
||||
0x01, 0x0E, 0x06,
|
||||
0x01, 0x20, 0x1a,
|
||||
0x01, 0x43, 0x40,
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x34, 0x03,
|
||||
0x01, 0x35, 0x44,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x31, 0x04,
|
||||
0x01, 0x4b, 0x09,
|
||||
0x01, 0x4c, 0x05,
|
||||
0x01, 0x4d, 0x04,
|
||||
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x44, 0x00,
|
||||
0x01, 0x45, 0x20,
|
||||
0x01, 0x47, 0x08,
|
||||
0x01, 0x48, 0x28,
|
||||
0x01, 0x67, 0x00,
|
||||
0x01, 0x70, 0x04,
|
||||
0x01, 0x71, 0x01,
|
||||
0x01, 0x72, 0xfe,
|
||||
0x01, 0x76, 0x00,
|
||||
0x01, 0x77, 0x00,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x0d, 0x01,
|
||||
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x80, 0x01,
|
||||
0x01, 0x01, 0xF8,
|
||||
|
||||
0x01, 0xFF, 0x01,
|
||||
0x01, 0x8e, 0x01,
|
||||
0x01, 0x00, 0x01,
|
||||
0x01, 0xFF, 0x00,
|
||||
0x01, 0x80, 0x00,
|
||||
|
||||
0x00, 0x00, 0x00
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L0X_TUNING_H_ */
|
||||
3141
App/VL53L0X_API/core/src/vl53l0x_api.c
Normal file
3141
App/VL53L0X_API/core/src/vl53l0x_api.c
Normal file
File diff suppressed because it is too large
Load Diff
1288
App/VL53L0X_API/core/src/vl53l0x_api_calibration.c
Normal file
1288
App/VL53L0X_API/core/src/vl53l0x_api_calibration.c
Normal file
File diff suppressed because it is too large
Load Diff
2128
App/VL53L0X_API/core/src/vl53l0x_api_core.c
Normal file
2128
App/VL53L0X_API/core/src/vl53l0x_api_core.c
Normal file
File diff suppressed because it is too large
Load Diff
42
App/VL53L0X_API/core/src/vl53l0x_api_ranging.c
Normal file
42
App/VL53L0X_API/core/src/vl53l0x_api_ranging.c
Normal file
@@ -0,0 +1,42 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
#include "vl53l0x_api.h"
|
||||
#include "vl53l0x_api_core.h"
|
||||
|
||||
|
||||
#ifndef __KERNEL__
|
||||
#include <stdlib.h>
|
||||
#endif
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
|
||||
|
||||
466
App/VL53L0X_API/core/src/vl53l0x_api_strings.c
Normal file
466
App/VL53L0X_API/core/src/vl53l0x_api_strings.c
Normal file
@@ -0,0 +1,466 @@
|
||||
/*******************************************************************************
|
||||
* Copyright © 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
#include "vl53l0x_api.h"
|
||||
#include "vl53l0x_api_core.h"
|
||||
#include "vl53l0x_api_strings.h"
|
||||
|
||||
#ifndef __KERNEL__
|
||||
#include <stdlib.h>
|
||||
#endif
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(TRACE_MODULE_API, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(TRACE_MODULE_API, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(TRACE_MODULE_API, status, fmt, ##__VA_ARGS__)
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_check_part_used(VL53L0X_DEV Dev,
|
||||
uint8_t *Revision,
|
||||
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
uint8_t ModuleIdInt;
|
||||
char *ProductId_tmp;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
Status = VL53L0X_get_info_from_device(Dev, 2);
|
||||
|
||||
if (Status == VL53L0X_ERROR_NONE) {
|
||||
ModuleIdInt = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, ModuleId);
|
||||
|
||||
if (ModuleIdInt == 0) {
|
||||
*Revision = 0;
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId, "");
|
||||
} else {
|
||||
*Revision = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev, Revision);
|
||||
ProductId_tmp = VL53L0X_GETDEVICESPECIFICPARAMETER(Dev,
|
||||
ProductId);
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->ProductId,
|
||||
ProductId_tmp);
|
||||
}
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_info(VL53L0X_DEV Dev,
|
||||
VL53L0X_DeviceInfo_t *pVL53L0X_DeviceInfo)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
uint8_t revision_id;
|
||||
uint8_t Revision;
|
||||
|
||||
Status = VL53L0X_check_part_used(Dev, &Revision, pVL53L0X_DeviceInfo);
|
||||
|
||||
if (Status == VL53L0X_ERROR_NONE) {
|
||||
if (Revision == 0) {
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
|
||||
VL53L0X_STRING_DEVICE_INFO_NAME_TS0);
|
||||
} else if ((Revision <= 34) && (Revision != 32)) {
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
|
||||
VL53L0X_STRING_DEVICE_INFO_NAME_TS1);
|
||||
} else if (Revision < 39) {
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
|
||||
VL53L0X_STRING_DEVICE_INFO_NAME_TS2);
|
||||
} else {
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Name,
|
||||
VL53L0X_STRING_DEVICE_INFO_NAME_ES1);
|
||||
}
|
||||
|
||||
VL53L0X_COPYSTRING(pVL53L0X_DeviceInfo->Type,
|
||||
VL53L0X_STRING_DEVICE_INFO_TYPE);
|
||||
|
||||
}
|
||||
|
||||
if (Status == VL53L0X_ERROR_NONE) {
|
||||
Status = VL53L0X_RdByte(Dev,
|
||||
VL53L0X_REG_IDENTIFICATION_MODEL_ID,
|
||||
&pVL53L0X_DeviceInfo->ProductType);
|
||||
}
|
||||
|
||||
if (Status == VL53L0X_ERROR_NONE) {
|
||||
Status = VL53L0X_RdByte(Dev,
|
||||
VL53L0X_REG_IDENTIFICATION_REVISION_ID,
|
||||
&revision_id);
|
||||
pVL53L0X_DeviceInfo->ProductRevisionMajor = 1;
|
||||
pVL53L0X_DeviceInfo->ProductRevisionMinor =
|
||||
(revision_id & 0xF0) >> 4;
|
||||
}
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_get_device_error_string(VL53L0X_DeviceError ErrorCode,
|
||||
char *pDeviceErrorString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (ErrorCode) {
|
||||
case VL53L0X_DEVICEERROR_NONE:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_NONE);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_VCSELCONTINUITYTESTFAILURE:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_VCSELCONTINUITYTESTFAILURE);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_VCSELWATCHDOGTESTFAILURE:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_VCSELWATCHDOGTESTFAILURE);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_NOVHVVALUEFOUND:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_NOVHVVALUEFOUND);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_MSRCNOTARGET:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_MSRCNOTARGET);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_SNRCHECK:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_SNRCHECK);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_RANGEPHASECHECK:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_RANGEPHASECHECK);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_SIGMATHRESHOLDCHECK:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_SIGMATHRESHOLDCHECK);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_TCC:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_TCC);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_PHASECONSISTENCY:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_PHASECONSISTENCY);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_MINCLIP:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_MINCLIP);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_RANGECOMPLETE:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_RANGECOMPLETE);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_ALGOUNDERFLOW:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_ALGOUNDERFLOW);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_ALGOOVERFLOW:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_ALGOOVERFLOW);
|
||||
break;
|
||||
case VL53L0X_DEVICEERROR_RANGEIGNORETHRESHOLD:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_DEVICEERROR_RANGEIGNORETHRESHOLD);
|
||||
break;
|
||||
|
||||
default:
|
||||
VL53L0X_COPYSTRING(pDeviceErrorString,
|
||||
VL53L0X_STRING_UNKNOW_ERROR_CODE);
|
||||
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_range_status_string(uint8_t RangeStatus,
|
||||
char *pRangeStatusString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (RangeStatus) {
|
||||
case 0:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_RANGEVALID);
|
||||
break;
|
||||
case 1:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_SIGMA);
|
||||
break;
|
||||
case 2:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_SIGNAL);
|
||||
break;
|
||||
case 3:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_MINRANGE);
|
||||
break;
|
||||
case 4:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_PHASE);
|
||||
break;
|
||||
case 5:
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_HW);
|
||||
break;
|
||||
|
||||
default: /**/
|
||||
VL53L0X_COPYSTRING(pRangeStatusString,
|
||||
VL53L0X_STRING_RANGESTATUS_NONE);
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_error_string(VL53L0X_Error PalErrorCode,
|
||||
char *pPalErrorString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (PalErrorCode) {
|
||||
case VL53L0X_ERROR_NONE:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_NONE);
|
||||
break;
|
||||
case VL53L0X_ERROR_CALIBRATION_WARNING:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_CALIBRATION_WARNING);
|
||||
break;
|
||||
case VL53L0X_ERROR_MIN_CLIPPED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_MIN_CLIPPED);
|
||||
break;
|
||||
case VL53L0X_ERROR_UNDEFINED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_UNDEFINED);
|
||||
break;
|
||||
case VL53L0X_ERROR_INVALID_PARAMS:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_INVALID_PARAMS);
|
||||
break;
|
||||
case VL53L0X_ERROR_NOT_SUPPORTED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L0X_ERROR_INTERRUPT_NOT_CLEARED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_INTERRUPT_NOT_CLEARED);
|
||||
break;
|
||||
case VL53L0X_ERROR_RANGE_ERROR:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_RANGE_ERROR);
|
||||
break;
|
||||
case VL53L0X_ERROR_TIME_OUT:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_TIME_OUT);
|
||||
break;
|
||||
case VL53L0X_ERROR_MODE_NOT_SUPPORTED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_MODE_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L0X_ERROR_BUFFER_TOO_SMALL:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_BUFFER_TOO_SMALL);
|
||||
break;
|
||||
case VL53L0X_ERROR_GPIO_NOT_EXISTING:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_GPIO_NOT_EXISTING);
|
||||
break;
|
||||
case VL53L0X_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L0X_ERROR_CONTROL_INTERFACE:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_CONTROL_INTERFACE);
|
||||
break;
|
||||
case VL53L0X_ERROR_INVALID_COMMAND:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_INVALID_COMMAND);
|
||||
break;
|
||||
case VL53L0X_ERROR_DIVISION_BY_ZERO:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_DIVISION_BY_ZERO);
|
||||
break;
|
||||
case VL53L0X_ERROR_REF_SPAD_INIT:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_REF_SPAD_INIT);
|
||||
break;
|
||||
case VL53L0X_ERROR_NOT_IMPLEMENTED:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_ERROR_NOT_IMPLEMENTED);
|
||||
break;
|
||||
|
||||
default:
|
||||
VL53L0X_COPYSTRING(pPalErrorString,
|
||||
VL53L0X_STRING_UNKNOW_ERROR_CODE);
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_pal_state_string(VL53L0X_State PalStateCode,
|
||||
char *pPalStateString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (PalStateCode) {
|
||||
case VL53L0X_STATE_POWERDOWN:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_POWERDOWN);
|
||||
break;
|
||||
case VL53L0X_STATE_WAIT_STATICINIT:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_WAIT_STATICINIT);
|
||||
break;
|
||||
case VL53L0X_STATE_STANDBY:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_STANDBY);
|
||||
break;
|
||||
case VL53L0X_STATE_IDLE:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_IDLE);
|
||||
break;
|
||||
case VL53L0X_STATE_RUNNING:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_RUNNING);
|
||||
break;
|
||||
case VL53L0X_STATE_UNKNOWN:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_UNKNOWN);
|
||||
break;
|
||||
case VL53L0X_STATE_ERROR:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_ERROR);
|
||||
break;
|
||||
|
||||
default:
|
||||
VL53L0X_COPYSTRING(pPalStateString,
|
||||
VL53L0X_STRING_STATE_UNKNOWN);
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_get_sequence_steps_info(
|
||||
VL53L0X_SequenceStepId SequenceStepId,
|
||||
char *pSequenceStepsString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (SequenceStepId) {
|
||||
case VL53L0X_SEQUENCESTEP_TCC:
|
||||
VL53L0X_COPYSTRING(pSequenceStepsString,
|
||||
VL53L0X_STRING_SEQUENCESTEP_TCC);
|
||||
break;
|
||||
case VL53L0X_SEQUENCESTEP_DSS:
|
||||
VL53L0X_COPYSTRING(pSequenceStepsString,
|
||||
VL53L0X_STRING_SEQUENCESTEP_DSS);
|
||||
break;
|
||||
case VL53L0X_SEQUENCESTEP_MSRC:
|
||||
VL53L0X_COPYSTRING(pSequenceStepsString,
|
||||
VL53L0X_STRING_SEQUENCESTEP_MSRC);
|
||||
break;
|
||||
case VL53L0X_SEQUENCESTEP_PRE_RANGE:
|
||||
VL53L0X_COPYSTRING(pSequenceStepsString,
|
||||
VL53L0X_STRING_SEQUENCESTEP_PRE_RANGE);
|
||||
break;
|
||||
case VL53L0X_SEQUENCESTEP_FINAL_RANGE:
|
||||
VL53L0X_COPYSTRING(pSequenceStepsString,
|
||||
VL53L0X_STRING_SEQUENCESTEP_FINAL_RANGE);
|
||||
break;
|
||||
|
||||
default:
|
||||
Status = VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
|
||||
VL53L0X_Error VL53L0X_get_limit_check_info(VL53L0X_DEV Dev,
|
||||
uint16_t LimitCheckId,
|
||||
char *pLimitCheckString)
|
||||
{
|
||||
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
switch (LimitCheckId) {
|
||||
case VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_SIGMA_FINAL_RANGE);
|
||||
break;
|
||||
case VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE);
|
||||
break;
|
||||
case VL53L0X_CHECKENABLE_SIGNAL_REF_CLIP:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_SIGNAL_REF_CLIP);
|
||||
break;
|
||||
case VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD);
|
||||
break;
|
||||
|
||||
case VL53L0X_CHECKENABLE_SIGNAL_RATE_MSRC:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_MSRC);
|
||||
break;
|
||||
|
||||
case VL53L0X_CHECKENABLE_SIGNAL_RATE_PRE_RANGE:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_CHECKENABLE_SIGNAL_RATE_PRE_RANGE);
|
||||
break;
|
||||
|
||||
default:
|
||||
VL53L0X_COPYSTRING(pLimitCheckString,
|
||||
VL53L0X_STRING_UNKNOW_ERROR_CODE);
|
||||
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
55
App/VL53L0X_API/platform/README.md
Normal file
55
App/VL53L0X_API/platform/README.md
Normal file
@@ -0,0 +1,55 @@
|
||||
# VL53L0X 多传感器模板（STM32H743 + FreeRTOS）
|
||||
|
||||
这组模板按固定顺序组织 4 颗 VL53L0X：
|
||||
|
||||
1. 左上
|
||||
2. 左下
|
||||
3. 右上
|
||||
4. 右下
|
||||
|
||||
## 文件说明
|
||||
|
||||
- `vl53l0x_platform.h/.c`
|
||||
- ST 官方 API 的平台适配层
|
||||
- 不再把 `hi2c1` 硬编码到驱动里
|
||||
- 读写失败时不会污染 `RdWord/RdDWord` 输出
|
||||
- `PollingDelay()` 在调度器已启动时使用 `vTaskDelay()`
|
||||
|
||||
- `vl53_board.h/.c`
|
||||
- 4 颗 VL53L0X 的板级管理层
|
||||
- 负责 `XSHUT` 拉低/逐个拉起/逐个改地址
|
||||
- 默认运行地址：0x54 / 0x56 / 0x58 / 0x5A（8-bit 写法）
|
||||
|
||||
- `vl53_task.h/.c`
|
||||
- FreeRTOS 任务骨架
|
||||
- 10ms 周期轮询一遍 4 颗传感器
|
||||
- 采集结果写成一个快照，避免上层散读散写
|
||||
|
||||
## 你需要改的地方
|
||||
|
||||
只要改 `vl53_board.c` 里的 `g_vl53_hw_cfg[]`：
|
||||
|
||||
- `hi2c`
|
||||
- `xshut_port`
|
||||
- `xshut_pin`
|
||||
- `gpio1_port` / `gpio1_pin`（如果暂时不用中断，可保持 `NULL/0`）
|
||||
|
||||
## 集成方式
|
||||
|
||||
1. 保留 ST 官方 API 的 `vl53l0x_api*.c`、`vl53l0x_def.h` 等核心文件不动。
|
||||
2. 用本模板替换你原来的 `vl53l0x_platform.h/.c`。
|
||||
3. 新增 `vl53_board.*` 和 `vl53_task.*` 到 `App/sensors/` 或你自己的目录。
|
||||
4. 在 CubeMX 里保留任务壳函数，在任务壳里调用 `AppVl53_TaskLoop()`。
|
||||
5. 不要把 VL53 逻辑塞进你 20ms 的 `canTxTask`。
|
||||
|
||||
## 建议任务频率
|
||||
|
||||
- `canTxTask`: 保持 20ms
|
||||
- `vl53Task`: 10ms 唤醒，内部读 ready 后取数
|
||||
- 估计/控制：50–100Hz
|
||||
|
||||
## 备注
|
||||
|
||||
- 地址写法用的是 **8-bit**，默认地址是 `0x52`。
|
||||
- 多颗同总线时，必须所有器件先 `XSHUT=RESET`，然后逐个拉起并改地址。
|
||||
- 这版模板默认走“单任务采集 + 快照共享”，适合你当前的 ASER 架构。
|
||||
217
App/VL53L0X_API/platform/vl53_board.c
Normal file
217
App/VL53L0X_API/platform/vl53_board.c
Normal file
@@ -0,0 +1,217 @@
|
||||
#include "robot_params.h"
|
||||
|
||||
#if !PARAM_VL53_USE_L1X
|
||||
|
||||
#include "vl53_board.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "vl53_calibration_config.h"
|
||||
|
||||
/* ================= EMA滤波底层实现 ================= */
|
||||
static void vl53_ema_init(Vl53EMA_t *ema, float alpha) {
|
||||
ema->x = 0.0f;
|
||||
ema->alpha = alpha;
|
||||
ema->initialized = 0;
|
||||
}
|
||||
|
||||
static float vl53_ema_update(Vl53EMA_t *ema, float measurement) {
|
||||
if (ema->initialized == 0) {
|
||||
ema->x = measurement;
|
||||
ema->initialized = 1;
|
||||
return ema->x;
|
||||
}
|
||||
// EMA公式: x_new = alpha * measurement + (1 - alpha) * x_old
|
||||
ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x;
|
||||
return ema->x;
|
||||
}
|
||||
|
||||
static const Vl53RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
|
||||
{
|
||||
switch (id) {
|
||||
case 0: return &k_vl53_left_calibration[0];
|
||||
case 1: return &k_vl53_left_calibration[1];
|
||||
case 2: return &k_vl53_right_calibration[0];
|
||||
case 3: return &k_vl53_right_calibration[1];
|
||||
default: return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
static VL53L0X_Error vl53_apply_runtime_calibration(VL53L0X_DEV dev, uint8_t id)
|
||||
{
|
||||
const Vl53RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id);
|
||||
VL53L0X_Error status;
|
||||
|
||||
if (cal == NULL) return VL53L0X_ERROR_NONE;
|
||||
|
||||
if (cal->offset_calibrated != 0U) {
|
||||
status = VL53L0X_SetOffsetCalibrationDataMicroMeter(dev, cal->offset_micro_meters);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
}
|
||||
|
||||
if (cal->xtalk_calibrated != 0U) {
|
||||
status = VL53L0X_SetXTalkCompensationRateMegaCps(dev, cal->xtalk_compensation_rate_mcps);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_SetXTalkCompensationEnable(dev, 1u);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
} else {
|
||||
status = VL53L0X_SetXTalkCompensationEnable(dev, 0u);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
}
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
/* ================= ST 官方配置序列 ================= */
|
||||
static VL53L0X_Error vl53_do_static_init(VL53L0X_DEV dev, uint32_t timing_budget_us)
|
||||
{
|
||||
VL53L0X_Error status;
|
||||
uint32_t ref_spad_count = 0u;
|
||||
uint8_t is_aperture_spads = 0u;
|
||||
uint8_t vhv_settings = 0u;
|
||||
uint8_t phase_cal = 0u;
|
||||
|
||||
status = VL53L0X_StaticInit(dev);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_PerformRefSpadManagement(dev, &ref_spad_count, &is_aperture_spads);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_PerformRefCalibration(dev, &vhv_settings, &phase_cal);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_SetDeviceMode(dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
/* 连续测量采用 back-to-back 模式，让下一次测量在上一帧完成后立即开始。
|
||||
* 这样任务轮询周期变快时，能尽可能拿到最新帧，而不是被固定间隔再次拖慢。 */
|
||||
status = VL53L0X_SetInterMeasurementPeriodMilliSeconds(dev, 0u);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1u);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L0X_SetLimitCheckEnable(dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u);
|
||||
if (status != VL53L0X_ERROR_NONE) return status;
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
/* ================= 核心管理器 API ================= */
|
||||
VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us)
|
||||
{
|
||||
if ((board == NULL) || (hw_cfgs == NULL) || (count == 0) || (count > VL53_MAX_DEVS_PER_BOARD)) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
|
||||
memset(board, 0, sizeof(Vl53Board_t));
|
||||
board->dev_count = count;
|
||||
board->timing_budget_us = (timing_budget_us == 0u) ? 33000u : timing_budget_us;
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
board->dev[i].name = hw_cfgs[i].name;
|
||||
board->dev[i].id = hw_cfgs[i].id;
|
||||
board->dev[i].comms_type = 1u;
|
||||
board->dev[i].comms_speed_khz = 400u;
|
||||
board->dev[i].is_present = 0u;
|
||||
|
||||
VL53L0X_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L0X_DEFAULT_ADDR_8BIT, 100u, NULL);
|
||||
VL53L0X_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0);
|
||||
|
||||
VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
|
||||
|
||||
/* 初始化EMA滤波器：alpha 从 robot_params.h 读取 */
|
||||
vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA);
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(10));
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (VL53L0X_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L0X_ERROR_NONE) continue;
|
||||
vTaskDelay(pdMS_TO_TICKS(20));
|
||||
|
||||
board->dev[i].I2cDevAddr = VL53L0X_DEFAULT_ADDR_8BIT;
|
||||
if (VL53L0X_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L0X_ERROR_NONE) continue;
|
||||
if (VL53L0X_DataInit(&board->dev[i]) != VL53L0X_ERROR_NONE) continue;
|
||||
if (vl53_do_static_init(&board->dev[i], board->timing_budget_us) != VL53L0X_ERROR_NONE) continue;
|
||||
if (vl53_apply_runtime_calibration(&board->dev[i], hw_cfgs[i].id) != VL53L0X_ERROR_NONE) continue;
|
||||
|
||||
board->init_mask |= (uint8_t)(1u << i);
|
||||
board->dev[i].is_present = 1u;
|
||||
}
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error Vl53Board_StartContinuous(Vl53Board_t *board)
|
||||
{
|
||||
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if (board->init_mask & (1u << i)) VL53L0X_StartMeasurement(&board->dev[i]);
|
||||
}
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error Vl53Board_StopContinuous(Vl53Board_t *board)
|
||||
{
|
||||
if (board == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if (board->init_mask & (1u << i)) VL53L0X_StopMeasurement(&board->dev[i]);
|
||||
}
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot)
|
||||
{
|
||||
if (board == NULL || snapshot == NULL) return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
|
||||
memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t));
|
||||
snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
|
||||
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if ((board->init_mask & (1u << i)) == 0u) {
|
||||
snapshot->range_status[i] = 255u;
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t ready = 0u;
|
||||
if (VL53L0X_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L0X_ERROR_NONE) continue;
|
||||
|
||||
if (ready) {
|
||||
VL53L0X_RangingMeasurementData_t data;
|
||||
memset(&data, 0, sizeof(data));
|
||||
|
||||
if (VL53L0X_GetRangingMeasurementData(&board->dev[i], &data) == VL53L0X_ERROR_NONE) {
|
||||
/* 1. 写入原始数据 */
|
||||
snapshot->range_mm[i] = data.RangeMilliMeter;
|
||||
snapshot->range_status[i] = data.RangeStatus;
|
||||
|
||||
if (data.RangeStatus == 0u) {
|
||||
/* 2. 标记有效并按开关决定是否应用EMA滤波 */
|
||||
snapshot->valid_mask |= (1u << i);
|
||||
#if PARAM_VL53_USE_EMA_FILTER
|
||||
snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)data.RangeMilliMeter);
|
||||
#else
|
||||
snapshot->range_mm_filtered[i] = (float)data.RangeMilliMeter;
|
||||
board->ema[i].x = (float)data.RangeMilliMeter;
|
||||
board->ema[i].initialized = 1u;
|
||||
#endif
|
||||
} else {
|
||||
/* 测距失败时，滤波值维持上一次的历史最佳估计不变 */
|
||||
snapshot->range_mm_filtered[i] = board->ema[i].x;
|
||||
}
|
||||
|
||||
VL53L0X_ClearInterruptMask(&board->dev[i], 0u);
|
||||
}
|
||||
} else {
|
||||
/* 如果没准备好，把上一帧的历史值顺延下来，防止读到 0 */
|
||||
snapshot->range_mm_filtered[i] = board->ema[i].x;
|
||||
}
|
||||
}
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
#endif
|
||||
57
App/VL53L0X_API/platform/vl53_board.h
Normal file
57
App/VL53L0X_API/platform/vl53_board.h
Normal file
@@ -0,0 +1,57 @@
|
||||
#ifndef VL53_BOARD_H
|
||||
#define VL53_BOARD_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "vl53l0x_api.h"
|
||||
#include "vl53l0x_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define VL53_MAX_DEVS_PER_BOARD 4
|
||||
|
||||
typedef struct {
|
||||
I2C_HandleTypeDef *hi2c;
|
||||
GPIO_TypeDef *xshut_port;
|
||||
uint16_t xshut_pin;
|
||||
uint8_t runtime_addr_8bit;
|
||||
const char *name;
|
||||
uint8_t id;
|
||||
} Vl53BoardHwCfg_t;
|
||||
|
||||
/* ================= 内部EMA滤波器结构 ================= */
|
||||
typedef struct {
|
||||
float x; // 当前滤波值 (滤波后的距离)
|
||||
float alpha; // 平滑系数 (0.0~1.0, 越大响应越快)
|
||||
uint8_t initialized; // 防止第一次开机从0缓慢爬升
|
||||
} Vl53EMA_t;
|
||||
|
||||
/* ================= 快照数据结构 (对外接口) ================= */
|
||||
typedef struct {
|
||||
uint32_t tick_ms;
|
||||
uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD]; /* 接口1：原始硬件数据 */
|
||||
float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD]; /* 接口2：EMA平滑数据 */
|
||||
uint8_t range_status[VL53_MAX_DEVS_PER_BOARD];
|
||||
uint8_t valid_mask;
|
||||
} Vl53BoardSnapshot_t;
|
||||
|
||||
typedef struct {
|
||||
VL53L0X_Dev_t dev[VL53_MAX_DEVS_PER_BOARD];
|
||||
Vl53EMA_t ema[VL53_MAX_DEVS_PER_BOARD]; /* 每个传感器配备一个专属EMA滤波器 */
|
||||
uint8_t init_mask;
|
||||
uint8_t dev_count;
|
||||
uint32_t timing_budget_us;
|
||||
} Vl53Board_t;
|
||||
|
||||
VL53L0X_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us);
|
||||
VL53L0X_Error Vl53Board_StartContinuous(Vl53Board_t *board);
|
||||
VL53L0X_Error Vl53Board_StopContinuous(Vl53Board_t *board);
|
||||
VL53L0X_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* VL53_BOARD_H */
|
||||
53
App/VL53L0X_API/platform/vl53_calibration_config.h
Normal file
53
App/VL53L0X_API/platform/vl53_calibration_config.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef VL53_CALIBRATION_CONFIG_H
|
||||
#define VL53_CALIBRATION_CONFIG_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
typedef struct {
|
||||
int32_t offset_micro_meters;
|
||||
uint8_t offset_calibrated;
|
||||
FixPoint1616_t xtalk_compensation_rate_mcps;
|
||||
uint8_t xtalk_calibrated;
|
||||
} Vl53RuntimeCalibration_t;
|
||||
|
||||
/*
|
||||
* 运行时直接加载的 VL53 校准值。
|
||||
*
|
||||
* 当前已确认：
|
||||
* - 左前 offset = 12000 um
|
||||
* - 左后 offset = 11000 um
|
||||
*
|
||||
* 右侧和 XTalk 目前未写入有效标定值，因此 calibrated 标志保持 0。
|
||||
*/
|
||||
static const Vl53RuntimeCalibration_t k_vl53_left_calibration[2] = {
|
||||
{
|
||||
.offset_micro_meters = 8000,
|
||||
.xtalk_compensation_rate_mcps = 0,
|
||||
.offset_calibrated = 0,
|
||||
.xtalk_calibrated = 0,
|
||||
},
|
||||
{
|
||||
.offset_micro_meters = 8000,
|
||||
.xtalk_compensation_rate_mcps = 0,
|
||||
.offset_calibrated = 0,
|
||||
.xtalk_calibrated = 0,
|
||||
},
|
||||
};
|
||||
|
||||
static const Vl53RuntimeCalibration_t k_vl53_right_calibration[2] = {
|
||||
{
|
||||
.offset_micro_meters = 2000,
|
||||
.xtalk_compensation_rate_mcps = 0,
|
||||
.offset_calibrated = 0,
|
||||
.xtalk_calibrated = 0,
|
||||
},
|
||||
{
|
||||
.offset_micro_meters = 9000,
|
||||
.xtalk_compensation_rate_mcps = 0,
|
||||
.offset_calibrated = 0,
|
||||
.xtalk_calibrated = 0,
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* VL53_CALIBRATION_CONFIG_H */
|
||||
315
App/VL53L0X_API/platform/vl53l0x_platform.c
Normal file
315
App/VL53L0X_API/platform/vl53l0x_platform.c
Normal file
@@ -0,0 +1,315 @@
|
||||
#include "vl53l0x_platform.h"
|
||||
#include "vl53l0x_api.h"
|
||||
|
||||
#if VL53_USE_FREERTOS_DELAY
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#endif
|
||||
|
||||
#if defined(configSUPPORT_STATIC_ALLOCATION) || defined(configUSE_MUTEXES)
|
||||
#include "semphr.h"
|
||||
#define VL53_USE_FREERTOS_MUTEX 1
|
||||
#else
|
||||
#define VL53_USE_FREERTOS_MUTEX 0
|
||||
#endif
|
||||
|
||||
static VL53L0X_Error vl53_hal_to_pal(HAL_StatusTypeDef hal_status)
|
||||
{
|
||||
switch (hal_status) {
|
||||
case HAL_OK:
|
||||
return VL53L0X_ERROR_NONE;
|
||||
case HAL_TIMEOUT:
|
||||
return VL53L0X_ERROR_TIME_OUT;
|
||||
default:
|
||||
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
}
|
||||
|
||||
static VL53L0X_Error vl53_validate_dev(VL53L0X_DEV Dev)
|
||||
{
|
||||
if ((Dev == NULL) || (Dev->hi2c == NULL)) {
|
||||
return VL53L0X_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
static VL53L0X_Error vl53_bus_lock(VL53L0X_DEV Dev)
|
||||
{
|
||||
#if VL53_USE_FREERTOS_MUTEX
|
||||
if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
const TickType_t wait_ticks = pdMS_TO_TICKS((Dev->io_timeout_ms == 0u) ? 10u : Dev->io_timeout_ms);
|
||||
if (xSemaphoreTake((SemaphoreHandle_t)Dev->bus_lock, wait_ticks) != pdTRUE) {
|
||||
return VL53L0X_ERROR_TIME_OUT;
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
(void)Dev;
|
||||
#endif
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
static void vl53_bus_unlock(VL53L0X_DEV Dev)
|
||||
{
|
||||
#if VL53_USE_FREERTOS_MUTEX
|
||||
if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
(void)xSemaphoreGive((SemaphoreHandle_t)Dev->bus_lock);
|
||||
}
|
||||
}
|
||||
#else
|
||||
(void)Dev;
|
||||
#endif
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
/* Shared I2C bus is already serialized per I/O transaction below. */
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
void VL53L0X_PlatformAttachBus(VL53L0X_DEV Dev,
|
||||
I2C_HandleTypeDef *hi2c,
|
||||
uint8_t i2c_addr_8bit,
|
||||
uint16_t io_timeout_ms,
|
||||
void *bus_lock)
|
||||
{
|
||||
if (Dev == NULL) {
|
||||
return;
|
||||
}
|
||||
Dev->hi2c = hi2c;
|
||||
Dev->I2cDevAddr = i2c_addr_8bit;
|
||||
Dev->io_timeout_ms = (io_timeout_ms == 0u) ? 100u : io_timeout_ms;
|
||||
Dev->bus_lock = bus_lock;
|
||||
}
|
||||
|
||||
void VL53L0X_PlatformAttachPins(VL53L0X_DEV Dev,
|
||||
GPIO_TypeDef *xshut_port,
|
||||
uint16_t xshut_pin,
|
||||
GPIO_TypeDef *gpio1_port,
|
||||
uint16_t gpio1_pin)
|
||||
{
|
||||
if (Dev == NULL) {
|
||||
return;
|
||||
}
|
||||
Dev->xshut_port = xshut_port;
|
||||
Dev->xshut_pin = xshut_pin;
|
||||
Dev->gpio1_port = gpio1_port;
|
||||
Dev->gpio1_pin = gpio1_pin;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_PlatformSetXShut(VL53L0X_DEV Dev, GPIO_PinState state)
|
||||
{
|
||||
if ((Dev == NULL) || (Dev->xshut_port == NULL)) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
HAL_GPIO_WritePin(Dev->xshut_port, Dev->xshut_pin, state);
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_PlatformBootDevice(VL53L0X_DEV Dev, uint32_t reset_low_ms, uint32_t boot_wait_ms)
|
||||
{
|
||||
VL53L0X_Error status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_RESET);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(reset_low_ms));
|
||||
} else {
|
||||
HAL_Delay(reset_low_ms);
|
||||
}
|
||||
|
||||
status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_SET);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(boot_wait_ms));
|
||||
} else {
|
||||
HAL_Delay(boot_wait_ms);
|
||||
}
|
||||
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
uint32_t VL53L0X_PlatformGetTick(void)
|
||||
{
|
||||
return HAL_GetTick();
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
|
||||
{
|
||||
HAL_StatusTypeDef hal_status;
|
||||
VL53L0X_Error status;
|
||||
|
||||
if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = vl53_validate_dev(Dev);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
status = vl53_bus_lock(Dev);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
|
||||
hal_status = HAL_I2C_Mem_Write(Dev->hi2c,
|
||||
Dev->I2cDevAddr,
|
||||
index,
|
||||
I2C_MEMADD_SIZE_8BIT,
|
||||
pdata,
|
||||
(uint16_t)count,
|
||||
Dev->io_timeout_ms);
|
||||
vl53_bus_unlock(Dev);
|
||||
return vl53_hal_to_pal(hal_status);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
|
||||
{
|
||||
HAL_StatusTypeDef hal_status;
|
||||
VL53L0X_Error status;
|
||||
|
||||
if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = vl53_validate_dev(Dev);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
status = vl53_bus_lock(Dev);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
|
||||
hal_status = HAL_I2C_Mem_Read(Dev->hi2c,
|
||||
Dev->I2cDevAddr,
|
||||
index,
|
||||
I2C_MEMADD_SIZE_8BIT,
|
||||
pdata,
|
||||
(uint16_t)count,
|
||||
Dev->io_timeout_ms);
|
||||
vl53_bus_unlock(Dev);
|
||||
return vl53_hal_to_pal(hal_status);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
|
||||
{
|
||||
return VL53L0X_WriteMulti(Dev, index, &data, 1u);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
|
||||
{
|
||||
uint8_t buffer[2];
|
||||
buffer[0] = (uint8_t)(data >> 8);
|
||||
buffer[1] = (uint8_t)(data & 0xFFu);
|
||||
return VL53L0X_WriteMulti(Dev, index, buffer, 2u);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
|
||||
{
|
||||
uint8_t buffer[4];
|
||||
buffer[0] = (uint8_t)(data >> 24);
|
||||
buffer[1] = (uint8_t)(data >> 16);
|
||||
buffer[2] = (uint8_t)(data >> 8);
|
||||
buffer[3] = (uint8_t)(data & 0xFFu);
|
||||
return VL53L0X_WriteMulti(Dev, index, buffer, 4u);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
|
||||
{
|
||||
if (data == NULL) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
return VL53L0X_ReadMulti(Dev, index, data, 1u);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
|
||||
{
|
||||
uint8_t buffer[2] = {0u, 0u};
|
||||
VL53L0X_Error status;
|
||||
|
||||
if (data == NULL) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L0X_ReadMulti(Dev, index, buffer, 2u);
|
||||
if (status == VL53L0X_ERROR_NONE) {
|
||||
*data = (((uint16_t)buffer[0]) << 8) | buffer[1];
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
|
||||
{
|
||||
uint8_t buffer[4] = {0u, 0u, 0u, 0u};
|
||||
VL53L0X_Error status;
|
||||
|
||||
if (data == NULL) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L0X_ReadMulti(Dev, index, buffer, 4u);
|
||||
if (status == VL53L0X_ERROR_NONE) {
|
||||
*data = (((uint32_t)buffer[0]) << 24)
|
||||
| (((uint32_t)buffer[1]) << 16)
|
||||
| (((uint32_t)buffer[2]) << 8)
|
||||
| ((uint32_t)buffer[3]);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
|
||||
{
|
||||
VL53L0X_Error status;
|
||||
uint8_t data = 0u;
|
||||
|
||||
status = VL53L0X_RdByte(Dev, index, &data);
|
||||
if (status != VL53L0X_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
data = (uint8_t)((data & AndData) | OrData);
|
||||
return VL53L0X_WrByte(Dev, index, data);
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
|
||||
{
|
||||
(void)Dev;
|
||||
#if VL53_USE_FREERTOS_DELAY
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(2u));
|
||||
} else {
|
||||
HAL_Delay(2u);
|
||||
}
|
||||
#else
|
||||
HAL_Delay(2u);
|
||||
#endif
|
||||
return VL53L0X_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L0X_Error VL53L0X_PlatformChangeAddress(VL53L0X_DEV Dev, uint8_t new_addr_8bit)
|
||||
{
|
||||
VL53L0X_Error status;
|
||||
|
||||
if (new_addr_8bit == 0u) {
|
||||
return VL53L0X_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L0X_SetDeviceAddress(Dev, new_addr_8bit);
|
||||
if (status == VL53L0X_ERROR_NONE) {
|
||||
Dev->I2cDevAddr = new_addr_8bit;
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(2u));
|
||||
} else {
|
||||
HAL_Delay(2u);
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
89
App/VL53L0X_API/platform/vl53l0x_platform.h
Normal file
89
App/VL53L0X_API/platform/vl53l0x_platform.h
Normal file
@@ -0,0 +1,89 @@
|
||||
#ifndef VL53L0X_PLATFORM_H
|
||||
#define VL53L0X_PLATFORM_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "main.h"
|
||||
|
||||
#ifndef VL53L0X_SINGLE_DEVICE_DRIVER
|
||||
#define VL53L0X_SINGLE_DEVICE_DRIVER 0
|
||||
#endif
|
||||
|
||||
#include "vl53l0x_def.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef VL53_USE_FREERTOS_DELAY
|
||||
#define VL53_USE_FREERTOS_DELAY 1
|
||||
#endif
|
||||
|
||||
#ifndef VL53L0X_COPYSTRING
|
||||
#define VL53L0X_COPYSTRING(dst, src) strcpy((dst), (src))
|
||||
#endif
|
||||
|
||||
#ifndef VL53L0X_DEFAULT_ADDR_8BIT
|
||||
#define VL53L0X_DEFAULT_ADDR_8BIT 0x52u
|
||||
#endif
|
||||
|
||||
typedef struct {
|
||||
VL53L0X_DevData_t Data;
|
||||
uint8_t I2cDevAddr; /* ST API expects 8-bit address, default 0x52 */
|
||||
uint8_t comms_type;
|
||||
uint16_t comms_speed_khz;
|
||||
|
||||
I2C_HandleTypeDef *hi2c;
|
||||
GPIO_TypeDef *xshut_port;
|
||||
uint16_t xshut_pin;
|
||||
GPIO_TypeDef *gpio1_port; /* optional */
|
||||
uint16_t gpio1_pin;
|
||||
|
||||
void *bus_lock; /* optional shared mutex/lock handle */
|
||||
const char *name;
|
||||
uint8_t id; /* user label: 1=左上, 2=左下, 3=右上, 4=右下 */
|
||||
uint8_t is_present;
|
||||
uint16_t io_timeout_ms;
|
||||
} VL53L0X_Dev_t;
|
||||
|
||||
typedef VL53L0X_Dev_t* VL53L0X_DEV;
|
||||
|
||||
#define PALDevDataGet(Dev, field) ((Dev)->Data.field)
|
||||
#define PALDevDataSet(Dev, field, data) ((Dev)->Data.field = (data))
|
||||
|
||||
VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev);
|
||||
VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev);
|
||||
|
||||
VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
|
||||
VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count);
|
||||
VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data);
|
||||
VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data);
|
||||
VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data);
|
||||
VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data);
|
||||
VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data);
|
||||
VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data);
|
||||
VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData);
|
||||
VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev);
|
||||
|
||||
void VL53L0X_PlatformAttachBus(VL53L0X_DEV Dev,
|
||||
I2C_HandleTypeDef *hi2c,
|
||||
uint8_t i2c_addr_8bit,
|
||||
uint16_t io_timeout_ms,
|
||||
void *bus_lock);
|
||||
|
||||
void VL53L0X_PlatformAttachPins(VL53L0X_DEV Dev,
|
||||
GPIO_TypeDef *xshut_port,
|
||||
uint16_t xshut_pin,
|
||||
GPIO_TypeDef *gpio1_port,
|
||||
uint16_t gpio1_pin);
|
||||
|
||||
VL53L0X_Error VL53L0X_PlatformSetXShut(VL53L0X_DEV Dev, GPIO_PinState state);
|
||||
VL53L0X_Error VL53L0X_PlatformBootDevice(VL53L0X_DEV Dev, uint32_t reset_low_ms, uint32_t boot_wait_ms);
|
||||
VL53L0X_Error VL53L0X_PlatformChangeAddress(VL53L0X_DEV Dev, uint8_t new_addr_8bit);
|
||||
uint32_t VL53L0X_PlatformGetTick(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* VL53L0X_PLATFORM_H */
|
||||
18
App/VL53L0X_API/platform/vl53l0x_platform_log.h
Normal file
18
App/VL53L0X_API/platform/vl53l0x_platform_log.h
Normal file
@@ -0,0 +1,18 @@
|
||||
#ifndef _VL53L0X_PLATFORM_LOG_H_
|
||||
#define _VL53L0X_PLATFORM_LOG_H_
|
||||
|
||||
/* 1. 解决 TRACE_MODULE_API 未定义的报错 */
|
||||
#define TRACE_MODULE_API 0
|
||||
|
||||
/* 2. 强行屏蔽官方所有【带下划线】的底层日志宏 */
|
||||
#define _LOG_FUNCTION_START(...) (void)0
|
||||
#define _LOG_FUNCTION_END(...) (void)0
|
||||
#define _LOG_FUNCTION_END_FMT(...) (void)0
|
||||
|
||||
/* 3. 强行屏蔽官方所有【不带下划线】的普通日志宏 */
|
||||
#define LOG_FUNCTION_START(...) (void)0
|
||||
#define LOG_FUNCTION_END(...) (void)0
|
||||
#define LOG_FUNCTION_END_FMT(...) (void)0
|
||||
#define VL53L0X_ErrLog(...) (void)0
|
||||
|
||||
#endif /* _VL53L0X_PLATFORM_LOG_H_ */
|
||||
15
App/VL53L0X_API/platform/vl53l0x_types.h
Normal file
15
App/VL53L0X_API/platform/vl53l0x_types.h
Normal file
@@ -0,0 +1,15 @@
|
||||
#ifndef _VL53L0X_TYPES_H_
|
||||
#define _VL53L0X_TYPES_H_
|
||||
|
||||
/* 引入现代 C 语言标准整数类型库 */
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#ifndef NULL
|
||||
#define NULL 0
|
||||
#endif
|
||||
|
||||
/* 核心修复：填补 ST 官方定义的定点数类型 */
|
||||
typedef uint32_t FixPoint1616_t;
|
||||
|
||||
#endif /* _VL53L0X_TYPES_H_ */
|
||||
BIN
App/VL53L1X_API/UM2356-VL53L1X-User_Manual_rev1.pdf
Normal file
BIN
App/VL53L1X_API/UM2356-VL53L1X-User_Manual_rev1.pdf
Normal file
Binary file not shown.
1142
App/VL53L1X_API/core/inc/vl53l1_api.h
Normal file
1142
App/VL53L1X_API/core/inc/vl53l1_api.h
Normal file
File diff suppressed because it is too large
Load Diff
236
App/VL53L1X_API/core/inc/vl53l1_api_calibration.h
Normal file
236
App/VL53L1X_API/core/inc/vl53l1_api_calibration.h
Normal file
@@ -0,0 +1,236 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_core.h
|
||||
*
|
||||
* @brief EwokPlus25 low level API function definitions
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_API_CALIBRATION_H_
|
||||
#define _VL53L1_API_CALIBRATION_H_
|
||||
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Run Reference Array SPAD Characterisation.
|
||||
*
|
||||
* This function finds the required number of reference SPAD
|
||||
* to meet the input required peak reference rate.
|
||||
*
|
||||
* The algorithm first tries the non apertured reference SPAD's,
|
||||
* if the rate is too high for the minimum allowed SPAD count (5)
|
||||
* then the algo switches to 5x apertured SPAD's and if the rate
|
||||
* is still to high then the 10x apertured SPAD are selected.
|
||||
*
|
||||
* The function reads the following results from the device and
|
||||
* both caches the values in the pdev->customer structure and
|
||||
* writes the data into the G02 customer register group.
|
||||
*
|
||||
* - num_ref_spads
|
||||
* - ref_location
|
||||
* - DCR SPAD enables for selected reference location
|
||||
*
|
||||
* Note power force is enabled as the function needs to read
|
||||
* data from the Patch RAM.
|
||||
*
|
||||
* Should only be called once per part with coverglass attached to
|
||||
* generate the required num of SPAD, Ref location and DCR SPAD enable
|
||||
* data
|
||||
*
|
||||
* @param[in] Dev : Device Handle
|
||||
* @param[out] pcal_status : Pointer to unfiltered calibration status
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS
|
||||
* Less than 5 Good SPAD available, output not valid
|
||||
* @return VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH
|
||||
* At end of search reference rate > 40.0 Mcps
|
||||
* Offset stability may be degraded.
|
||||
* @return VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW
|
||||
* At end of search reference rate < 10.0 Mcps
|
||||
* Offset stability may be degraded.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_ref_spad_char(VL53L1_DEV Dev, VL53L1_Error *pcal_status);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Runs the input Device Test
|
||||
*
|
||||
* Calls
|
||||
*
|
||||
* - VL53L1_enable_powerforce()
|
||||
* - VL53L1_start_test()
|
||||
* - VL53L1_poll_for_range_completion()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] device_test_mode : Device test mode register value
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_device_test(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_DeviceTestMode device_test_mode);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Runs SPAD rate map
|
||||
*
|
||||
* Output structure contains SPAD rate data in SPAD number order
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] device_test_mode : Device test mode register value.
|
||||
* Valid options: \n
|
||||
* - VL53L1_DEVICETESTMODE_LCR_VCSEL_OFF \n
|
||||
* - VL53L1_DEVICETESTMODE_LCR_VCSEL_ON
|
||||
* @param[in] array_select : Device SPAD array select
|
||||
* Valid options: \n
|
||||
* - VL53L1_DEVICESSCARRAY_RTN \n
|
||||
* - VL53L1_DEVICESSCARRAY_REF
|
||||
* @param[in] ssc_config_timeout_us : SSC timeout in [us] e.g 36000us
|
||||
* @param[out] pspad_rate_data : pointer to output rates structure
|
||||
* 1.15 format for LCR_VCSEL_OFF
|
||||
* 9.7 format for LCR_VCSEL_ON
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_spad_rate_map(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_DeviceTestMode device_test_mode,
|
||||
VL53L1_DeviceSscArray array_select,
|
||||
uint32_t ssc_config_timeout_us,
|
||||
VL53L1_spad_rate_data_t *pspad_rate_data);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Run offset calibration
|
||||
*
|
||||
* Runs the standard ranging MM1 and MM2 calibration presets
|
||||
* to generate the MM1 and MM2 range offset data
|
||||
*
|
||||
* The range config timeout is used for both MM1 and MM2 so that
|
||||
* the sigma delta settling is the same as for the 'real' range
|
||||
*
|
||||
* Places results into VL53L1_customer_nvm_managed_t within pdev
|
||||
*
|
||||
* Use VL53L1_get_part_to_part_data() to get the offset calibration
|
||||
* results
|
||||
*
|
||||
* Current FMT settings:
|
||||
*
|
||||
* - offset_calibration_mode = VL53L1_OFFSETCALIBRATIONMODE__STANDARD_RANGING
|
||||
* - dss_config__target_total_rate_mcps = 0x0A00 (20.0Mcps) to 0x1400 (40.0Mcps)
|
||||
* - phasecal_config_timeout_us = 1000
|
||||
* - range_config_timeout_us = 13000
|
||||
* - pre_num_of_samples = 32
|
||||
* - mm1_num_of_samples = 100
|
||||
* - mm2_range_num_of_samples = 64
|
||||
* - target_distance_mm = 140 mm
|
||||
* - target reflectance = 5%
|
||||
*
|
||||
* Note: function parms simplified as part of Patch_CalFunctionSimplification_11791
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] cal_distance_mm : Distance to target in [mm] - the ground truth
|
||||
* @param[out] pcal_status : Pointer to unfiltered calibration status
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return VL53L1_WARNING_OFFSET_CAL_INSUFFICIENT_MM1_SPADS
|
||||
* Effective MM1 SPAD count too low (<5.0).
|
||||
* Out with recommended calibration condition.
|
||||
* Accuracy of offset calibration may be degraded.
|
||||
* @return VL53L1_WARNING_OFFSET_CAL_PRE_RANGE_RATE_TOO_HIGH
|
||||
* Pre range too high (>40.0) in pile up region.
|
||||
* Out with recommended calibration condition.
|
||||
* Accuracy of offset calibration may be degraded.
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_offset_calibration(
|
||||
VL53L1_DEV Dev,
|
||||
int16_t cal_distance_mm,
|
||||
VL53L1_Error *pcal_status);
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_API_CALIBRATION_H_ */
|
||||
1183
App/VL53L1X_API/core/inc/vl53l1_api_core.h
Normal file
1183
App/VL53L1X_API/core/inc/vl53l1_api_core.h
Normal file
File diff suppressed because it is too large
Load Diff
354
App/VL53L1X_API/core/inc/vl53l1_api_debug.h
Normal file
354
App/VL53L1X_API/core/inc/vl53l1_api_debug.h
Normal file
@@ -0,0 +1,354 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_debug.h
|
||||
*
|
||||
* @brief EwokPlus25 low level API function definitions
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_API_DEBUG_H_
|
||||
#define _VL53L1_API_DEBUG_H_
|
||||
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/* Start Patch_AdditionalDebugData_11823 */
|
||||
|
||||
/**
|
||||
* @brief Gets the current LL Driver configuration parameters and the last
|
||||
* set of histogram data for debug
|
||||
*
|
||||
* @param[in] Dev : Device Handle
|
||||
* @param[out] pdata : pointer to VL53L1_additional_data_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_get_additional_data(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_additional_data_t *pdata);
|
||||
|
||||
/* End Patch_AdditionalDebugData_11823 */
|
||||
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
|
||||
/**
|
||||
* @brief Implements an sprintf function for signed fixed point numbers
|
||||
*
|
||||
* @param[in] fp_value : input signed fixed point number
|
||||
* @param[in] frac_bits : number of fixed point fractional bits
|
||||
* @param[in] buf_size : size of supplied text buffer
|
||||
* @param[out] pbuffer : pointer to text buffer
|
||||
*
|
||||
*/
|
||||
|
||||
void VL53L1_signed_fixed_point_sprintf(
|
||||
int32_t fp_value,
|
||||
uint8_t frac_bits,
|
||||
uint16_t buf_size,
|
||||
char *pbuffer);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out VL53L1_static_nvm_managed_t for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to VL53L1_static_nvm_managed_t
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_static_nvm_managed(
|
||||
VL53L1_static_nvm_managed_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out VL53L1_customer_nvm_managed_t for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to VL53L1_customer_nvm_managed_t
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_customer_nvm_managed(
|
||||
VL53L1_customer_nvm_managed_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out VL53L1_nvm_copy_data_t for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to VL53L1_nvm_copy_data_t
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_nvm_copy_data(
|
||||
VL53L1_nvm_copy_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the Range Results structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_range_results_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_range_results(
|
||||
VL53L1_range_results_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the Range Data structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_range_data_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_range_data(
|
||||
VL53L1_range_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the offset range results structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_offset_range_results_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_offset_range_results(
|
||||
VL53L1_offset_range_results_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the offset range data structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_offset_range_data_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_offset_range_data(
|
||||
VL53L1_offset_range_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the peak rate map calibration data structure
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_cal_peak_rate_map_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_cal_peak_rate_map(
|
||||
VL53L1_cal_peak_rate_map_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the additional offset calibration data structure
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_additional_offset_cal_data_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_additional_offset_cal_data(
|
||||
VL53L1_additional_offset_cal_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the additional data structure
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_additional_data_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_additional_data(
|
||||
VL53L1_additional_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the LL driver gain calibration data structure
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_gain_calibration_data_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_gain_calibration_data(
|
||||
VL53L1_gain_calibration_data_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the xtalk configuration data for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_xtalk_config_t Structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_xtalk_config(
|
||||
VL53L1_xtalk_config_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the Optical Centre structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_optical_centre_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_optical_centre(
|
||||
VL53L1_optical_centre_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function to print out the contents of
|
||||
* the User Zone (ROI) structure for debug
|
||||
*
|
||||
* @param[in] pdata : pointer to a VL53L1_user_zone_t structure
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_user_zone(
|
||||
VL53L1_user_zone_t *pdata,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
/**
|
||||
* @brief Convenience function for printing out VL53L1_spad_rate_data_t
|
||||
*
|
||||
* @param[in] pspad_rates : pointer to VL53L1_spad_rate_data_t
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_spad_rate_data(
|
||||
VL53L1_spad_rate_data_t *pspad_rates,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Convenience function for printing out VL53L1_spad_rate_map_t
|
||||
*
|
||||
* @param[in] pspad_rates : pointer to VL53L1_spad_rate_map_t
|
||||
* @param[in] pprefix : pointer to name prefix string
|
||||
* @param[in] trace_flags : logging module enable bit flags
|
||||
*/
|
||||
|
||||
void VL53L1_print_spad_rate_map(
|
||||
VL53L1_spad_rate_data_t *pspad_rates,
|
||||
char *pprefix,
|
||||
uint32_t trace_flags);
|
||||
|
||||
|
||||
#endif /* VL53L1_LOG_ENABLE */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_API_DEBUG_H_ */
|
||||
471
App/VL53L1X_API/core/inc/vl53l1_api_preset_modes.h
Normal file
471
App/VL53L1X_API/core/inc/vl53l1_api_preset_modes.h
Normal file
@@ -0,0 +1,471 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_preset_modes.h
|
||||
*
|
||||
* @brief EwokPlus25 API core function definition
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_API_PRESET_MODES_H_
|
||||
#define _VL53L1_API_PRESET_MODES_H_
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes Ref SPAD Char Configuration Parameters
|
||||
*
|
||||
* @param[out] pdata : pointer to VL53L1_refspadchar_config_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_init_refspadchar_config_struct(
|
||||
VL53L1_refspadchar_config_t *pdata);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes SPAD Self Check (SSC) Configuration Parameters
|
||||
*
|
||||
* @param[out] pdata : pointer to VL53L1_ssc_config_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_init_ssc_config_struct(
|
||||
VL53L1_ssc_config_t *pdata);
|
||||
|
||||
/**
|
||||
* @brief Initializes Xtalk Configuration Parameters
|
||||
*
|
||||
* @param[in] pnvm : pointer to VL53L1_customer_nvm_managed_t data structure
|
||||
* @param[out] pdata : pointer to VL53L1_xtalk_config_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_init_xtalk_config_struct(
|
||||
VL53L1_customer_nvm_managed_t *pnvm,
|
||||
VL53L1_xtalk_config_t *pdata);
|
||||
|
||||
/**
|
||||
* @brief Initializes Offset Calibration Configuration Parameters
|
||||
*
|
||||
* @param[out] pdata : pointer to VL53L1_offsetcal_config_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_init_offset_cal_config_struct(
|
||||
VL53L1_offsetcal_config_t *pdata);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes Tuning Parameter Storage Values
|
||||
*
|
||||
* @param[out] pdata : pointer to VL53L1_tuning_parm_storage_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_init_tuning_parm_storage_struct(
|
||||
VL53L1_tuning_parm_storage_t *pdata);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_STANDARD_RANGING
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_standard_ranging(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_SHORT_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_standard_ranging_short_range(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_LONG_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_standard_ranging_long_range(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_preset_mode_standard_ranging_mm1_cal(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_preset_mode_standard_ranging_mm2_cal(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_TIMED_RANGING
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_timed_ranging(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_TIMED_RANGING_SHORT_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_timed_ranging_short_range(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_TIMED_RANGING_LONG_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_timed_ranging_long_range(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_MEDIUM_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
* @param[out] plpadata : pointer to VL53L1_low_power_auto_data_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_low_power_auto_ranging(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms,
|
||||
VL53L1_low_power_auto_data_t *plpadata);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_SHORT_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
* @param[out] plpadata : pointer to VL53L1_low_power_auto_data_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_low_power_auto_short_ranging(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms,
|
||||
VL53L1_low_power_auto_data_t *plpadata);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_LONG_RANGE
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
* @param[out] plpadata : pointer to VL53L1_low_power_auto_data_t data structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_low_power_auto_long_ranging(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms,
|
||||
VL53L1_low_power_auto_data_t *plpadata);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_OLT
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_olt(
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
/**
|
||||
* @brief Initializes static and dynamic configuration settings for
|
||||
* preset mode VL53L1_DEVICEPRESETMODE_SINGLESHOT_RANGING
|
||||
*
|
||||
* @param[out] pstatic : pointer to VL53L1_static_config_t data structure
|
||||
* @param[out] pgeneral : pointer to VL53L1_general_config_t data structure
|
||||
* @param[out] ptiming : pointer to VL53L1_timing_config_t data structure
|
||||
* @param[out] pdynamic : pointer to VL53L1_dynamic_config_t data structure
|
||||
* @param[out] psystem : pointer to VL53L1_system_control_t data structure
|
||||
* @param[out] ptuning_parms : pointer to VL53L1_tuning_parms_storage_t structure
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_preset_mode_singleshot_ranging(
|
||||
|
||||
VL53L1_static_config_t *pstatic,
|
||||
VL53L1_general_config_t *pgeneral,
|
||||
VL53L1_timing_config_t *ptiming,
|
||||
VL53L1_dynamic_config_t *pdynamic,
|
||||
VL53L1_system_control_t *psystem,
|
||||
VL53L1_tuning_parm_storage_t *ptuning_parms);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_API_CORE_H_ */
|
||||
159
App/VL53L1X_API/core/inc/vl53l1_api_strings.h
Normal file
159
App/VL53L1X_API/core/inc/vl53l1_api_strings.h
Normal file
@@ -0,0 +1,159 @@
|
||||
|
||||
/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
|
||||
/******************************************************************************
|
||||
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
|
||||
|
||||
This file is part of VL53L1 and is dual licensed,
|
||||
either GPL-2.0+
|
||||
or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_strings.h
|
||||
* @brief VL53L1 API function declarations for decoding error codes to a
|
||||
* text strings
|
||||
*/
|
||||
|
||||
|
||||
#ifndef VL53L1_API_STRINGS_H_
|
||||
#define VL53L1_API_STRINGS_H_
|
||||
|
||||
#include "vl53l1_def.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates a string for the input device range status code
|
||||
*
|
||||
* @param[in] RangeStatus : Device Range AStatus Code
|
||||
* @param[out] pRangeStatusString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_get_range_status_string(
|
||||
uint8_t RangeStatus,
|
||||
char *pRangeStatusString);
|
||||
|
||||
/**
|
||||
* @brief Generates an error string for the input PAL error code
|
||||
*
|
||||
* @param[in] PalErrorCode : PAL Error Code
|
||||
* @param[out] pPalErrorString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_get_pal_error_string(
|
||||
VL53L1_Error PalErrorCode,
|
||||
char *pPalErrorString);
|
||||
|
||||
/**
|
||||
* @brief Generates a string for the input PAL State code
|
||||
*
|
||||
* @param[in] PalStateCode : PAL State Code
|
||||
* @param[out] pPalStateString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_get_pal_state_string(
|
||||
VL53L1_State PalStateCode,
|
||||
char *pPalStateString);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates a string for the sequence step Id
|
||||
*
|
||||
* @param[in] SequenceStepId : Sequence Step Id
|
||||
* @param[out] pSequenceStepsString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
VL53L1_Error VL53L1_get_sequence_steps_info(
|
||||
VL53L1_SequenceStepId SequenceStepId,
|
||||
char *pSequenceStepsString);
|
||||
|
||||
/**
|
||||
* @brief Generates a string for the limit check Id
|
||||
*
|
||||
* @param[in] LimitCheckId : Limit check Id
|
||||
* @param[out] pLimitCheckString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
VL53L1_Error VL53L1_get_limit_check_info(uint16_t LimitCheckId,
|
||||
char *pLimitCheckString);
|
||||
|
||||
#ifndef VL53L1_USE_EMPTY_STRING
|
||||
#define VL53L1_STRING_DEVICE_INFO_NAME0 "VL53L1 cut1.0"
|
||||
#define VL53L1_STRING_DEVICE_INFO_NAME1 "VL53L1 cut1.1"
|
||||
#define VL53L1_STRING_DEVICE_INFO_TYPE "VL53L1"
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L1_STRING_RANGESTATUS_NONE "No Update"
|
||||
#define VL53L1_STRING_RANGESTATUS_RANGEVALID "Range Valid"
|
||||
#define VL53L1_STRING_RANGESTATUS_SIGMA "Sigma Fail"
|
||||
#define VL53L1_STRING_RANGESTATUS_SIGNAL "Signal Fail"
|
||||
#define VL53L1_STRING_RANGESTATUS_MINRANGE "Min Range Fail"
|
||||
#define VL53L1_STRING_RANGESTATUS_PHASE "Phase Fail"
|
||||
#define VL53L1_STRING_RANGESTATUS_HW "Hardware Fail"
|
||||
|
||||
|
||||
/* Range Status */
|
||||
#define VL53L1_STRING_STATE_POWERDOWN "POWERDOWN State"
|
||||
#define VL53L1_STRING_STATE_WAIT_STATICINIT \
|
||||
"Wait for staticinit State"
|
||||
#define VL53L1_STRING_STATE_STANDBY "STANDBY State"
|
||||
#define VL53L1_STRING_STATE_IDLE "IDLE State"
|
||||
#define VL53L1_STRING_STATE_RUNNING "RUNNING State"
|
||||
#define VL53L1_STRING_STATE_RESET "RESET State"
|
||||
#define VL53L1_STRING_STATE_UNKNOWN "UNKNOWN State"
|
||||
#define VL53L1_STRING_STATE_ERROR "ERROR State"
|
||||
|
||||
|
||||
|
||||
/* Check Enable */
|
||||
#define VL53L1_STRING_CHECKENABLE_SIGMA_FINAL_RANGE \
|
||||
"SIGMA FINAL RANGE"
|
||||
#define VL53L1_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE \
|
||||
"SIGNAL RATE FINAL RANGE"
|
||||
#define VL53L1_STRING_CHECKENABLE_SIGNAL_MIN_CLIP \
|
||||
"SIGNAL MIN CLIP"
|
||||
#define VL53L1_STRING_CHECKENABLE_RANGE_IGNORE_THRESHOLD \
|
||||
"RANGE IGNORE THRESHOLD"
|
||||
#define VL53L1_STRING_CHECKENABLE_RANGE_PHASE_HIGH \
|
||||
"RANGE PHASE HIGH"
|
||||
#define VL53L1_STRING_CHECKENABLE_RANGE_PHASE_LOW \
|
||||
"RANGE PHASE LOW"
|
||||
#define VL53L1_STRING_CHECKENABLE_RANGE_PHASE_CONSISTENCY \
|
||||
"RANGE PHASE CONSISTENCY"
|
||||
|
||||
/* Sequence Step */
|
||||
#define VL53L1_STRING_SEQUENCESTEP_VHV "VHV"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_PHASECAL "PHASE CAL"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_REFPHASE "REF PHASE"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_DSS1 "DSS1"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_DSS2 "DSS2"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_MM1 "MM1"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_MM2 "MM2"
|
||||
#define VL53L1_STRING_SEQUENCESTEP_RANGE "RANGE"
|
||||
#endif /* VL53L1_USE_EMPTY_STRING */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
1090
App/VL53L1X_API/core/inc/vl53l1_core.h
Normal file
1090
App/VL53L1X_API/core/inc/vl53l1_core.h
Normal file
File diff suppressed because it is too large
Load Diff
222
App/VL53L1X_API/core/inc/vl53l1_core_support.h
Normal file
222
App/VL53L1X_API/core/inc/vl53l1_core_support.h
Normal file
@@ -0,0 +1,222 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_core_support.h
|
||||
*
|
||||
* @brief EwokPlus25 core function definitions
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_CORE_SUPPORT_H_
|
||||
#define _VL53L1_CORE_SUPPORT_H_
|
||||
|
||||
#include "vl53l1_types.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculates the PLL period in [us] from the input
|
||||
* fast_osc_frequency
|
||||
*
|
||||
* @param[in] fast_osc_frequency : fast oscillator frequency in 4.12 MHz format
|
||||
*
|
||||
* @return pll_period_us : PLL period in [us] 8.24 format
|
||||
*/
|
||||
|
||||
uint32_t VL53L1_calc_pll_period_us(
|
||||
uint16_t fast_osc_frequency);
|
||||
|
||||
|
||||
|
||||
#ifdef PAL_EXTENDED
|
||||
/**
|
||||
* @brief Calculates the ranging duration in ns using fixed point maths
|
||||
* *
|
||||
* Uses a temporary uint64_t variable internally
|
||||
*
|
||||
* @param[in] pll_period_us : PLL Period in [us] 0.25 format
|
||||
* @param[in] vcsel_parm_pclks : period, width or WOI window in PLL clocks
|
||||
* in 6.4 format.
|
||||
* @param[in] window_vclks : ranging or ambient window duration in VCSEL clocks
|
||||
* @param[in] periods_elapsed_mclks : elapsed time in macro clocks
|
||||
*
|
||||
* @return duration_us : uint32_t containing the duration time in us
|
||||
*/
|
||||
|
||||
uint32_t VL53L1_duration_maths(
|
||||
uint32_t pll_period_us,
|
||||
uint32_t vcsel_parm_pclks,
|
||||
uint32_t window_vclks,
|
||||
uint32_t periods_elapsed_mclks);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculates the square root of the input integer
|
||||
*
|
||||
* Reference : http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
|
||||
*
|
||||
* @param[in] num : input integer
|
||||
*
|
||||
* @return res : square root result
|
||||
*/
|
||||
|
||||
uint32_t VL53L1_isqrt(
|
||||
uint32_t num);
|
||||
|
||||
/**
|
||||
* @brief Calculates the count rate using fixed point maths
|
||||
*
|
||||
* Uses a temporary uint64_t variable internally
|
||||
* Any negative negative rates are clipped to 0.
|
||||
*
|
||||
* @param[in] events : accumulated SPAD events
|
||||
* @param[in] time_us : elapsed time in us
|
||||
*
|
||||
* @return rate_mcps : uint16_t count rate in 9.7 format
|
||||
*/
|
||||
|
||||
uint16_t VL53L1_rate_maths(
|
||||
int32_t events,
|
||||
uint32_t time_us);
|
||||
|
||||
/**
|
||||
* @brief Calculates the Rate per spad
|
||||
*
|
||||
* Uses a temporary uint32_t variable internally
|
||||
* Any output rates larger than 16 bit are clipped to 0xFFFF.
|
||||
*
|
||||
* @param[in] frac_bits : required output fractional precision - 7 \
|
||||
* due to inherent resolution of pk_rate
|
||||
* @param[in] peak_count_rate : peak signal count rate in mcps
|
||||
* @param[in] num_spads : actual effective spads in 8.8
|
||||
* @param[in] max_output_value : User set value to clip output
|
||||
*
|
||||
* @return rate_per_spad : uint16_t count rate in variable fractional format
|
||||
*/
|
||||
|
||||
uint16_t VL53L1_rate_per_spad_maths(
|
||||
uint32_t frac_bits,
|
||||
uint32_t peak_count_rate,
|
||||
uint16_t num_spads,
|
||||
uint32_t max_output_value);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Calculates the range from the phase data
|
||||
*
|
||||
* Uses a temporary int64_t variable internally
|
||||
*
|
||||
* @param[in] fast_osc_frequency : Fast oscillator freq [MHz] 4.12 format
|
||||
* @param[in] phase : phase in 5.11 format
|
||||
* @param[in] zero_distance_phase : zero distance phase in 5.11 format
|
||||
* @param[in] fractional_bits : valid options : 0, 1, 2
|
||||
* @param[in] gain_factor : gain correction factor 1.11 format
|
||||
* @param[in] range_offset_mm : range offset [mm] 14.2 format
|
||||
|
||||
* @return range_mm : signed range in [mm]
|
||||
* format depends on fractional_bits input
|
||||
*/
|
||||
|
||||
int32_t VL53L1_range_maths(
|
||||
uint16_t fast_osc_frequency,
|
||||
uint16_t phase,
|
||||
uint16_t zero_distance_phase,
|
||||
uint8_t fractional_bits,
|
||||
int32_t gain_factor,
|
||||
int32_t range_offset_mm);
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Decodes VCSEL period register value into the real period in PLL clocks
|
||||
*
|
||||
* @param[in] vcsel_period_reg : 8 -bit register value
|
||||
*
|
||||
* @return vcsel_period_pclks : 8-bit decoded value
|
||||
*
|
||||
*/
|
||||
|
||||
uint8_t VL53L1_decode_vcsel_period(
|
||||
uint8_t vcsel_period_reg);
|
||||
|
||||
/**
|
||||
* @brief Decodes the Byte.Bit coord encoding into an (x,y) coord value
|
||||
*
|
||||
* @param[in] spad_number : Coord location in Byte.Bit format
|
||||
* @param[out] prow : Decoded row
|
||||
* @param[out] pcol : Decoded column
|
||||
*
|
||||
*/
|
||||
|
||||
void VL53L1_decode_row_col(
|
||||
uint8_t spad_number,
|
||||
uint8_t *prow,
|
||||
uint8_t *pcol);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_CORE_SUPPORT_H_ */
|
||||
603
App/VL53L1X_API/core/inc/vl53l1_def.h
Normal file
603
App/VL53L1X_API/core/inc/vl53l1_def.h
Normal file
@@ -0,0 +1,603 @@
|
||||
|
||||
/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
|
||||
/******************************************************************************
|
||||
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
|
||||
|
||||
This file is part of VL53L1 and is dual licensed,
|
||||
either GPL-2.0+
|
||||
or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_def.h
|
||||
*
|
||||
* @brief Type definitions for VL53L1 API.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L1_DEF_H_
|
||||
#define _VL53L1_DEF_H_
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @defgroup VL53L1_globaldefine_group VL53L1 Defines
|
||||
* @brief VL53L1 Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
/** VL53L1 IMPLEMENTATION major version */
|
||||
#define VL53L1_IMPLEMENTATION_VER_MAJOR 2
|
||||
/** VL53L1 IMPLEMENTATION minor version */
|
||||
#define VL53L1_IMPLEMENTATION_VER_MINOR 4
|
||||
/** VL53L1 IMPLEMENTATION sub version */
|
||||
#define VL53L1_IMPLEMENTATION_VER_SUB 5
|
||||
/** VL53L1 IMPLEMENTATION sub version */
|
||||
#define VL53L1_IMPLEMENTATION_VER_REVISION 2548
|
||||
|
||||
/****************************************
|
||||
* PRIVATE define do not edit
|
||||
****************************************/
|
||||
|
||||
/** @brief Defines the parameters of the Get Version Functions
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t revision; /*!< revision number */
|
||||
uint8_t major; /*!< major number */
|
||||
uint8_t minor; /*!< minor number */
|
||||
uint8_t build; /*!< build number */
|
||||
} VL53L1_Version_t;
|
||||
|
||||
|
||||
#define VL53L1_DEVINFO_STRLEN 32
|
||||
|
||||
/** @brief Defines the parameters of the Get Device Info Functions
|
||||
*/
|
||||
typedef struct {
|
||||
char Name[VL53L1_DEVINFO_STRLEN];
|
||||
/*!< Full Name of the Device e.g. VL53L1 cut1.1 */
|
||||
char Type[VL53L1_DEVINFO_STRLEN];
|
||||
/*!< Type of the Device e.g VL53L1 */
|
||||
char ProductId[VL53L1_DEVINFO_STRLEN];
|
||||
/*!< Product Identifier String
|
||||
* @warning Not yet implemented
|
||||
*/
|
||||
uint8_t ProductType;
|
||||
/*!< Product Type, VL53L1 = 0xCC, VL53L3 = 0xAA
|
||||
* Stands as module_type in the datasheet
|
||||
*/
|
||||
uint8_t ProductRevisionMajor;
|
||||
/*!< Product revision major */
|
||||
uint8_t ProductRevisionMinor;
|
||||
/*!< Product revision minor */
|
||||
} VL53L1_DeviceInfo_t;
|
||||
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_PresetModes_group Defines Preset modes
|
||||
* Defines all possible preset modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_PresetModes;
|
||||
|
||||
#define VL53L1_PRESETMODE_AUTONOMOUS ((VL53L1_PresetModes) 3)
|
||||
#define VL53L1_PRESETMODE_LITE_RANGING ((VL53L1_PresetModes) 4)
|
||||
#define VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS ((VL53L1_PresetModes) 8)
|
||||
|
||||
/* ... Modes to be added depending on device */
|
||||
/** @} VL53L1_define_PresetModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_DistanceModes_group Defines Distance modes
|
||||
* Defines all possible Distance modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DistanceModes;
|
||||
|
||||
#define VL53L1_DISTANCEMODE_SHORT ((VL53L1_DistanceModes) 1)
|
||||
#define VL53L1_DISTANCEMODE_MEDIUM ((VL53L1_DistanceModes) 2)
|
||||
#define VL53L1_DISTANCEMODE_LONG ((VL53L1_DistanceModes) 3)
|
||||
/** @} VL53L1_define_DistanceModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_XtalkCal_group Defines Xtalk Calibration modes
|
||||
* Defines all possible Offset Calibration modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_XtalkCalibrationModes;
|
||||
|
||||
#define VL53L1_XTALKCALIBRATIONMODE_NO_TARGET \
|
||||
((VL53L1_OffsetCalibrationModes) 0)
|
||||
/*!< To perform Xtalk calibration with no target below 80 cm */
|
||||
#define VL53L1_XTALKCALIBRATIONMODE_SINGLE_TARGET \
|
||||
((VL53L1_OffsetCalibrationModes) 1)
|
||||
/*!< To perform Xtalk calibration with one target */
|
||||
#define VL53L1_XTALKCALIBRATIONMODE_FULL_ROI \
|
||||
((VL53L1_OffsetCalibrationModes) 2)
|
||||
/*!< To perform Xtalk calibration based on histogram with full ROI */
|
||||
|
||||
/** @} VL53L1_define_XtalkCal_group */
|
||||
|
||||
/** @defgroup VL53L1_define_OffsetCal_group Defines Offset Calibration modes
|
||||
* Defines all possible Offset Calibration modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_OffsetCalibrationModes;
|
||||
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE_STANDARD \
|
||||
((VL53L1_OffsetCalibrationModes) 1)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE_PRERANGE_ONLY \
|
||||
((VL53L1_OffsetCalibrationModes) 2)
|
||||
|
||||
/** @} VL53L1_define_OffsetCal_group */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/** @defgroup VL53L1_CheckEnable_group Check Enable list
|
||||
* @brief Check Enable code
|
||||
*
|
||||
* Define used to specify the LimitCheckId.
|
||||
* Use @a VL53L1_GetLimitCheckInfo() to get the string.
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE 0
|
||||
#define VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE 1
|
||||
|
||||
#define VL53L1_CHECKENABLE_NUMBER_OF_CHECKS 2
|
||||
|
||||
/** @} end of VL53L1_CheckEnable_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_ThresholdMode_gropup Detection Functionality
|
||||
* @brief Defines the different functionalities for the detection feature
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_ThresholdMode;
|
||||
|
||||
#define VL53L1_THRESHOLD_CROSSED_LOW \
|
||||
((VL53L1_ThresholdMode) 0)
|
||||
/*!< Trigger interrupt if value < thresh_low */
|
||||
#define VL53L1_THRESHOLD_CROSSED_HIGH \
|
||||
((VL53L1_ThresholdMode) 1)
|
||||
/*!< Trigger interrupt if value > thresh_high */
|
||||
#define VL53L1_THRESHOLD_OUT_OF_WINDOW \
|
||||
((VL53L1_ThresholdMode) 2)
|
||||
/*!< Trigger interrupt if value < thresh_low OR value > thresh_high */
|
||||
#define VL53L1_THRESHOLD_IN_WINDOW \
|
||||
((VL53L1_ThresholdMode) 3)
|
||||
/*!< Trigger interrupt if value > thresh_low AND value < thresh_high */
|
||||
|
||||
/** @} end of VL53L1_ThresholdMode_gropup */
|
||||
|
||||
/** @brief Defines parameters for Distance detection Thresholds configuration
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L1_ThresholdMode CrossMode;
|
||||
uint16_t High; /*!< Distance threshold high limit in mm */
|
||||
uint16_t Low; /*!< Distance threshold low limit in mm */
|
||||
} VL53L1_DistanceThreshold_t;
|
||||
|
||||
/** @brief Defines parameters for Signal rate detection Thresholds configuration
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L1_ThresholdMode CrossMode;
|
||||
FixPoint1616_t High; /*!< Signal rate threshold high limit */
|
||||
FixPoint1616_t Low; /*!< Signal rate threshold low limit */
|
||||
} VL53L1_RateThreshold_t;
|
||||
|
||||
/** @defgroup VL53L1_DetectionMode_group Gpio Functionality
|
||||
* @brief Defines conditions leading to device's IT on GPIO
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DetectionMode;
|
||||
|
||||
#define VL53L1_DETECTION_NORMAL_RUN \
|
||||
((VL53L1_DetectionMode) 0)
|
||||
/*!< Trigger interrupt on new measurement regardless of threshold
|
||||
* just like after a VL53L1_SetPresetMode() call
|
||||
*/
|
||||
#define VL53L1_DETECTION_DISTANCE_ONLY \
|
||||
((VL53L1_DetectionMode) 1)
|
||||
/*!< Trigger interrupt if "threshold event" occurs on distance */
|
||||
#define VL53L1_DETECTION_RATE_ONLY \
|
||||
((VL53L1_DetectionMode) 2)
|
||||
/*!< Trigger interrupt if "threshold event" occurs on signal rate */
|
||||
#define VL53L1_DETECTION_DISTANCE_AND_RATE \
|
||||
((VL53L1_DetectionMode) 3)
|
||||
/*!< Trigger interrupt if "threshold event" occurs on distance AND rate
|
||||
*/
|
||||
#define VL53L1_DETECTION_DISTANCE_OR_RATE \
|
||||
((VL53L1_DetectionMode) 4)
|
||||
/*!< Trigger interrupt if "threshold event" occurs on distance OR rate
|
||||
*/
|
||||
|
||||
/** @} end of VL53L1_DetectionMode_group */
|
||||
|
||||
/** @brief Defines parameters for User/object Detection configuration
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L1_DetectionMode DetectionMode; /*!< See #VL53L1_DetectionMode*/
|
||||
uint8_t IntrNoTarget; /*!< 1 to trigger IT in case of no target found */
|
||||
VL53L1_DistanceThreshold_t Distance; /*!< limits in mm */
|
||||
VL53L1_RateThreshold_t Rate;/*!< limits in FixPoint1616_t */
|
||||
} VL53L1_DetectionConfig_t;
|
||||
|
||||
|
||||
/** @brief Defines all parameters for the device
|
||||
*/
|
||||
typedef struct {
|
||||
VL53L1_PresetModes PresetMode;
|
||||
/*!< Defines the operating mode to be used for the next measure */
|
||||
VL53L1_DistanceModes DistanceMode;
|
||||
/*!< Defines the operating mode to be used for the next measure */
|
||||
uint32_t MeasurementTimingBudgetMicroSeconds;
|
||||
/*!< Defines the allowed total time for a single measurement */
|
||||
uint8_t LimitChecksEnable[VL53L1_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array store all the Limit Check enable for this device. */
|
||||
uint8_t LimitChecksStatus[VL53L1_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array stores all the Status of the check linked to last
|
||||
* measurement.
|
||||
*/
|
||||
FixPoint1616_t LimitChecksValue[VL53L1_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array stores all the Limit Check value for this device */
|
||||
FixPoint1616_t LimitChecksCurrent[VL53L1_CHECKENABLE_NUMBER_OF_CHECKS];
|
||||
/*!< This Array stores all the Limit Check current value from latest
|
||||
* ranging
|
||||
*/
|
||||
} VL53L1_DeviceParameters_t;
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_State_group Defines the current status of the device
|
||||
* Defines the current status of the device
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L1_State;
|
||||
|
||||
#define VL53L1_STATE_POWERDOWN ((VL53L1_State) 0)
|
||||
/*!< Device is in HW reset */
|
||||
#define VL53L1_STATE_WAIT_STATICINIT ((VL53L1_State) 1)
|
||||
/*!< Device is initialized and wait for static initialization */
|
||||
#define VL53L1_STATE_STANDBY ((VL53L1_State) 2)
|
||||
/*!< Device is in Low power Standby mode */
|
||||
#define VL53L1_STATE_IDLE ((VL53L1_State) 3)
|
||||
/*!< Device has been initialized and ready to do measurements */
|
||||
#define VL53L1_STATE_RUNNING ((VL53L1_State) 4)
|
||||
/*!< Device is performing measurement */
|
||||
#define VL53L1_STATE_RESET ((VL53L1_State) 5)
|
||||
/*!< Soft reset has been run on Device */
|
||||
#define VL53L1_STATE_UNKNOWN ((VL53L1_State) 98)
|
||||
/*!< Device is in unknown state and need to be rebooted */
|
||||
#define VL53L1_STATE_ERROR ((VL53L1_State) 99)
|
||||
/*!< Device is in error state and need to be rebooted */
|
||||
|
||||
/** @} VL53L1_define_State_group */
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @struct VL53L1_RangingMeasurementData_t
|
||||
* @brief Single Range measurement data.
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t TimeStamp;
|
||||
/*!< 32-bit time stamp.
|
||||
* @warning Not yet implemented
|
||||
*/
|
||||
|
||||
uint8_t StreamCount;
|
||||
/*!< 8-bit Stream Count. */
|
||||
|
||||
uint8_t RangeQualityLevel;
|
||||
/*!< indicate a quality level in percentage from 0 to 100
|
||||
* @warning Not yet implemented
|
||||
*/
|
||||
|
||||
FixPoint1616_t SignalRateRtnMegaCps;
|
||||
/*!< Return signal rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of target
|
||||
* reflectance.
|
||||
*/
|
||||
|
||||
FixPoint1616_t AmbientRateRtnMegaCps;
|
||||
/*!< Return ambient rate (MCPS)\n these is a 16.16 fix point
|
||||
* value, which is effectively a measure of the ambient
|
||||
* light.
|
||||
*/
|
||||
|
||||
uint16_t EffectiveSpadRtnCount;
|
||||
/*!< Return the effective SPAD count for the return signal.
|
||||
* To obtain Real value it should be divided by 256
|
||||
*/
|
||||
|
||||
FixPoint1616_t SigmaMilliMeter;
|
||||
/*!< Return the Sigma value in millimeter */
|
||||
|
||||
int16_t RangeMilliMeter;
|
||||
/*!< range distance in millimeter. This should be between
|
||||
* RangeMinMilliMeter and RangeMaxMilliMeter
|
||||
*/
|
||||
|
||||
uint8_t RangeFractionalPart;
|
||||
/*!< Fractional part of range distance. Final value is a
|
||||
* RangeMilliMeter + RangeFractionalPart/256.
|
||||
* @warning Not yet implemented
|
||||
*/
|
||||
|
||||
uint8_t RangeStatus;
|
||||
/*!< Range Status for the current measurement. This is device
|
||||
* dependent. Value = 0 means value is valid.
|
||||
*/
|
||||
} VL53L1_RangingMeasurementData_t;
|
||||
|
||||
|
||||
|
||||
/** @brief Defines User Zone(ROI) parameters
|
||||
*
|
||||
*/
|
||||
typedef struct {
|
||||
|
||||
uint8_t TopLeftX; /*!< Top Left x coordinate: 0-15 range */
|
||||
uint8_t TopLeftY; /*!< Top Left y coordinate: 0-15 range */
|
||||
uint8_t BotRightX; /*!< Bot Right x coordinate: 0-15 range */
|
||||
uint8_t BotRightY; /*!< Bot Right y coordinate: 0-15 range */
|
||||
|
||||
} VL53L1_UserRoi_t;
|
||||
|
||||
|
||||
/** @brief Defines ROI configuration parameters
|
||||
*
|
||||
* Support up a max of 16 zones, Each Zone has the same size
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @struct VL53L1_CustomerNvmManaged_t
|
||||
*
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
uint8_t global_config__spad_enables_ref_0;
|
||||
uint8_t global_config__spad_enables_ref_1;
|
||||
uint8_t global_config__spad_enables_ref_2;
|
||||
uint8_t global_config__spad_enables_ref_3;
|
||||
uint8_t global_config__spad_enables_ref_4;
|
||||
uint8_t global_config__spad_enables_ref_5;
|
||||
uint8_t global_config__ref_en_start_select;
|
||||
uint8_t ref_spad_man__num_requested_ref_spads;
|
||||
uint8_t ref_spad_man__ref_location;
|
||||
uint32_t algo__crosstalk_compensation_plane_offset_kcps;
|
||||
int16_t algo__crosstalk_compensation_x_plane_gradient_kcps;
|
||||
int16_t algo__crosstalk_compensation_y_plane_gradient_kcps;
|
||||
uint16_t ref_spad_char__total_rate_target_mcps;
|
||||
int16_t algo__part_to_part_range_offset_mm;
|
||||
int16_t mm_config__inner_offset_mm;
|
||||
int16_t mm_config__outer_offset_mm;
|
||||
} VL53L1_CustomerNvmManaged_t;
|
||||
|
||||
/**
|
||||
* @struct VL53L1_CalibrationData_t
|
||||
* @brief Structure for storing the Calibration Data
|
||||
*
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint32_t struct_version;
|
||||
VL53L1_CustomerNvmManaged_t customer;
|
||||
VL53L1_additional_offset_cal_data_t add_off_cal_data;
|
||||
VL53L1_optical_centre_t optical_centre;
|
||||
VL53L1_gain_calibration_data_t gain_cal;
|
||||
VL53L1_cal_peak_rate_map_t cal_peak_rate_map;
|
||||
} VL53L1_CalibrationData_t;
|
||||
|
||||
#define VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION 0x20
|
||||
/** VL53L1 additional Calibration Data struct version final struct version
|
||||
* is given by adding it to VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION
|
||||
*/
|
||||
|
||||
#define VL53L1_CALIBRATION_DATA_STRUCT_VERSION \
|
||||
(VL53L1_LL_CALIBRATION_DATA_STRUCT_VERSION + \
|
||||
VL53L1_ADDITIONAL_CALIBRATION_DATA_STRUCT_VERSION)
|
||||
/* VL53L1 Calibration Data struct version */
|
||||
|
||||
/**
|
||||
* @struct VL53L1_AdditionalData_t
|
||||
* @brief Structure for storing the Additional Data
|
||||
*
|
||||
*/
|
||||
typedef VL53L1_additional_data_t VL53L1_AdditionalData_t;
|
||||
|
||||
|
||||
typedef uint8_t VL53L1_SequenceStepId;
|
||||
|
||||
#define VL53L1_SEQUENCESTEP_VHV ((VL53L1_SequenceStepId) 0)
|
||||
/*!<VHV. */
|
||||
#define VL53L1_SEQUENCESTEP_PHASECAL ((VL53L1_SequenceStepId) 1)
|
||||
/*!<Phase Calibration. */
|
||||
#define VL53L1_SEQUENCESTEP_REFPHASE ((VL53L1_SequenceStepId) 2)
|
||||
/*!<Reference Phase. */
|
||||
#define VL53L1_SEQUENCESTEP_DSS1 ((VL53L1_SequenceStepId) 3)
|
||||
/*!<DSS1. */
|
||||
#define VL53L1_SEQUENCESTEP_DSS2 ((VL53L1_SequenceStepId) 4)
|
||||
/*!<DSS2. */
|
||||
#define VL53L1_SEQUENCESTEP_MM1 ((VL53L1_SequenceStepId) 5)
|
||||
/*!<Mode Mitigation 1. */
|
||||
#define VL53L1_SEQUENCESTEP_MM2 ((VL53L1_SequenceStepId) 6)
|
||||
/*!<Mode Mitigation 2. */
|
||||
#define VL53L1_SEQUENCESTEP_RANGE ((VL53L1_SequenceStepId) 7)
|
||||
/*!<Final Range step. */
|
||||
|
||||
#define VL53L1_SEQUENCESTEP_NUMBER_OF_ITEMS 8
|
||||
/*!<Number of Sequence Step Managed by the API. */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_RangeStatus_group Defines the Range Status
|
||||
* @{
|
||||
*/
|
||||
#define VL53L1_RANGESTATUS_RANGE_VALID 0
|
||||
/*!<The Range is valid. */
|
||||
#define VL53L1_RANGESTATUS_SIGMA_FAIL 1
|
||||
/*!<Sigma Fail. */
|
||||
#define VL53L1_RANGESTATUS_SIGNAL_FAIL 2
|
||||
/*!<Signal fail. */
|
||||
#define VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED 3
|
||||
/*!<Target is below minimum detection threshold. */
|
||||
#define VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL 4
|
||||
/*!<Phase out of valid limits - different to a wrap exit. */
|
||||
#define VL53L1_RANGESTATUS_HARDWARE_FAIL 5
|
||||
/*!<Hardware fail. */
|
||||
#define VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL 6
|
||||
/*!<The Range is valid but the wraparound check has not been done. */
|
||||
#define VL53L1_RANGESTATUS_WRAP_TARGET_FAIL 7
|
||||
/*!<Wrapped target - no matching phase in other VCSEL period timing. */
|
||||
#define VL53L1_RANGESTATUS_PROCESSING_FAIL 8
|
||||
/*!<Internal algo underflow or overflow in lite ranging. */
|
||||
#define VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL 9
|
||||
/*!<Specific to lite ranging. */
|
||||
#define VL53L1_RANGESTATUS_SYNCRONISATION_INT 10
|
||||
/*!<1st interrupt when starting ranging in back to back mode. Ignore data. */
|
||||
#define VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE 11
|
||||
/*!<All Range ok but object is result of multiple pulses merging together.
|
||||
* Used by RQL for merged pulse detection
|
||||
*/
|
||||
#define VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL 12
|
||||
/*!<Used by RQL as different to phase fail. */
|
||||
#define VL53L1_RANGESTATUS_MIN_RANGE_FAIL 13
|
||||
/*!<User ROI input is not valid e.g. beyond SPAD Array.*/
|
||||
#define VL53L1_RANGESTATUS_RANGE_INVALID 14
|
||||
/*!<lld returned valid range but negative value ! */
|
||||
#define VL53L1_RANGESTATUS_NONE 255
|
||||
/*!<No Update. */
|
||||
|
||||
/** @} VL53L1_define_RangeStatus_group */
|
||||
|
||||
|
||||
/** @brief Contains the Internal data of the Bare Driver
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
VL53L1_LLDriverData_t LLData;
|
||||
/*!< Low Level Driver data structure */
|
||||
|
||||
VL53L1_LLDriverResults_t llresults;
|
||||
/*!< Low Level Driver data structure */
|
||||
|
||||
VL53L1_State PalState; /* Store the pal state */
|
||||
VL53L1_DeviceParameters_t CurrentParameters;
|
||||
/*!< Current Device Parameter */
|
||||
|
||||
} VL53L1_DevData_t;
|
||||
|
||||
|
||||
/* MACRO Definitions */
|
||||
/** @defgroup VL53L1_define_GeneralMacro_group General Macro Defines
|
||||
* General Macro Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/* Defines */
|
||||
#define VL53L1_SETPARAMETERFIELD(Dev, field, value) \
|
||||
(VL53L1DevDataSet(Dev, CurrentParameters.field, value))
|
||||
|
||||
#define VL53L1_GETPARAMETERFIELD(Dev, field, variable) \
|
||||
(variable = VL53L1DevDataGet(Dev, CurrentParameters).field)
|
||||
|
||||
|
||||
#define VL53L1_SETARRAYPARAMETERFIELD(Dev, field, index, value) \
|
||||
(VL53L1DevDataSet(Dev, CurrentParameters.field[index], value))
|
||||
|
||||
#define VL53L1_GETARRAYPARAMETERFIELD(Dev, field, index, variable) \
|
||||
(variable = VL53L1DevDataGet(Dev, CurrentParameters).field[index])
|
||||
|
||||
|
||||
#define VL53L1_SETDEVICESPECIFICPARAMETER(Dev, field, value) \
|
||||
(VL53L1DevDataSet(Dev, DeviceSpecificParameters.field, value))
|
||||
|
||||
#define VL53L1_GETDEVICESPECIFICPARAMETER(Dev, field) \
|
||||
(VL53L1DevDataGet(Dev, DeviceSpecificParameters).field)
|
||||
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT44(Value) \
|
||||
(uint16_t)((Value>>12)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT44TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<12)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT72(Value) \
|
||||
(uint16_t)((Value>>14)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT72TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<14)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT97(Value) \
|
||||
(uint16_t)((Value>>9)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT97TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<9)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT88(Value) \
|
||||
(uint16_t)((Value>>8)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT88TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<8)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT412(Value) \
|
||||
(uint16_t)((Value>>4)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT412TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<4)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT313(Value) \
|
||||
(uint16_t)((Value>>3)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT313TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<3)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT08(Value) \
|
||||
(uint8_t)((Value>>8)&0x00FF)
|
||||
#define VL53L1_FIXPOINT08TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<8)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT53(Value) \
|
||||
(uint8_t)((Value>>13)&0x00FF)
|
||||
#define VL53L1_FIXPOINT53TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<13)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT102(Value) \
|
||||
(uint16_t)((Value>>14)&0x0FFF)
|
||||
#define VL53L1_FIXPOINT102TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<14)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT142(Value) \
|
||||
(uint16_t)((Value>>14)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT142TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<14)
|
||||
|
||||
#define VL53L1_FIXPOINT1616TOFIXPOINT160(Value) \
|
||||
(uint16_t)((Value>>16)&0xFFFF)
|
||||
#define VL53L1_FIXPOINT160TOFIXPOINT1616(Value) \
|
||||
(FixPoint1616_t)((uint32_t)Value<<16)
|
||||
|
||||
#define VL53L1_MAKEUINT16(lsb, msb) (uint16_t)((((uint16_t)msb)<<8) + \
|
||||
(uint16_t)lsb)
|
||||
|
||||
#ifndef SUPPRESS_UNUSED_WARNING
|
||||
#define SUPPRESS_UNUSED_WARNING(x) ((void) (x))
|
||||
#endif
|
||||
|
||||
/** @} VL53L1_define_GeneralMacro_group */
|
||||
|
||||
/** @} VL53L1_globaldefine_group */
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* _VL53L1_DEF_H_ */
|
||||
253
App/VL53L1X_API/core/inc/vl53l1_error_codes.h
Normal file
253
App/VL53L1X_API/core/inc/vl53l1_error_codes.h
Normal file
@@ -0,0 +1,253 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_error_codes.h
|
||||
*
|
||||
* @brief Error Code definitions for VL53L1 API.
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_ERROR_CODES_H_
|
||||
#define _VL53L1_ERROR_CODES_H_
|
||||
|
||||
#include "vl53l1_types.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/****************************************
|
||||
* PRIVATE define do not edit
|
||||
****************************************/
|
||||
|
||||
/** @defgroup VL53L1_define_Error_group Error and Warning code returned by API
|
||||
* The following DEFINE are used to identify the PAL ERROR
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef int8_t VL53L1_Error;
|
||||
|
||||
#define VL53L1_ERROR_NONE ((VL53L1_Error) 0)
|
||||
#define VL53L1_ERROR_CALIBRATION_WARNING ((VL53L1_Error) - 1)
|
||||
/*!< Warning invalid calibration data may be in used
|
||||
\a VL53L1_InitData()
|
||||
\a VL53L1_GetOffsetCalibrationData
|
||||
\a VL53L1_SetOffsetCalibrationData */
|
||||
#define VL53L1_ERROR_MIN_CLIPPED ((VL53L1_Error) - 2)
|
||||
/*!< Warning parameter passed was clipped to min before to be applied */
|
||||
|
||||
#define VL53L1_ERROR_UNDEFINED ((VL53L1_Error) - 3)
|
||||
/*!< Unqualified error */
|
||||
#define VL53L1_ERROR_INVALID_PARAMS ((VL53L1_Error) - 4)
|
||||
/*!< Parameter passed is invalid or out of range */
|
||||
#define VL53L1_ERROR_NOT_SUPPORTED ((VL53L1_Error) - 5)
|
||||
/*!< Function is not supported in current mode or configuration */
|
||||
#define VL53L1_ERROR_RANGE_ERROR ((VL53L1_Error) - 6)
|
||||
/*!< Device report a ranging error interrupt status */
|
||||
#define VL53L1_ERROR_TIME_OUT ((VL53L1_Error) - 7)
|
||||
/*!< Aborted due to time out */
|
||||
#define VL53L1_ERROR_MODE_NOT_SUPPORTED ((VL53L1_Error) - 8)
|
||||
/*!< Asked mode is not supported by the device */
|
||||
#define VL53L1_ERROR_BUFFER_TOO_SMALL ((VL53L1_Error) - 9)
|
||||
/*!< ... */
|
||||
#define VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL ((VL53L1_Error) - 10)
|
||||
/*!< Supplied buffer is larger than I2C supports */
|
||||
#define VL53L1_ERROR_GPIO_NOT_EXISTING ((VL53L1_Error) - 11)
|
||||
/*!< User tried to setup a non-existing GPIO pin */
|
||||
#define VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L1_Error) - 12)
|
||||
/*!< unsupported GPIO functionality */
|
||||
#define VL53L1_ERROR_CONTROL_INTERFACE ((VL53L1_Error) - 13)
|
||||
/*!< error reported from IO functions */
|
||||
#define VL53L1_ERROR_INVALID_COMMAND ((VL53L1_Error) - 14)
|
||||
/*!< The command is not allowed in the current device state
|
||||
* (power down) */
|
||||
#define VL53L1_ERROR_DIVISION_BY_ZERO ((VL53L1_Error) - 15)
|
||||
/*!< In the function a division by zero occurs */
|
||||
#define VL53L1_ERROR_REF_SPAD_INIT ((VL53L1_Error) - 16)
|
||||
/*!< Error during reference SPAD initialization */
|
||||
#define VL53L1_ERROR_GPH_SYNC_CHECK_FAIL ((VL53L1_Error) - 17)
|
||||
/*!< GPH sync interrupt check fail - API out of sync with device*/
|
||||
#define VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL ((VL53L1_Error) - 18)
|
||||
/*!< Stream count check fail - API out of sync with device */
|
||||
#define VL53L1_ERROR_GPH_ID_CHECK_FAIL ((VL53L1_Error) - 19)
|
||||
/*!< GPH ID check fail - API out of sync with device */
|
||||
#define VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL ((VL53L1_Error) - 20)
|
||||
/*!< Zone dynamic config stream count check failed - API out of sync */
|
||||
#define VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL ((VL53L1_Error) - 21)
|
||||
/*!< Zone dynamic config GPH ID check failed - API out of sync */
|
||||
|
||||
#define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL ((VL53L1_Error) - 22)
|
||||
/*!< Thrown when run_xtalk_extraction fn has 0 succesful samples
|
||||
* when using the full array to sample the xtalk. In this case there is
|
||||
* not enough information to generate new Xtalk parm info. The function
|
||||
* will exit and leave the current xtalk parameters unaltered */
|
||||
#define VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL ((VL53L1_Error) - 23)
|
||||
/*!< Thrown when run_xtalk_extraction fn has found that the
|
||||
* avg sigma estimate of the full array xtalk sample is > than the
|
||||
* maximal limit allowed. In this case the xtalk sample is too noisy for
|
||||
* measurement. The function will exit and leave the current xtalk parameters
|
||||
* unaltered. */
|
||||
|
||||
|
||||
#define VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL ((VL53L1_Error) - 24)
|
||||
/*!< Thrown if there one of stages has no valid offset calibration
|
||||
* samples. A fatal error calibration not valid */
|
||||
#define VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL ((VL53L1_Error) - 25)
|
||||
/*!< Thrown if there one of stages has zero effective SPADS
|
||||
* Traps the case when MM1 SPADs is zero.
|
||||
* A fatal error calibration not valid */
|
||||
#define VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL ((VL53L1_Error) - 26)
|
||||
/*!< Thrown if then some of the zones have no valid samples
|
||||
* A fatal error calibration not valid */
|
||||
|
||||
#define VL53L1_ERROR_TUNING_PARM_KEY_MISMATCH ((VL53L1_Error) - 27)
|
||||
/*!< Thrown if the tuning file key table version does not match with
|
||||
* expected value. The driver expects the key table version to match
|
||||
* the compiled default version number in the define
|
||||
* #VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT
|
||||
* */
|
||||
|
||||
#define VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS ((VL53L1_Error) - 28)
|
||||
/*!< Thrown if there are less than 5 good SPADs are available. */
|
||||
#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH ((VL53L1_Error) - 29)
|
||||
/*!< Thrown if the final reference rate is greater than
|
||||
the upper reference rate limit - default is 40 Mcps.
|
||||
Implies a minimum Q3 (x10) SPAD (5) selected */
|
||||
#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW ((VL53L1_Error) - 30)
|
||||
/*!< Thrown if the final reference rate is less than
|
||||
the lower reference rate limit - default is 10 Mcps.
|
||||
Implies maximum Q1 (x1) SPADs selected */
|
||||
|
||||
|
||||
#define VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES ((VL53L1_Error) - 31)
|
||||
/*!< Thrown if there is less than the requested number of
|
||||
* valid samples. */
|
||||
#define VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH ((VL53L1_Error) - 32)
|
||||
/*!< Thrown if the offset calibration range sigma estimate is greater
|
||||
* than 8.0 mm. This is the recommended min value to yield a stable
|
||||
* offset measurement */
|
||||
#define VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH ((VL53L1_Error) - 33)
|
||||
/*!< Thrown when VL53L1_run_offset_calibration() peak rate is greater
|
||||
than that 50.0Mcps. This is the recommended max rate to avoid
|
||||
pile-up influencing the offset measurement */
|
||||
#define VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW ((VL53L1_Error) - 34)
|
||||
/*!< Thrown when VL53L1_run_offset_calibration() when one of stages
|
||||
range has less that 5.0 effective SPADS. This is the recommended
|
||||
min value to yield a stable offset */
|
||||
|
||||
|
||||
#define VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES ((VL53L1_Error) - 35)
|
||||
/*!< Thrown if one of more of the zones have less than
|
||||
the requested number of valid samples */
|
||||
#define VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH ((VL53L1_Error) - 36)
|
||||
/*!< Thrown if one or more zones have sigma estimate value greater
|
||||
* than 8.0 mm. This is the recommended min value to yield a stable
|
||||
* offset measurement */
|
||||
#define VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH ((VL53L1_Error) - 37)
|
||||
/*!< Thrown if one of more zones have peak rate higher than
|
||||
that 50.0Mcps. This is the recommended max rate to avoid
|
||||
pile-up influencing the offset measurement */
|
||||
|
||||
|
||||
#define VL53L1_WARNING_XTALK_MISSING_SAMPLES ((VL53L1_Error) - 38)
|
||||
/*!< Thrown to notify that some of the xtalk samples did not yield
|
||||
* valid ranging pulse data while attempting to measure
|
||||
* the xtalk signal in vl53l1_run_xtalk_extract(). This can signify any of
|
||||
* the zones are missing samples, for further debug information the
|
||||
* xtalk_results struct should be referred to. This warning is for
|
||||
* notification only, the xtalk pulse and shape have still been generated
|
||||
*/
|
||||
#define VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT ((VL53L1_Error) - 39)
|
||||
/*!< Thrown to notify that some of teh xtalk samples used for gradient
|
||||
* generation did not yield valid ranging pulse data while attempting to
|
||||
* measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify
|
||||
* that any one of the zones 0-3 yielded no successful samples. The
|
||||
* xtalk_results struct should be referred to for further debug info.
|
||||
* This warning is for notification only, the xtalk pulse and shape
|
||||
* have still been generated.
|
||||
*/
|
||||
#define VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT ((VL53L1_Error) - 40)
|
||||
/*!< Thrown to notify that some of the xtalk samples used for gradient
|
||||
* generation did not pass the sigma limit check while attempting to
|
||||
* measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify
|
||||
* that any one of the zones 0-3 yielded an avg sigma_mm value > the limit.
|
||||
* The xtalk_results struct should be referred to for further debug info.
|
||||
* This warning is for notification only, the xtalk pulse and shape
|
||||
* have still been generated.
|
||||
*/
|
||||
|
||||
#define VL53L1_ERROR_NOT_IMPLEMENTED ((VL53L1_Error) - 41)
|
||||
/*!< Tells requested functionality has not been implemented yet or
|
||||
* not compatible with the device */
|
||||
#define VL53L1_ERROR_PLATFORM_SPECIFIC_START ((VL53L1_Error) - 60)
|
||||
/*!< Tells the starting code for platform */
|
||||
/** @} VL53L1_define_Error_group */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* _VL53L1_ERROR_CODES_H_ */
|
||||
93
App/VL53L1X_API/core/inc/vl53l1_error_exceptions.h
Normal file
93
App/VL53L1X_API/core/inc/vl53l1_error_exceptions.h
Normal file
@@ -0,0 +1,93 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_error_exceptions.h
|
||||
*
|
||||
* @brief EwokPlus25 LL Driver definitions for control of error handling in LL driver
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_ERROR_EXCEPTIONS_H_
|
||||
#define _VL53L1_ERROR_EXCEPTIONS_H_
|
||||
|
||||
#define IGNORE_DIVISION_BY_ZERO 0
|
||||
|
||||
#define IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FAIL 0
|
||||
#define IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL 0
|
||||
#define IGNORE_XTALK_EXTRACTION_NO_SAMPLE_FOR_GRADIENT_WARN 0
|
||||
#define IGNORE_XTALK_EXTRACTION_SIGMA_LIMIT_FOR_GRADIENT_WARN 0
|
||||
#define IGNORE_XTALK_EXTRACTION_MISSING_SAMPLES_WARN 0
|
||||
|
||||
#define IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS 0
|
||||
#define IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH 0
|
||||
#define IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW 0
|
||||
|
||||
#define IGNORE_OFFSET_CAL_MISSING_SAMPLES 0
|
||||
#define IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH 0
|
||||
#define IGNORE_OFFSET_CAL_RATE_TOO_HIGH 0
|
||||
#define IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW 0
|
||||
|
||||
#define IGNORE_ZONE_CAL_MISSING_SAMPLES 0
|
||||
#define IGNORE_ZONE_CAL_SIGMA_TOO_HIGH 0
|
||||
#define IGNORE_ZONE_CAL_RATE_TOO_HIGH 0
|
||||
|
||||
#endif /* _VL53L1_ERROR_EXCEPTIONS_H_ */
|
||||
209
App/VL53L1X_API/core/inc/vl53l1_error_strings.h
Normal file
209
App/VL53L1X_API/core/inc/vl53l1_error_strings.h
Normal file
@@ -0,0 +1,209 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_error_strings.h
|
||||
* @brief VL53L1 function declarations for decoding error codes to a
|
||||
* text strings
|
||||
*/
|
||||
|
||||
|
||||
#ifndef VL53L1_ERROR_STRINGS_H_
|
||||
#define VL53L1_ERROR_STRINGS_H_
|
||||
|
||||
#include "vl53l1_error_codes.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates an error string for the input PAL error code
|
||||
*
|
||||
* @param[in] PalErrorCode : PAL Error Code
|
||||
* @param[out] pPalErrorString : pointer to character buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_get_pal_error_string(
|
||||
VL53L1_Error PalErrorCode,
|
||||
char *pPalErrorString);
|
||||
|
||||
|
||||
#ifndef VL53L1_USE_EMPTY_STRING
|
||||
|
||||
/* PAL ERROR strings */
|
||||
#define VL53L1_STRING_ERROR_NONE \
|
||||
"No Error"
|
||||
#define VL53L1_STRING_ERROR_CALIBRATION_WARNING \
|
||||
"Calibration Warning Error"
|
||||
#define VL53L1_STRING_ERROR_MIN_CLIPPED \
|
||||
"Min clipped error"
|
||||
#define VL53L1_STRING_ERROR_UNDEFINED \
|
||||
"Undefined error"
|
||||
#define VL53L1_STRING_ERROR_INVALID_PARAMS \
|
||||
"Invalid parameters error"
|
||||
#define VL53L1_STRING_ERROR_NOT_SUPPORTED \
|
||||
"Not supported error"
|
||||
#define VL53L1_STRING_ERROR_RANGE_ERROR \
|
||||
"Range error"
|
||||
#define VL53L1_STRING_ERROR_TIME_OUT \
|
||||
"Time out error"
|
||||
#define VL53L1_STRING_ERROR_MODE_NOT_SUPPORTED \
|
||||
"Mode not supported error"
|
||||
#define VL53L1_STRING_ERROR_BUFFER_TOO_SMALL \
|
||||
"Buffer too small"
|
||||
#define VL53L1_STRING_ERROR_COMMS_BUFFER_TOO_SMALL \
|
||||
"Comms Buffer too small"
|
||||
#define VL53L1_STRING_ERROR_GPIO_NOT_EXISTING \
|
||||
"GPIO not existing"
|
||||
#define VL53L1_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED \
|
||||
"GPIO funct not supported"
|
||||
#define VL53L1_STRING_ERROR_CONTROL_INTERFACE \
|
||||
"Control Interface Error"
|
||||
#define VL53L1_STRING_ERROR_INVALID_COMMAND \
|
||||
"Invalid Command Error"
|
||||
#define VL53L1_STRING_ERROR_DIVISION_BY_ZERO \
|
||||
"Division by zero Error"
|
||||
#define VL53L1_STRING_ERROR_REF_SPAD_INIT \
|
||||
"Reference Spad Init Error"
|
||||
#define VL53L1_STRING_ERROR_GPH_SYNC_CHECK_FAIL \
|
||||
"GPH Sync Check Fail - API out of sync"
|
||||
#define VL53L1_STRING_ERROR_STREAM_COUNT_CHECK_FAIL \
|
||||
"Stream Count Check Fail - API out of sync"
|
||||
#define VL53L1_STRING_ERROR_GPH_ID_CHECK_FAIL \
|
||||
"GPH ID Check Fail - API out of sync"
|
||||
#define VL53L1_STRING_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL \
|
||||
"Zone Stream Count Check Fail - API out of sync"
|
||||
#define VL53L1_STRING_ERROR_ZONE_GPH_ID_CHECK_FAIL \
|
||||
"Zone GPH ID Check Fail - API out of sync"
|
||||
|
||||
#define VL53L1_STRING_ERROR_XTALK_EXTRACTION_NO_SAMPLES_FAIL \
|
||||
"No Xtalk using full array - Xtalk Extract Fail"
|
||||
#define VL53L1_STRING_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL \
|
||||
"Xtalk does not meet required sigma limit - Xtalk Extract Fail"
|
||||
|
||||
#define VL53L1_STRING_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL \
|
||||
"Offset Cal - one of more stages with no valid samples - fatal"
|
||||
#define VL53L1_STRING_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL \
|
||||
"Offset Cal - one of more stages with no SPADS enables - fatal"
|
||||
#define VL53L1_STRING_ERROR_ZONE_CAL_NO_SAMPLE_FAIL \
|
||||
"Zone Cal - one of more zones with no valid samples - fatal"
|
||||
|
||||
#define VL53L1_STRING_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS \
|
||||
"Ref SPAD Char - Not Enough Good SPADs"
|
||||
#define VL53L1_STRING_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH \
|
||||
"Ref SPAD Char - Final Ref Rate too high"
|
||||
#define VL53L1_STRING_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW \
|
||||
"Ref SPAD Char - Final Ref Rate too low"
|
||||
|
||||
#define VL53L1_STRING_WARNING_OFFSET_CAL_MISSING_SAMPLES \
|
||||
"Offset Cal - Less than the requested number of valid samples"
|
||||
#define VL53L1_STRING_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH \
|
||||
"Offset Cal - Sigma estimate value too high - offset not stable"
|
||||
#define VL53L1_STRING_WARNING_OFFSET_CAL_RATE_TOO_HIGH \
|
||||
"Offset Cal - Rate too high - in pile up"
|
||||
#define VL53L1_STRING_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW \
|
||||
"Offset Cal - Insufficient SPADs - offset may not be stable"
|
||||
|
||||
#define VL53L1_STRING_WARNING_ZONE_CAL_MISSING_SAMPLES \
|
||||
"Zone Cal - One or more zone with less than requested valid samples"
|
||||
#define VL53L1_STRING_WARNING_ZONE_CAL_SIGMA_TOO_HIGH \
|
||||
"Zone Cal - One of more zones the sigma estimate too high"
|
||||
#define VL53L1_STRING_WARNING_ZONE_CAL_RATE_TOO_HIGH \
|
||||
"Zone Cal - One of more zones with rate too high - in pile up"
|
||||
|
||||
#define VL53L1_STRING_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT \
|
||||
"Xtalk - Gradient sample num = 0"
|
||||
#define VL53L1_STRING_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT \
|
||||
"Xtalk - Gradient Sigma > Limit"
|
||||
#define VL53L1_STRING_WARNING_XTALK_MISSING_SAMPLES \
|
||||
"Xtalk - Some missing and invalid samples"
|
||||
|
||||
#define VL53L1_STRING_ERROR_DEVICE_FIRMWARE_TOO_OLD \
|
||||
"Device Firmware too old"
|
||||
#define VL53L1_STRING_ERROR_DEVICE_FIRMWARE_TOO_NEW \
|
||||
"Device Firmware too new"
|
||||
#define VL53L1_STRING_ERROR_UNIT_TEST_FAIL \
|
||||
"Unit Test Fail"
|
||||
#define VL53L1_STRING_ERROR_FILE_READ_FAIL \
|
||||
"File Read Fail"
|
||||
#define VL53L1_STRING_ERROR_FILE_WRITE_FAIL \
|
||||
"File Write Fail"
|
||||
|
||||
#define VL53L1_STRING_ERROR_NOT_IMPLEMENTED \
|
||||
"Not implemented error"
|
||||
#define VL53L1_STRING_UNKNOW_ERROR_CODE \
|
||||
"Unknown Error Code"
|
||||
|
||||
#endif /* VL53L1_USE_EMPTY_STRING */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
1098
App/VL53L1X_API/core/inc/vl53l1_ll_def.h
Normal file
1098
App/VL53L1X_API/core/inc/vl53l1_ll_def.h
Normal file
File diff suppressed because it is too large
Load Diff
698
App/VL53L1X_API/core/inc/vl53l1_ll_device.h
Normal file
698
App/VL53L1X_API/core/inc/vl53l1_ll_device.h
Normal file
@@ -0,0 +1,698 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_ll_device.h
|
||||
*
|
||||
* @brief LL Driver Device specific defines. To be adapted by implementer for the
|
||||
* targeted device.
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_LL_DEVICE_H_
|
||||
#define _VL53L1_LL_DEVICE_H_
|
||||
|
||||
#include "vl53l1_types.h"
|
||||
#include "vl53l1_platform_user_config.h"
|
||||
|
||||
#define VL53L1_I2C 0x01
|
||||
#define VL53L1_SPI 0x00
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DevSpecDefines_group VL53L1 Device Specific Defines
|
||||
* @brief VL53L1 cut1.0 Device Specific Defines
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @defgroup VL53L1_define_WaitMethod_group Defines Wait method used
|
||||
* For example blocking versus non blocking
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_WaitMethod;
|
||||
|
||||
#define VL53L1_WAIT_METHOD_BLOCKING ((VL53L1_WaitMethod) 0)
|
||||
#define VL53L1_WAIT_METHOD_NON_BLOCKING ((VL53L1_WaitMethod) 1)
|
||||
|
||||
/** @} end of VL53L1_define_WaitMethod_group */
|
||||
|
||||
/** @defgroup VL53L1_define_DeviceState_group Device State
|
||||
*
|
||||
* @brief Defines all possible device states for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceState;
|
||||
|
||||
#define VL53L1_DEVICESTATE_POWERDOWN ((VL53L1_DeviceState) 0)
|
||||
#define VL53L1_DEVICESTATE_HW_STANDBY ((VL53L1_DeviceState) 1)
|
||||
#define VL53L1_DEVICESTATE_FW_COLDBOOT ((VL53L1_DeviceState) 2)
|
||||
#define VL53L1_DEVICESTATE_SW_STANDBY ((VL53L1_DeviceState) 3)
|
||||
#define VL53L1_DEVICESTATE_RANGING_DSS_AUTO ((VL53L1_DeviceState) 4)
|
||||
#define VL53L1_DEVICESTATE_RANGING_DSS_MANUAL ((VL53L1_DeviceState) 5)
|
||||
#define VL53L1_DEVICESTATE_RANGING_WAIT_GPH_SYNC ((VL53L1_DeviceState) 6)
|
||||
#define VL53L1_DEVICESTATE_RANGING_GATHER_DATA ((VL53L1_DeviceState) 7)
|
||||
#define VL53L1_DEVICESTATE_RANGING_OUTPUT_DATA ((VL53L1_DeviceState) 8)
|
||||
|
||||
#define VL53L1_DEVICESTATE_UNKNOWN ((VL53L1_DeviceState) 98)
|
||||
#define VL53L1_DEVICESTATE_ERROR ((VL53L1_DeviceState) 99)
|
||||
|
||||
/** @} end of VL53L1_define_DeviceState_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_DevicePresetModes_group Device Preset Modes
|
||||
*
|
||||
* @brief Defines all possible device preset modes. The integer values are
|
||||
* kept the same as main EwokPlus driver branch
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DevicePresetModes;
|
||||
|
||||
#define VL53L1_DEVICEPRESETMODE_NONE ((VL53L1_DevicePresetModes) 0)
|
||||
#define VL53L1_DEVICEPRESETMODE_STANDARD_RANGING ((VL53L1_DevicePresetModes) 1)
|
||||
#define VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_SHORT_RANGE ((VL53L1_DevicePresetModes) 2)
|
||||
#define VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_LONG_RANGE ((VL53L1_DevicePresetModes) 3)
|
||||
#define VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL ((VL53L1_DevicePresetModes) 4)
|
||||
#define VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL ((VL53L1_DevicePresetModes) 5)
|
||||
#define VL53L1_DEVICEPRESETMODE_TIMED_RANGING ((VL53L1_DevicePresetModes) 6)
|
||||
#define VL53L1_DEVICEPRESETMODE_TIMED_RANGING_SHORT_RANGE ((VL53L1_DevicePresetModes) 7)
|
||||
#define VL53L1_DEVICEPRESETMODE_TIMED_RANGING_LONG_RANGE ((VL53L1_DevicePresetModes) 8)
|
||||
#define VL53L1_DEVICEPRESETMODE_OLT ((VL53L1_DevicePresetModes) 17)
|
||||
#define VL53L1_DEVICEPRESETMODE_SINGLESHOT_RANGING ((VL53L1_DevicePresetModes) 18)
|
||||
#define VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_SHORT_RANGE ((VL53L1_DevicePresetModes) 36)
|
||||
#define VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_MEDIUM_RANGE ((VL53L1_DevicePresetModes) 37)
|
||||
#define VL53L1_DEVICEPRESETMODE_LOWPOWERAUTO_LONG_RANGE ((VL53L1_DevicePresetModes) 38)
|
||||
|
||||
/** @} end of VL53L1_define_DevicePresetModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_DeviceMeasurementModes_group Device Measurement Modes
|
||||
*
|
||||
* @brief Defines all possible measurement modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceMeasurementModes;
|
||||
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_STOP ((VL53L1_DeviceMeasurementModes) 0x00)
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_SINGLESHOT ((VL53L1_DeviceMeasurementModes) 0x10)
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK ((VL53L1_DeviceMeasurementModes) 0x20)
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_TIMED ((VL53L1_DeviceMeasurementModes) 0x40)
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_ABORT ((VL53L1_DeviceMeasurementModes) 0x80)
|
||||
|
||||
/** @} VL53L1_define_DeviceMeasurementModes_group */
|
||||
|
||||
/** @defgroup VL53L1_define_OffsetCalibrationModes_group Device Offset Calibration Mode
|
||||
*
|
||||
* @brief Defines possible offset calibration modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_OffsetCalibrationMode;
|
||||
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__NONE ((VL53L1_OffsetCalibrationMode) 0)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD ((VL53L1_OffsetCalibrationMode) 1)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM ((VL53L1_OffsetCalibrationMode) 2)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY ((VL53L1_OffsetCalibrationMode) 3)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__HISTOGRAM_PRE_RANGE_ONLY ((VL53L1_OffsetCalibrationMode) 4)
|
||||
#define VL53L1_OFFSETCALIBRATIONMODE__PER_ZONE ((VL53L1_OffsetCalibrationMode) 5)
|
||||
|
||||
/** @} VL53L1_define_OffsetCalibrationModes_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_define_OffsetCalibrationModes_group Device Offset Correction Mode
|
||||
*
|
||||
* @brief Defines all possible offset correction modes for the device
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_OffsetCorrectionMode;
|
||||
|
||||
#define VL53L1_OFFSETCORRECTIONMODE__NONE ((VL53L1_OffsetCorrectionMode) 0)
|
||||
#define VL53L1_OFFSETCORRECTIONMODE__MM1_MM2_OFFSETS ((VL53L1_OffsetCorrectionMode) 1)
|
||||
#define VL53L1_OFFSETCORRECTIONMODE__PER_ZONE_OFFSETS ((VL53L1_OffsetCorrectionMode) 2)
|
||||
|
||||
/** @} VL53L1_define_OffsetCalibrationModes_group */
|
||||
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceSequenceConfig_group Device Sequence Config
|
||||
*
|
||||
* @brief Individual bit enables for each stage in the ranging scheduler
|
||||
* The values below encode the bit shift for each bit
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceSequenceConfig;
|
||||
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_VHV ((VL53L1_DeviceSequenceConfig) 0)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_PHASECAL ((VL53L1_DeviceSequenceConfig) 1)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_REFERENCE_PHASE ((VL53L1_DeviceSequenceConfig) 2)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_DSS1 ((VL53L1_DeviceSequenceConfig) 3)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_DSS2 ((VL53L1_DeviceSequenceConfig) 4)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_MM1 ((VL53L1_DeviceSequenceConfig) 5)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_MM2 ((VL53L1_DeviceSequenceConfig) 6)
|
||||
#define VL53L1_DEVICESEQUENCECONFIG_RANGE ((VL53L1_DeviceSequenceConfig) 7)
|
||||
|
||||
/** @} VL53L1_DeviceSequenceConfig_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceInterruptPolarity_group Device Interrupt Polarity
|
||||
*
|
||||
* @brief Device Interrupt Polarity
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceInterruptPolarity;
|
||||
|
||||
#define VL53L1_DEVICEINTERRUPTPOLARITY_ACTIVE_HIGH ((VL53L1_DeviceInterruptPolarity) 0x00)
|
||||
#define VL53L1_DEVICEINTERRUPTPOLARITY_ACTIVE_LOW ((VL53L1_DeviceInterruptPolarity) 0x10)
|
||||
#define VL53L1_DEVICEINTERRUPTPOLARITY_BIT_MASK ((VL53L1_DeviceInterruptPolarity) 0x10)
|
||||
#define VL53L1_DEVICEINTERRUPTPOLARITY_CLEAR_MASK ((VL53L1_DeviceInterruptPolarity) 0xEF)
|
||||
|
||||
/** @} VL53L1_DeviceInterruptPolarity_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceGpioMode_group Device GPIO Mode
|
||||
*
|
||||
* @brief Device Gpio Mode
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceGpioMode;
|
||||
|
||||
#define VL53L1_DEVICEGPIOMODE_OUTPUT_CONSTANT_ZERO ((VL53L1_DeviceGpioMode) 0x00)
|
||||
#define VL53L1_DEVICEGPIOMODE_OUTPUT_RANGE_AND_ERROR_INTERRUPTS ((VL53L1_DeviceGpioMode) 0x01)
|
||||
#define VL53L1_DEVICEGPIOMODE_OUTPUT_TIMIER_INTERRUPTS ((VL53L1_DeviceGpioMode) 0x02)
|
||||
#define VL53L1_DEVICEGPIOMODE_OUTPUT_RANGE_MODE_INTERRUPT_STATUS ((VL53L1_DeviceGpioMode) 0x03)
|
||||
#define VL53L1_DEVICEGPIOMODE_OUTPUT_SLOW_OSCILLATOR_CLOCK ((VL53L1_DeviceGpioMode) 0x04)
|
||||
#define VL53L1_DEVICEGPIOMODE_BIT_MASK ((VL53L1_DeviceGpioMode) 0x0F)
|
||||
#define VL53L1_DEVICEGPIOMODE_CLEAR_MASK ((VL53L1_DeviceGpioMode) 0xF0)
|
||||
|
||||
/** @} VL53L1_DeviceGpioMode_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceError_group Device Error
|
||||
*
|
||||
* @brief Device Error code in the range status
|
||||
*
|
||||
* This enum is Device specific it should be updated in the implementation
|
||||
* Use @a VL53L1_GetStatusErrorString() to get the string.
|
||||
* It is related to Status Register of the Device.
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceError;
|
||||
|
||||
#define VL53L1_DEVICEERROR_NOUPDATE ((VL53L1_DeviceError) 0)
|
||||
/*!< 0 No Update */
|
||||
#define VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE ((VL53L1_DeviceError) 1)
|
||||
#define VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE ((VL53L1_DeviceError) 2)
|
||||
#define VL53L1_DEVICEERROR_NOVHVVALUEFOUND ((VL53L1_DeviceError) 3)
|
||||
#define VL53L1_DEVICEERROR_MSRCNOTARGET ((VL53L1_DeviceError) 4)
|
||||
#define VL53L1_DEVICEERROR_RANGEPHASECHECK ((VL53L1_DeviceError) 5)
|
||||
#define VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK ((VL53L1_DeviceError) 6)
|
||||
#define VL53L1_DEVICEERROR_PHASECONSISTENCY ((VL53L1_DeviceError) 7)
|
||||
#define VL53L1_DEVICEERROR_MINCLIP ((VL53L1_DeviceError) 8)
|
||||
#define VL53L1_DEVICEERROR_RANGECOMPLETE ((VL53L1_DeviceError) 9)
|
||||
#define VL53L1_DEVICEERROR_ALGOUNDERFLOW ((VL53L1_DeviceError) 10)
|
||||
#define VL53L1_DEVICEERROR_ALGOOVERFLOW ((VL53L1_DeviceError) 11)
|
||||
#define VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD ((VL53L1_DeviceError) 12)
|
||||
#define VL53L1_DEVICEERROR_USERROICLIP ((VL53L1_DeviceError) 13)
|
||||
#define VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS ((VL53L1_DeviceError) 14)
|
||||
#define VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET ((VL53L1_DeviceError) 15)
|
||||
#define VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET ((VL53L1_DeviceError) 16)
|
||||
#define VL53L1_DEVICEERROR_MULTCLIPFAIL ((VL53L1_DeviceError) 17)
|
||||
#define VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY ((VL53L1_DeviceError) 18)
|
||||
#define VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ((VL53L1_DeviceError) 19)
|
||||
#define VL53L1_DEVICEERROR_EVENTCONSISTENCY ((VL53L1_DeviceError) 20)
|
||||
#define VL53L1_DEVICEERROR_MINSIGNALEVENTCHECK ((VL53L1_DeviceError) 21)
|
||||
#define VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE ((VL53L1_DeviceError) 22)
|
||||
|
||||
/* Patch_NewDeviceErrorCodePrevRangeNoTargets_11786 */
|
||||
#define VL53L1_DEVICEERROR_PREV_RANGE_NO_TARGETS ((VL53L1_DeviceError) 23)
|
||||
|
||||
/** @} end of VL53L1_DeviceError_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceReportStatus_group Device Report Status
|
||||
* @brief Device Report Status code
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceReportStatus;
|
||||
|
||||
#define VL53L1_DEVICEREPORTSTATUS_NOUPDATE ((VL53L1_DeviceReportStatus) 0)
|
||||
/*!< 0 No Update */
|
||||
#define VL53L1_DEVICEREPORTSTATUS_ROI_SETUP ((VL53L1_DeviceReportStatus) 1)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_VHV ((VL53L1_DeviceReportStatus) 2)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_PHASECAL ((VL53L1_DeviceReportStatus) 3)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_REFERENCE_PHASE ((VL53L1_DeviceReportStatus) 4)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_DSS1 ((VL53L1_DeviceReportStatus) 5)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_DSS2 ((VL53L1_DeviceReportStatus) 6)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_MM1 ((VL53L1_DeviceReportStatus) 7)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_MM2 ((VL53L1_DeviceReportStatus) 8)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_RANGE ((VL53L1_DeviceReportStatus) 9)
|
||||
#define VL53L1_DEVICEREPORTSTATUS_HISTOGRAM ((VL53L1_DeviceReportStatus) 10)
|
||||
|
||||
/** @} end of VL53L1_DeviceReportStatus_group */
|
||||
|
||||
/** @defgroup VL53L1_DeviceDssMode_group Dynamic SPAD Selection Mode
|
||||
* @brief Selects the device Dynamic SPAD Selection Mode
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L1_DeviceDssMode;
|
||||
|
||||
#define VL53L1_DEVICEDSSMODE__DISABLED \
|
||||
((VL53L1_DeviceDssMode) 0)
|
||||
#define VL53L1_DEVICEDSSMODE__TARGET_RATE \
|
||||
((VL53L1_DeviceDssMode) 1)
|
||||
#define VL53L1_DEVICEDSSMODE__REQUESTED_EFFFECTIVE_SPADS \
|
||||
((VL53L1_DeviceDssMode) 2)
|
||||
#define VL53L1_DEVICEDSSMODE__BLOCK_SELECT \
|
||||
((VL53L1_DeviceDssMode) 3)
|
||||
|
||||
/** @} end of VL53L1_DeviceDssMode_group */
|
||||
|
||||
/** @defgroup VL53L1_DeviceConfigLevel_group Device Config Level
|
||||
*
|
||||
* @brief Defines the contents of the config & start range I2C multi byte transaction
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceConfigLevel;
|
||||
|
||||
#define VL53L1_DEVICECONFIGLEVEL_SYSTEM_CONTROL \
|
||||
((VL53L1_DeviceConfigLevel) 0)
|
||||
/*!< Configs system control & start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_DYNAMIC_ONWARDS \
|
||||
((VL53L1_DeviceConfigLevel) 1)
|
||||
/*!< Dynamic config onwards (dynamic_config, system_control) & start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_TIMING_ONWARDS \
|
||||
((VL53L1_DeviceConfigLevel) 2)
|
||||
/*!< Dynamic config onwards (timing config, dynamic_config, system_control) &
|
||||
start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_GENERAL_ONWARDS \
|
||||
((VL53L1_DeviceConfigLevel) 3)
|
||||
/*!< General config onwards (general_config, timing config, dynamic_config,
|
||||
system_control) & start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_STATIC_ONWARDS \
|
||||
((VL53L1_DeviceConfigLevel) 4)
|
||||
/*!< Static config onwards (static_config, general_config, timing_config,
|
||||
dynamic_config, system_control) & start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS \
|
||||
((VL53L1_DeviceConfigLevel) 5)
|
||||
/*!< Full device config (customer_nvm_managed, static_config, general_config,
|
||||
timing config, dynamic_config, system_control) & start range */
|
||||
#define VL53L1_DEVICECONFIGLEVEL_FULL \
|
||||
((VL53L1_DeviceConfigLevel) 6)
|
||||
/*!< Full device config (static_nvm_managed, customer_nvm_managed, static_config,
|
||||
general_config, timing config, dynamic_config, system_control) & start range */
|
||||
|
||||
/** @} end of VL53L1_DeviceConfigLevel_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceResultsLevel_group Device Results Level
|
||||
*
|
||||
* @brief Defines the contents of the read results I2C multi byte transaction
|
||||
* @{
|
||||
*/
|
||||
typedef uint8_t VL53L1_DeviceResultsLevel;
|
||||
|
||||
#define VL53L1_DEVICERESULTSLEVEL_SYSTEM_RESULTS \
|
||||
((VL53L1_DeviceResultsLevel) 0)
|
||||
/*!< Read just system_results */
|
||||
#define VL53L1_DEVICERESULTSLEVEL_UPTO_CORE \
|
||||
((VL53L1_DeviceResultsLevel) 1)
|
||||
/*!< Read both system and core results */
|
||||
#define VL53L1_DEVICERESULTSLEVEL_FULL \
|
||||
((VL53L1_DeviceResultsLevel) 2)
|
||||
/*!< Read system, core and debug results */
|
||||
|
||||
/** @} end of VL53L1_DeviceResultsLevel_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceTestMode_group Device Test Mode
|
||||
*
|
||||
* @brief Values below match the the TEST_MODE__CTRL register
|
||||
* do not change
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L1_DeviceTestMode;
|
||||
|
||||
#define VL53L1_DEVICETESTMODE_NONE \
|
||||
((VL53L1_DeviceTestMode) 0x00)
|
||||
/*!< Idle */
|
||||
#define VL53L1_DEVICETESTMODE_NVM_ZERO \
|
||||
((VL53L1_DeviceTestMode) 0x01)
|
||||
/*!< NVM zero */
|
||||
#define VL53L1_DEVICETESTMODE_NVM_COPY \
|
||||
((VL53L1_DeviceTestMode) 0x02)
|
||||
/*!< NVM copy */
|
||||
#define VL53L1_DEVICETESTMODE_PATCH \
|
||||
((VL53L1_DeviceTestMode) 0x03)
|
||||
/*!< Patch */
|
||||
#define VL53L1_DEVICETESTMODE_DCR \
|
||||
((VL53L1_DeviceTestMode) 0x04)
|
||||
/*!< DCR - SPAD Self-Check (Pass if Count Rate is less than Threshold) */
|
||||
#define VL53L1_DEVICETESTMODE_LCR_VCSEL_OFF \
|
||||
((VL53L1_DeviceTestMode) 0x05)
|
||||
/*!< LCR - SPAD Self-Check (Pass if Count Rate is greater than Threshold
|
||||
and VCSEL off) */
|
||||
#define VL53L1_DEVICETESTMODE_LCR_VCSEL_ON \
|
||||
((VL53L1_DeviceTestMode) 0x06)
|
||||
/*!< LCR - SPAD Self-Check (Pass if Count Rate is greater than Threshold
|
||||
and VCSEL on) */
|
||||
#define VL53L1_DEVICETESTMODE_SPOT_CENTRE_LOCATE \
|
||||
((VL53L1_DeviceTestMode) 0x07)
|
||||
/*!< Spot centre locate */
|
||||
#define VL53L1_DEVICETESTMODE_REF_SPAD_CHAR_WITH_PRE_VHV \
|
||||
((VL53L1_DeviceTestMode) 0x08)
|
||||
/*!<Reference SPAD Characterisation with pre-VHV */
|
||||
#define VL53L1_DEVICETESTMODE_REF_SPAD_CHAR_ONLY \
|
||||
((VL53L1_DeviceTestMode) 0x09)
|
||||
/*!< Reference SPAD Characterisation Only */
|
||||
|
||||
/** @} end of VL53L1_DeviceTestMode_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_DeviceSscArray_group Device Test Mode
|
||||
*
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L1_DeviceSscArray;
|
||||
|
||||
#define VL53L1_DEVICESSCARRAY_RTN ((VL53L1_DeviceSscArray) 0x00)
|
||||
/*!<Return Array Rates */
|
||||
#define VL53L1_DEVICETESTMODE_REF ((VL53L1_DeviceSscArray) 0x01)
|
||||
/*!< Reference Array Rates Only */
|
||||
|
||||
/** @} end of VL53L1_DeviceSscArray_group */
|
||||
|
||||
|
||||
/** @defgroup VL53L1_SpadArraySelection_group SPAD Array Selection Functionality
|
||||
* @brief SPAD array selection definitions
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_RETURN_ARRAY_ONLY 0x01
|
||||
/*!< Return SPAD Array only */
|
||||
#define VL53L1_REFERENCE_ARRAY_ONLY 0x10
|
||||
/*!< Reference SPAD Array only */
|
||||
#define VL53L1_BOTH_RETURN_AND_REFERENCE_ARRAYS 0x11
|
||||
/*!< Both Return and Reference SPAD Arrays */
|
||||
#define VL53L1_NEITHER_RETURN_AND_REFERENCE_ARRAYS 0x00
|
||||
/*!< Neither Return or Reference SPAD Array */
|
||||
|
||||
/** @} end of VL53L1_SpadArraySelection_group */
|
||||
|
||||
/** @defgroup VL53L1_DeviceInterruptLevel_group Interrupt Level Functionality
|
||||
* @brief Interrupt Output Level types
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_HIGH 0x00
|
||||
/*!< Active High Interrupt */
|
||||
#define VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_LOW 0x10
|
||||
/*!< Active Low Interrupt */
|
||||
#define VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_MASK 0x10
|
||||
/*!< Active Bit Mask */
|
||||
|
||||
/** @} end of VL53L1_DeviceInterruptLevel_group */
|
||||
|
||||
/** @defgroup VL53L1_ApiCore_group Misc Functionality
|
||||
* @brief API core specific definitions
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_POLLING_DELAY_US 1000
|
||||
/*!< 1000us delay for register polling */
|
||||
#define VL53L1_SOFTWARE_RESET_DURATION_US 100
|
||||
/*!< 100us software reset duration */
|
||||
#define VL53L1_FIRMWARE_BOOT_TIME_US 1200
|
||||
/*!< Duration of firmware boot time for which I2C
|
||||
access is blocked. Real Device 1ms, FPGA 15ms */
|
||||
#define VL53L1_ENABLE_POWERFORCE_SETTLING_TIME_US 250
|
||||
/*!< After enabling power force a delay is required
|
||||
to bring regulator, bandgap, oscillator time
|
||||
to power up and settle */
|
||||
#define VL53L1_SPAD_ARRAY_WIDTH 16
|
||||
/*!< SPAD array width */
|
||||
#define VL53L1_SPAD_ARRAY_HEIGHT 16
|
||||
/*!< SPAD array height */
|
||||
#define VL53L1_NVM_SIZE_IN_BYTES 512
|
||||
/*!< NVM (OTP) size in bytes */
|
||||
#define VL53L1_NO_OF_SPAD_ENABLES 256
|
||||
/*!< Number of SPADs each SPAD array */
|
||||
#define VL53L1_RTN_SPAD_BUFFER_SIZE 32
|
||||
/*!< Number of Return SPAD enable registers (bytes) */
|
||||
#define VL53L1_REF_SPAD_BUFFER_SIZE 6
|
||||
/*!< Number of Reference SPAD enable registers (bytes) */
|
||||
#define VL53L1_AMBIENT_WINDOW_VCSEL_PERIODS 256
|
||||
/*!< Sigma Delta Ambient window in VCSEL Periods */
|
||||
#define VL53L1_RANGING_WINDOW_VCSEL_PERIODS 2048
|
||||
/*!< Sigma Delta Ranging window in VCSEL periods */
|
||||
#define VL53L1_MACRO_PERIOD_VCSEL_PERIODS \
|
||||
(VL53L1_AMBIENT_WINDOW_VCSEL_PERIODS + VL53L1_RANGING_WINDOW_VCSEL_PERIODS)
|
||||
/*!< Macro Period in VCSEL periods */
|
||||
#define VL53L1_MAX_ALLOWED_PHASE 0xFFFF
|
||||
/*!< Maximum Allowed phase 0xFFFF means 31.999 PLL Clocks */
|
||||
|
||||
#define VL53L1_RTN_SPAD_UNITY_TRANSMISSION 0x0100
|
||||
/*!< SPAD unity transmission value - 1.0 in 8.8 format */
|
||||
#define VL53L1_RTN_SPAD_APERTURE_TRANSMISSION 0x0038
|
||||
/*!< Apertured SPAD transmission value - 8.8 format
|
||||
Nominal: 5x -> 0.200000 * 256 = 51 = 0x33
|
||||
Measured: 4.6x -> 0.217391 * 256 = 56 = 0x38 */
|
||||
|
||||
#define VL53L1_SPAD_TOTAL_COUNT_MAX ((0x01 << 29) - 1)
|
||||
/*!< Maximum SPAD count - 512Mcps * 1sec = 29bits) */
|
||||
#define VL53L1_SPAD_TOTAL_COUNT_RES_THRES (0x01 << 24)
|
||||
/*!< SPAD count threshold for reduced 3-bit fractional resolution */
|
||||
#define VL53L1_COUNT_RATE_INTERNAL_MAX ((0x01 << 24) - 1)
|
||||
/*!< Maximum internal count rate is a 17.7 (24-b) value */
|
||||
#define VL53L1_SPEED_OF_LIGHT_IN_AIR 299704
|
||||
/*!< Speed of light in air in mm/sec */
|
||||
#define VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8 (299704 >> 3)
|
||||
/*!< Speed of light in air in divided by 8, 2 for round trip
|
||||
and 4 as an additional scaling factor */
|
||||
|
||||
/** @} end of VL53L1_ApiCore_group */
|
||||
|
||||
/** @} end of VL53L1_DevSpecDefines_group */
|
||||
|
||||
/** @defgroup VL53L1_GPIO_Interrupt_Mode_group Interrupt modes
|
||||
* @brief Selects between four interrupt modes
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint8_t VL53L1_GPIO_Interrupt_Mode;
|
||||
|
||||
#define VL53L1_GPIOINTMODE_LEVEL_LOW \
|
||||
((VL53L1_GPIO_Interrupt_Mode) 0)
|
||||
/*!< Trigger interupt if value < thresh_low */
|
||||
#define VL53L1_GPIOINTMODE_LEVEL_HIGH \
|
||||
((VL53L1_GPIO_Interrupt_Mode) 1)
|
||||
/*!< Trigger interupt if value > thresh_high */
|
||||
#define VL53L1_GPIOINTMODE_OUT_OF_WINDOW \
|
||||
((VL53L1_GPIO_Interrupt_Mode) 2)
|
||||
/*!< Trigger interupt if value < thresh_low OR value > thresh_high */
|
||||
#define VL53L1_GPIOINTMODE_IN_WINDOW \
|
||||
((VL53L1_GPIO_Interrupt_Mode) 3)
|
||||
/*!< Trigger interupt if value > thresh_low AND value < thresh_high */
|
||||
|
||||
/** @} end of VL53L1_GPIO_Interrupt_Mode_group */
|
||||
|
||||
/** @defgroup VL53L1_TuningParms_group Tuning Parameters
|
||||
* @brief Selects specific tuning parameter inputs to get/set \
|
||||
* Added as part of Patch_AddedTuningParms_11761
|
||||
* @{
|
||||
*/
|
||||
|
||||
typedef uint16_t VL53L1_TuningParms;
|
||||
|
||||
#define VL53L1_TUNINGPARMS_LLD_PUBLIC_MIN_ADDRESS \
|
||||
((VL53L1_TuningParms) VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS)
|
||||
#define VL53L1_TUNINGPARMS_LLD_PUBLIC_MAX_ADDRESS \
|
||||
((VL53L1_TuningParms) VL53L1_TUNINGPARM_LOWPOWERAUTO_RANGE_CONFIG_TIMEOUT_US)
|
||||
|
||||
#define VL53L1_TUNINGPARMS_LLD_PRIVATE_MIN_ADDRESS \
|
||||
((VL53L1_TuningParms) VL53L1_TUNINGPARM_PRIVATE_PAGE_BASE_ADDRESS)
|
||||
#define VL53L1_TUNINGPARMS_LLD_PRIVATE_MAX_ADDRESS \
|
||||
((VL53L1_TuningParms) VL53L1_TUNINGPARMS_LLD_PRIVATE_MIN_ADDRESS)
|
||||
|
||||
#define VL53L1_TUNINGPARM_VERSION \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 0))
|
||||
#define VL53L1_TUNINGPARM_KEY_TABLE_VERSION \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 1))
|
||||
#define VL53L1_TUNINGPARM_LLD_VERSION \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 2))
|
||||
#define VL53L1_TUNINGPARM_CONSISTENCY_LITE_PHASE_TOLERANCE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 3))
|
||||
#define VL53L1_TUNINGPARM_PHASECAL_TARGET \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 4))
|
||||
#define VL53L1_TUNINGPARM_LITE_CAL_REPEAT_RATE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 5))
|
||||
#define VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 6))
|
||||
#define VL53L1_TUNINGPARM_LITE_MIN_CLIP_MM \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 7))
|
||||
#define VL53L1_TUNINGPARM_LITE_LONG_SIGMA_THRESH_MM \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 8))
|
||||
#define VL53L1_TUNINGPARM_LITE_MED_SIGMA_THRESH_MM \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 9))
|
||||
#define VL53L1_TUNINGPARM_LITE_SHORT_SIGMA_THRESH_MM \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 10))
|
||||
#define VL53L1_TUNINGPARM_LITE_LONG_MIN_COUNT_RATE_RTN_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 11))
|
||||
#define VL53L1_TUNINGPARM_LITE_MED_MIN_COUNT_RATE_RTN_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 12))
|
||||
#define VL53L1_TUNINGPARM_LITE_SHORT_MIN_COUNT_RATE_RTN_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 13))
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_EST_PULSE_WIDTH \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 14))
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_EST_AMB_WIDTH_NS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 15))
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_REF_MM \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 16))
|
||||
#define VL53L1_TUNINGPARM_LITE_RIT_MULT \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 17))
|
||||
#define VL53L1_TUNINGPARM_LITE_SEED_CONFIG \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 18))
|
||||
#define VL53L1_TUNINGPARM_LITE_QUANTIFIER \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 19))
|
||||
#define VL53L1_TUNINGPARM_LITE_FIRST_ORDER_SELECT \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 20))
|
||||
#define VL53L1_TUNINGPARM_LITE_XTALK_MARGIN_KCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 21))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_LONG_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 22))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_MED_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 23))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_SHORT_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 24))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_LONG_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 25))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_MED_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 26))
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_SHORT_RANGE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 27))
|
||||
#define VL53L1_TUNINGPARM_TIMED_SEED_CONFIG \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 28))
|
||||
#define VL53L1_TUNINGPARM_VHV_LOOPBOUND \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 29))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_DEVICE_TEST_MODE \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 30))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_VCSEL_PERIOD \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 31))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_PHASECAL_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 32))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_TARGET_COUNT_RATE_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 33))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_MIN_COUNTRATE_LIMIT_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 34))
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_MAX_COUNTRATE_LIMIT_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 35))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_DSS_RATE_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 36))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_PHASECAL_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 37))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 38))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_RANGE_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 39))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_PRE_SAMPLES \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 40))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM1_SAMPLES \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 41))
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM2_SAMPLES \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 42))
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_VCSEL_PERIOD \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 43))
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_VCSEL_START \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 44))
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_RATE_LIMIT_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 45))
|
||||
#define VL53L1_TUNINGPARM_LITE_DSS_CONFIG_TARGET_TOTAL_RATE_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 46))
|
||||
#define VL53L1_TUNINGPARM_TIMED_DSS_CONFIG_TARGET_TOTAL_RATE_MCPS \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 47))
|
||||
#define VL53L1_TUNINGPARM_LITE_PHASECAL_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 48))
|
||||
#define VL53L1_TUNINGPARM_TIMED_PHASECAL_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 49))
|
||||
#define VL53L1_TUNINGPARM_LITE_MM_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 50))
|
||||
#define VL53L1_TUNINGPARM_TIMED_MM_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 51))
|
||||
#define VL53L1_TUNINGPARM_LITE_RANGE_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 52))
|
||||
#define VL53L1_TUNINGPARM_TIMED_RANGE_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 53))
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 54))
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 55))
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_RANGE_CONFIG_TIMEOUT_US \
|
||||
((VL53L1_TuningParms) (VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS + 56))
|
||||
|
||||
|
||||
/** @} end of VL53L1_TuningParms_group */
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/* _VL53L1_DEVICE_H_ */
|
||||
|
||||
|
||||
3022
App/VL53L1X_API/core/inc/vl53l1_nvm_map.h
Normal file
3022
App/VL53L1X_API/core/inc/vl53l1_nvm_map.h
Normal file
File diff suppressed because it is too large
Load Diff
72
App/VL53L1X_API/core/inc/vl53l1_preset_setup.h
Normal file
72
App/VL53L1X_API/core/inc/vl53l1_preset_setup.h
Normal file
@@ -0,0 +1,72 @@
|
||||
|
||||
/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
|
||||
/******************************************************************************
|
||||
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
|
||||
|
||||
This file is part of VL53L1 and is dual licensed,
|
||||
either GPL-2.0+
|
||||
or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_PRESET_SETUP_H_
|
||||
#define _VL53L1_PRESET_SETUP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/* indexes for the bare driver tuning setting API function */
|
||||
enum VL53L1_Tuning_t {
|
||||
VL53L1_TUNING_VERSION = 0,
|
||||
VL53L1_TUNING_PROXY_MIN,
|
||||
VL53L1_TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM,
|
||||
VL53L1_TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER,
|
||||
VL53L1_TUNING_MIN_AMBIENT_DMAX_VALID,
|
||||
VL53L1_TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER,
|
||||
VL53L1_TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM,
|
||||
VL53L1_TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT,
|
||||
VL53L1_TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN,
|
||||
VL53L1_TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET,
|
||||
VL53L1_TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR,
|
||||
VL53L1_TUNING_PHASECAL_PATCH_POWER,
|
||||
VL53L1_TUNING_MAX_TUNABLE_KEY
|
||||
};
|
||||
|
||||
/* default values for the tuning settings parameters */
|
||||
#define TUNING_VERSION 0x0007
|
||||
|
||||
#define TUNING_PROXY_MIN -30 /* min distance in mm */
|
||||
#define TUNING_SINGLE_TARGET_XTALK_TARGET_DISTANCE_MM 600
|
||||
/* Target distance in mm for single target Xtalk */
|
||||
#define TUNING_SINGLE_TARGET_XTALK_SAMPLE_NUMBER 50
|
||||
/* Number of sample used for single target Xtalk */
|
||||
#define TUNING_MIN_AMBIENT_DMAX_VALID 8
|
||||
/* Minimum ambient level to state the Dmax returned by the device is valid */
|
||||
#define TUNING_MAX_SIMPLE_OFFSET_CALIBRATION_SAMPLE_NUMBER 50
|
||||
/* Maximum loops to perform simple offset calibration */
|
||||
#define TUNING_XTALK_FULL_ROI_TARGET_DISTANCE_MM 600
|
||||
/* Target distance in mm for target Xtalk from Bins method*/
|
||||
#define TUNING_SIMPLE_OFFSET_CALIBRATION_REPEAT 1
|
||||
/* Number of loops done during the simple offset calibration*/
|
||||
#define TUNING_ZERO_DISTANCE_OFFSET_NON_LINEAR_FACTOR_DEFAULT 9
|
||||
/* zero distance offset calibration non linear compensation default value */
|
||||
|
||||
/* The following settings are related to the fix for ticket EwokP #558410 */
|
||||
#define TUNING_XTALK_FULL_ROI_BIN_SUM_MARGIN 24
|
||||
/* Acceptance margin for the xtalk_shape bin_data sum computation */
|
||||
#define TUNING_XTALK_FULL_ROI_DEFAULT_OFFSET 50
|
||||
/* Recovery value for Xtalk compensation plane offset in kcps */
|
||||
/* 50 stands for ~0.10 kcps cover glass in 7.9 format */
|
||||
/* End of settings related to the fix for ticket EwokP #558410 */
|
||||
#define TUNING_PHASECAL_PATCH_POWER 0
|
||||
/* PhaseCal duration patch tuning param
|
||||
* 0 default duration 15ms, 1 leads to 60ms, 2 for 240ms and 3 for 3580ms
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_PRESET_SETUP_H_ */
|
||||
1635
App/VL53L1X_API/core/inc/vl53l1_register_funcs.h
Normal file
1635
App/VL53L1X_API/core/inc/vl53l1_register_funcs.h
Normal file
File diff suppressed because it is too large
Load Diff
11895
App/VL53L1X_API/core/inc/vl53l1_register_map.h
Normal file
11895
App/VL53L1X_API/core/inc/vl53l1_register_map.h
Normal file
File diff suppressed because it is too large
Load Diff
222
App/VL53L1X_API/core/inc/vl53l1_register_settings.h
Normal file
222
App/VL53L1X_API/core/inc/vl53l1_register_settings.h
Normal file
@@ -0,0 +1,222 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_register_settings.h
|
||||
*
|
||||
* @brief Device register setting defines.
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_REGISTER_SETTINGS_H_
|
||||
#define _VL53L1_REGISTER_SETTINGS_H_
|
||||
|
||||
|
||||
/** @defgroup VL53L1_RegisterSettings_group Functionality
|
||||
* @brief Defines the register settings for key device
|
||||
* configuration registers
|
||||
* @{
|
||||
*/
|
||||
|
||||
/** @defgroup VL53L1_DeviceSchedulerMode_group - Pseudo, Streaming & Hist
|
||||
* @brief Values below match the bit positions in the SYSTEM__MODE_START
|
||||
* register do not change
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_DEVICESCHEDULERMODE_PSEUDO_SOLO 0x00
|
||||
#define VL53L1_DEVICESCHEDULERMODE_STREAMING 0x01
|
||||
#define VL53L1_DEVICESCHEDULERMODE_HISTOGRAM 0x02
|
||||
|
||||
/** @} end of VL53L1_DeviceReadoutMode_group */
|
||||
|
||||
/** @defgroup VL53L1_DeviceReadoutMode_group - Single, Dual, Split & Manual
|
||||
* @brief Values below match the bit positions in the SYSTEM__MODE_START
|
||||
* register do not change
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_DEVICEREADOUTMODE_SINGLE_SD (0x00 << 2)
|
||||
#define VL53L1_DEVICEREADOUTMODE_DUAL_SD (0x01 << 2)
|
||||
#define VL53L1_DEVICEREADOUTMODE_SPLIT_READOUT (0x02 << 2)
|
||||
#define VL53L1_DEVICEREADOUTMODE_SPLIT_MANUAL (0x03 << 2)
|
||||
|
||||
/** @} end of VL53L1_DeviceReadoutMode_group */
|
||||
|
||||
/** @defgroup VL53L1_DeviceMeasurementMode_group - SingleShot, BackToBack & timed
|
||||
* @brief Values below match the bit positions in the SYSTEM__MODE_START
|
||||
* register do not change
|
||||
* @{
|
||||
*/
|
||||
|
||||
/*
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_STOP 0x00
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_SINGLESHOT 0x10
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK 0x20
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_TIMED 0x40
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_ABORT 0x80
|
||||
*/
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK 0xF0
|
||||
#define VL53L1_DEVICEMEASUREMENTMODE_STOP_MASK 0x0F
|
||||
|
||||
#define VL53L1_GROUPEDPARAMETERHOLD_ID_MASK 0x02
|
||||
|
||||
/** @} end of VL53L1_DeviceMeasurementMode_group */
|
||||
|
||||
#define VL53L1_EWOK_I2C_DEV_ADDR_DEFAULT 0x29
|
||||
/*!< Device default 7-bit I2C address */
|
||||
#define VL53L1_OSC_FREQUENCY 0x00
|
||||
#define VL53L1_OSC_TRIM_DEFAULT 0x00
|
||||
#define VL53L1_OSC_FREQ_SET_DEFAULT 0x00
|
||||
|
||||
#define VL53L1_RANGE_HISTOGRAM_REF 0x08
|
||||
#define VL53L1_RANGE_HISTOGRAM_RET 0x10
|
||||
#define VL53L1_RANGE_HISTOGRAM_BOTH 0x18
|
||||
#define VL53L1_RANGE_HISTOGRAM_INIT 0x20
|
||||
#define VL53L1_RANGE_VHV_INIT 0x40
|
||||
|
||||
/* Result Status */
|
||||
#define VL53L1_RESULT_RANGE_STATUS 0x1F
|
||||
|
||||
/* */
|
||||
#define VL53L1_SYSTEM__SEED_CONFIG__MANUAL 0x00
|
||||
#define VL53L1_SYSTEM__SEED_CONFIG__STANDARD 0x01
|
||||
#define VL53L1_SYSTEM__SEED_CONFIG__EVEN_UPDATE_ONLY 0x02
|
||||
|
||||
/* Interrupt Config */
|
||||
#define VL53L1_INTERRUPT_CONFIG_LEVEL_LOW 0x00
|
||||
#define VL53L1_INTERRUPT_CONFIG_LEVEL_HIGH 0x01
|
||||
#define VL53L1_INTERRUPT_CONFIG_OUT_OF_WINDOW 0x02
|
||||
#define VL53L1_INTERRUPT_CONFIG_IN_WINDOW 0x03
|
||||
#define VL53L1_INTERRUPT_CONFIG_NEW_SAMPLE_READY 0x20
|
||||
|
||||
/* Interrupt Clear */
|
||||
#define VL53L1_CLEAR_RANGE_INT 0x01
|
||||
#define VL53L1_CLEAR_ERROR_INT 0x02
|
||||
|
||||
/* Sequence Config */
|
||||
#define VL53L1_SEQUENCE_VHV_EN 0x01
|
||||
#define VL53L1_SEQUENCE_PHASECAL_EN 0x02
|
||||
#define VL53L1_SEQUENCE_REFERENCE_PHASE_EN 0x04
|
||||
#define VL53L1_SEQUENCE_DSS1_EN 0x08
|
||||
#define VL53L1_SEQUENCE_DSS2_EN 0x10
|
||||
#define VL53L1_SEQUENCE_MM1_EN 0x20
|
||||
#define VL53L1_SEQUENCE_MM2_EN 0x40
|
||||
#define VL53L1_SEQUENCE_RANGE_EN 0x80
|
||||
|
||||
/* defines for DSS__ROI_CONTROL */
|
||||
#define VL53L1_DSS_CONTROL__ROI_SUBTRACT 0x20
|
||||
#define VL53L1_DSS_CONTROL__ROI_INTERSECT 0x10
|
||||
|
||||
#define VL53L1_DSS_CONTROL__MODE_DISABLED 0x00
|
||||
#define VL53L1_DSS_CONTROL__MODE_TARGET_RATE 0x01
|
||||
#define VL53L1_DSS_CONTROL__MODE_EFFSPADS 0x02
|
||||
#define VL53L1_DSS_CONTROL__MODE_BLOCKSELECT 0x03
|
||||
|
||||
/* SPAD Readout defines
|
||||
*
|
||||
* 7:6 - SPAD_IN_SEL_REF
|
||||
* 5:4 - SPAD_IN_SEL_RTN
|
||||
* 2 - SPAD_PS_BYPASS
|
||||
* 0 - SPAD_EN_PULSE_EXTENDER
|
||||
*/
|
||||
|
||||
#define VL53L1_RANGING_CORE__SPAD_READOUT__STANDARD 0x45
|
||||
#define VL53L1_RANGING_CORE__SPAD_READOUT__RETURN_ARRAY_ONLY 0x05
|
||||
#define VL53L1_RANGING_CORE__SPAD_READOUT__REFERENCE_ARRAY_ONLY 0x55
|
||||
#define VL53L1_RANGING_CORE__SPAD_READOUT__RETURN_SPLIT_ARRAY 0x25
|
||||
#define VL53L1_RANGING_CORE__SPAD_READOUT__CALIB_PULSES 0xF5
|
||||
|
||||
|
||||
#define VL53L1_LASER_SAFETY__KEY_VALUE 0x6C
|
||||
|
||||
/* Range Status defines
|
||||
*
|
||||
* 7 - GPH ID
|
||||
* 6 - Min threshold hit
|
||||
* 5 - Max threshold hit
|
||||
* 4:0 - Range Status
|
||||
*/
|
||||
|
||||
#define VL53L1_RANGE_STATUS__RANGE_STATUS_MASK 0x1F
|
||||
#define VL53L1_RANGE_STATUS__MAX_THRESHOLD_HIT_MASK 0x20
|
||||
#define VL53L1_RANGE_STATUS__MIN_THRESHOLD_HIT_MASK 0x40
|
||||
#define VL53L1_RANGE_STATUS__GPH_ID_RANGE_STATUS_MASK 0x80
|
||||
|
||||
/* Interrupt Status defines
|
||||
*
|
||||
* 5 - GPH ID
|
||||
* 4:3 - Interrupt Error Status
|
||||
* 2:0 - Interrupt Status
|
||||
*/
|
||||
|
||||
#define VL53L1_INTERRUPT_STATUS__INT_STATUS_MASK 0x07
|
||||
#define VL53L1_INTERRUPT_STATUS__INT_ERROR_STATUS_MASK 0x18
|
||||
#define VL53L1_INTERRUPT_STATUS__GPH_ID_INT_STATUS_MASK 0x20
|
||||
|
||||
/** @} end of VL53L1_RegisterSettings_group */
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
/* _VL53L1_REGISTER_SETTINGS_H_ */
|
||||
|
||||
|
||||
4402
App/VL53L1X_API/core/inc/vl53l1_register_structs.h
Normal file
4402
App/VL53L1X_API/core/inc/vl53l1_register_structs.h
Normal file
File diff suppressed because it is too large
Load Diff
100
App/VL53L1X_API/core/inc/vl53l1_silicon_core.h
Normal file
100
App/VL53L1X_API/core/inc/vl53l1_silicon_core.h
Normal file
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_silicon_core.h
|
||||
*
|
||||
* @brief EwokPlus25 low level silicon specific API function definitions
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_SILICON_CORE_H_
|
||||
#define _VL53L1_SILICON_CORE_H_
|
||||
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Checks if the firmware is ready for ranging (Silicon variant)
|
||||
*
|
||||
* @param[in] Dev : Device Handle
|
||||
* @param[out] pready : pointer to data ready flag \n
|
||||
* 0 = firmware not ready \n
|
||||
* 1 = firmware ready
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_is_firmware_ready_silicon(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_SILICON_CORE_H_ */
|
||||
210
App/VL53L1X_API/core/inc/vl53l1_tuning_parm_defaults.h
Normal file
210
App/VL53L1X_API/core/inc/vl53l1_tuning_parm_defaults.h
Normal file
@@ -0,0 +1,210 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_tuning_parm_defaults.h
|
||||
*
|
||||
* @brief Define defaults for tuning parm list
|
||||
*
|
||||
* - Ensures coherence of internal defaults to tuning table
|
||||
* - Allows reduction of tuning parm list to only changes from the
|
||||
* standard settings defined below
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L1_TUNING_PARM_DEFAULTS_H_
|
||||
#define _VL53L1_TUNING_PARM_DEFAULTS_H_
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** @defgroup VL53L1_tuningparmdefault_group VL53L1 Defines
|
||||
* @brief VL53L1 Tuning Parm Default Values
|
||||
* @{
|
||||
*/
|
||||
|
||||
#define VL53L1_TUNINGPARM_VERSION_DEFAULT \
|
||||
((uint16_t) 32771)
|
||||
#define VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT \
|
||||
((uint16_t) 32769)
|
||||
#define VL53L1_TUNINGPARM_LLD_VERSION_DEFAULT \
|
||||
((uint16_t) 32833)
|
||||
#define VL53L1_TUNINGPARM_CONSISTENCY_LITE_PHASE_TOLERANCE_DEFAULT \
|
||||
((uint8_t) 2)
|
||||
#define VL53L1_TUNINGPARM_PHASECAL_TARGET_DEFAULT \
|
||||
((uint8_t) 33)
|
||||
#define VL53L1_TUNINGPARM_LITE_CAL_REPEAT_RATE_DEFAULT \
|
||||
((uint16_t) 0)
|
||||
#define VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT \
|
||||
((uint16_t) 2011)
|
||||
#define VL53L1_TUNINGPARM_LITE_MIN_CLIP_MM_DEFAULT \
|
||||
((uint8_t) 0)
|
||||
#define VL53L1_TUNINGPARM_LITE_LONG_SIGMA_THRESH_MM_DEFAULT \
|
||||
((uint16_t) 360)
|
||||
#define VL53L1_TUNINGPARM_LITE_MED_SIGMA_THRESH_MM_DEFAULT \
|
||||
((uint16_t) 360)
|
||||
#define VL53L1_TUNINGPARM_LITE_SHORT_SIGMA_THRESH_MM_DEFAULT \
|
||||
((uint16_t) 360)
|
||||
#define VL53L1_TUNINGPARM_LITE_LONG_MIN_COUNT_RATE_RTN_MCPS_DEFAULT \
|
||||
((uint16_t) 192)
|
||||
#define VL53L1_TUNINGPARM_LITE_MED_MIN_COUNT_RATE_RTN_MCPS_DEFAULT \
|
||||
((uint16_t) 192)
|
||||
#define VL53L1_TUNINGPARM_LITE_SHORT_MIN_COUNT_RATE_RTN_MCPS_DEFAULT \
|
||||
((uint16_t) 192)
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_EST_PULSE_WIDTH_DEFAULT \
|
||||
((uint8_t) 8)
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_EST_AMB_WIDTH_NS_DEFAULT \
|
||||
((uint8_t) 16)
|
||||
#define VL53L1_TUNINGPARM_LITE_SIGMA_REF_MM_DEFAULT \
|
||||
((uint8_t) 1)
|
||||
#define VL53L1_TUNINGPARM_LITE_RIT_MULT_DEFAULT \
|
||||
((uint8_t) 64)
|
||||
#define VL53L1_TUNINGPARM_LITE_SEED_CONFIG_DEFAULT \
|
||||
((uint8_t) 2)
|
||||
#define VL53L1_TUNINGPARM_LITE_QUANTIFIER_DEFAULT \
|
||||
((uint8_t) 2)
|
||||
#define VL53L1_TUNINGPARM_LITE_FIRST_ORDER_SELECT_DEFAULT \
|
||||
((uint8_t) 0)
|
||||
#define VL53L1_TUNINGPARM_LITE_XTALK_MARGIN_KCPS_DEFAULT \
|
||||
((int16_t) 0)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_LONG_RANGE_DEFAULT \
|
||||
((uint8_t) 14)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_MED_RANGE_DEFAULT \
|
||||
((uint8_t) 10)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_RTN_LITE_SHORT_RANGE_DEFAULT \
|
||||
((uint8_t) 6)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_LONG_RANGE_DEFAULT \
|
||||
((uint8_t) 14)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_MED_RANGE_DEFAULT \
|
||||
((uint8_t) 10)
|
||||
#define VL53L1_TUNINGPARM_INITIAL_PHASE_REF_LITE_SHORT_RANGE_DEFAULT \
|
||||
((uint8_t) 6)
|
||||
#define VL53L1_TUNINGPARM_TIMED_SEED_CONFIG_DEFAULT \
|
||||
((uint8_t) 1)
|
||||
#define VL53L1_TUNINGPARM_VHV_LOOPBOUND_DEFAULT \
|
||||
((uint8_t) 32)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_DEVICE_TEST_MODE_DEFAULT \
|
||||
((uint8_t) 8)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_VCSEL_PERIOD_DEFAULT \
|
||||
((uint8_t) 11)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_PHASECAL_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 1000)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_TARGET_COUNT_RATE_MCPS_DEFAULT \
|
||||
((uint16_t) 2560)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_MIN_COUNTRATE_LIMIT_MCPS_DEFAULT \
|
||||
((uint16_t) 1280)
|
||||
#define VL53L1_TUNINGPARM_REFSPADCHAR_MAX_COUNTRATE_LIMIT_MCPS_DEFAULT \
|
||||
((uint16_t) 5120)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_DSS_RATE_MCPS_DEFAULT \
|
||||
((uint16_t) 2560)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_PHASECAL_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 1000)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 13000)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_RANGE_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 13000)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_PRE_SAMPLES_DEFAULT \
|
||||
((uint8_t) 8)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM1_SAMPLES_DEFAULT \
|
||||
((uint8_t) 40)
|
||||
#define VL53L1_TUNINGPARM_OFFSET_CAL_MM2_SAMPLES_DEFAULT \
|
||||
((uint8_t) 9)
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_VCSEL_PERIOD_DEFAULT \
|
||||
((uint8_t) 18)
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_VCSEL_START_DEFAULT \
|
||||
((uint8_t) 15)
|
||||
#define VL53L1_TUNINGPARM_SPADMAP_RATE_LIMIT_MCPS_DEFAULT \
|
||||
((uint16_t) 12)
|
||||
#define VL53L1_TUNINGPARM_LITE_DSS_CONFIG_TARGET_TOTAL_RATE_MCPS_DEFAULT \
|
||||
((uint16_t) 2560)
|
||||
#define VL53L1_TUNINGPARM_TIMED_DSS_CONFIG_TARGET_TOTAL_RATE_MCPS_DEFAULT \
|
||||
((uint16_t) 2560)
|
||||
#define VL53L1_TUNINGPARM_LITE_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 1000)
|
||||
#define VL53L1_TUNINGPARM_TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 1000)
|
||||
#define VL53L1_TUNINGPARM_LITE_MM_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 2000)
|
||||
#define VL53L1_TUNINGPARM_TIMED_MM_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 2000)
|
||||
#define VL53L1_TUNINGPARM_LITE_RANGE_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 63000)
|
||||
#define VL53L1_TUNINGPARM_TIMED_RANGE_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 13000)
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT \
|
||||
((uint8_t) 3)
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 700)
|
||||
#define VL53L1_TUNINGPARM_LOWPOWERAUTO_RANGE_CONFIG_TIMEOUT_US_DEFAULT \
|
||||
((uint32_t) 8000)
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_LL_DEF_H_ */
|
||||
|
||||
|
||||
276
App/VL53L1X_API/core/inc/vl53l1_wait.h
Normal file
276
App/VL53L1X_API/core/inc/vl53l1_wait.h
Normal file
@@ -0,0 +1,276 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_wait.h
|
||||
*
|
||||
* @brief EwokPlus25 low level Driver wait function definitions
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_WAIT_H_
|
||||
#define _VL53L1_WAIT_H_
|
||||
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief Wait for initial firmware boot to finish
|
||||
*
|
||||
* Calls VL53L1_poll_for_boot_completion()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_boot_completion(
|
||||
VL53L1_DEV Dev);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits for initial firmware ready
|
||||
*
|
||||
* Only waits to see if the firmware is ready in timed and
|
||||
* single shot modes.
|
||||
*
|
||||
* Calls VL53L1_poll_for_firmware_ready()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_firmware_ready(
|
||||
VL53L1_DEV Dev);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits for the next ranging interrupt
|
||||
*
|
||||
* Calls VL53L1_poll_for_range_completion()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_range_completion(
|
||||
VL53L1_DEV Dev);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits for a device test mode to complete.
|
||||
|
||||
* Calls VL53L1_poll_for_test_completion()
|
||||
*
|
||||
* @param[in] Dev : Device Handle
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_test_completion(
|
||||
VL53L1_DEV Dev);
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads FIRMWARE__SYSTEM_STATUS register to detect if the
|
||||
* firmware was finished booting
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[out] pready : pointer to data ready flag \n
|
||||
* 0 = boot not complete \n
|
||||
* 1 = boot complete
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_is_boot_complete(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready);
|
||||
|
||||
/**
|
||||
* @brief Reads FIRMWARE__SYSTEM_STATUS register to detect if the
|
||||
* firmware is ready for ranging.
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[out] pready : pointer to data ready flag \n
|
||||
* 0 = firmware not ready \n
|
||||
* 1 = firmware ready
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_is_firmware_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads bit 0 of VL53L1_GPIO__TIO_HV_STATUS register to determine
|
||||
* if new range data is ready (available).
|
||||
*
|
||||
* Interrupt may be either active high or active low. The behaviour of bit 0
|
||||
* of the VL53L1_GPIO__TIO_HV_STATUS register is the same as the interrupt
|
||||
* signal generated on the GPIO pin.
|
||||
*
|
||||
* pdev->stat_cfg.gpio_hv_mux_ctrl bit 4 is used to select required check level
|
||||
*
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[out] pready : pointer to data ready flag \n
|
||||
* 0 = data not ready \n
|
||||
* 1 = new range data available
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_is_new_data_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready);
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits (polls) for initial firmware boot to finish
|
||||
*
|
||||
* After power on or a device reset via XSHUTDOWN EwokPlus25 firmware takes
|
||||
* about 2ms to boot. During this boot sequence elected NVM data is copied
|
||||
* to the device's Host & MCU G02 registers
|
||||
*
|
||||
* This function polls the FIRMWARE__SYSTEM_STATUS register to detect when
|
||||
* firmware is ready.
|
||||
*
|
||||
* Polling is implemented using VL53L1_WaitValueMaskEx()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] timeout_ms : Wait timeout in [ms]
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_boot_completion(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Waits (polls) for initial firmware ready
|
||||
*
|
||||
* Polling is implemented using VL53L1_WaitValueMaskEx()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] timeout_ms : Wait timeout in [ms]
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_firmware_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Polls bit 0 of VL53L1_GPIO__TIO_HV_STATUS register to determine
|
||||
* the state of the GPIO (Interrupt) pin.
|
||||
*
|
||||
* Interrupt may be either active high or active low. The behaviour of bit 0
|
||||
* of the VL53L1_GPIO__TIO_HV_STATUS register is the same as the interrupt
|
||||
* signal generated on the GPIO pin.
|
||||
*
|
||||
* pdev->stat_cfg.gpio_hv_mux_ctrl bit 4 is used to select required check level
|
||||
*
|
||||
* Polling is implemented using VL53L1_WaitValueMaskEx()
|
||||
*
|
||||
* @param[in] Dev : Device handle
|
||||
* @param[in] timeout_ms : Wait timeout in [ms]
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_range_completion(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* _VL53L1_WAIT_H_ */
|
||||
2937
App/VL53L1X_API/core/src/vl53l1_api.c
Normal file
2937
App/VL53L1X_API/core/src/vl53l1_api.c
Normal file
File diff suppressed because it is too large
Load Diff
876
App/VL53L1X_API/core/src/vl53l1_api_calibration.c
Normal file
876
App/VL53L1X_API/core/src/vl53l1_api_calibration.c
Normal file
@@ -0,0 +1,876 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_core.c
|
||||
*
|
||||
* @brief EwokPlus25 low level API function definition
|
||||
*/
|
||||
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_ll_device.h"
|
||||
#include "vl53l1_platform.h"
|
||||
#include "vl53l1_register_map.h"
|
||||
#include "vl53l1_register_funcs.h"
|
||||
#include "vl53l1_register_settings.h"
|
||||
#include "vl53l1_core.h"
|
||||
#include "vl53l1_wait.h"
|
||||
#include "vl53l1_api_preset_modes.h"
|
||||
#include "vl53l1_silicon_core.h"
|
||||
#include "vl53l1_api_core.h"
|
||||
#include "vl53l1_api_calibration.h"
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
#include "vl53l1_api_debug.h"
|
||||
#endif
|
||||
#ifdef VL53L1_LOGGING
|
||||
#include "vl53l1_debug.h"
|
||||
#endif
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \
|
||||
fmt, ##__VA_ARGS__)
|
||||
|
||||
#define trace_print(level, ...) \
|
||||
_LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
|
||||
level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
|
||||
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_ref_spad_char(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_Error *pcal_status)
|
||||
{
|
||||
/*
|
||||
* Runs Reference SPAD Characterisation
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t comms_buffer[6];
|
||||
|
||||
VL53L1_refspadchar_config_t *prefspadchar = &(pdev->refspadchar);
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/*
|
||||
* Ensure power force is enabled
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
|
||||
status = VL53L1_enable_powerforce(Dev);
|
||||
|
||||
/*
|
||||
* Configure device
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_set_ref_spad_char_config(
|
||||
Dev,
|
||||
prefspadchar->vcsel_period,
|
||||
prefspadchar->timeout_us,
|
||||
prefspadchar->target_count_rate_mcps,
|
||||
prefspadchar->max_count_rate_limit_mcps,
|
||||
prefspadchar->min_count_rate_limit_mcps,
|
||||
pdev->stat_nvm.osc_measured__fast_osc__frequency);
|
||||
|
||||
/*
|
||||
* Run device test
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_run_device_test(
|
||||
Dev,
|
||||
prefspadchar->device_test_mode);
|
||||
|
||||
/*
|
||||
* Read results
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_ReadMulti(
|
||||
Dev,
|
||||
VL53L1_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS,
|
||||
comms_buffer,
|
||||
2);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads =
|
||||
comms_buffer[0];
|
||||
pdev->dbg_results.ref_spad_char_result__ref_location =
|
||||
comms_buffer[1];
|
||||
}
|
||||
|
||||
/*
|
||||
* copy results to customer nvm managed G02 registers
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_WriteMulti(
|
||||
Dev,
|
||||
VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
|
||||
comms_buffer,
|
||||
2);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->customer.ref_spad_man__num_requested_ref_spads =
|
||||
comms_buffer[0];
|
||||
pdev->customer.ref_spad_man__ref_location =
|
||||
comms_buffer[1];
|
||||
}
|
||||
|
||||
/* After Ref Spad Char the final set of good SPAD enables
|
||||
* are stored in the NCY results registers below
|
||||
*
|
||||
* - RESULT__SPARE_0_SD_1
|
||||
* - RESULT__SPARE_1_SD_1
|
||||
* - RESULT__SPARE_2_SD_1
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_ReadMulti(
|
||||
Dev,
|
||||
VL53L1_RESULT__SPARE_0_SD1,
|
||||
comms_buffer,
|
||||
6);
|
||||
|
||||
/*
|
||||
* copy reference SPAD enables to customer nvm managed
|
||||
* G02 registers
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_WriteMulti(
|
||||
Dev,
|
||||
VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
|
||||
comms_buffer,
|
||||
6);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->customer.global_config__spad_enables_ref_0 =
|
||||
comms_buffer[0];
|
||||
pdev->customer.global_config__spad_enables_ref_1 =
|
||||
comms_buffer[1];
|
||||
pdev->customer.global_config__spad_enables_ref_2 =
|
||||
comms_buffer[2];
|
||||
pdev->customer.global_config__spad_enables_ref_3 =
|
||||
comms_buffer[3];
|
||||
pdev->customer.global_config__spad_enables_ref_4 =
|
||||
comms_buffer[4];
|
||||
pdev->customer.global_config__spad_enables_ref_5 =
|
||||
comms_buffer[5];
|
||||
}
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
/* Print customer nvm managed data */
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
VL53L1_print_customer_nvm_managed(
|
||||
&(pdev->customer),
|
||||
"run_ref_spad_char():pdev->lldata.customer.",
|
||||
VL53L1_TRACE_MODULE_REF_SPAD_CHAR);
|
||||
#endif
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
|
||||
switch (pdev->sys_results.result__range_status) {
|
||||
|
||||
case VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS:
|
||||
status = VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS;
|
||||
break;
|
||||
|
||||
case VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET:
|
||||
status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH;
|
||||
break;
|
||||
|
||||
case VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET:
|
||||
status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Save unfiltered status
|
||||
*/
|
||||
|
||||
*pcal_status = status;
|
||||
|
||||
/* Status exception code */
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
|
||||
VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
|
||||
status);
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH,
|
||||
VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH,
|
||||
status);
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW,
|
||||
VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW,
|
||||
status);
|
||||
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_run_offset_calibration(
|
||||
VL53L1_DEV Dev,
|
||||
int16_t cal_distance_mm,
|
||||
VL53L1_Error *pcal_status)
|
||||
{
|
||||
/*
|
||||
* Runs offset calibration
|
||||
*
|
||||
* Recommended tuning parm settings:
|
||||
*
|
||||
* - pre_num_of_samples = 32
|
||||
* - mm1_num_of_samples = 100
|
||||
* - mm2_num_of_samples = 64
|
||||
* - target_distance_mm = 140mm
|
||||
* - target reflectance = 5%
|
||||
*
|
||||
* Standard Ranging (sigma delta mode):
|
||||
* - dss_config__target_total_rate_mcps = 20.0 -40.0 Mcps
|
||||
* - phasecal_config_timeout_us = 1000
|
||||
* - range_config_timeout_us = 13000
|
||||
* - mm_config_timeout_us = 13000
|
||||
*
|
||||
*
|
||||
* Note: function parms simplified as part of
|
||||
* Patch_CalFunctionSimplification_11791
|
||||
*
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev =
|
||||
VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
VL53L1_DevicePresetModes device_preset_modes[VL53L1_MAX_OFFSET_RANGE_RESULTS];
|
||||
|
||||
VL53L1_range_results_t range_results;
|
||||
VL53L1_range_results_t *prange_results = &range_results;
|
||||
VL53L1_range_data_t *prange_data = NULL;
|
||||
VL53L1_offset_range_data_t *poffset = NULL;
|
||||
|
||||
uint8_t i = 0;
|
||||
uint8_t m = 0;
|
||||
uint8_t measurement_mode =
|
||||
VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
|
||||
uint16_t manual_effective_spads =
|
||||
pdev->gen_cfg.dss_config__manual_effective_spads_select;
|
||||
|
||||
uint8_t num_of_samples[VL53L1_MAX_OFFSET_RANGE_RESULTS];
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/* select requested offset calibration mode */
|
||||
|
||||
switch (pdev->offset_calibration_mode) {
|
||||
|
||||
default:
|
||||
device_preset_modes[0] =
|
||||
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
|
||||
device_preset_modes[1] =
|
||||
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL;
|
||||
device_preset_modes[2] =
|
||||
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL;
|
||||
break;
|
||||
}
|
||||
|
||||
/* initialise num_of_samples */
|
||||
/* Start Patch_CalFunctionSimplification_11791 */
|
||||
num_of_samples[0] = pdev->offsetcal_cfg.pre_num_of_samples;
|
||||
num_of_samples[1] = pdev->offsetcal_cfg.mm1_num_of_samples;
|
||||
num_of_samples[2] = pdev->offsetcal_cfg.mm2_num_of_samples;
|
||||
/* End Patch_CalFunctionSimplification_11791 */
|
||||
|
||||
/* force all offsets to zero */
|
||||
|
||||
switch (pdev->offset_calibration_mode) {
|
||||
|
||||
case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
|
||||
/* only run pre range */
|
||||
pdev->offset_results.active_results = 1;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
pdev->customer.mm_config__inner_offset_mm = 0;
|
||||
pdev->customer.mm_config__outer_offset_mm = 0;
|
||||
pdev->offset_results.active_results =
|
||||
VL53L1_MAX_OFFSET_RANGE_RESULTS;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
pdev->customer.algo__part_to_part_range_offset_mm = 0;
|
||||
|
||||
/* initialise offset range results */
|
||||
|
||||
pdev->offset_results.max_results = VL53L1_MAX_OFFSET_RANGE_RESULTS;
|
||||
pdev->offset_results.cal_distance_mm = cal_distance_mm;
|
||||
|
||||
for (m = 0 ; m < VL53L1_MAX_OFFSET_RANGE_RESULTS; m++) {
|
||||
|
||||
poffset = &(pdev->offset_results.data[m]);
|
||||
poffset->preset_mode = 0;
|
||||
poffset->no_of_samples = 0;
|
||||
poffset->effective_spads = 0;
|
||||
poffset->peak_rate_mcps = 0;
|
||||
poffset->sigma_mm = 0;
|
||||
poffset->median_range_mm = 0;
|
||||
}
|
||||
|
||||
for (m = 0 ; m < pdev->offset_results.active_results ; m++) {
|
||||
|
||||
poffset = &(pdev->offset_results.data[m]);
|
||||
|
||||
poffset->preset_mode = device_preset_modes[m];
|
||||
|
||||
/* Apply preset mode */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_set_preset_mode(
|
||||
Dev,
|
||||
device_preset_modes[m],
|
||||
/* Start Patch_CalFunctionSimplification_11791 */
|
||||
pdev->offsetcal_cfg.dss_config__target_total_rate_mcps,
|
||||
pdev->offsetcal_cfg.phasecal_config_timeout_us,
|
||||
pdev->offsetcal_cfg.mm_config_timeout_us,
|
||||
pdev->offsetcal_cfg.range_config_timeout_us,
|
||||
/* End Patch_CalFunctionSimplification_11791 */
|
||||
100);
|
||||
|
||||
pdev->gen_cfg.dss_config__manual_effective_spads_select =
|
||||
manual_effective_spads;
|
||||
|
||||
/* Initialise device and start range */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_init_and_start_range(
|
||||
Dev,
|
||||
measurement_mode,
|
||||
VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);
|
||||
|
||||
for (i = 0 ; i <= (num_of_samples[m]+2) ; i++) {
|
||||
|
||||
/* Wait for range completion */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_wait_for_range_completion(Dev);
|
||||
|
||||
/*
|
||||
* Get Device Results
|
||||
* - Checks the stream count is the expected one
|
||||
* - Read device system results
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_get_device_results(
|
||||
Dev,
|
||||
VL53L1_DEVICERESULTSLEVEL_FULL,
|
||||
prange_results);
|
||||
|
||||
/*
|
||||
* Ignore 1st two ranges to give the sigma delta initial
|
||||
* phase time to settle
|
||||
*
|
||||
* accummulate range results if range is successful
|
||||
*/
|
||||
|
||||
prange_data = &(prange_results->data[0]);
|
||||
|
||||
if (prange_results->stream_count > 1) {
|
||||
|
||||
if (prange_data->range_status ==
|
||||
VL53L1_DEVICEERROR_RANGECOMPLETE) {
|
||||
|
||||
poffset->no_of_samples++;
|
||||
poffset->effective_spads +=
|
||||
(uint32_t)prange_data->actual_effective_spads;
|
||||
poffset->peak_rate_mcps +=
|
||||
(uint32_t)prange_data->peak_signal_count_rate_mcps;
|
||||
poffset->sigma_mm +=
|
||||
(uint32_t)prange_data->sigma_mm;
|
||||
poffset->median_range_mm +=
|
||||
(int32_t)prange_data->median_range_mm;
|
||||
|
||||
poffset->dss_config__roi_mode_control =
|
||||
pdev->gen_cfg.dss_config__roi_mode_control;
|
||||
poffset->dss_config__manual_effective_spads_select =
|
||||
pdev->gen_cfg.dss_config__manual_effective_spads_select;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Conditional wait for firmware ready. Only waits for timed
|
||||
* and single shot modes. Mode check is performed inside the
|
||||
* wait function
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_wait_for_firmware_ready(Dev);
|
||||
|
||||
/*
|
||||
* Send ranging handshake
|
||||
*
|
||||
* - Update Zone management
|
||||
* - Update GPH registers
|
||||
* - Clear current interrupt
|
||||
* - Initialise SYSTEM__MODE_START for next range (if there is one!)
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_clear_interrupt_and_enable_next_range(
|
||||
Dev,
|
||||
measurement_mode);
|
||||
}
|
||||
|
||||
/* Stop range */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_stop_range(Dev);
|
||||
|
||||
/* Wait for Stop (abort) range to complete */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_WaitUs(Dev, 1000);
|
||||
|
||||
/* generate average values */
|
||||
if (poffset->no_of_samples > 0) {
|
||||
|
||||
poffset->effective_spads += (poffset->no_of_samples/2);
|
||||
poffset->effective_spads /= poffset->no_of_samples;
|
||||
|
||||
poffset->peak_rate_mcps += (poffset->no_of_samples/2);
|
||||
poffset->peak_rate_mcps /= poffset->no_of_samples;
|
||||
|
||||
poffset->sigma_mm += (poffset->no_of_samples/2);
|
||||
poffset->sigma_mm /= poffset->no_of_samples;
|
||||
|
||||
poffset->median_range_mm += (poffset->no_of_samples/2);
|
||||
poffset->median_range_mm /= poffset->no_of_samples;
|
||||
|
||||
poffset->range_mm_offset = (int32_t)cal_distance_mm;
|
||||
poffset->range_mm_offset -= poffset->median_range_mm;
|
||||
|
||||
/* remember the number of SPADs for standard ranging */
|
||||
if (poffset->preset_mode ==
|
||||
VL53L1_DEVICEPRESETMODE_STANDARD_RANGING)
|
||||
manual_effective_spads =
|
||||
(uint16_t)poffset->effective_spads;
|
||||
}
|
||||
}
|
||||
|
||||
/* Calculate offsets */
|
||||
|
||||
switch (pdev->offset_calibration_mode) {
|
||||
|
||||
case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
|
||||
|
||||
/* copy offsets to customer data structure */
|
||||
pdev->customer.mm_config__inner_offset_mm +=
|
||||
(int16_t)pdev->offset_results.data[0].range_mm_offset;
|
||||
pdev->customer.mm_config__outer_offset_mm +=
|
||||
(int16_t)pdev->offset_results.data[0].range_mm_offset;
|
||||
break;
|
||||
|
||||
default:
|
||||
/* copy offsets to customer data structure */
|
||||
pdev->customer.mm_config__inner_offset_mm =
|
||||
(int16_t)pdev->offset_results.data[1].range_mm_offset;
|
||||
pdev->customer.mm_config__outer_offset_mm =
|
||||
(int16_t)pdev->offset_results.data[2].range_mm_offset;
|
||||
pdev->customer.algo__part_to_part_range_offset_mm = 0;
|
||||
|
||||
/* copy average rate and effective SPAD count to
|
||||
additional offset calibration data structure */
|
||||
|
||||
pdev->add_off_cal_data.result__mm_inner_actual_effective_spads =
|
||||
(uint16_t)pdev->offset_results.data[1].effective_spads;
|
||||
pdev->add_off_cal_data.result__mm_outer_actual_effective_spads =
|
||||
(uint16_t)pdev->offset_results.data[2].effective_spads;
|
||||
|
||||
pdev->add_off_cal_data.result__mm_inner_peak_signal_count_rtn_mcps =
|
||||
(uint16_t)pdev->offset_results.data[1].peak_rate_mcps;
|
||||
pdev->add_off_cal_data.result__mm_outer_peak_signal_count_rtn_mcps =
|
||||
(uint16_t)pdev->offset_results.data[2].peak_rate_mcps;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
/* apply to device */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_set_customer_nvm_managed(
|
||||
Dev,
|
||||
&(pdev->customer));
|
||||
|
||||
/*
|
||||
* Check the peak rates, sigma, min spads for each stage
|
||||
*/
|
||||
|
||||
for (m = 0 ; m < pdev->offset_results.active_results ; m++) {
|
||||
|
||||
poffset = &(pdev->offset_results.data[m]);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
|
||||
pdev->offset_results.cal_report = m;
|
||||
|
||||
if (poffset->no_of_samples < num_of_samples[m])
|
||||
status = VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES;
|
||||
|
||||
/* only check sigma for the pre-range as
|
||||
* the it is not calculated by the device
|
||||
* for the MM1 and MM2 stages
|
||||
*/
|
||||
if (m == 0 && poffset->sigma_mm >
|
||||
((uint32_t)VL53L1_OFFSET_CAL_MAX_SIGMA_MM<<5))
|
||||
status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;
|
||||
|
||||
if (poffset->peak_rate_mcps >
|
||||
VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS)
|
||||
status = VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH;
|
||||
|
||||
if (poffset->dss_config__manual_effective_spads_select <
|
||||
VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS)
|
||||
status = VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW;
|
||||
|
||||
if (poffset->dss_config__manual_effective_spads_select == 0)
|
||||
status = VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL;
|
||||
|
||||
if (poffset->no_of_samples == 0)
|
||||
status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Save unfiltered status
|
||||
*/
|
||||
|
||||
pdev->offset_results.cal_status = status;
|
||||
*pcal_status = pdev->offset_results.cal_status;
|
||||
|
||||
/* Status exception codes */
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_OFFSET_CAL_MISSING_SAMPLES,
|
||||
VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES,
|
||||
status);
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH,
|
||||
VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH,
|
||||
status);
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_OFFSET_CAL_RATE_TOO_HIGH,
|
||||
VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH,
|
||||
status);
|
||||
|
||||
IGNORE_STATUS(
|
||||
IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
|
||||
VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
|
||||
status);
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
|
||||
/* Prints out the offset calibration data for debug */
|
||||
|
||||
VL53L1_print_customer_nvm_managed(
|
||||
&(pdev->customer),
|
||||
"run_offset_calibration():pdev->lldata.customer.",
|
||||
VL53L1_TRACE_MODULE_OFFSET_DATA);
|
||||
|
||||
VL53L1_print_additional_offset_cal_data(
|
||||
&(pdev->add_off_cal_data),
|
||||
"run_offset_calibration():pdev->lldata.add_off_cal_data.",
|
||||
VL53L1_TRACE_MODULE_OFFSET_DATA);
|
||||
|
||||
VL53L1_print_offset_range_results(
|
||||
&(pdev->offset_results),
|
||||
"run_offset_calibration():pdev->lldata.offset_results.",
|
||||
VL53L1_TRACE_MODULE_OFFSET_DATA);
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_spad_rate_map(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_DeviceTestMode device_test_mode,
|
||||
VL53L1_DeviceSscArray array_select,
|
||||
uint32_t ssc_config_timeout_us,
|
||||
VL53L1_spad_rate_data_t *pspad_rate_data)
|
||||
{
|
||||
|
||||
/**
|
||||
* Runs SPAD Rate Map
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
VL53L1_LLDriverData_t *pdev =
|
||||
VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/*
|
||||
* Ensure power force is enabled
|
||||
*/
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_enable_powerforce(Dev);
|
||||
|
||||
/*
|
||||
* Configure the test
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->ssc_cfg.array_select = array_select;
|
||||
pdev->ssc_cfg.timeout_us = ssc_config_timeout_us;
|
||||
status =
|
||||
VL53L1_set_ssc_config(
|
||||
Dev,
|
||||
&(pdev->ssc_cfg),
|
||||
pdev->stat_nvm.osc_measured__fast_osc__frequency);
|
||||
}
|
||||
|
||||
/*
|
||||
* Run device test
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_run_device_test(
|
||||
Dev,
|
||||
device_test_mode);
|
||||
|
||||
/*
|
||||
* Read Rate Data from Patch Ram
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_get_spad_rate_data(
|
||||
Dev,
|
||||
pspad_rate_data);
|
||||
|
||||
if (device_test_mode == VL53L1_DEVICETESTMODE_LCR_VCSEL_ON)
|
||||
pspad_rate_data->fractional_bits = 7;
|
||||
else
|
||||
pspad_rate_data->fractional_bits = 15;
|
||||
|
||||
/* Ensure power force is disabled */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_disable_powerforce(Dev);
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
/* Print return rate data and map */
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
VL53L1_print_spad_rate_data(
|
||||
pspad_rate_data,
|
||||
"run_spad_rate_map():",
|
||||
VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
|
||||
VL53L1_print_spad_rate_map(
|
||||
pspad_rate_data,
|
||||
"run_spad_rate_map():",
|
||||
VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
|
||||
}
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef VL53L1_NOCALIB
|
||||
VL53L1_Error VL53L1_run_device_test(
|
||||
VL53L1_DEV Dev,
|
||||
VL53L1_DeviceTestMode device_test_mode)
|
||||
{
|
||||
/*
|
||||
* Runs the selected Device Test Mode
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t comms_buffer[2];
|
||||
uint8_t gpio_hv_mux__ctrl = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/*
|
||||
* Get current interrupt config
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
|
||||
status =
|
||||
VL53L1_RdByte(
|
||||
Dev,
|
||||
VL53L1_GPIO_HV_MUX__CTRL,
|
||||
&gpio_hv_mux__ctrl);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl;
|
||||
|
||||
/*
|
||||
* Trigger the test
|
||||
*/
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_start_test(
|
||||
Dev,
|
||||
device_test_mode);
|
||||
|
||||
/*
|
||||
* Wait for test completion
|
||||
*/
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_wait_for_test_completion(Dev);
|
||||
|
||||
/*
|
||||
* Read range and report status
|
||||
*/
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_ReadMulti(
|
||||
Dev,
|
||||
VL53L1_RESULT__RANGE_STATUS,
|
||||
comms_buffer,
|
||||
2);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->sys_results.result__range_status = comms_buffer[0];
|
||||
pdev->sys_results.result__report_status = comms_buffer[1];
|
||||
}
|
||||
|
||||
/* mask range status bits */
|
||||
|
||||
pdev->sys_results.result__range_status &=
|
||||
VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
trace_print(
|
||||
VL53L1_TRACE_LEVEL_INFO,
|
||||
" Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n",
|
||||
"result__range_status",
|
||||
pdev->sys_results.result__range_status,
|
||||
"result__report_status",
|
||||
pdev->sys_results.result__report_status);
|
||||
|
||||
/*
|
||||
* Clear interrupt
|
||||
*/
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_clear_interrupt(Dev);
|
||||
}
|
||||
|
||||
/*
|
||||
* Clear test mode register
|
||||
* - required so that next test command will trigger
|
||||
* internal MCU interrupt
|
||||
*/
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_start_test(
|
||||
Dev,
|
||||
0x00);
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
#endif
|
||||
3533
App/VL53L1X_API/core/src/vl53l1_api_core.c
Normal file
3533
App/VL53L1X_API/core/src/vl53l1_api_core.c
Normal file
File diff suppressed because it is too large
Load Diff
1741
App/VL53L1X_API/core/src/vl53l1_api_debug.c
Normal file
1741
App/VL53L1X_API/core/src/vl53l1_api_debug.c
Normal file
File diff suppressed because it is too large
Load Diff
1407
App/VL53L1X_API/core/src/vl53l1_api_preset_modes.c
Normal file
1407
App/VL53L1X_API/core/src/vl53l1_api_preset_modes.c
Normal file
File diff suppressed because it is too large
Load Diff
220
App/VL53L1X_API/core/src/vl53l1_api_strings.c
Normal file
220
App/VL53L1X_API/core/src/vl53l1_api_strings.c
Normal file
@@ -0,0 +1,220 @@
|
||||
|
||||
// SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause
|
||||
/******************************************************************************
|
||||
* Copyright (c) 2020, STMicroelectronics - All Rights Reserved
|
||||
|
||||
This file is part of VL53L1 and is dual licensed,
|
||||
either GPL-2.0+
|
||||
or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
******************************************************************************
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_api_strings.c
|
||||
* @brief VL53L1 API functions for decoding error codes to a text string
|
||||
*/
|
||||
|
||||
#include "vl53l1_api_core.h"
|
||||
#include "vl53l1_api_strings.h"
|
||||
#include "vl53l1_error_codes.h"
|
||||
#include "vl53l1_error_strings.h"
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_API, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_API, status, fmt, \
|
||||
##__VA_ARGS__)
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_get_range_status_string(
|
||||
uint8_t RangeStatus,
|
||||
char *pRangeStatusString)
|
||||
{
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
SUPPRESS_UNUSED_WARNING(RangeStatus);
|
||||
VL53L1_COPYSTRING(pRangeStatusString, "");
|
||||
#else
|
||||
switch (RangeStatus) {
|
||||
case 0:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_RANGEVALID);
|
||||
break;
|
||||
case 1:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_SIGMA);
|
||||
break;
|
||||
case 2:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_SIGNAL);
|
||||
break;
|
||||
case 3:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_MINRANGE);
|
||||
break;
|
||||
case 4:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_PHASE);
|
||||
break;
|
||||
case 5:
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_HW);
|
||||
break;
|
||||
|
||||
default: /**/
|
||||
VL53L1_COPYSTRING(pRangeStatusString,
|
||||
VL53L1_STRING_RANGESTATUS_NONE);
|
||||
}
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_get_pal_state_string(
|
||||
VL53L1_State PalStateCode,
|
||||
char *pPalStateString)
|
||||
{
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
SUPPRESS_UNUSED_WARNING(PalStateCode);
|
||||
VL53L1_COPYSTRING(pPalStateString, "");
|
||||
#else
|
||||
switch (PalStateCode) {
|
||||
case VL53L1_STATE_POWERDOWN:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_POWERDOWN);
|
||||
break;
|
||||
case VL53L1_STATE_WAIT_STATICINIT:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_WAIT_STATICINIT);
|
||||
break;
|
||||
case VL53L1_STATE_STANDBY:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_STANDBY);
|
||||
break;
|
||||
case VL53L1_STATE_IDLE:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_IDLE);
|
||||
break;
|
||||
case VL53L1_STATE_RUNNING:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_RUNNING);
|
||||
break;
|
||||
case VL53L1_STATE_RESET:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_RESET);
|
||||
break;
|
||||
case VL53L1_STATE_UNKNOWN:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_UNKNOWN);
|
||||
break;
|
||||
case VL53L1_STATE_ERROR:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_ERROR);
|
||||
break;
|
||||
|
||||
default:
|
||||
VL53L1_COPYSTRING(pPalStateString,
|
||||
VL53L1_STRING_STATE_UNKNOWN);
|
||||
}
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_get_sequence_steps_info(
|
||||
VL53L1_SequenceStepId SequenceStepId,
|
||||
char *pSequenceStepsString)
|
||||
{
|
||||
VL53L1_Error Status = VL53L1_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
SUPPRESS_UNUSED_WARNING(SequenceStepId);
|
||||
VL53L1_COPYSTRING(pSequenceStepsString, "");
|
||||
#else
|
||||
switch (SequenceStepId) {
|
||||
case VL53L1_SEQUENCESTEP_VHV:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_VHV);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_PHASECAL:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_PHASECAL);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_REFPHASE:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_DSS1);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_DSS1:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_DSS1);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_DSS2:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_DSS2);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_MM1:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_MM1);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_MM2:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_MM2);
|
||||
break;
|
||||
case VL53L1_SEQUENCESTEP_RANGE:
|
||||
VL53L1_COPYSTRING(pSequenceStepsString,
|
||||
VL53L1_STRING_SEQUENCESTEP_RANGE);
|
||||
break;
|
||||
default:
|
||||
Status = VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
|
||||
return Status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_get_limit_check_info(uint16_t LimitCheckId,
|
||||
char *pLimitCheckString)
|
||||
{
|
||||
VL53L1_Error Status = VL53L1_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
SUPPRESS_UNUSED_WARNING(LimitCheckId);
|
||||
VL53L1_COPYSTRING(pLimitCheckString, "");
|
||||
#else
|
||||
switch (LimitCheckId) {
|
||||
case VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE:
|
||||
VL53L1_COPYSTRING(pLimitCheckString,
|
||||
VL53L1_STRING_CHECKENABLE_SIGMA_FINAL_RANGE);
|
||||
break;
|
||||
case VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE:
|
||||
VL53L1_COPYSTRING(pLimitCheckString,
|
||||
VL53L1_STRING_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE);
|
||||
break;
|
||||
default:
|
||||
VL53L1_COPYSTRING(pLimitCheckString,
|
||||
VL53L1_STRING_UNKNOW_ERROR_CODE);
|
||||
}
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
return Status;
|
||||
}
|
||||
|
||||
2349
App/VL53L1X_API/core/src/vl53l1_core.c
Normal file
2349
App/VL53L1X_API/core/src/vl53l1_core.c
Normal file
File diff suppressed because it is too large
Load Diff
445
App/VL53L1X_API/core/src/vl53l1_core_support.c
Normal file
445
App/VL53L1X_API/core/src/vl53l1_core_support.c
Normal file
@@ -0,0 +1,445 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_core_support.c
|
||||
*
|
||||
* @brief EwokPlus25 core function definition
|
||||
*/
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_ll_device.h"
|
||||
#include "vl53l1_platform_log.h"
|
||||
#include "vl53l1_core_support.h"
|
||||
#include "vl53l1_platform_user_data.h"
|
||||
#include "vl53l1_platform_user_defines.h"
|
||||
|
||||
#ifdef VL53L1_LOGGING
|
||||
#include "vl53l1_debug.h"
|
||||
#include "vl53l1_register_debug.h"
|
||||
#endif
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, \
|
||||
status, fmt, ##__VA_ARGS__)
|
||||
|
||||
#define trace_print(level, ...) \
|
||||
_LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
|
||||
level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)
|
||||
|
||||
|
||||
uint32_t VL53L1_calc_pll_period_us(
|
||||
uint16_t fast_osc_frequency)
|
||||
{
|
||||
/* Calculates PLL frequency using NVM fast_osc_frequency
|
||||
* Fast osc frequency fixed point format = unsigned 4.12
|
||||
*
|
||||
* PLL period fixed point format = unsigned 0.24
|
||||
* Min input fast osc frequency = 1 MHz
|
||||
* PLL Multiplier = 64 (fixed)
|
||||
* Min PLL freq = 64.0MHz
|
||||
* -> max PLL period = 1/ 64
|
||||
* -> only the 18 LS bits are used
|
||||
*
|
||||
* 2^30 = (2^24) (1.0us) * 4096 (2^12) / 64 (PLL Multiplier)
|
||||
*/
|
||||
|
||||
uint32_t pll_period_us = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
pll_period_us = (0x01 << 30) / fast_osc_frequency;
|
||||
|
||||
#ifdef VL53L1_LOGGING
|
||||
trace_print(VL53L1_TRACE_LEVEL_DEBUG,
|
||||
" %-48s : %10u\n", "pll_period_us",
|
||||
pll_period_us);
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(0);
|
||||
|
||||
return pll_period_us;
|
||||
}
|
||||
|
||||
|
||||
#ifdef PAL_EXTENDED
|
||||
uint32_t VL53L1_duration_maths(
|
||||
uint32_t pll_period_us,
|
||||
uint32_t vcsel_parm_pclks,
|
||||
uint32_t window_vclks,
|
||||
uint32_t elapsed_mclks)
|
||||
{
|
||||
/*
|
||||
* Generates the ranging duration in us
|
||||
*
|
||||
* duration_us = elapsed_mclks * vcsel_perm_pclks *
|
||||
* window_vclks * pll_period_us
|
||||
*
|
||||
* returned value in [us] with no fraction bits
|
||||
*/
|
||||
|
||||
uint64_t tmp_long_int = 0;
|
||||
uint32_t duration_us = 0;
|
||||
|
||||
/* PLL period us = 0.24 18 LS bits used
|
||||
* window_vclks = 12.0 (2304 max)
|
||||
* output 30b (6.24)
|
||||
*/
|
||||
duration_us = window_vclks * pll_period_us;
|
||||
|
||||
/* down shift by 12
|
||||
* output 18b (6.12)
|
||||
*/
|
||||
duration_us = duration_us >> 12;
|
||||
|
||||
/* Save first part of the calc (#1) */
|
||||
tmp_long_int = (uint64_t)duration_us;
|
||||
|
||||
/* Multiply elapsed macro periods (22-bit)
|
||||
* by VCSEL parameter 6.4 (max 63.9999)
|
||||
* output 32b (28.4)
|
||||
*/
|
||||
duration_us = elapsed_mclks * vcsel_parm_pclks;
|
||||
|
||||
/* down shift by 4 to remove fractional bits (#2)
|
||||
* output 28b (28.0)
|
||||
*/
|
||||
duration_us = duration_us >> 4;
|
||||
|
||||
/* Multiply #1 18b (6.12) by #2 28b (28.0)
|
||||
* output 46b (34.12)
|
||||
*/
|
||||
tmp_long_int = tmp_long_int * (uint64_t)duration_us;
|
||||
|
||||
/* Remove fractional part
|
||||
* output 34b (34.0)
|
||||
*/
|
||||
tmp_long_int = tmp_long_int >> 12;
|
||||
|
||||
/* Clip to 32-bits */
|
||||
if (tmp_long_int > 0xFFFFFFFF) {
|
||||
tmp_long_int = 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
duration_us = (uint32_t)tmp_long_int;
|
||||
|
||||
return duration_us;
|
||||
}
|
||||
|
||||
|
||||
uint32_t VL53L1_isqrt(uint32_t num)
|
||||
{
|
||||
|
||||
/*
|
||||
* Implements an integer square root
|
||||
*
|
||||
* From: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots
|
||||
*/
|
||||
|
||||
uint32_t res = 0;
|
||||
uint32_t bit = 1 << 30; /* The second-to-top bit is set: 1 << 14 for 16-bits, 1 << 30 for 32 bits */
|
||||
|
||||
/* "bit" starts at the highest power of four <= the argument. */
|
||||
while (bit > num) {
|
||||
bit >>= 2;
|
||||
}
|
||||
|
||||
while (bit != 0) {
|
||||
if (num >= res + bit) {
|
||||
num -= res + bit;
|
||||
res = (res >> 1) + bit;
|
||||
} else {
|
||||
res >>= 1;
|
||||
}
|
||||
bit >>= 2;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
uint16_t VL53L1_rate_maths(
|
||||
int32_t events,
|
||||
uint32_t time_us)
|
||||
{
|
||||
/*
|
||||
* Converts events into count rate
|
||||
*
|
||||
* Max events = 512 Mcps * 1sec
|
||||
* = 512,000,000 events
|
||||
* = 29b
|
||||
*
|
||||
* If events > 2^24 use 3-bit fractional bits is used internally
|
||||
* otherwise 7-bit fractional bits are used
|
||||
*/
|
||||
|
||||
uint32_t tmp_int = 0;
|
||||
uint32_t frac_bits = 7;
|
||||
uint16_t rate_mcps = 0; /* 9.7 format */
|
||||
|
||||
/*
|
||||
* Clip input event range
|
||||
*/
|
||||
|
||||
if (events > VL53L1_SPAD_TOTAL_COUNT_MAX) {
|
||||
tmp_int = VL53L1_SPAD_TOTAL_COUNT_MAX;
|
||||
} else if (events > 0) {
|
||||
tmp_int = (uint32_t)events;
|
||||
}
|
||||
|
||||
/*
|
||||
* if events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES use 3 rather
|
||||
* than 7 fractional bits internal to function
|
||||
*/
|
||||
|
||||
if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
|
||||
frac_bits = 3;
|
||||
} else {
|
||||
frac_bits = 7;
|
||||
}
|
||||
|
||||
/*
|
||||
* Create 3 or 7 fractional bits
|
||||
* output 32b (29.3 or 25.7)
|
||||
* Divide by range duration in [us] - no fractional bits
|
||||
*/
|
||||
if (time_us > 0) {
|
||||
tmp_int = ((tmp_int << frac_bits) + (time_us / 2)) / time_us;
|
||||
}
|
||||
|
||||
/*
|
||||
* Re align if reduced resolution
|
||||
*/
|
||||
if (events > VL53L1_SPAD_TOTAL_COUNT_RES_THRES) {
|
||||
tmp_int = tmp_int << 4;
|
||||
}
|
||||
|
||||
/*
|
||||
* Firmware internal count is 17.7 (24b) but it this
|
||||
* case clip to 16-bit value for reporting
|
||||
*/
|
||||
|
||||
if (tmp_int > 0xFFFF) {
|
||||
tmp_int = 0xFFFF;
|
||||
}
|
||||
|
||||
rate_mcps = (uint16_t)tmp_int;
|
||||
|
||||
return rate_mcps;
|
||||
}
|
||||
|
||||
uint16_t VL53L1_rate_per_spad_maths(
|
||||
uint32_t frac_bits,
|
||||
uint32_t peak_count_rate,
|
||||
uint16_t num_spads,
|
||||
uint32_t max_output_value)
|
||||
{
|
||||
|
||||
uint32_t tmp_int = 0;
|
||||
|
||||
/* rate_per_spad Format varies with prog frac_bits */
|
||||
uint16_t rate_per_spad = 0;
|
||||
|
||||
/* Calculate rate per spad with variable fractional bits */
|
||||
|
||||
/* Frac_bits should be programmed as final frac_bits - 7 as
|
||||
* the pk_rate contains an inherent 7 bit resolution
|
||||
*/
|
||||
|
||||
if (num_spads > 0) {
|
||||
tmp_int = (peak_count_rate << 8) << frac_bits;
|
||||
tmp_int = (tmp_int + ((uint32_t)num_spads / 2)) / (uint32_t)num_spads;
|
||||
} else {
|
||||
tmp_int = ((peak_count_rate) << frac_bits);
|
||||
}
|
||||
|
||||
/* Clip in case of overwrap - special code */
|
||||
|
||||
if (tmp_int > max_output_value) {
|
||||
tmp_int = max_output_value;
|
||||
}
|
||||
|
||||
rate_per_spad = (uint16_t)tmp_int;
|
||||
|
||||
return rate_per_spad;
|
||||
}
|
||||
|
||||
int32_t VL53L1_range_maths(
|
||||
uint16_t fast_osc_frequency,
|
||||
uint16_t phase,
|
||||
uint16_t zero_distance_phase,
|
||||
uint8_t fractional_bits,
|
||||
int32_t gain_factor,
|
||||
int32_t range_offset_mm)
|
||||
{
|
||||
/*
|
||||
* Converts phase information into distance in [mm]
|
||||
*/
|
||||
|
||||
uint32_t pll_period_us = 0; /* 0.24 format */
|
||||
int64_t tmp_long_int = 0;
|
||||
int32_t range_mm = 0;
|
||||
|
||||
/* Calculate PLL period in [ps] */
|
||||
|
||||
pll_period_us = VL53L1_calc_pll_period_us(fast_osc_frequency);
|
||||
|
||||
/* Raw range in [mm]
|
||||
*
|
||||
* calculate the phase difference between return and reference phases
|
||||
*
|
||||
* phases 16b (5.11)
|
||||
* output 17b including sign bit
|
||||
*/
|
||||
|
||||
tmp_long_int = (int64_t)phase - (int64_t)zero_distance_phase;
|
||||
|
||||
/*
|
||||
* multiply by the PLL period
|
||||
*
|
||||
* PLL period 24bit (0.24) but only 18 LS bits used
|
||||
*
|
||||
* Output 35b (0.35) (17b + 18b)
|
||||
*/
|
||||
|
||||
tmp_long_int = tmp_long_int * (int64_t)pll_period_us;
|
||||
|
||||
/*
|
||||
* Down shift by 9 - Output 26b (0.26)
|
||||
*/
|
||||
|
||||
tmp_long_int = tmp_long_int / (0x01 << 9);
|
||||
|
||||
/*
|
||||
* multiply by speed of light in air divided by 8
|
||||
* Factor of 8 includes 2 for the round trip and 4 scaling
|
||||
*
|
||||
* VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8 = 16b (16.2)
|
||||
*
|
||||
* Output 42b (18.24) (16b + 26b)
|
||||
*/
|
||||
|
||||
tmp_long_int = tmp_long_int * VL53L1_SPEED_OF_LIGHT_IN_AIR_DIV_8;
|
||||
|
||||
/*
|
||||
* Down shift by 22 - Output 20b (18.2)
|
||||
*/
|
||||
|
||||
tmp_long_int = tmp_long_int / (0x01 << 22);
|
||||
|
||||
/* Add range offset */
|
||||
range_mm = (int32_t)tmp_long_int + range_offset_mm;
|
||||
|
||||
/* apply correction gain */
|
||||
range_mm *= gain_factor;
|
||||
range_mm += 0x0400;
|
||||
range_mm /= 0x0800;
|
||||
|
||||
/* Remove fractional bits */
|
||||
if (fractional_bits == 0)
|
||||
range_mm = range_mm / (0x01 << 2);
|
||||
else if (fractional_bits == 1)
|
||||
range_mm = range_mm / (0x01 << 1);
|
||||
|
||||
return range_mm;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint8_t VL53L1_decode_vcsel_period(uint8_t vcsel_period_reg)
|
||||
{
|
||||
/*
|
||||
* Converts the encoded VCSEL period register value into
|
||||
* the real period in PLL clocks
|
||||
*/
|
||||
|
||||
uint8_t vcsel_period_pclks = 0;
|
||||
|
||||
vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
|
||||
|
||||
return vcsel_period_pclks;
|
||||
}
|
||||
|
||||
|
||||
void VL53L1_decode_row_col(
|
||||
uint8_t spad_number,
|
||||
uint8_t *prow,
|
||||
uint8_t *pcol)
|
||||
{
|
||||
|
||||
/**
|
||||
* Decodes the array (row,col) location from
|
||||
* the input SPAD number
|
||||
*/
|
||||
|
||||
if (spad_number > 127) {
|
||||
*prow = 8 + ((255-spad_number) & 0x07);
|
||||
*pcol = (spad_number-128) >> 3;
|
||||
} else {
|
||||
*prow = spad_number & 0x07;
|
||||
*pcol = (127-spad_number) >> 3;
|
||||
}
|
||||
}
|
||||
|
||||
304
App/VL53L1X_API/core/src/vl53l1_error_strings.c
Normal file
304
App/VL53L1X_API/core/src/vl53l1_error_strings.c
Normal file
@@ -0,0 +1,304 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_error_strings.c
|
||||
* @brief VL53L1 API functions for decoding error codes to a text string
|
||||
*/
|
||||
|
||||
#include "vl53l1_error_codes.h"
|
||||
#include "vl53l1_error_strings.h"
|
||||
#include "vl53l1_platform_log.h"
|
||||
#include "vl53l1_ll_def.h"
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_API, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_API, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_API, \
|
||||
status, fmt, ##__VA_ARGS__)
|
||||
|
||||
|
||||
#ifndef VL53L1_DEBUG
|
||||
#define VL53L1_USE_EMPTY_STRING
|
||||
#endif
|
||||
|
||||
VL53L1_Error VL53L1_get_pal_error_string(
|
||||
VL53L1_Error PalErrorCode,
|
||||
char *pPalErrorString)
|
||||
{
|
||||
VL53L1_Error Status = VL53L1_ERROR_NONE;
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
SUPPRESS_UNUSED_WARNING(PalErrorCode);
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
#ifdef VL53L1_USE_EMPTY_STRING
|
||||
VL53L1_COPYSTRING(pPalErrorString, "");
|
||||
#else
|
||||
|
||||
switch (PalErrorCode) {
|
||||
case VL53L1_ERROR_NONE:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_NONE);
|
||||
break;
|
||||
case VL53L1_ERROR_CALIBRATION_WARNING:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_CALIBRATION_WARNING);
|
||||
break;
|
||||
case VL53L1_ERROR_MIN_CLIPPED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_MIN_CLIPPED);
|
||||
break;
|
||||
case VL53L1_ERROR_UNDEFINED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_UNDEFINED);
|
||||
break;
|
||||
case VL53L1_ERROR_INVALID_PARAMS:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_INVALID_PARAMS);
|
||||
break;
|
||||
case VL53L1_ERROR_NOT_SUPPORTED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L1_ERROR_RANGE_ERROR:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_RANGE_ERROR);
|
||||
break;
|
||||
case VL53L1_ERROR_TIME_OUT:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_TIME_OUT);
|
||||
break;
|
||||
case VL53L1_ERROR_MODE_NOT_SUPPORTED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_MODE_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L1_ERROR_BUFFER_TOO_SMALL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_BUFFER_TOO_SMALL);
|
||||
break;
|
||||
case VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_COMMS_BUFFER_TOO_SMALL);
|
||||
break;
|
||||
case VL53L1_ERROR_GPIO_NOT_EXISTING:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_GPIO_NOT_EXISTING);
|
||||
break;
|
||||
case VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED);
|
||||
break;
|
||||
case VL53L1_ERROR_CONTROL_INTERFACE:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_CONTROL_INTERFACE);
|
||||
break;
|
||||
case VL53L1_ERROR_INVALID_COMMAND:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_INVALID_COMMAND);
|
||||
break;
|
||||
case VL53L1_ERROR_DIVISION_BY_ZERO:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_DIVISION_BY_ZERO);
|
||||
break;
|
||||
case VL53L1_ERROR_REF_SPAD_INIT:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_REF_SPAD_INIT);
|
||||
break;
|
||||
case VL53L1_ERROR_GPH_SYNC_CHECK_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_GPH_SYNC_CHECK_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_STREAM_COUNT_CHECK_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_GPH_ID_CHECK_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_GPH_ID_CHECK_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_ZONE_GPH_ID_CHECK_FAIL);
|
||||
break;
|
||||
|
||||
case VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_XTALK_EXTRACTION_NO_SAMPLES_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL);
|
||||
break;
|
||||
|
||||
case VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_ZONE_CAL_NO_SAMPLE_FAIL);
|
||||
break;
|
||||
|
||||
case VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_OFFSET_CAL_MISSING_SAMPLES);
|
||||
break;
|
||||
case VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH);
|
||||
break;
|
||||
case VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_OFFSET_CAL_RATE_TOO_HIGH);
|
||||
break;
|
||||
case VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW);
|
||||
break;
|
||||
|
||||
case VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_ZONE_CAL_MISSING_SAMPLES);
|
||||
break;
|
||||
case VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_ZONE_CAL_SIGMA_TOO_HIGH);
|
||||
break;
|
||||
case VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_ZONE_CAL_RATE_TOO_HIGH);
|
||||
break;
|
||||
|
||||
case VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS);
|
||||
break;
|
||||
case VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH);
|
||||
break;
|
||||
case VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW);
|
||||
break;
|
||||
|
||||
case VL53L1_WARNING_XTALK_MISSING_SAMPLES:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_XTALK_MISSING_SAMPLES);
|
||||
break;
|
||||
case VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT);
|
||||
break;
|
||||
case VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT);
|
||||
break;
|
||||
|
||||
case VL53L1_ERROR_DEVICE_FIRMWARE_TOO_OLD:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_DEVICE_FIRMWARE_TOO_OLD);
|
||||
break;
|
||||
case VL53L1_ERROR_DEVICE_FIRMWARE_TOO_NEW:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_DEVICE_FIRMWARE_TOO_NEW);
|
||||
break;
|
||||
case VL53L1_ERROR_UNIT_TEST_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_UNIT_TEST_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_FILE_READ_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_FILE_READ_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_FILE_WRITE_FAIL:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_FILE_WRITE_FAIL);
|
||||
break;
|
||||
case VL53L1_ERROR_NOT_IMPLEMENTED:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_ERROR_NOT_IMPLEMENTED);
|
||||
break;
|
||||
default:
|
||||
VL53L1_COPYSTRING(pPalErrorString,
|
||||
VL53L1_STRING_UNKNOW_ERROR_CODE);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
LOG_FUNCTION_END(Status);
|
||||
|
||||
return Status;
|
||||
}
|
||||
4489
App/VL53L1X_API/core/src/vl53l1_register_funcs.c
Normal file
4489
App/VL53L1X_API/core/src/vl53l1_register_funcs.c
Normal file
File diff suppressed because it is too large
Load Diff
146
App/VL53L1X_API/core/src/vl53l1_silicon_core.c
Normal file
146
App/VL53L1X_API/core/src/vl53l1_silicon_core.c
Normal file
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_silicon_core.c
|
||||
*
|
||||
* @brief EwokPlus25 low level silicon LL Driver function definition
|
||||
*/
|
||||
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_platform.h"
|
||||
#include "vl53l1_register_map.h"
|
||||
#include "vl53l1_core.h"
|
||||
#include "vl53l1_silicon_core.h"
|
||||
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, fmt, ##__VA_ARGS__)
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_is_firmware_ready_silicon(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready)
|
||||
{
|
||||
/**
|
||||
* Determines if the firmware is ready to range
|
||||
*
|
||||
* There are 2 different behaviors depending on whether
|
||||
* power force is enabled or not
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t comms_buffer[5];
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/* read interrupt and power force reset status */
|
||||
|
||||
status = VL53L1_ReadMulti(
|
||||
Dev,
|
||||
VL53L1_INTERRUPT_MANAGER__ENABLES,
|
||||
comms_buffer,
|
||||
5);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
|
||||
pdev->dbg_results.interrupt_manager__enables =
|
||||
comms_buffer[0];
|
||||
pdev->dbg_results.interrupt_manager__clear =
|
||||
comms_buffer[1];
|
||||
pdev->dbg_results.interrupt_manager__status =
|
||||
comms_buffer[2];
|
||||
pdev->dbg_results.mcu_to_host_bank__wr_access_en =
|
||||
comms_buffer[3];
|
||||
pdev->dbg_results.power_management__go1_reset_status =
|
||||
comms_buffer[4];
|
||||
|
||||
if ((pdev->sys_ctrl.power_management__go1_power_force & 0x01) == 0x01) {
|
||||
|
||||
if (((pdev->dbg_results.interrupt_manager__enables & 0x1F) == 0x1F) &&
|
||||
((pdev->dbg_results.interrupt_manager__clear & 0x1F) == 0x1F))
|
||||
*pready = 0x01;
|
||||
else
|
||||
*pready = 0x00;
|
||||
|
||||
} else {
|
||||
|
||||
/* set ready flag if bit 0 is zero i.g G01 is in reset */
|
||||
if ((pdev->dbg_results.power_management__go1_reset_status & 0x01) == 0x00)
|
||||
*pready = 0x01;
|
||||
else
|
||||
*pready = 0x00;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
558
App/VL53L1X_API/core/src/vl53l1_wait.c
Normal file
558
App/VL53L1X_API/core/src/vl53l1_wait.c
Normal file
@@ -0,0 +1,558 @@
|
||||
/*
|
||||
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
|
||||
*
|
||||
* This file is part of VL53L1 Core and is dual licensed,
|
||||
* either 'STMicroelectronics
|
||||
* Proprietary license'
|
||||
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* 'STMicroelectronics Proprietary license'
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: STMicroelectronics Proprietary in accordance with licensing
|
||||
* terms at www.st.com/sla0081
|
||||
*
|
||||
* STMicroelectronics confidential
|
||||
* Reproduction and Communication of this document is strictly prohibited unless
|
||||
* specifically authorized in writing by STMicroelectronics.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* Alternatively, VL53L1 Core may be distributed under the terms of
|
||||
* 'BSD 3-clause "New" or "Revised" License', in which case the following
|
||||
* provisions apply instead of the ones mentioned above :
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
* License terms: BSD 3-clause "New" or "Revised" License.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* 3. Neither the name of the copyright holder nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*
|
||||
********************************************************************************
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_wait.c
|
||||
*
|
||||
* @brief EwokPlus25 low level Driver wait function definition
|
||||
*/
|
||||
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_ll_device.h"
|
||||
#include "vl53l1_platform.h"
|
||||
#include "vl53l1_core.h"
|
||||
#include "vl53l1_silicon_core.h"
|
||||
#include "vl53l1_wait.h"
|
||||
#include "vl53l1_register_settings.h"
|
||||
|
||||
|
||||
#define LOG_FUNCTION_START(fmt, ...) \
|
||||
_LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END(status, ...) \
|
||||
_LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
|
||||
#define LOG_FUNCTION_END_FMT(status, fmt, ...) \
|
||||
_LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \
|
||||
fmt, ##__VA_ARGS__)
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_boot_completion(
|
||||
VL53L1_DEV Dev)
|
||||
{
|
||||
|
||||
/* Waits for firmware boot to finish
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t fw_ready = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
if (pdev->wait_method == VL53L1_WAIT_METHOD_BLOCKING) {
|
||||
|
||||
/* blocking version */
|
||||
|
||||
status =
|
||||
VL53L1_poll_for_boot_completion(
|
||||
Dev,
|
||||
VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS);
|
||||
|
||||
} else {
|
||||
|
||||
/* implement non blocking version below */
|
||||
|
||||
fw_ready = 0;
|
||||
while (fw_ready == 0x00 && status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_is_boot_complete(
|
||||
Dev,
|
||||
&fw_ready);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_WaitMs(
|
||||
Dev,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_firmware_ready(
|
||||
VL53L1_DEV Dev)
|
||||
{
|
||||
|
||||
/* If in timed mode or single shot then check firmware is ready
|
||||
* before sending handshake
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t fw_ready = 0;
|
||||
uint8_t mode_start = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/* Filter out tje measure mode part of the mode
|
||||
* start register
|
||||
*/
|
||||
mode_start =
|
||||
pdev->sys_ctrl.system__mode_start &
|
||||
VL53L1_DEVICEMEASUREMENTMODE_MODE_MASK;
|
||||
|
||||
/*
|
||||
* conditional wait for firmware ready
|
||||
* only waits for timed and single shot modes
|
||||
*/
|
||||
|
||||
if ((mode_start == VL53L1_DEVICEMEASUREMENTMODE_TIMED) ||
|
||||
(mode_start == VL53L1_DEVICEMEASUREMENTMODE_SINGLESHOT)) {
|
||||
|
||||
if (pdev->wait_method == VL53L1_WAIT_METHOD_BLOCKING) {
|
||||
|
||||
/* blocking version */
|
||||
|
||||
status =
|
||||
VL53L1_poll_for_firmware_ready(
|
||||
Dev,
|
||||
VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
|
||||
|
||||
} else {
|
||||
|
||||
/* implement non blocking version below */
|
||||
|
||||
fw_ready = 0;
|
||||
while (fw_ready == 0x00 && status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_is_firmware_ready(
|
||||
Dev,
|
||||
&fw_ready);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_WaitMs(
|
||||
Dev,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_range_completion(
|
||||
VL53L1_DEV Dev)
|
||||
{
|
||||
|
||||
/* Wrapper function for waiting for range completion
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t data_ready = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
if (pdev->wait_method == VL53L1_WAIT_METHOD_BLOCKING) {
|
||||
|
||||
/* blocking version */
|
||||
|
||||
status =
|
||||
VL53L1_poll_for_range_completion(
|
||||
Dev,
|
||||
VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS);
|
||||
|
||||
} else {
|
||||
|
||||
/* implement non blocking version below */
|
||||
|
||||
data_ready = 0;
|
||||
while (data_ready == 0x00 && status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_is_new_data_ready(
|
||||
Dev,
|
||||
&data_ready);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_WaitMs(
|
||||
Dev,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_wait_for_test_completion(
|
||||
VL53L1_DEV Dev)
|
||||
{
|
||||
|
||||
/* Wrapper function for waiting for test mode completion
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t data_ready = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
if (pdev->wait_method == VL53L1_WAIT_METHOD_BLOCKING) {
|
||||
|
||||
/* blocking version */
|
||||
|
||||
status =
|
||||
VL53L1_poll_for_range_completion(
|
||||
Dev,
|
||||
VL53L1_TEST_COMPLETION_POLLING_TIMEOUT_MS);
|
||||
|
||||
} else {
|
||||
|
||||
/* implement non blocking version below */
|
||||
|
||||
data_ready = 0;
|
||||
while (data_ready == 0x00 && status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_is_new_data_ready(
|
||||
Dev,
|
||||
&data_ready);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_WaitMs(
|
||||
Dev,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_is_boot_complete(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready)
|
||||
{
|
||||
/**
|
||||
* Determines if the firmware finished booting by reading
|
||||
* bit 0 of firmware__system_status register
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
uint8_t firmware__system_status = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/* read current range interrupt state */
|
||||
|
||||
status =
|
||||
VL53L1_RdByte(
|
||||
Dev,
|
||||
VL53L1_FIRMWARE__SYSTEM_STATUS,
|
||||
&firmware__system_status);
|
||||
|
||||
/* set *pready = 1 if new range data ready complete
|
||||
* zero otherwise
|
||||
*/
|
||||
|
||||
if ((firmware__system_status & 0x01) == 0x01) {
|
||||
*pready = 0x01;
|
||||
VL53L1_init_ll_driver_state(
|
||||
Dev,
|
||||
VL53L1_DEVICESTATE_SW_STANDBY);
|
||||
} else {
|
||||
*pready = 0x00;
|
||||
VL53L1_init_ll_driver_state(
|
||||
Dev,
|
||||
VL53L1_DEVICESTATE_FW_COLDBOOT);
|
||||
}
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_is_firmware_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready)
|
||||
{
|
||||
/**
|
||||
* Determines if the firmware is ready to range
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
status = VL53L1_is_firmware_ready_silicon(
|
||||
Dev,
|
||||
pready);
|
||||
|
||||
pdev->fw_ready = *pready;
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_is_new_data_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint8_t *pready)
|
||||
{
|
||||
/**
|
||||
* Determines if new range data is ready by reading bit 0 of
|
||||
* VL53L1_GPIO__TIO_HV_STATUS to determine the current state
|
||||
* of output interrupt pin
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t gpio__mux_active_high_hv = 0;
|
||||
uint8_t gpio__tio_hv_status = 0;
|
||||
uint8_t interrupt_ready = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
gpio__mux_active_high_hv =
|
||||
pdev->stat_cfg.gpio_hv_mux__ctrl &
|
||||
VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_MASK;
|
||||
|
||||
if (gpio__mux_active_high_hv == VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_HIGH)
|
||||
interrupt_ready = 0x01;
|
||||
else
|
||||
interrupt_ready = 0x00;
|
||||
|
||||
/* read current range interrupt state */
|
||||
|
||||
status = VL53L1_RdByte(
|
||||
Dev,
|
||||
VL53L1_GPIO__TIO_HV_STATUS,
|
||||
&gpio__tio_hv_status);
|
||||
|
||||
/* set *pready = 1 if new range data ready complete zero otherwise */
|
||||
|
||||
if ((gpio__tio_hv_status & 0x01) == interrupt_ready)
|
||||
*pready = 0x01;
|
||||
else
|
||||
*pready = 0x00;
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_boot_completion(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms)
|
||||
{
|
||||
/**
|
||||
* Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if
|
||||
* the firmware is ready.
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
/* after reset for the firmware blocks I2C access while
|
||||
* it copies the NVM data into the G02 host register banks
|
||||
* The host must wait the required time to allow the copy
|
||||
* to complete before attempting to read the firmware status
|
||||
*/
|
||||
|
||||
status = VL53L1_WaitUs(
|
||||
Dev,
|
||||
VL53L1_FIRMWARE_BOOT_TIME_US);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status =
|
||||
VL53L1_WaitValueMaskEx(
|
||||
Dev,
|
||||
timeout_ms,
|
||||
VL53L1_FIRMWARE__SYSTEM_STATUS,
|
||||
0x01,
|
||||
0x01,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
VL53L1_init_ll_driver_state(Dev, VL53L1_DEVICESTATE_SW_STANDBY);
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_firmware_ready(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms)
|
||||
{
|
||||
/**
|
||||
* Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if
|
||||
* the firmware is ready.
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint32_t start_time_ms = 0;
|
||||
uint32_t current_time_ms = 0;
|
||||
int32_t poll_delay_ms = VL53L1_POLLING_DELAY_MS;
|
||||
uint8_t fw_ready = 0;
|
||||
|
||||
/* calculate time limit in absolute time */
|
||||
|
||||
VL53L1_GetTickCount(Dev, &start_time_ms); /*lint !e534 ignoring return*/
|
||||
pdev->fw_ready_poll_duration_ms = 0;
|
||||
|
||||
/* wait until firmware is ready, timeout reached on error occurred */
|
||||
|
||||
while ((status == VL53L1_ERROR_NONE) &&
|
||||
(pdev->fw_ready_poll_duration_ms < timeout_ms) &&
|
||||
(fw_ready == 0)) {
|
||||
|
||||
status = VL53L1_is_firmware_ready(
|
||||
Dev,
|
||||
&fw_ready);
|
||||
|
||||
if (status == VL53L1_ERROR_NONE &&
|
||||
fw_ready == 0 &&
|
||||
poll_delay_ms > 0) {
|
||||
status = VL53L1_WaitMs(
|
||||
Dev,
|
||||
poll_delay_ms);
|
||||
}
|
||||
|
||||
/*
|
||||
* Update polling time (Compare difference rather than
|
||||
* absolute to negate 32bit wrap around issue)
|
||||
*/
|
||||
VL53L1_GetTickCount(Dev, ¤t_time_ms); /*lint !e534 ignoring return*/
|
||||
pdev->fw_ready_poll_duration_ms =
|
||||
current_time_ms - start_time_ms;
|
||||
}
|
||||
|
||||
if (fw_ready == 0 && status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_ERROR_TIME_OUT;
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_poll_for_range_completion(
|
||||
VL53L1_DEV Dev,
|
||||
uint32_t timeout_ms)
|
||||
{
|
||||
/**
|
||||
* Polls bit 0 of VL53L1_GPIO__TIO_HV_STATUS to determine
|
||||
* the state of output interrupt pin
|
||||
*
|
||||
* Interrupt may be either active high or active low. Use active_high to
|
||||
* select the required level check
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
VL53L1_LLDriverData_t *pdev = VL53L1DevStructGetLLDriverHandle(Dev);
|
||||
|
||||
uint8_t gpio__mux_active_high_hv = 0;
|
||||
uint8_t interrupt_ready = 0;
|
||||
|
||||
LOG_FUNCTION_START("");
|
||||
|
||||
gpio__mux_active_high_hv =
|
||||
pdev->stat_cfg.gpio_hv_mux__ctrl &
|
||||
VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_MASK;
|
||||
|
||||
if (gpio__mux_active_high_hv == VL53L1_DEVICEINTERRUPTLEVEL_ACTIVE_HIGH)
|
||||
interrupt_ready = 0x01;
|
||||
else
|
||||
interrupt_ready = 0x00;
|
||||
|
||||
status =
|
||||
VL53L1_WaitValueMaskEx(
|
||||
Dev,
|
||||
timeout_ms,
|
||||
VL53L1_GPIO__TIO_HV_STATUS,
|
||||
interrupt_ready,
|
||||
0x01,
|
||||
VL53L1_POLLING_DELAY_MS);
|
||||
|
||||
LOG_FUNCTION_END(status);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
469
App/VL53L1X_API/platform/inc/vl53l1_platform.h
Normal file
469
App/VL53L1X_API/platform/inc/vl53l1_platform.h
Normal file
@@ -0,0 +1,469 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_H_
|
||||
#define _VL53L1_PLATFORM_H_
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_platform_log.h"
|
||||
|
||||
#define VL53L1_IPP_API
|
||||
#include "vl53l1_platform_ipp_imports.h"
|
||||
#include "vl53l1_platform_user_data.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#ifndef VL53_USE_FREERTOS_DELAY
|
||||
#define VL53_USE_FREERTOS_DELAY 1
|
||||
#endif
|
||||
|
||||
#ifndef VL53L1_DEFAULT_ADDR_8BIT
|
||||
#define VL53L1_DEFAULT_ADDR_8BIT 0x52u
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform.h
|
||||
*
|
||||
* @brief All end user OS/platform/application porting
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initialise platform comms.
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] comms_type : selects between I2C and SPI
|
||||
* @param[in] comms_speed_khz : unsigned short containing the I2C speed in kHz
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_CommsInitialise(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t comms_type,
|
||||
uint16_t comms_speed_khz);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Close platform comms.
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_CommsClose(
|
||||
VL53L1_Dev_t *pdev);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Writes the supplied byte buffer to the device
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[in] pdata : pointer to uint8_t (byte) buffer containing the data to be written
|
||||
* @param[in] count : number of bytes in the supplied byte buffer
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WriteMulti(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata,
|
||||
uint32_t count);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads the requested number of bytes from the device
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[out] pdata : pointer to the uint8_t (byte) buffer to store read data
|
||||
* @param[in] count : number of bytes to read
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_ReadMulti(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata,
|
||||
uint32_t count);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Writes a single byte to the device
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[in] data : uint8_t data value to write
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WrByte(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t data);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Writes a single word (16-bit unsigned) to the device
|
||||
*
|
||||
* Manages the big-endian nature of the device register map
|
||||
* (first byte written is the MS byte).
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[in] data : uin16_t data value write
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WrWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint16_t data);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Writes a single dword (32-bit unsigned) to the device
|
||||
*
|
||||
* Manages the big-endian nature of the device register map
|
||||
* (first byte written is the MS byte).
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[in] data : uint32_t data value to write
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WrDWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint32_t data);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads a single byte from the device
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index
|
||||
* @param[out] pdata : pointer to uint8_t data value
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_RdByte(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads a single word (16-bit unsigned) from the device
|
||||
*
|
||||
* Manages the big-endian nature of the device (first byte read is the MS byte).
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[out] pdata : pointer to uint16_t data value
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_RdWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint16_t *pdata);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Reads a single dword (32-bit unsigned) from the device
|
||||
*
|
||||
* Manages the big-endian nature of the device (first byte read is the MS byte).
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[out] pdata : pointer to uint32_t data value
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_RdDWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint32_t *pdata);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Implements a programmable wait in us
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] wait_us : integer wait in micro seconds
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WaitUs(
|
||||
VL53L1_Dev_t *pdev,
|
||||
int32_t wait_us);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Implements a programmable wait in ms
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] wait_ms : integer wait in milliseconds
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WaitMs(
|
||||
VL53L1_Dev_t *pdev,
|
||||
int32_t wait_ms);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the frequency of the timer used for ranging results time stamps
|
||||
*
|
||||
* @param[out] ptimer_freq_hz : pointer for timer frequency
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GetTimerFrequency(int32_t *ptimer_freq_hz);
|
||||
|
||||
/**
|
||||
* @brief Get the timer value in units of timer_freq_hz (see VL53L1_get_timestamp_frequency())
|
||||
*
|
||||
* @param[out] ptimer_count : pointer for timer count value
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GetTimerValue(int32_t *ptimer_count);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the mode of a specified GPIO pin
|
||||
*
|
||||
* @param pin - an identifier specifying the pin being modified - defined per platform
|
||||
*
|
||||
* @param mode - an identifier specifying the requested mode - defined per platform
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioSetMode(uint8_t pin, uint8_t mode);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the value of a specified GPIO pin
|
||||
*
|
||||
* @param pin - an identifier specifying the pin being modified - defined per platform
|
||||
*
|
||||
* @param value - a value to set on the GPIO pin - typically 0 or 1
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioSetValue(uint8_t pin, uint8_t value);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the value of a specified GPIO pin
|
||||
*
|
||||
* @param pin - an identifier specifying the pin being modified - defined per platform
|
||||
*
|
||||
* @param pvalue - a value retrieved from the GPIO pin - typically 0 or 1
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioGetValue(uint8_t pin, uint8_t *pvalue);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Sets and clears the XShutdown pin on the Ewok
|
||||
*
|
||||
* @param value - the value for xshutdown - 0 = in reset, 1 = operational
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioXshutdown(uint8_t value);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Sets and clears the Comms Mode pin (NCS) on the Ewok
|
||||
*
|
||||
* @param value - the value for comms select - 0 = I2C, 1 = SPI
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioCommsSelect(uint8_t value);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Enables and disables the power to the Ewok module
|
||||
*
|
||||
* @param value - the state of the power supply - 0 = power off, 1 = power on
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioPowerEnable(uint8_t value);
|
||||
|
||||
/**
|
||||
* @brief Enables callbacks to the supplied funtion pointer when Ewok interrupts ocurr
|
||||
*
|
||||
* @param function - a function callback supplies by the caller, for interrupt notification
|
||||
* @param edge_type - falling edge or rising edge interrupt detection
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioInterruptEnable(void (*function)(void), uint8_t edge_type);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Disables the callback on Ewok interrupts
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GpioInterruptDisable(void);
|
||||
|
||||
|
||||
/*
|
||||
* @brief Gets current system tick count in [ms]
|
||||
*
|
||||
* @return time_ms : current time in [ms]
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_GetTickCount(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t *ptime_ms);
|
||||
|
||||
void VL53L1_PlatformAttachBus(
|
||||
VL53L1_Dev_t *pdev,
|
||||
I2C_HandleTypeDef *hi2c,
|
||||
uint8_t i2c_addr_8bit,
|
||||
uint16_t io_timeout_ms,
|
||||
void *bus_lock);
|
||||
|
||||
void VL53L1_PlatformAttachPins(
|
||||
VL53L1_Dev_t *pdev,
|
||||
GPIO_TypeDef *xshut_port,
|
||||
uint16_t xshut_pin,
|
||||
GPIO_TypeDef *gpio1_port,
|
||||
uint16_t gpio1_pin);
|
||||
|
||||
VL53L1_Error VL53L1_PlatformSetXShut(
|
||||
VL53L1_Dev_t *pdev,
|
||||
GPIO_PinState state);
|
||||
|
||||
VL53L1_Error VL53L1_PlatformBootDevice(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t reset_low_ms,
|
||||
uint32_t boot_wait_ms);
|
||||
|
||||
VL53L1_Error VL53L1_PlatformChangeAddress(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t new_addr_8bit);
|
||||
|
||||
uint32_t VL53L1_PlatformGetTick(void);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Register "wait for value" polling routine
|
||||
*
|
||||
* Port of the V2WReg Script function WaitValueMaskEx()
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] timeout_ms : timeout in [ms]
|
||||
* @param[in] index : uint16_t register index value
|
||||
* @param[in] value : value to wait for
|
||||
* @param[in] mask : mask to be applied before comparison with value
|
||||
* @param[in] poll_delay_ms : polling delay been each read transaction in [ms]
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_WaitValueMaskEx(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t timeout_ms,
|
||||
uint16_t index,
|
||||
uint8_t value,
|
||||
uint8_t mask,
|
||||
uint32_t poll_delay_ms);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
90
App/VL53L1X_API/platform/inc/vl53l1_platform_init.h
Normal file
90
App/VL53L1X_API/platform/inc/vl53l1_platform_init.h
Normal file
@@ -0,0 +1,90 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_INIT_H_
|
||||
#define _VL53L1_PLATFORM_INIT_H_
|
||||
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_init.h
|
||||
*
|
||||
* @brief EwokPlus25 comms and GPIO init
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initialise platform comms, GPIO and reset device
|
||||
*
|
||||
* Initialises comms, sets the states of GPIO (xshutdown, ncs,
|
||||
* EVK device power regulator enable) and resets the device
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
* @param[in] i2c_slave_address : I2C slave address
|
||||
* @param[in] comms_type : Comms type: VL53L1_I2C or VL53L1_SPI
|
||||
* @param[in] comms_speed_khz : 400kHz recommended for I2C
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_platform_init(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t i2c_slave_address,
|
||||
uint8_t comms_type,
|
||||
uint16_t comms_speed_khz);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Close platform comms and GPIO
|
||||
*
|
||||
* Puts the device into reset, disables the EVK device power regulator
|
||||
* and closes comms
|
||||
*
|
||||
* @param[in] pdev : pointer to device structure (device handle)
|
||||
*
|
||||
* @return VL53L1_ERROR_NONE Success
|
||||
* @return "Other error code" See ::VL53L1_Error
|
||||
*/
|
||||
|
||||
VL53L1_Error VL53L1_platform_terminate(
|
||||
VL53L1_Dev_t *pdev);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,6 @@
|
||||
|
||||
#ifdef VL53L1_NEEDS_IPP
|
||||
# undef VL53L1_IPP_API
|
||||
# define VL53L1_IPP_API __declspec(dllimport)
|
||||
# pragma comment (lib, "EwokPlus25API_IPP")
|
||||
#endif
|
||||
223
App/VL53L1X_API/platform/inc/vl53l1_platform_log.h
Normal file
223
App/VL53L1X_API/platform/inc/vl53l1_platform_log.h
Normal file
@@ -0,0 +1,223 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2015, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
********************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_log.h
|
||||
*
|
||||
* @brief EwokPlus25 platform logging function definition
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_LOG_H_
|
||||
#define _VL53L1_PLATFORM_LOG_H_
|
||||
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
#include "vl53l1_platform_user_config.h"
|
||||
|
||||
#ifdef _MSC_VER
|
||||
# define EWOKPLUS_EXPORTS __declspec(dllexport)
|
||||
#else
|
||||
# define EWOKPLUS_EXPORTS
|
||||
#endif
|
||||
|
||||
#include "vl53l1_types.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <time.h>
|
||||
|
||||
/**
|
||||
* @brief Set the level, output and specific functions for module logging.
|
||||
*
|
||||
*
|
||||
* @param filename - full path of output log file, NULL for print to stdout
|
||||
*
|
||||
* @param modules - Module or None or All to trace
|
||||
* VL53L1_TRACE_MODULE_NONE
|
||||
* VL53L1_TRACE_MODULE_API
|
||||
* VL53L1_TRACE_MODULE_CORE
|
||||
* VL53L1_TRACE_MODULE_TUNING
|
||||
* VL53L1_TRACE_MODULE_CHARACTERISATION
|
||||
* VL53L1_TRACE_MODULE_PLATFORM
|
||||
* VL53L1_TRACE_MODULE_ALL
|
||||
*
|
||||
* @param level - trace level
|
||||
* VL53L1_TRACE_LEVEL_NONE
|
||||
* VL53L1_TRACE_LEVEL_ERRORS
|
||||
* VL53L1_TRACE_LEVEL_WARNING
|
||||
* VL53L1_TRACE_LEVEL_INFO
|
||||
* VL53L1_TRACE_LEVEL_DEBUG
|
||||
* VL53L1_TRACE_LEVEL_ALL
|
||||
* VL53L1_TRACE_LEVEL_IGNORE
|
||||
*
|
||||
* @param functions - function level to trace;
|
||||
* VL53L1_TRACE_FUNCTION_NONE
|
||||
* VL53L1_TRACE_FUNCTION_I2C
|
||||
* VL53L1_TRACE_FUNCTION_ALL
|
||||
*
|
||||
* @return status - always VL53L1_ERROR_NONE
|
||||
*
|
||||
*/
|
||||
|
||||
#define VL53L1_TRACE_LEVEL_NONE 0x00000000
|
||||
#define VL53L1_TRACE_LEVEL_ERRORS 0x00000001
|
||||
#define VL53L1_TRACE_LEVEL_WARNING 0x00000002
|
||||
#define VL53L1_TRACE_LEVEL_INFO 0x00000004
|
||||
#define VL53L1_TRACE_LEVEL_DEBUG 0x00000008
|
||||
#define VL53L1_TRACE_LEVEL_ALL 0x00000010
|
||||
#define VL53L1_TRACE_LEVEL_IGNORE 0x00000020
|
||||
|
||||
#define VL53L1_TRACE_FUNCTION_NONE 0x00000000
|
||||
#define VL53L1_TRACE_FUNCTION_I2C 0x00000001
|
||||
#define VL53L1_TRACE_FUNCTION_ALL 0x7fffffff
|
||||
|
||||
#define VL53L1_TRACE_MODULE_NONE 0x00000000
|
||||
#define VL53L1_TRACE_MODULE_API 0x00000001
|
||||
#define VL53L1_TRACE_MODULE_CORE 0x00000002
|
||||
#define VL53L1_TRACE_MODULE_PROTECTED 0x00000004
|
||||
#define VL53L1_TRACE_MODULE_HISTOGRAM 0x00000008
|
||||
#define VL53L1_TRACE_MODULE_REGISTERS 0x00000010
|
||||
#define VL53L1_TRACE_MODULE_PLATFORM 0x00000020
|
||||
#define VL53L1_TRACE_MODULE_NVM 0x00000040
|
||||
#define VL53L1_TRACE_MODULE_CALIBRATION_DATA 0x00000080
|
||||
#define VL53L1_TRACE_MODULE_NVM_DATA 0x00000100
|
||||
#define VL53L1_TRACE_MODULE_HISTOGRAM_DATA 0x00000200
|
||||
#define VL53L1_TRACE_MODULE_RANGE_RESULTS_DATA 0x00000400
|
||||
#define VL53L1_TRACE_MODULE_XTALK_DATA 0x00000800
|
||||
#define VL53L1_TRACE_MODULE_OFFSET_DATA 0x00001000
|
||||
#define VL53L1_TRACE_MODULE_DATA_INIT 0x00002000
|
||||
#define VL53L1_TRACE_MODULE_REF_SPAD_CHAR 0x00004000
|
||||
#define VL53L1_TRACE_MODULE_SPAD_RATE_MAP 0x00008000
|
||||
#ifdef PAL_EXTENDED
|
||||
#define VL53L1_TRACE_MODULE_SPAD 0x01000000
|
||||
#define VL53L1_TRACE_MODULE_FMT 0x02000000
|
||||
#define VL53L1_TRACE_MODULE_UTILS 0x04000000
|
||||
#define VL53L1_TRACE_MODULE_BENCH_FUNCS 0x08000000
|
||||
#endif
|
||||
#define VL53L1_TRACE_MODULE_CUSTOMER_API 0x40000000
|
||||
#define VL53L1_TRACE_MODULE_ALL 0x7fffffff
|
||||
|
||||
|
||||
extern uint32_t _trace_level;
|
||||
|
||||
/*
|
||||
* NOTE: dynamically exported if we enable logging.
|
||||
* this way, Python interfaces can access this function, but we don't
|
||||
* need to include it in the .def files.
|
||||
*/
|
||||
EWOKPLUS_EXPORTS int8_t VL53L1_trace_config(
|
||||
char *filename,
|
||||
uint32_t modules,
|
||||
uint32_t level,
|
||||
uint32_t functions);
|
||||
|
||||
/**
|
||||
* @brief Print trace module function.
|
||||
*
|
||||
* @param module - ??
|
||||
* @param level - ??
|
||||
* @param function - ??
|
||||
* @param format - ??
|
||||
*
|
||||
*/
|
||||
|
||||
EWOKPLUS_EXPORTS void VL53L1_trace_print_module_function(
|
||||
uint32_t module,
|
||||
uint32_t level,
|
||||
uint32_t function,
|
||||
const char *format, ...);
|
||||
|
||||
/**
|
||||
* @brief Get global _trace_functions parameter
|
||||
*
|
||||
* @return _trace_functions
|
||||
*/
|
||||
|
||||
uint32_t VL53L1_get_trace_functions(void);
|
||||
|
||||
/**
|
||||
* @brief Set global _trace_functions parameter
|
||||
*
|
||||
* @param[in] function : new function code
|
||||
*/
|
||||
|
||||
void VL53L1_set_trace_functions(uint32_t function);
|
||||
|
||||
|
||||
/*
|
||||
* @brief Returns the current system tick count in [ms]
|
||||
*
|
||||
* @return time_ms : current time in [ms]
|
||||
*
|
||||
*/
|
||||
|
||||
uint32_t VL53L1_clock(void);
|
||||
|
||||
#define LOG_GET_TIME() \
|
||||
((int)VL53L1_clock())
|
||||
|
||||
#define _LOG_TRACE_PRINT(module, level, function, ...) \
|
||||
VL53L1_trace_print_module_function(module, level, function, ##__VA_ARGS__);
|
||||
|
||||
#define _LOG_FUNCTION_START(module, fmt, ...) \
|
||||
VL53L1_trace_print_module_function(module, _trace_level, VL53L1_TRACE_FUNCTION_ALL, "%6ld <START> %s "fmt"\n", LOG_GET_TIME(), __FUNCTION__, ##__VA_ARGS__);
|
||||
|
||||
#define _LOG_FUNCTION_END(module, status, ...)\
|
||||
VL53L1_trace_print_module_function(module, _trace_level, VL53L1_TRACE_FUNCTION_ALL, "%6ld <END> %s %d\n", LOG_GET_TIME(), __FUNCTION__, (int)status, ##__VA_ARGS__)
|
||||
|
||||
#define _LOG_FUNCTION_END_FMT(module, status, fmt, ...)\
|
||||
VL53L1_trace_print_module_function(module, _trace_level, VL53L1_TRACE_FUNCTION_ALL, "%6ld <END> %s %d "fmt"\n", LOG_GET_TIME(), __FUNCTION__, (int)status, ##__VA_ARGS__)
|
||||
|
||||
#define _LOG_GET_TRACE_FUNCTIONS()\
|
||||
VL53L1_get_trace_functions()
|
||||
|
||||
#define _LOG_SET_TRACE_FUNCTIONS(functions)\
|
||||
VL53L1_set_trace_functions(functions)
|
||||
|
||||
#define _LOG_STRING_BUFFER(x) char x[VL53L1_MAX_STRING_LENGTH]
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#else /* VL53L1_LOG_ENABLE - no logging */
|
||||
|
||||
#define _LOG_TRACE_PRINT(module, level, function, ...)
|
||||
#define _LOG_FUNCTION_START(module, fmt, ...)
|
||||
#define _LOG_FUNCTION_END(module, status, ...)
|
||||
#define _LOG_FUNCTION_END_FMT(module, status, fmt, ...)
|
||||
#define _LOG_GET_TRACE_FUNCTIONS() 0
|
||||
#define _LOG_SET_TRACE_FUNCTIONS(functions)
|
||||
#define _LOG_STRING_BUFFER(x)
|
||||
|
||||
#endif /* VL53L1_LOG_ENABLE */
|
||||
|
||||
#endif /* _VL53L1_PLATFORM_LOG_H_ */
|
||||
84
App/VL53L1X_API/platform/inc/vl53l1_platform_user_config.h
Normal file
84
App/VL53L1X_API/platform/inc/vl53l1_platform_user_config.h
Normal file
@@ -0,0 +1,84 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2015, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
********************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_user_config.h
|
||||
*
|
||||
* @brief EwokPlus compile time user modifiable configuration
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_USER_CONFIG_H_
|
||||
#define _VL53L1_PLATFORM_USER_CONFIG_H_
|
||||
|
||||
#define VL53L1_BYTES_PER_WORD 2
|
||||
#define VL53L1_BYTES_PER_DWORD 4
|
||||
|
||||
/* Define polling delays */
|
||||
#define VL53L1_BOOT_COMPLETION_POLLING_TIMEOUT_MS 500
|
||||
#define VL53L1_RANGE_COMPLETION_POLLING_TIMEOUT_MS 2000
|
||||
#define VL53L1_TEST_COMPLETION_POLLING_TIMEOUT_MS 60000
|
||||
|
||||
#define VL53L1_POLLING_DELAY_MS 1
|
||||
|
||||
/* Define LLD TuningParms Page Base Address
|
||||
* - Part of Patch_AddedTuningParms_11761
|
||||
*/
|
||||
#define VL53L1_TUNINGPARM_PUBLIC_PAGE_BASE_ADDRESS 0x8000
|
||||
#define VL53L1_TUNINGPARM_PRIVATE_PAGE_BASE_ADDRESS 0xC000
|
||||
|
||||
#define VL53L1_GAIN_FACTOR__STANDARD_DEFAULT 0x0800
|
||||
/*!< Default standard ranging gain correction factor
|
||||
1.11 format. 1.0 = 0x0800, 0.980 = 0x07D7 */
|
||||
|
||||
#define VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS 0x0500
|
||||
/*!< Lower Limit for the MM1 effective SPAD count during offset
|
||||
calibration Format 8.8 0x0500 -> 5.0 effective SPADs */
|
||||
|
||||
#define VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS 0x1900
|
||||
/*!< Max Limit for the pre range peak rate during offset
|
||||
calibration Format 9.7 0x1900 -> 50.0 Mcps.
|
||||
If larger then in pile up */
|
||||
|
||||
#define VL53L1_OFFSET_CAL_MAX_SIGMA_MM 0x0040
|
||||
/*!< Max sigma estimate limit during offset calibration
|
||||
Check applies to pre-range, mm1 and mm2 ranges
|
||||
Format 14.2 0x0040 -> 16.0mm. */
|
||||
|
||||
#define VL53L1_MAX_USER_ZONES 1
|
||||
/*!< Max number of user Zones - maximal limitation from
|
||||
FW stream divide - value of 254 */
|
||||
|
||||
#define VL53L1_MAX_RANGE_RESULTS 2
|
||||
/*!< Allocates storage for return and reference restults */
|
||||
|
||||
|
||||
#define VL53L1_MAX_STRING_LENGTH 512
|
||||
|
||||
#endif /* _VL53L1_PLATFORM_USER_CONFIG_H_ */
|
||||
|
||||
135
App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h
Normal file
135
App/VL53L1X_API/platform/inc/vl53l1_platform_user_data.h
Normal file
@@ -0,0 +1,135 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_USER_DATA_H_
|
||||
#define _VL53L1_PLATFORM_USER_DATA_H_
|
||||
|
||||
#ifndef __KERNEL__
|
||||
#include <stdlib.h>
|
||||
#endif
|
||||
|
||||
#include "main.h"
|
||||
#include "vl53l1_def.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_user_data.h
|
||||
*
|
||||
* @brief All end user OS/platform/application porting
|
||||
*/
|
||||
|
||||
/** @brief Contains the current state and internal values of the API
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
|
||||
VL53L1_DevData_t Data;
|
||||
/*!< Low Level Driver data structure */
|
||||
|
||||
uint8_t i2c_slave_address;
|
||||
/*!< ST API expects the 8-bit I2C address, default 0x52 */
|
||||
uint8_t comms_type;
|
||||
/*!< Type of comms : VL53L1_I2C or VL53L1_SPI */
|
||||
uint16_t comms_speed_khz;
|
||||
/*!< Comms speed [kHz] : typically 400kHz for I2C */
|
||||
|
||||
I2C_HandleTypeDef *hi2c;
|
||||
GPIO_TypeDef *xshut_port;
|
||||
uint16_t xshut_pin;
|
||||
GPIO_TypeDef *gpio1_port;
|
||||
uint16_t gpio1_pin;
|
||||
|
||||
void *bus_lock;
|
||||
const char *name;
|
||||
uint8_t id;
|
||||
uint8_t is_present;
|
||||
uint16_t io_timeout_ms;
|
||||
|
||||
uint32_t new_data_ready_poll_duration_ms;
|
||||
/*!< New data ready poll duration in ms - for debug */
|
||||
} VL53L1_Dev_t;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Declare the device Handle as a pointer of the structure @a VL53L1_Dev_t.
|
||||
*
|
||||
*/
|
||||
typedef VL53L1_Dev_t *VL53L1_DEV;
|
||||
|
||||
/**
|
||||
* @def PALDevDataGet
|
||||
* @brief Get ST private structure @a VL53L1_DevData_t data access
|
||||
*
|
||||
* @param Dev Device Handle
|
||||
* @param field ST structure field name
|
||||
* It maybe used and as real data "ref" not just as "get" for sub-structure item
|
||||
* like PALDevDataGet(FilterData.field)[i] or
|
||||
* PALDevDataGet(FilterData.MeasurementIndex)++
|
||||
*/
|
||||
#define PALDevDataGet(Dev, field) (Dev->Data.field)
|
||||
|
||||
|
||||
/**
|
||||
* @def PALDevDataSet(Dev, field, data)
|
||||
* @brief Set ST private structure @a VL53L1_DevData_t data field
|
||||
* @param Dev Device Handle
|
||||
* @param field ST structure field name
|
||||
* @param data Data to be set
|
||||
*/
|
||||
#define PALDevDataSet(Dev, field, data) ((Dev->Data.field) = (data))
|
||||
|
||||
|
||||
/**
|
||||
* @def VL53L1DevStructGetLLDriverHandle
|
||||
* @brief Get LL Driver handle @a VL53L0_Dev_t data access
|
||||
*
|
||||
* @param Dev Device Handle
|
||||
*/
|
||||
#define VL53L1DevStructGetLLDriverHandle(Dev) (&Dev->Data.LLData)
|
||||
|
||||
/**
|
||||
* @def VL53L1DevStructGetLLResultsHandle
|
||||
* @brief Get LL Results handle @a VL53L0_Dev_t data access
|
||||
*
|
||||
* @param Dev Device Handle
|
||||
*/
|
||||
#define VL53L1DevStructGetLLResultsHandle(Dev) (&Dev->Data.llresults)
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
95
App/VL53L1X_API/platform/inc/vl53l1_platform_user_defines.h
Normal file
95
App/VL53L1X_API/platform/inc/vl53l1_platform_user_defines.h
Normal file
@@ -0,0 +1,95 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
|
||||
#ifndef _VL53L1_PLATFORM_USER_DEFINES_H_
|
||||
#define _VL53L1_PLATFORM_USER_DEFINES_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_user_defines.h
|
||||
*
|
||||
* @brief All end user OS/platform/application definitions
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @def do_division_u
|
||||
* @brief customer supplied division operation - 64-bit unsigned
|
||||
*
|
||||
* @param dividend unsigned 64-bit numerator
|
||||
* @param divisor unsigned 64-bit denominator
|
||||
*/
|
||||
#define do_division_u(dividend, divisor) (dividend / divisor)
|
||||
|
||||
|
||||
/**
|
||||
* @def do_division_s
|
||||
* @brief customer supplied division operation - 64-bit signed
|
||||
*
|
||||
* @param dividend signed 64-bit numerator
|
||||
* @param divisor signed 64-bit denominator
|
||||
*/
|
||||
#define do_division_s(dividend, divisor) (dividend / divisor)
|
||||
|
||||
|
||||
/**
|
||||
* @def WARN_OVERRIDE_STATUS
|
||||
* @brief customer supplied macro to optionally output info when a specific
|
||||
error has been overridden with success within the EwokPlus driver
|
||||
*
|
||||
* @param __X__ the macro which enabled the suppression
|
||||
*/
|
||||
#define WARN_OVERRIDE_STATUS(__X__)\
|
||||
trace_print (VL53L1_TRACE_LEVEL_WARNING, #__X__);
|
||||
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#define DISABLE_WARNINGS() { \
|
||||
__pragma (warning (push)); \
|
||||
__pragma (warning (disable:4127)); \
|
||||
}
|
||||
#define ENABLE_WARNINGS() { \
|
||||
__pragma (warning (pop)); \
|
||||
}
|
||||
#else
|
||||
#define DISABLE_WARNINGS()
|
||||
#define ENABLE_WARNINGS()
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _VL53L1_PLATFORM_USER_DEFINES_H_
|
||||
|
||||
114
App/VL53L1X_API/platform/inc/vl53l1_types.h
Normal file
114
App/VL53L1X_API/platform/inc/vl53l1_types.h
Normal file
@@ -0,0 +1,114 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2015, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
********************************************************************************/
|
||||
/**
|
||||
* @file vl53l1_types.h
|
||||
* @brief VL53L1 types definition
|
||||
*/
|
||||
|
||||
#ifndef _VL53L1_TYPES_H_
|
||||
#define _VL53L1_TYPES_H_
|
||||
|
||||
/** @defgroup porting_type Basic type definition
|
||||
* @ingroup api_platform
|
||||
*
|
||||
* @brief file vl53l1_types.h files hold basic type definition that may requires porting
|
||||
*
|
||||
* contains type that must be defined for the platform\n
|
||||
* when target platform and compiler provide stdint.h and stddef.h it is enough to include it.\n
|
||||
* If stdint.h is not available review and adapt all signed and unsigned 8/16/32 bits basic types. \n
|
||||
* If stddef.h is not available review and adapt NULL definition .
|
||||
*/
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#ifndef NULL
|
||||
#error "Error NULL definition should be done. Please add required include "
|
||||
#endif
|
||||
|
||||
|
||||
#if !defined(STDINT_H) && !defined(_STDINT_H) && !defined(_GCC_STDINT_H) && !defined(__STDINT_DECLS) && !defined(_GCC_WRAP_STDINT_H) && !defined(_STDINT)
|
||||
|
||||
#pragma message("Please review type definition of STDINT define for your platform and add to list above ")
|
||||
|
||||
/*
|
||||
* target platform do not provide stdint or use a different #define than above
|
||||
* to avoid seeing the message below addapt the #define list above or implement
|
||||
* all type and delete these pragma
|
||||
*/
|
||||
|
||||
/** \ingroup VL53L1_portingType_group
|
||||
* @{
|
||||
*/
|
||||
|
||||
|
||||
typedef unsigned long long uint64_t;
|
||||
|
||||
|
||||
/** @brief Typedef defining 32 bit unsigned int type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef unsigned int uint32_t;
|
||||
|
||||
/** @brief Typedef defining 32 bit int type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef int int32_t;
|
||||
|
||||
/** @brief Typedef defining 16 bit unsigned short type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef unsigned short uint16_t;
|
||||
|
||||
/** @brief Typedef defining 16 bit short type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef short int16_t;
|
||||
|
||||
/** @brief Typedef defining 8 bit unsigned char type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef unsigned char uint8_t;
|
||||
|
||||
/** @brief Typedef defining 8 bit char type.\n
|
||||
* The developer should modify this to suit the platform being deployed.
|
||||
*/
|
||||
typedef signed char int8_t;
|
||||
|
||||
/** @} */
|
||||
#endif /* _STDINT_H */
|
||||
|
||||
|
||||
/** use where fractional values are expected
|
||||
*
|
||||
* Given a floating point value f it's .16 bit point is (int)(f*(1<<16))*/
|
||||
typedef uint32_t FixPoint1616_t;
|
||||
|
||||
#endif /* VL53L1_TYPES_H_ */
|
||||
539
App/VL53L1X_API/platform/src/vl53l1_platform.c
Normal file
539
App/VL53L1X_API/platform/src/vl53l1_platform.c
Normal file
@@ -0,0 +1,539 @@
|
||||
#include "vl53l1_platform.h"
|
||||
#include "vl53l1_api.h"
|
||||
|
||||
#if VL53_USE_FREERTOS_DELAY
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#endif
|
||||
|
||||
#if defined(configSUPPORT_STATIC_ALLOCATION) || defined(configUSE_MUTEXES)
|
||||
#include "semphr.h"
|
||||
#define VL53L1_USE_FREERTOS_MUTEX 1
|
||||
#else
|
||||
#define VL53L1_USE_FREERTOS_MUTEX 0
|
||||
#endif
|
||||
|
||||
static VL53L1_Dev_t *s_platform_default_dev;
|
||||
|
||||
static VL53L1_Error vl53l1_hal_to_pal(HAL_StatusTypeDef hal_status)
|
||||
{
|
||||
switch (hal_status) {
|
||||
case HAL_OK:
|
||||
return VL53L1_ERROR_NONE;
|
||||
case HAL_TIMEOUT:
|
||||
return VL53L1_ERROR_TIME_OUT;
|
||||
default:
|
||||
return VL53L1_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
}
|
||||
|
||||
static VL53L1_Error vl53l1_validate_dev(VL53L1_Dev_t *pdev)
|
||||
{
|
||||
if ((pdev == NULL) || (pdev->hi2c == NULL)) {
|
||||
return VL53L1_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
if (pdev->i2c_slave_address == 0u) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
static VL53L1_Error vl53l1_bus_lock(VL53L1_Dev_t *pdev)
|
||||
{
|
||||
#if VL53L1_USE_FREERTOS_MUTEX
|
||||
if ((pdev != NULL) && (pdev->bus_lock != NULL)) {
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
const TickType_t wait_ticks = pdMS_TO_TICKS((pdev->io_timeout_ms == 0u) ? 10u : pdev->io_timeout_ms);
|
||||
if (xSemaphoreTake((SemaphoreHandle_t)pdev->bus_lock, wait_ticks) != pdTRUE) {
|
||||
return VL53L1_ERROR_TIME_OUT;
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
#endif
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
static void vl53l1_bus_unlock(VL53L1_Dev_t *pdev)
|
||||
{
|
||||
#if VL53L1_USE_FREERTOS_MUTEX
|
||||
if ((pdev != NULL) && (pdev->bus_lock != NULL)) {
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
(void)xSemaphoreGive((SemaphoreHandle_t)pdev->bus_lock);
|
||||
}
|
||||
}
|
||||
#else
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
#endif
|
||||
}
|
||||
|
||||
static void vl53l1_delay_ms(uint32_t wait_ms)
|
||||
{
|
||||
if (wait_ms == 0u) {
|
||||
return;
|
||||
}
|
||||
#if VL53_USE_FREERTOS_DELAY
|
||||
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
|
||||
vTaskDelay(pdMS_TO_TICKS(wait_ms));
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
HAL_Delay(wait_ms);
|
||||
}
|
||||
|
||||
static void vl53l1_enable_cycle_counter(void)
|
||||
{
|
||||
if ((CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) == 0u) {
|
||||
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
|
||||
}
|
||||
if ((DWT->CTRL & DWT_CTRL_CYCCNTENA_Msk) == 0u) {
|
||||
DWT->CYCCNT = 0u;
|
||||
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
|
||||
}
|
||||
}
|
||||
|
||||
static void vl53l1_delay_us_blocking(uint32_t wait_us)
|
||||
{
|
||||
uint32_t cycles_per_us;
|
||||
uint32_t start_cycles;
|
||||
uint32_t wait_cycles;
|
||||
|
||||
if (wait_us == 0u) {
|
||||
return;
|
||||
}
|
||||
|
||||
cycles_per_us = HAL_RCC_GetHCLKFreq() / 1000000u;
|
||||
if (cycles_per_us == 0u) {
|
||||
HAL_Delay(1u);
|
||||
return;
|
||||
}
|
||||
|
||||
vl53l1_enable_cycle_counter();
|
||||
start_cycles = DWT->CYCCNT;
|
||||
wait_cycles = cycles_per_us * wait_us;
|
||||
while ((DWT->CYCCNT - start_cycles) < wait_cycles) {
|
||||
}
|
||||
}
|
||||
|
||||
void VL53L1_PlatformAttachBus(
|
||||
VL53L1_Dev_t *pdev,
|
||||
I2C_HandleTypeDef *hi2c,
|
||||
uint8_t i2c_addr_8bit,
|
||||
uint16_t io_timeout_ms,
|
||||
void *bus_lock)
|
||||
{
|
||||
if (pdev == NULL) {
|
||||
return;
|
||||
}
|
||||
pdev->hi2c = hi2c;
|
||||
pdev->i2c_slave_address = i2c_addr_8bit;
|
||||
pdev->io_timeout_ms = (io_timeout_ms == 0u) ? 100u : io_timeout_ms;
|
||||
pdev->bus_lock = bus_lock;
|
||||
pdev->comms_type = VL53L1_I2C;
|
||||
if (pdev->comms_speed_khz == 0u) {
|
||||
pdev->comms_speed_khz = 400u;
|
||||
}
|
||||
if (s_platform_default_dev == NULL) {
|
||||
s_platform_default_dev = pdev;
|
||||
}
|
||||
}
|
||||
|
||||
void VL53L1_PlatformAttachPins(
|
||||
VL53L1_Dev_t *pdev,
|
||||
GPIO_TypeDef *xshut_port,
|
||||
uint16_t xshut_pin,
|
||||
GPIO_TypeDef *gpio1_port,
|
||||
uint16_t gpio1_pin)
|
||||
{
|
||||
if (pdev == NULL) {
|
||||
return;
|
||||
}
|
||||
pdev->xshut_port = xshut_port;
|
||||
pdev->xshut_pin = xshut_pin;
|
||||
pdev->gpio1_port = gpio1_port;
|
||||
pdev->gpio1_pin = gpio1_pin;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_PlatformSetXShut(
|
||||
VL53L1_Dev_t *pdev,
|
||||
GPIO_PinState state)
|
||||
{
|
||||
if ((pdev == NULL) || (pdev->xshut_port == NULL)) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
HAL_GPIO_WritePin(pdev->xshut_port, pdev->xshut_pin, state);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_PlatformBootDevice(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t reset_low_ms,
|
||||
uint32_t boot_wait_ms)
|
||||
{
|
||||
VL53L1_Error status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_RESET);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
vl53l1_delay_ms(reset_low_ms);
|
||||
status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_SET);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
vl53l1_delay_ms(boot_wait_ms);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
uint32_t VL53L1_PlatformGetTick(void)
|
||||
{
|
||||
return HAL_GetTick();
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_CommsInitialise(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t comms_type,
|
||||
uint16_t comms_speed_khz)
|
||||
{
|
||||
if (pdev == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
pdev->comms_type = comms_type;
|
||||
pdev->comms_speed_khz = comms_speed_khz;
|
||||
if (comms_type != VL53L1_I2C) {
|
||||
return VL53L1_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
pdev->comms_type = VL53L1_I2C;
|
||||
return vl53l1_validate_dev(pdev);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_CommsClose(VL53L1_Dev_t *pdev)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WriteMulti(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata,
|
||||
uint32_t count)
|
||||
{
|
||||
HAL_StatusTypeDef hal_status;
|
||||
VL53L1_Error status;
|
||||
|
||||
if ((pdata == NULL) || (count == 0u) || (count > 0xFFFFu)) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = vl53l1_validate_dev(pdev);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
status = vl53l1_bus_lock(pdev);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
hal_status = HAL_I2C_Mem_Write(
|
||||
pdev->hi2c,
|
||||
pdev->i2c_slave_address,
|
||||
index,
|
||||
I2C_MEMADD_SIZE_16BIT,
|
||||
pdata,
|
||||
(uint16_t)count,
|
||||
pdev->io_timeout_ms);
|
||||
vl53l1_bus_unlock(pdev);
|
||||
return vl53l1_hal_to_pal(hal_status);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_ReadMulti(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata,
|
||||
uint32_t count)
|
||||
{
|
||||
HAL_StatusTypeDef hal_status;
|
||||
VL53L1_Error status;
|
||||
|
||||
if ((pdata == NULL) || (count == 0u) || (count > 0xFFFFu)) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = vl53l1_validate_dev(pdev);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
status = vl53l1_bus_lock(pdev);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
hal_status = HAL_I2C_Mem_Read(
|
||||
pdev->hi2c,
|
||||
pdev->i2c_slave_address,
|
||||
index,
|
||||
I2C_MEMADD_SIZE_16BIT,
|
||||
pdata,
|
||||
(uint16_t)count,
|
||||
pdev->io_timeout_ms);
|
||||
vl53l1_bus_unlock(pdev);
|
||||
return vl53l1_hal_to_pal(hal_status);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WrByte(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t data)
|
||||
{
|
||||
return VL53L1_WriteMulti(pdev, index, &data, 1u);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WrWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint16_t data)
|
||||
{
|
||||
uint8_t buffer[2];
|
||||
|
||||
buffer[0] = (uint8_t)(data >> 8);
|
||||
buffer[1] = (uint8_t)(data & 0xFFu);
|
||||
return VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_WORD);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WrDWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint32_t data)
|
||||
{
|
||||
uint8_t buffer[4];
|
||||
|
||||
buffer[0] = (uint8_t)(data >> 24);
|
||||
buffer[1] = (uint8_t)(data >> 16);
|
||||
buffer[2] = (uint8_t)(data >> 8);
|
||||
buffer[3] = (uint8_t)(data & 0xFFu);
|
||||
return VL53L1_WriteMulti(pdev, index, buffer, VL53L1_BYTES_PER_DWORD);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_RdByte(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint8_t *pdata)
|
||||
{
|
||||
if (pdata == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
return VL53L1_ReadMulti(pdev, index, pdata, 1u);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_RdWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint16_t *pdata)
|
||||
{
|
||||
uint8_t buffer[2] = {0u, 0u};
|
||||
VL53L1_Error status;
|
||||
|
||||
if (pdata == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L1_ReadMulti(pdev, index, buffer, VL53L1_BYTES_PER_WORD);
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
*pdata = (uint16_t)(((uint16_t)buffer[0] << 8) | buffer[1]);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_RdDWord(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint16_t index,
|
||||
uint32_t *pdata)
|
||||
{
|
||||
uint8_t buffer[4] = {0u, 0u, 0u, 0u};
|
||||
VL53L1_Error status;
|
||||
|
||||
if (pdata == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L1_ReadMulti(pdev, index, buffer, VL53L1_BYTES_PER_DWORD);
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
*pdata = ((uint32_t)buffer[0] << 24)
|
||||
| ((uint32_t)buffer[1] << 16)
|
||||
| ((uint32_t)buffer[2] << 8)
|
||||
| (uint32_t)buffer[3];
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WaitUs(
|
||||
VL53L1_Dev_t *pdev,
|
||||
int32_t wait_us)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
if (wait_us <= 0) {
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
if (wait_us >= 1000) {
|
||||
vl53l1_delay_ms((uint32_t)wait_us / 1000u);
|
||||
wait_us %= 1000;
|
||||
}
|
||||
vl53l1_delay_us_blocking((uint32_t)wait_us);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WaitMs(
|
||||
VL53L1_Dev_t *pdev,
|
||||
int32_t wait_ms)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
if (wait_ms > 0) {
|
||||
vl53l1_delay_ms((uint32_t)wait_ms);
|
||||
}
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GetTimerFrequency(int32_t *ptimer_freq_hz)
|
||||
{
|
||||
if (ptimer_freq_hz == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
*ptimer_freq_hz = 1000;
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GetTimerValue(int32_t *ptimer_count)
|
||||
{
|
||||
if (ptimer_count == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
*ptimer_count = (int32_t)HAL_GetTick();
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioSetMode(uint8_t pin, uint8_t mode)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pin);
|
||||
SUPPRESS_UNUSED_WARNING(mode);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioSetValue(uint8_t pin, uint8_t value)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pin);
|
||||
SUPPRESS_UNUSED_WARNING(value);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioGetValue(uint8_t pin, uint8_t *pvalue)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pin);
|
||||
if (pvalue == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
*pvalue = 0u;
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioXshutdown(uint8_t value)
|
||||
{
|
||||
if (s_platform_default_dev == NULL) {
|
||||
return VL53L1_ERROR_CONTROL_INTERFACE;
|
||||
}
|
||||
return VL53L1_PlatformSetXShut(s_platform_default_dev, value ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioCommsSelect(uint8_t value)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(value);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioPowerEnable(uint8_t value)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(value);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioInterruptEnable(void (*function)(void), uint8_t edge_type)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(function);
|
||||
SUPPRESS_UNUSED_WARNING(edge_type);
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GpioInterruptDisable(void)
|
||||
{
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_GetTickCount(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t *ptick_count_ms)
|
||||
{
|
||||
SUPPRESS_UNUSED_WARNING(pdev);
|
||||
if (ptick_count_ms == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
*ptick_count_ms = HAL_GetTick();
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_WaitValueMaskEx(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint32_t timeout_ms,
|
||||
uint16_t index,
|
||||
uint8_t value,
|
||||
uint8_t mask,
|
||||
uint32_t poll_delay_ms)
|
||||
{
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
uint32_t start_time_ms = 0u;
|
||||
uint32_t current_time_ms = 0u;
|
||||
uint8_t byte_value = 0u;
|
||||
uint8_t found = 0u;
|
||||
|
||||
if (pdev == NULL) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L1_GetTickCount(pdev, &start_time_ms);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
return status;
|
||||
}
|
||||
pdev->new_data_ready_poll_duration_ms = 0u;
|
||||
|
||||
while ((status == VL53L1_ERROR_NONE) &&
|
||||
(pdev->new_data_ready_poll_duration_ms < timeout_ms) &&
|
||||
(found == 0u)) {
|
||||
status = VL53L1_RdByte(pdev, index, &byte_value);
|
||||
if (status != VL53L1_ERROR_NONE) {
|
||||
break;
|
||||
}
|
||||
if ((byte_value & mask) == value) {
|
||||
found = 1u;
|
||||
break;
|
||||
}
|
||||
if (poll_delay_ms > 0u) {
|
||||
status = VL53L1_WaitMs(pdev, (int32_t)poll_delay_ms);
|
||||
}
|
||||
VL53L1_GetTickCount(pdev, ¤t_time_ms);
|
||||
pdev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
|
||||
}
|
||||
|
||||
if ((found == 0u) && (status == VL53L1_ERROR_NONE)) {
|
||||
status = VL53L1_ERROR_TIME_OUT;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
VL53L1_Error VL53L1_PlatformChangeAddress(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t new_addr_8bit)
|
||||
{
|
||||
VL53L1_Error status;
|
||||
|
||||
if ((pdev == NULL) || (new_addr_8bit == 0u)) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
status = VL53L1_SetDeviceAddress(pdev, new_addr_8bit);
|
||||
if (status == VL53L1_ERROR_NONE) {
|
||||
pdev->i2c_slave_address = new_addr_8bit;
|
||||
vl53l1_delay_ms(2u);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
103
App/VL53L1X_API/platform/src/vl53l1_platform_init.c
Normal file
103
App/VL53L1X_API/platform/src/vl53l1_platform_init.c
Normal file
@@ -0,0 +1,103 @@
|
||||
/*******************************************************************************
|
||||
Copyright (C) 2016, STMicroelectronics International N.V.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of STMicroelectronics nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT OF INTELLECTUAL PROPERTY RIGHTS ARE DISCLAIMED.
|
||||
IN NO EVENT SHALL STMICROELECTRONICS INTERNATIONAL N.V. BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
******************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_init.c
|
||||
* @brief EwokPlus25 comms and GPIO init
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include "vl53l1_ll_def.h"
|
||||
#include "vl53l1_platform.h"
|
||||
#include "vl53l1_platform_init.h"
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_platform_init(
|
||||
VL53L1_Dev_t *pdev,
|
||||
uint8_t i2c_slave_address,
|
||||
uint8_t comms_type,
|
||||
uint16_t comms_speed_khz)
|
||||
{
|
||||
/*
|
||||
* Initialise comms, GPIOs (xshutdown, ncs, EVK power regulator enable)
|
||||
* and reset Device
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
if (pdev == NULL)
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
|
||||
/* remember comms settings */
|
||||
|
||||
pdev->i2c_slave_address = (i2c_slave_address == 0u) ? VL53L1_DEFAULT_ADDR_8BIT : i2c_slave_address;
|
||||
pdev->comms_type = comms_type;
|
||||
pdev->comms_speed_khz = (comms_speed_khz == 0u) ? 400u : comms_speed_khz;
|
||||
|
||||
if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
|
||||
status =
|
||||
VL53L1_CommsInitialise(
|
||||
pdev,
|
||||
pdev->comms_type,
|
||||
pdev->comms_speed_khz);
|
||||
|
||||
pdev->io_timeout_ms = (pdev->io_timeout_ms == 0u) ? 100u : pdev->io_timeout_ms;
|
||||
|
||||
if ((status == VL53L1_ERROR_NONE) && (pdev->xshut_port != NULL)) {
|
||||
status = VL53L1_PlatformBootDevice(pdev, 2u, 2u);
|
||||
} else if (status == VL53L1_ERROR_NONE) {
|
||||
status = VL53L1_WaitMs(pdev, 2);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
VL53L1_Error VL53L1_platform_terminate(
|
||||
VL53L1_Dev_t *pdev)
|
||||
{
|
||||
/*
|
||||
* Puts the device into reset, disables EVK power regulator
|
||||
* and closes comms
|
||||
*/
|
||||
|
||||
VL53L1_Error status = VL53L1_ERROR_NONE;
|
||||
|
||||
/* put device in reset when an XSHUT pin is attached */
|
||||
if ((status == VL53L1_ERROR_NONE) && (pdev != NULL) && (pdev->xshut_port != NULL))
|
||||
status = VL53L1_PlatformSetXShut(pdev, GPIO_PIN_RESET);
|
||||
|
||||
/* close the comms interfaces */
|
||||
if (status == VL53L1_ERROR_NONE)
|
||||
status = VL53L1_CommsClose(pdev);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
131
App/VL53L1X_API/platform/src/vl53l1_platform_log.c
Normal file
131
App/VL53L1X_API/platform/src/vl53l1_platform_log.c
Normal file
@@ -0,0 +1,131 @@
|
||||
/*
|
||||
* COPYRIGHT (C) STMicroelectronics 2015. All rights reserved.
|
||||
*
|
||||
* This software is the confidential and proprietary information of
|
||||
* STMicroelectronics ("Confidential Information"). You shall not
|
||||
* disclose such Confidential Information and shall use it only in
|
||||
* accordance with the terms of the license agreement you entered into
|
||||
* with STMicroelectronics
|
||||
*
|
||||
* Programming Golden Rule: Keep it Simple!
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file vl53l1_platform_log.c
|
||||
*
|
||||
* @brief Code function definitions for EwokPlus25 Platform Logging Layer
|
||||
*/
|
||||
|
||||
#include <stdio.h> // sprintf(), vsnprintf(), printf()
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "vl53l1_platform_log.h"
|
||||
#include "vl53l1_platform_user_config.h"
|
||||
|
||||
#ifdef VL53L1_LOG_ENABLE
|
||||
|
||||
char * _trace_filename = NULL;
|
||||
FILE *_tracefile = NULL;
|
||||
|
||||
uint32_t _trace_level = VL53L1_TRACE_LEVEL_WARNING;
|
||||
uint32_t _trace_modules = VL53L1_TRACE_MODULE_NONE;
|
||||
uint32_t _trace_functions = VL53L1_TRACE_FUNCTION_ALL;
|
||||
|
||||
int8_t VL53L1_trace_config(
|
||||
char *filename,
|
||||
uint32_t modules,
|
||||
uint32_t level,
|
||||
uint32_t functions)
|
||||
{
|
||||
int8_t status = 0;
|
||||
|
||||
// // Something in the commented out code below causes ncsim to crash!
|
||||
//
|
||||
// if (((_trace_filename != NULL) && (_trace_filename != filename)) ||strcmp(filename,"")==0)
|
||||
// {
|
||||
// if ( _tracefile != NULL )
|
||||
// {
|
||||
// fclose(_tracefile);
|
||||
// _tracefile = NULL;
|
||||
// }
|
||||
// free(_trace_filename);
|
||||
// _trace_filename = NULL;
|
||||
// }
|
||||
//
|
||||
|
||||
if (((filename != NULL) && (_tracefile == NULL)) && strcmp(filename,""))
|
||||
{
|
||||
_tracefile = fopen(filename, "w+");
|
||||
|
||||
//TODO: Add time and header banner to the log file to indicate we've just opened a new log session
|
||||
|
||||
if ( _tracefile != NULL )
|
||||
{
|
||||
_trace_filename = (char*)malloc((strlen(filename) + 1) * sizeof(char));
|
||||
strcpy(_trace_filename, filename);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("VL53L1_trace_config(): failed to open log file (%s)\n", filename);
|
||||
status = 1;
|
||||
}
|
||||
}
|
||||
|
||||
_trace_modules = modules;
|
||||
_trace_level = level;
|
||||
_trace_functions = functions;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
void VL53L1_trace_print_module_function(uint32_t module, uint32_t level, uint32_t function, const char *format, ...)
|
||||
{
|
||||
if ( ((level <=_trace_level) && ((module & _trace_modules) > 0))
|
||||
|| ((function & _trace_functions) > 0) )
|
||||
{
|
||||
va_list arg_list;
|
||||
char message[VL53L1_MAX_STRING_LENGTH];
|
||||
|
||||
va_start(arg_list, format);
|
||||
vsnprintf(message, VL53L1_MAX_STRING_LENGTH-1, format, arg_list); /*lint !e534 ignore return*/
|
||||
va_end(arg_list);
|
||||
|
||||
if (_tracefile != NULL)
|
||||
{
|
||||
fprintf(_tracefile, message); /*lint !e592 ignore format specifier*/
|
||||
}
|
||||
else
|
||||
{
|
||||
printf(message); /*lint !e592 ignore format specifier*/
|
||||
}
|
||||
|
||||
// if (_tracefile != NULL)
|
||||
// fprintf(_tracefile, message);
|
||||
// else
|
||||
// printf(message);
|
||||
} /*lint !e438 ignore issues with arg_list*/
|
||||
}
|
||||
|
||||
|
||||
uint32_t VL53L1_get_trace_functions(void)
|
||||
{
|
||||
return _trace_functions;
|
||||
}
|
||||
|
||||
|
||||
void VL53L1_set_trace_functions(uint32_t function)
|
||||
{
|
||||
_trace_functions = function;
|
||||
}
|
||||
|
||||
|
||||
uint32_t VL53L1_clock(void)
|
||||
{
|
||||
/* Returns current tick count in [ms] */
|
||||
uint32_t tick_count_ms = (uint32_t)clock();
|
||||
return tick_count_ms;
|
||||
}
|
||||
#endif // VL53L1_LOG_ENABLE
|
||||
229
App/VL53L1X_API/platform/vl53_board.c
Normal file
229
App/VL53L1X_API/platform/vl53_board.c
Normal file
@@ -0,0 +1,229 @@
|
||||
#include "robot_params.h"
|
||||
|
||||
#if PARAM_VL53_USE_L1X
|
||||
|
||||
#include "vl53_board.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "vl53_calibration_config.h"
|
||||
|
||||
static void vl53_ema_init(Vl53EMA_t *ema, float alpha)
|
||||
{
|
||||
ema->x = 0.0f;
|
||||
ema->alpha = alpha;
|
||||
ema->initialized = 0u;
|
||||
}
|
||||
|
||||
static float vl53_ema_update(Vl53EMA_t *ema, float measurement)
|
||||
{
|
||||
if (ema->initialized == 0u) {
|
||||
ema->x = measurement;
|
||||
ema->initialized = 1u;
|
||||
return ema->x;
|
||||
}
|
||||
|
||||
ema->x = ema->alpha * measurement + (1.0f - ema->alpha) * ema->x;
|
||||
return ema->x;
|
||||
}
|
||||
|
||||
static const Vl53L1RuntimeCalibration_t *vl53_get_runtime_calibration(uint8_t id)
|
||||
{
|
||||
switch (id) {
|
||||
case 0: return &k_vl53l1_left_calibration[0];
|
||||
case 1: return &k_vl53l1_left_calibration[1];
|
||||
case 2: return &k_vl53l1_right_calibration[0];
|
||||
case 3: return &k_vl53l1_right_calibration[1];
|
||||
default: return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
static VL53L1_Error vl53_apply_runtime_calibration(VL53L1_DEV dev, uint8_t id)
|
||||
{
|
||||
const Vl53L1RuntimeCalibration_t *cal = vl53_get_runtime_calibration(id);
|
||||
|
||||
if ((cal == NULL) || (cal->calibrated == 0u)) {
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
return VL53L1_SetCalibrationData(dev, (VL53L1_CalibrationData_t *)&cal->data);
|
||||
}
|
||||
|
||||
static uint32_t vl53_sanitize_timing_budget_us(uint32_t timing_budget_us)
|
||||
{
|
||||
if (timing_budget_us < 20000u) {
|
||||
return 20000u;
|
||||
}
|
||||
if (timing_budget_us > 1000000u) {
|
||||
return 1000000u;
|
||||
}
|
||||
return timing_budget_us;
|
||||
}
|
||||
|
||||
static uint32_t vl53_compute_inter_measurement_ms(uint32_t timing_budget_us)
|
||||
{
|
||||
uint32_t timing_budget_ms = (timing_budget_us + 999u) / 1000u;
|
||||
|
||||
/* UM2356: inter-measurement period shorter than timing budget时会立即开始下一帧。 */
|
||||
if (timing_budget_ms < 25u) {
|
||||
timing_budget_ms = 25u;
|
||||
}
|
||||
return timing_budget_ms;
|
||||
}
|
||||
|
||||
static VL53L1_Error vl53_configure_ranging_profile(VL53L1_DEV dev, uint32_t timing_budget_us)
|
||||
{
|
||||
VL53L1_Error status;
|
||||
const uint32_t inter_measurement_ms = vl53_compute_inter_measurement_ms(timing_budget_us);
|
||||
|
||||
status = VL53L1_SetPresetMode(dev, VL53L1_PRESETMODE_LITE_RANGING);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L1_SetDistanceMode(dev, VL53L1_DISTANCEMODE_LONG);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L1_SetMeasurementTimingBudgetMicroSeconds(dev, timing_budget_us);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L1_SetInterMeasurementPeriodMilliSeconds(dev, inter_measurement_ms);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, 1u);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = VL53L1_SetLimitCheckEnable(dev, VL53L1_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1u);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
static VL53L1_Error vl53_do_static_init(VL53L1_DEV dev, uint32_t timing_budget_us, uint8_t id)
|
||||
{
|
||||
VL53L1_Error status;
|
||||
|
||||
status = VL53L1_StaticInit(dev);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
status = vl53_apply_runtime_calibration(dev, id);
|
||||
if (status != VL53L1_ERROR_NONE) return status;
|
||||
|
||||
return vl53_configure_ranging_profile(dev, timing_budget_us);
|
||||
}
|
||||
|
||||
VL53L1_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us)
|
||||
{
|
||||
if ((board == NULL) || (hw_cfgs == NULL) || (count == 0u) || (count > VL53_MAX_DEVS_PER_BOARD)) {
|
||||
return VL53L1_ERROR_INVALID_PARAMS;
|
||||
}
|
||||
|
||||
memset(board, 0, sizeof(Vl53Board_t));
|
||||
board->dev_count = count;
|
||||
board->timing_budget_us = vl53_sanitize_timing_budget_us((timing_budget_us == 0u) ? 33000u : timing_budget_us);
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
board->dev[i].name = hw_cfgs[i].name;
|
||||
board->dev[i].id = hw_cfgs[i].id;
|
||||
board->dev[i].comms_type = VL53L1_I2C;
|
||||
board->dev[i].comms_speed_khz = 400u;
|
||||
board->dev[i].is_present = 0u;
|
||||
|
||||
VL53L1_PlatformAttachBus(&board->dev[i], hw_cfgs[i].hi2c, VL53L1_DEFAULT_ADDR_8BIT, 100u, NULL);
|
||||
VL53L1_PlatformAttachPins(&board->dev[i], hw_cfgs[i].xshut_port, hw_cfgs[i].xshut_pin, NULL, 0u);
|
||||
(void)VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_RESET);
|
||||
|
||||
vl53_ema_init(&board->ema[i], PARAM_VL53_EMA_ALPHA);
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(10u));
|
||||
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
if (VL53L1_PlatformSetXShut(&board->dev[i], GPIO_PIN_SET) != VL53L1_ERROR_NONE) continue;
|
||||
vTaskDelay(pdMS_TO_TICKS(20u));
|
||||
|
||||
board->dev[i].i2c_slave_address = VL53L1_DEFAULT_ADDR_8BIT;
|
||||
if (VL53L1_CommsInitialise(&board->dev[i], VL53L1_I2C, board->dev[i].comms_speed_khz) != VL53L1_ERROR_NONE) continue;
|
||||
if (VL53L1_WaitDeviceBooted(&board->dev[i]) != VL53L1_ERROR_NONE) continue;
|
||||
if (VL53L1_PlatformChangeAddress(&board->dev[i], hw_cfgs[i].runtime_addr_8bit) != VL53L1_ERROR_NONE) continue;
|
||||
if (VL53L1_DataInit(&board->dev[i]) != VL53L1_ERROR_NONE) continue;
|
||||
if (vl53_do_static_init(&board->dev[i], board->timing_budget_us, hw_cfgs[i].id) != VL53L1_ERROR_NONE) continue;
|
||||
|
||||
board->init_mask |= (uint8_t)(1u << i);
|
||||
board->dev[i].is_present = 1u;
|
||||
}
|
||||
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error Vl53Board_StartContinuous(Vl53Board_t *board)
|
||||
{
|
||||
if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS;
|
||||
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if (board->init_mask & (1u << i)) {
|
||||
(void)VL53L1_StartMeasurement(&board->dev[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error Vl53Board_StopContinuous(Vl53Board_t *board)
|
||||
{
|
||||
if (board == NULL) return VL53L1_ERROR_INVALID_PARAMS;
|
||||
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if (board->init_mask & (1u << i)) {
|
||||
(void)VL53L1_StopMeasurement(&board->dev[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
VL53L1_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot)
|
||||
{
|
||||
if ((board == NULL) || (snapshot == NULL)) return VL53L1_ERROR_INVALID_PARAMS;
|
||||
|
||||
memset(snapshot, 0, sizeof(Vl53BoardSnapshot_t));
|
||||
snapshot->tick_ms = xTaskGetTickCount() * portTICK_PERIOD_MS;
|
||||
|
||||
for (uint8_t i = 0; i < board->dev_count; i++) {
|
||||
if ((board->init_mask & (1u << i)) == 0u) {
|
||||
snapshot->range_status[i] = 255u;
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t ready = 0u;
|
||||
if (VL53L1_GetMeasurementDataReady(&board->dev[i], &ready) != VL53L1_ERROR_NONE) continue;
|
||||
|
||||
if (ready != 0u) {
|
||||
VL53L1_RangingMeasurementData_t data;
|
||||
memset(&data, 0, sizeof(data));
|
||||
|
||||
if (VL53L1_GetRangingMeasurementData(&board->dev[i], &data) == VL53L1_ERROR_NONE) {
|
||||
snapshot->range_mm[i] = (data.RangeMilliMeter < 0) ? 0u : (uint16_t)data.RangeMilliMeter;
|
||||
snapshot->range_status[i] = data.RangeStatus;
|
||||
|
||||
if (data.RangeStatus == 0u) {
|
||||
snapshot->valid_mask |= (uint8_t)(1u << i);
|
||||
#if PARAM_VL53_USE_EMA_FILTER
|
||||
snapshot->range_mm_filtered[i] = vl53_ema_update(&board->ema[i], (float)snapshot->range_mm[i]);
|
||||
#else
|
||||
snapshot->range_mm_filtered[i] = (float)snapshot->range_mm[i];
|
||||
board->ema[i].x = (float)snapshot->range_mm[i];
|
||||
board->ema[i].initialized = 1u;
|
||||
#endif
|
||||
} else {
|
||||
snapshot->range_mm_filtered[i] = board->ema[i].x;
|
||||
}
|
||||
|
||||
(void)VL53L1_ClearInterruptAndStartMeasurement(&board->dev[i]);
|
||||
}
|
||||
} else {
|
||||
snapshot->range_mm_filtered[i] = board->ema[i].x;
|
||||
}
|
||||
}
|
||||
|
||||
return VL53L1_ERROR_NONE;
|
||||
}
|
||||
|
||||
#endif
|
||||
56
App/VL53L1X_API/platform/vl53_board.h
Normal file
56
App/VL53L1X_API/platform/vl53_board.h
Normal file
@@ -0,0 +1,56 @@
|
||||
#ifndef VL53_BOARD_H
|
||||
#define VL53_BOARD_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "vl53l1_api.h"
|
||||
#include "vl53l1_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define VL53_MAX_DEVS_PER_BOARD 4
|
||||
|
||||
typedef struct {
|
||||
I2C_HandleTypeDef *hi2c;
|
||||
GPIO_TypeDef *xshut_port;
|
||||
uint16_t xshut_pin;
|
||||
uint8_t runtime_addr_8bit;
|
||||
const char *name;
|
||||
uint8_t id;
|
||||
} Vl53BoardHwCfg_t;
|
||||
|
||||
typedef struct {
|
||||
float x;
|
||||
float alpha;
|
||||
uint8_t initialized;
|
||||
} Vl53EMA_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t tick_ms;
|
||||
uint16_t range_mm[VL53_MAX_DEVS_PER_BOARD];
|
||||
float range_mm_filtered[VL53_MAX_DEVS_PER_BOARD];
|
||||
uint8_t range_status[VL53_MAX_DEVS_PER_BOARD];
|
||||
uint8_t valid_mask;
|
||||
} Vl53BoardSnapshot_t;
|
||||
|
||||
typedef struct {
|
||||
VL53L1_Dev_t dev[VL53_MAX_DEVS_PER_BOARD];
|
||||
Vl53EMA_t ema[VL53_MAX_DEVS_PER_BOARD];
|
||||
uint8_t init_mask;
|
||||
uint8_t dev_count;
|
||||
uint32_t timing_budget_us;
|
||||
} Vl53Board_t;
|
||||
|
||||
VL53L1_Error Vl53Board_Init(Vl53Board_t *board, const Vl53BoardHwCfg_t *hw_cfgs, uint8_t count, uint32_t timing_budget_us);
|
||||
VL53L1_Error Vl53Board_StartContinuous(Vl53Board_t *board);
|
||||
VL53L1_Error Vl53Board_StopContinuous(Vl53Board_t *board);
|
||||
VL53L1_Error Vl53Board_ReadAll(Vl53Board_t *board, Vl53BoardSnapshot_t *snapshot);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* VL53_BOARD_H */
|
||||
221
App/VL53L1X_API/platform/vl53_calibration_config.h
Normal file
221
App/VL53L1X_API/platform/vl53_calibration_config.h
Normal file
@@ -0,0 +1,221 @@
|
||||
#ifndef VL53L1_CALIBRATION_CONFIG_H
|
||||
#define VL53L1_CALIBRATION_CONFIG_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "vl53l1_def.h"
|
||||
|
||||
typedef struct {
|
||||
uint8_t calibrated;
|
||||
VL53L1_CalibrationData_t data;
|
||||
} Vl53L1RuntimeCalibration_t;
|
||||
|
||||
/*
|
||||
* VL53L1X 按 UM2356 建议应在产线完成 RefSPAD / Offset / Xtalk 校准，
|
||||
* 上电后在 DataInit() + StaticInit() 之后用 SetCalibrationData() 回灌。
|
||||
*
|
||||
* 当前先提供空白占位，未标定时保持 calibrated = 0，驱动将跳过加载。
|
||||
*/
|
||||
static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = {
|
||||
{
|
||||
.calibrated = 0u,
|
||||
.data = {
|
||||
.struct_version = 3970629922u,
|
||||
.customer = {
|
||||
.global_config__spad_enables_ref_0 = 223u,
|
||||
.global_config__spad_enables_ref_1 = 247u,
|
||||
.global_config__spad_enables_ref_2 = 251u,
|
||||
.global_config__spad_enables_ref_3 = 254u,
|
||||
.global_config__spad_enables_ref_4 = 255u,
|
||||
.global_config__spad_enables_ref_5 = 7u,
|
||||
.global_config__ref_en_start_select = 0u,
|
||||
.ref_spad_man__num_requested_ref_spads = 11u,
|
||||
.ref_spad_man__ref_location = 1u,
|
||||
.algo__crosstalk_compensation_plane_offset_kcps = 0u,
|
||||
.algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
|
||||
.algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
|
||||
.ref_spad_char__total_rate_target_mcps = 2560u,
|
||||
.algo__part_to_part_range_offset_mm = 0,
|
||||
.mm_config__inner_offset_mm = 35,
|
||||
.mm_config__outer_offset_mm = 9,
|
||||
},
|
||||
.add_off_cal_data = {
|
||||
.result__mm_inner_actual_effective_spads = 280u,
|
||||
.result__mm_outer_actual_effective_spads = 1344u,
|
||||
.result__mm_inner_peak_signal_count_rtn_mcps = 397u,
|
||||
.result__mm_outer_peak_signal_count_rtn_mcps = 1305u,
|
||||
},
|
||||
.optical_centre = {
|
||||
.x_centre = 144u,
|
||||
.y_centre = 112u,
|
||||
},
|
||||
.gain_cal = {
|
||||
.standard_ranging_gain_factor = 2011u,
|
||||
},
|
||||
.cal_peak_rate_map = {
|
||||
.cal_distance_mm = 0,
|
||||
.max_samples = 0u,
|
||||
.width = 0u,
|
||||
.height = 0u,
|
||||
.peak_rate_mcps = {
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
.calibrated = 0u,
|
||||
.data = {
|
||||
.struct_version = 3970629922u,
|
||||
.customer = {
|
||||
.global_config__spad_enables_ref_0 = 255u,
|
||||
.global_config__spad_enables_ref_1 = 189u,
|
||||
.global_config__spad_enables_ref_2 = 255u,
|
||||
.global_config__spad_enables_ref_3 = 255u,
|
||||
.global_config__spad_enables_ref_4 = 255u,
|
||||
.global_config__spad_enables_ref_5 = 15u,
|
||||
.global_config__ref_en_start_select = 0u,
|
||||
.ref_spad_man__num_requested_ref_spads = 14u,
|
||||
.ref_spad_man__ref_location = 1u,
|
||||
.algo__crosstalk_compensation_plane_offset_kcps = 0u,
|
||||
.algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
|
||||
.algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
|
||||
.ref_spad_char__total_rate_target_mcps = 2560u,
|
||||
.algo__part_to_part_range_offset_mm = 0,
|
||||
.mm_config__inner_offset_mm = 37,
|
||||
.mm_config__outer_offset_mm = 9,
|
||||
},
|
||||
.add_off_cal_data = {
|
||||
.result__mm_inner_actual_effective_spads = 280u,
|
||||
.result__mm_outer_actual_effective_spads = 1288u,
|
||||
.result__mm_inner_peak_signal_count_rtn_mcps = 470u,
|
||||
.result__mm_outer_peak_signal_count_rtn_mcps = 1456u,
|
||||
},
|
||||
.optical_centre = {
|
||||
.x_centre = 112u,
|
||||
.y_centre = 128u,
|
||||
},
|
||||
.gain_cal = {
|
||||
.standard_ranging_gain_factor = 2011u,
|
||||
},
|
||||
.cal_peak_rate_map = {
|
||||
.cal_distance_mm = 0,
|
||||
.max_samples = 0u,
|
||||
.width = 0u,
|
||||
.height = 0u,
|
||||
.peak_rate_mcps = {
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
};
|
||||
|
||||
static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = {
|
||||
{
|
||||
.calibrated = 0u,
|
||||
.data = {
|
||||
.struct_version = 3970629922u,
|
||||
.customer = {
|
||||
.global_config__spad_enables_ref_0 = 159u,
|
||||
.global_config__spad_enables_ref_1 = 254u,
|
||||
.global_config__spad_enables_ref_2 = 255u,
|
||||
.global_config__spad_enables_ref_3 = 255u,
|
||||
.global_config__spad_enables_ref_4 = 239u,
|
||||
.global_config__spad_enables_ref_5 = 15u,
|
||||
.global_config__ref_en_start_select = 0u,
|
||||
.ref_spad_man__num_requested_ref_spads = 6u,
|
||||
.ref_spad_man__ref_location = 1u,
|
||||
.algo__crosstalk_compensation_plane_offset_kcps = 0u,
|
||||
.algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
|
||||
.algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
|
||||
.ref_spad_char__total_rate_target_mcps = 2560u,
|
||||
.algo__part_to_part_range_offset_mm = 0,
|
||||
.mm_config__inner_offset_mm = 54,
|
||||
.mm_config__outer_offset_mm = 31,
|
||||
},
|
||||
.add_off_cal_data = {
|
||||
.result__mm_inner_actual_effective_spads = 224u,
|
||||
.result__mm_outer_actual_effective_spads = 1456u,
|
||||
.result__mm_inner_peak_signal_count_rtn_mcps = 336u,
|
||||
.result__mm_outer_peak_signal_count_rtn_mcps = 1382u,
|
||||
},
|
||||
.optical_centre = {
|
||||
.x_centre = 112u,
|
||||
.y_centre = 112u,
|
||||
},
|
||||
.gain_cal = {
|
||||
.standard_ranging_gain_factor = 2011u,
|
||||
},
|
||||
.cal_peak_rate_map = {
|
||||
.cal_distance_mm = 0,
|
||||
.max_samples = 0u,
|
||||
.width = 0u,
|
||||
.height = 0u,
|
||||
.peak_rate_mcps = {
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
.calibrated = 0u,
|
||||
.data = {
|
||||
.struct_version = 3970629922u,
|
||||
.customer = {
|
||||
.global_config__spad_enables_ref_0 = 255u,
|
||||
.global_config__spad_enables_ref_1 = 255u,
|
||||
.global_config__spad_enables_ref_2 = 255u,
|
||||
.global_config__spad_enables_ref_3 = 191u,
|
||||
.global_config__spad_enables_ref_4 = 191u,
|
||||
.global_config__spad_enables_ref_5 = 11u,
|
||||
.global_config__ref_en_start_select = 0u,
|
||||
.ref_spad_man__num_requested_ref_spads = 11u,
|
||||
.ref_spad_man__ref_location = 2u,
|
||||
.algo__crosstalk_compensation_plane_offset_kcps = 0u,
|
||||
.algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
|
||||
.algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
|
||||
.ref_spad_char__total_rate_target_mcps = 2560u,
|
||||
.algo__part_to_part_range_offset_mm = 0,
|
||||
.mm_config__inner_offset_mm = 30,
|
||||
.mm_config__outer_offset_mm = 5,
|
||||
},
|
||||
.add_off_cal_data = {
|
||||
.result__mm_inner_actual_effective_spads = 224u,
|
||||
.result__mm_outer_actual_effective_spads = 1456u,
|
||||
.result__mm_inner_peak_signal_count_rtn_mcps = 334u,
|
||||
.result__mm_outer_peak_signal_count_rtn_mcps = 1524u,
|
||||
},
|
||||
.optical_centre = {
|
||||
.x_centre = 112u,
|
||||
.y_centre = 96u,
|
||||
},
|
||||
.gain_cal = {
|
||||
.standard_ranging_gain_factor = 2011u,
|
||||
},
|
||||
.cal_peak_rate_map = {
|
||||
.cal_distance_mm = 0,
|
||||
.max_samples = 0u,
|
||||
.width = 0u,
|
||||
.height = 0u,
|
||||
.peak_rate_mcps = {
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
|
||||
0u
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
};
|
||||
|
||||
#endif /* VL53L1_CALIBRATION_CONFIG_H */
|
||||
522
App/app_tasks.c
Normal file
522
App/app_tasks.c
Normal file
@@ -0,0 +1,522 @@
|
||||
#include "app_tasks.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "main.h"
|
||||
#include "snc_can_app.h"
|
||||
#include "robot_params.h" /* 统一参数配置 */
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
/* 引入各个传感器和黑板的头文件 */
|
||||
#include "Contract/robot_blackboard.h"
|
||||
#include "Contract/robot_odom.h"
|
||||
#include "laser/laser_manager.h"
|
||||
#include "IMU/hwt101.h"
|
||||
#include "vl53_board.h"
|
||||
#include "Contract/robot_cmd_slot.h"
|
||||
#include "preproc/corridor_preproc.h"
|
||||
#include "est/corridor_filter.h"
|
||||
#include "nav/corridor_ctrl.h"
|
||||
#include "nav/segment_fsm.h"
|
||||
#include "nav/nav_script.h"
|
||||
#include "nav/global_nav.h"
|
||||
#include "nav/track_map.h"
|
||||
extern osMutexId_t logMutexHandle;
|
||||
|
||||
/* 如果你的项目中没有引入 i2c.h，可以通过 extern 声明 I2C 句柄 */
|
||||
extern I2C_HandleTypeDef hi2c1;
|
||||
extern I2C_HandleTypeDef hi2c2;
|
||||
|
||||
static float g_test_vx = 0.0f;
|
||||
static float g_test_wz = 0.0f;
|
||||
|
||||
static uint32_t AppTasks_MsToTicks(uint32_t ms)
|
||||
{
|
||||
uint32_t ticks = pdMS_TO_TICKS(ms);
|
||||
return (ticks == 0U) ? 1U : ticks;
|
||||
}
|
||||
|
||||
static void App_UpdateTestCommand(void)
|
||||
{
|
||||
g_test_vx = 0.0f;
|
||||
g_test_wz = 0.0f;
|
||||
}
|
||||
|
||||
static void App_PrintStatus(void)
|
||||
{
|
||||
// 【新增】静态计数器，由于是 static 修饰，它的值在函数退出后不会丢失
|
||||
static uint8_t print_divider = 0;
|
||||
|
||||
// 假设外部任务是 100ms 调用一次该函数：
|
||||
// 如果设定 >= 5，就是 5 * 100ms = 500ms 打印一次
|
||||
// 如果设定 >= 10，就是 10 * 100ms = 1000ms (1秒) 打印一次
|
||||
if (++print_divider < 5) {
|
||||
return; // 还没到设定的次数，直接 return，不执行后续获取快照和打印的动作
|
||||
}
|
||||
print_divider = 0; // 计数器清零，重新开始下一轮计数
|
||||
|
||||
// 1. 获取全局黑板的快照
|
||||
RobotBlackboard_t snap;
|
||||
Blackboard_GetSnapshot(&snap);
|
||||
|
||||
// 2. 申请串口互斥锁，防止多线程打印打架导致乱码
|
||||
if (logMutexHandle != NULL) {
|
||||
(void)osMutexAcquire(logMutexHandle, osWaitForever);
|
||||
}
|
||||
|
||||
// 3. 打印表头和当前系统时间
|
||||
printf("\r\n========== Robot Status [%lu ms] ==========\r\n", HAL_GetTick());
|
||||
|
||||
// 4. 打印 IMU 姿态
|
||||
if (snap.imu_yaw.is_valid) {
|
||||
printf("[IMU] Yaw: %7.2f deg | Cont: %7.2f deg | Wz: %7.2f\r\n",
|
||||
snap.imu_yaw.value, snap.imu_yaw_continuous.value, snap.imu_wz.value);
|
||||
} else {
|
||||
printf("[IMU] --- OFFLINE or INVALID ---\r\n");
|
||||
}
|
||||
|
||||
// 5. 打印侧向阵列雷达 (强制转为 int，去掉多余的小数点)
|
||||
printf("[%s] Left-F : %4d mm | Left-R : %4d mm\r\n",
|
||||
VL53_BOARD_DRIVER_NAME,
|
||||
(int)snap.dist_left_f.value, (int)snap.dist_left_r.value);
|
||||
printf(" Right-F: %4d mm | Right-R: %4d mm\r\n",
|
||||
(int)snap.dist_right_f.value, (int)snap.dist_right_r.value);
|
||||
|
||||
// 6. 打印前后激光测距雷达 (四个探头独立显示)
|
||||
printf("[LASER] F-STP: %4d mm | F-ATK: %4d mm\r\n",
|
||||
(int)snap.dist_front_stp.value, (int)snap.dist_front_atk.value);
|
||||
printf(" R-STP: %4d mm | R-ATK: %4d mm\r\n",
|
||||
(int)snap.dist_rear_stp.value, (int)snap.dist_rear_atk.value);
|
||||
printf("==================================================\r\n");
|
||||
|
||||
// 7. 释放锁
|
||||
if (logMutexHandle != NULL) {
|
||||
(void)osMutexRelease(logMutexHandle);
|
||||
}
|
||||
}
|
||||
|
||||
void AppTasks_RunCanTxTask(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
uint32_t wake_tick = osKernelGetTickCount();
|
||||
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
|
||||
// 用于存放从指令槽里取出来的控制指令
|
||||
RobotTargetCmd_t current_cmd;
|
||||
|
||||
for (;;) {
|
||||
// 维持心跳发送
|
||||
SNC_CAN_20msTask();
|
||||
|
||||
// 从指令槽中获取最新指令，设置 100ms 超时容忍度
|
||||
if (CmdSlot_Pop(¤t_cmd, 100)) {
|
||||
// 如果算法层在 100ms 内有更新指令，正常下发给底盘
|
||||
(void)SNC_CAN_SendCmdVel(current_cmd.target_vx, current_cmd.target_wz, current_cmd.ctrl_flags);
|
||||
} else {
|
||||
// 🚨 触发安全保护：如果超过 100ms 没收到上层新指令（可能算法死机或网络断开）
|
||||
// 强制发送 0 速度，让底盘立刻停车，防止飞车撞墙
|
||||
(void)SNC_CAN_SendCmdVel(0.0f, 0.0f, 0U);
|
||||
}
|
||||
|
||||
wake_tick += period_ticks;
|
||||
(void)osDelayUntil(wake_tick);
|
||||
}
|
||||
}
|
||||
|
||||
void AppTasks_RunMonitorTask(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
uint32_t wake_tick = osKernelGetTickCount();
|
||||
const uint32_t period_ticks = AppTasks_MsToTicks(100U);
|
||||
|
||||
for (;;) {
|
||||
uint32_t now_ms = HAL_GetTick();
|
||||
|
||||
// 1. 执行 CAN 侧的周期性监控 (心跳/超时判断)
|
||||
SNC_CAN_100msTask();
|
||||
SNC_CAN_PollOnlineState(now_ms);
|
||||
|
||||
// 2. 获取 CAN 数据上下文
|
||||
const SNC_CAN_AppContext_t *can_ctx = SNC_CAN_GetContext();
|
||||
|
||||
// 3. 将底盘的核心状态写入全局黑板
|
||||
Blackboard_UpdateChassisState(can_ctx->status.state,
|
||||
can_ctx->status.diag_bits,
|
||||
can_ctx->status.online);
|
||||
|
||||
// 4. 里程计更新已迁移到 navTask(20ms)，此处不再消费
|
||||
// 原因：odom 实时性对 EKF 预测至关重要，需要与导航闭环同频
|
||||
|
||||
/* 指令下发已由 navTask 接管，此处不再发送测试指令 */
|
||||
// 调用纯净版播报员 (内部自带分频逻辑)
|
||||
// App_PrintStatus();
|
||||
|
||||
wake_tick += period_ticks;
|
||||
(void)osDelayUntil(wake_tick);
|
||||
}
|
||||
}
|
||||
/* =========================================================
|
||||
* 1. 前后激光雷达任务 (Laser Manager)
|
||||
* ========================================================= */
|
||||
void AppTasks_RunLaserTestTask_Impl(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
|
||||
// 初始化前后防撞雷达
|
||||
LASER_SIMPLE_Init();
|
||||
|
||||
for (;;)
|
||||
{
|
||||
// 底层高频巡检
|
||||
LASER_SIMPLE_Poll(HAL_GetTick());
|
||||
|
||||
// 获取无撕裂快照
|
||||
const laser_simple_snapshot_t *snap = LASER_SIMPLE_GetSnapshot();
|
||||
|
||||
// 写入全局数据黑板
|
||||
Blackboard_UpdateLaser(snap);
|
||||
|
||||
// 维持 50ms 巡检频率 (避障数据建议稍快一些)
|
||||
osDelay(50);
|
||||
}
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 2. IMU 姿态传感器任务 (HWT101)
|
||||
* ========================================================= */
|
||||
void AppTasks_RunImuTask_Impl(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
|
||||
// 初始化 IMU (开启 DMA 接收)，使用 UART7
|
||||
HWT101_Init(&huart7);
|
||||
osDelay(100);
|
||||
HWT101_ZeroYaw();
|
||||
for (;;) {
|
||||
// 高频解析 DMA 数据
|
||||
HWT101_Process();
|
||||
|
||||
// 安全拷贝 IMU 数据并写入黑板
|
||||
HWT101_Data_t imu_data;
|
||||
HWT101_GetData(&imu_data);
|
||||
Blackboard_UpdateIMU(&imu_data);
|
||||
|
||||
osDelay(10); // 10ms 解析与更新周期
|
||||
}
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 3. 侧向阵列雷达任务 (VL53 双总线)
|
||||
* ========================================================= */
|
||||
void AppTasks_RunVl53Task_Impl(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
static Vl53Board_t left_board;
|
||||
static Vl53Board_t right_board;
|
||||
/* --- 配置左侧雷达 (I2C2) --- */
|
||||
Vl53BoardHwCfg_t left_cfgs[2] = {
|
||||
{&hi2c2, GPIOB, GPIO_PIN_1, 0x62, "LF", 0}, // 左前 (PB1)
|
||||
{&hi2c2, GPIOC, GPIO_PIN_5, 0x64, "LR", 1} // 左后 (PC5)
|
||||
};
|
||||
|
||||
// 【修改 1】：测距预算从 robot_params.h 读取
|
||||
Vl53Board_Init(&left_board, left_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
|
||||
Vl53Board_StartContinuous(&left_board);
|
||||
|
||||
/* --- 配置右侧雷达 (I2C1) --- */
|
||||
Vl53BoardHwCfg_t right_cfgs[2] = {
|
||||
{&hi2c1, GPIOD, GPIO_PIN_13, 0x56, "RF", 2}, // 右前 (PD13)
|
||||
{&hi2c1, GPIOD, GPIO_PIN_14, 0x58, "RR", 3} // 右后 (PD14)
|
||||
};
|
||||
|
||||
// 【修改 2】：同样从 robot_params.h 读取
|
||||
Vl53Board_Init(&right_board, right_cfgs, 2, PARAM_VL53_TIMING_BUDGET);
|
||||
Vl53Board_StartContinuous(&right_board);
|
||||
|
||||
Vl53BoardSnapshot_t left_snap, right_snap, combined_snap;
|
||||
|
||||
for(;;) {
|
||||
// 读取左右两侧的雷达快照
|
||||
Vl53Board_ReadAll(&left_board, &left_snap);
|
||||
Vl53Board_ReadAll(&right_board, &right_snap);
|
||||
|
||||
// 合并到一个统一的 Snapshot 中送给黑板
|
||||
memset(&combined_snap, 0, sizeof(combined_snap));
|
||||
combined_snap.tick_ms = HAL_GetTick();
|
||||
|
||||
// 映射约定：[0]左前，[1]左后，[2]右前，[3]右后
|
||||
for(int i = 0; i < 2; i++) {
|
||||
// 左侧复制到 [0], [1]
|
||||
combined_snap.range_mm[i] = left_snap.range_mm[i];
|
||||
combined_snap.range_mm_filtered[i] = left_snap.range_mm_filtered[i];
|
||||
combined_snap.range_status[i] = left_snap.range_status[i];
|
||||
|
||||
// 右侧偏移 2 个身位，复制到 [2], [3]
|
||||
combined_snap.range_mm[i+2] = right_snap.range_mm[i];
|
||||
combined_snap.range_mm_filtered[i+2] = right_snap.range_mm_filtered[i];
|
||||
combined_snap.range_status[i+2] = right_snap.range_status[i];
|
||||
}
|
||||
|
||||
// 合并掩码 (右侧掩码左移 2 位)
|
||||
combined_snap.valid_mask = (left_snap.valid_mask & 0x03) | ((right_snap.valid_mask & 0x03) << 2);
|
||||
|
||||
// 统一推送到黑板
|
||||
Blackboard_UpdateVl53(&combined_snap);
|
||||
|
||||
// 与 VL53 约 33ms 的连续测量节奏匹配，避免高频空轮询旧数据。
|
||||
osDelay(33);
|
||||
}
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 4. 走廊导航控制任务 (Nav Pipeline)
|
||||
* 周期：20ms (50Hz)，与 CAN 发送同频
|
||||
* 流水线：Obs → Filter → Ctrl → Script → FSM → CmdSlot_Push
|
||||
* ========================================================= */
|
||||
void AppTasks_RunNavTask_Impl(void *argument)
|
||||
{
|
||||
(void)argument;
|
||||
uint32_t wake_tick = osKernelGetTickCount();
|
||||
const uint32_t period_ticks = AppTasks_MsToTicks(20U);
|
||||
uint32_t last_ms = HAL_GetTick();
|
||||
|
||||
/* 等传感器全部就绪再启动 (避免刚上电全是脏数据)。
|
||||
* 实车上 IMU 清零和多路测距任务稳定需要数秒，500ms 不够。 */
|
||||
osDelay(PARAM_NAV_STARTUP_DELAY_MS);
|
||||
|
||||
#if USE_GLOBAL_NAV
|
||||
GlobalNav_Start(); /* 开始赛道级导航 */
|
||||
#else
|
||||
NavScript_Start(); /* 单沟测试模式 */
|
||||
#endif
|
||||
|
||||
for (;;) {
|
||||
uint32_t now_ms = HAL_GetTick();
|
||||
float dt_s = (float)(now_ms - last_ms) / 1000.0f;
|
||||
if (dt_s <= 0.0f || dt_s > 0.5f) {
|
||||
dt_s = 0.02f; /* 容错：防止首拍或溢出 */
|
||||
}
|
||||
last_ms = now_ms;
|
||||
|
||||
/* --- Step 0: 里程计更新（从 monitorTask 迁移至此，20ms 实时消费）--- */
|
||||
{
|
||||
int16_t odom_fl, odom_rl, odom_fr, odom_rr;
|
||||
uint32_t odom_span_ms;
|
||||
uint8_t odom_frames = SNC_CAN_ConsumeOdomDelta(
|
||||
&odom_fl, &odom_rl,
|
||||
&odom_fr, &odom_rr,
|
||||
&odom_span_ms);
|
||||
|
||||
if (odom_frames > 0U) {
|
||||
/* 有新的增量帧需要积分 */
|
||||
Odom_Update(now_ms,
|
||||
odom_fl, odom_rl,
|
||||
odom_fr, odom_rr,
|
||||
odom_span_ms);
|
||||
} else {
|
||||
/* odom 断流保护：超过协议允许时间后主动清零黑板里的速度 */
|
||||
Odom_HandleTimeout(now_ms, SNC_ODOM_TIMEOUT_MS);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- Step 1: 拍摄黑板快照 --- */
|
||||
RobotBlackboard_t board;
|
||||
Blackboard_GetSnapshot(&board);
|
||||
|
||||
/* --- Step 2: 预处理 → 清洗观测 --- */
|
||||
CorridorObs_t obs;
|
||||
CorridorPreproc_ExtractObs(&board, now_ms, &obs);
|
||||
|
||||
/* --- Step 3: EKF → 走廊状态估计 --- */
|
||||
/* 注意: HWT101 输出 wz 单位是 °/s，EKF 需要 rad/s，必须转换！ */
|
||||
float imu_wz_raw = board.imu_wz.is_valid ? board.imu_wz.value : 0.0f;
|
||||
float imu_wz = PARAM_DEG2RAD(imu_wz_raw);
|
||||
float odom_vx = board.odom_vx;
|
||||
|
||||
/* IMU 连续 yaw → rad，作为 EKF 额外航向观测 */
|
||||
float imu_yaw_cont_rad = board.imu_yaw_continuous.is_valid
|
||||
? PARAM_DEG2RAD(board.imu_yaw_continuous.value) : 0.0f;
|
||||
bool imu_yaw_ok = board.imu_yaw_continuous.is_valid;
|
||||
|
||||
CorridorState_t corridor_state;
|
||||
CorridorFilter_Update(&obs, imu_wz, odom_vx, dt_s,
|
||||
imu_yaw_cont_rad, imu_yaw_ok, &corridor_state);
|
||||
|
||||
#if USE_GLOBAL_NAV
|
||||
/* ========== 赛道级导航模式 (6沟 S 型遍历) ========== */
|
||||
|
||||
/* --- Step 4: 赛道级导航 --- */
|
||||
GlobalNavOutput_t nav_out;
|
||||
GlobalNav_Update(&obs, &corridor_state, &board, now_ms, &nav_out);
|
||||
|
||||
/* --- Step 5: 控制律 --- */
|
||||
RawCmd_t raw_cmd;
|
||||
memset(&raw_cmd, 0, sizeof(raw_cmd));
|
||||
raw_cmd.t_ms = now_ms;
|
||||
|
||||
if (nav_out.request_corridor) {
|
||||
/* 沟内闭环：使用走廊控制器 */
|
||||
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
|
||||
} else if (nav_out.use_override) {
|
||||
/* 赛道级覆盖：直接用导航输出 */
|
||||
raw_cmd.v = nav_out.override_v;
|
||||
raw_cmd.w = nav_out.override_w;
|
||||
}
|
||||
/* else: raw_cmd 已是零速 */
|
||||
|
||||
/* --- Step 6: 安全仲裁 (带动作模式感知) --- */
|
||||
SegFsmOutput_t fsm_out;
|
||||
SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);
|
||||
|
||||
#else
|
||||
/* ========== 单沟测试模式 (原 nav_script) ========== */
|
||||
|
||||
/* --- Step 4: 段脚本执行器 --- */
|
||||
NavScriptOutput_t script_out;
|
||||
float imu_yaw_cont_deg = board.imu_yaw_continuous.is_valid
|
||||
? board.imu_yaw_continuous.value : 0.0f;
|
||||
NavScript_Update(&obs, &corridor_state, imu_yaw_cont_deg, &script_out);
|
||||
|
||||
/* --- Step 5: 控制律 --- */
|
||||
RawCmd_t raw_cmd;
|
||||
if (script_out.use_override) {
|
||||
raw_cmd.t_ms = now_ms;
|
||||
raw_cmd.v = script_out.override_v;
|
||||
raw_cmd.w = script_out.override_w;
|
||||
raw_cmd.flags = 0U;
|
||||
} else if (script_out.request_corridor) {
|
||||
CorridorCtrl_Compute(&corridor_state, &obs, imu_wz, &raw_cmd);
|
||||
} else {
|
||||
raw_cmd.t_ms = now_ms;
|
||||
raw_cmd.v = 0.0f;
|
||||
raw_cmd.w = 0.0f;
|
||||
raw_cmd.flags = 0U;
|
||||
}
|
||||
|
||||
/* --- Step 6: 安全仲裁 (按脚本阶段切换模式) --- */
|
||||
SegFsmOutput_t fsm_out;
|
||||
SafetyMode_t safety_mode = SAFETY_MODE_CORRIDOR;
|
||||
if (!script_out.active) {
|
||||
safety_mode = SAFETY_MODE_IDLE;
|
||||
} else if (script_out.stage == SCRIPT_STAGE_TURN_AT_END) {
|
||||
safety_mode = SAFETY_MODE_TURN;
|
||||
}
|
||||
SegFsm_Update(&raw_cmd, &obs, &corridor_state, safety_mode, &fsm_out);
|
||||
|
||||
#endif
|
||||
|
||||
/* --- Step 7: 将安全后的指令喂给 CAN 发送层 --- */
|
||||
CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0U);
|
||||
|
||||
wake_tick += period_ticks;
|
||||
(void)osDelayUntil(wake_tick);
|
||||
}
|
||||
}
|
||||
|
||||
void AppTasks_Init(void)
|
||||
{
|
||||
SNC_CAN_AppInit();
|
||||
Odom_Init();
|
||||
|
||||
/* --- 初始化走廊滤波器 (EKF) --- */
|
||||
CorridorFilterConfig_t filter_cfg = {
|
||||
.sensor_base_length = PARAM_SENSOR_BASE_LENGTH, /* 实测：同侧前后雷达间距 */
|
||||
.corridor_width = PARAM_CORRIDOR_WIDTH, /* 实测：走廊宽度 */
|
||||
.y_offset = 0.0f, /* 0 = 绝对居中 */
|
||||
.side_sensor_inset = PARAM_VL53_SIDE_INSET, /* [兼容] 统一内缩距离 */
|
||||
.left_sensor_inset = PARAM_VL53_LEFT_INSET, /* [改进A] 左侧独立内缩 (实测后填入!) */
|
||||
.right_sensor_inset = PARAM_VL53_RIGHT_INSET, /* [改进A] 右侧独立内缩 (实测后填入!) */
|
||||
.robot_width = PARAM_ROBOT_WIDTH, /* 实测：车体宽度 */
|
||||
.alpha_theta = 0.98f, /* 保留兼容，EKF 内部使用 Q/R */
|
||||
.alpha_y = 0.7f,
|
||||
};
|
||||
CorridorFilter_Init(&filter_cfg);
|
||||
|
||||
/* --- 初始化走廊控制器 --- */
|
||||
CorridorCtrlConfig_t ctrl_cfg = {
|
||||
.kp_theta = PARAM_CTRL_KP_THETA, /* 调优：航向比例增益 */
|
||||
.kd_theta = PARAM_CTRL_KD_THETA, /* 调优：航向微分增益 */
|
||||
.kp_y = PARAM_CTRL_KP_Y, /* 调优：横向比例增益 */
|
||||
.v_cruise = PARAM_CTRL_V_CRUISE, /* 调优：巡航速度 */
|
||||
.w_max = PARAM_CTRL_W_MAX, /* 角速度限幅 */
|
||||
.v_max = PARAM_CTRL_V_MAX, /* 线速度限幅 */
|
||||
.speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */
|
||||
.exit_front_dist = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */
|
||||
.wall_escape_dist = PARAM_CTRL_WALL_ESCAPE_DIST,
|
||||
.wall_escape_kp = PARAM_CTRL_WALL_ESCAPE_KP,
|
||||
.wall_escape_w_max = PARAM_CTRL_WALL_ESCAPE_WMAX,
|
||||
};
|
||||
CorridorCtrl_Init(&ctrl_cfg);
|
||||
|
||||
/* --- 初始化段状态机 --- */
|
||||
SegFsmConfig_t fsm_cfg = {
|
||||
.d_front_stop = PARAM_SAFE_D_FRONT_STOP, /* 安全：前向停车距离 */
|
||||
.d_front_approach = PARAM_SAFE_D_FRONT_APPROACH, /* 安全：前向减速距离 */
|
||||
.approach_min_v = PARAM_SAFE_APPROACH_MIN_V, /* 安全：减速区最低速度 */
|
||||
.conf_estop_thresh = PARAM_SAFE_CONF_ESTOP, /* 安全：E-Stop 置信度阈值 */
|
||||
};
|
||||
SegFsm_Init(&fsm_cfg);
|
||||
SegFsm_Start(); /* P0 修复: 必须显式启动安全状态机，否则 IDLE 状态直接输出零速 */
|
||||
|
||||
#if USE_GLOBAL_NAV
|
||||
/* --- 初始化赛道地图 --- */
|
||||
TrackMap_Init();
|
||||
|
||||
/* --- 初始化赛道级导航状态机 --- */
|
||||
GlobalNavConfig_t gnav_cfg = {
|
||||
.entry_v = PARAM_GNAV_ENTRY_V,
|
||||
.entry_distance = PARAM_GNAV_ENTRY_DISTANCE,
|
||||
.entry_timeout_ms = PARAM_GNAV_ENTRY_TIMEOUT,
|
||||
.turn_omega = PARAM_GNAV_TURN_OMEGA,
|
||||
.turn_tolerance_rad = PARAM_GNAV_TURN_TOLERANCE,
|
||||
.turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE,
|
||||
.turn_min_omega = PARAM_GNAV_TURN_MIN_OMEGA,
|
||||
.turn_timeout_ms = PARAM_GNAV_TURN_TIMEOUT,
|
||||
.reacquire_v = PARAM_GNAV_REACQUIRE_V,
|
||||
.reacquire_conf_thresh = PARAM_GNAV_REACQUIRE_CONF,
|
||||
.reacquire_width_tol = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
|
||||
.reacquire_min_odom = PARAM_GNAV_REACQUIRE_MIN_ODOM,
|
||||
.reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
|
||||
.reacquire_timeout_ms = PARAM_GNAV_REACQUIRE_TIMEOUT,
|
||||
.align_kp_th = PARAM_GNAV_ALIGN_KP_TH,
|
||||
.align_kp_y = PARAM_GNAV_ALIGN_KP_Y,
|
||||
.align_th_tol_rad = PARAM_GNAV_ALIGN_TH_TOL,
|
||||
.align_y_tol_m = PARAM_GNAV_ALIGN_Y_TOL,
|
||||
.align_confirm_ticks = PARAM_GNAV_ALIGN_TICKS,
|
||||
.align_timeout_ms = PARAM_GNAV_ALIGN_TIMEOUT,
|
||||
.reacquire_min_back_dist = PARAM_GNAV_REACQUIRE_MIN_BACK,
|
||||
.corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
|
||||
.corridor_length_max = PARAM_GNAV_CORRIDOR_MAX_LEN,
|
||||
.link_v = PARAM_GNAV_LINK_V,
|
||||
.link_distance = PARAM_GNAV_LINK_DISTANCE,
|
||||
.link_timeout_ms = PARAM_GNAV_LINK_TIMEOUT,
|
||||
.link_gap_runout = PARAM_GNAV_LINK_GAP_RUNOUT,
|
||||
.link_wall_target = PARAM_GNAV_LINK_WALL_TARGET,
|
||||
.link_wall_kp = PARAM_GNAV_LINK_WALL_KP,
|
||||
.link_wall_heading_kp = PARAM_GNAV_LINK_WALL_HEADING_KP,
|
||||
.link_wall_blend = PARAM_GNAV_LINK_WALL_BLEND,
|
||||
.exit_v = PARAM_GNAV_EXIT_V,
|
||||
.exit_runout = PARAM_GNAV_EXIT_RUNOUT,
|
||||
.exit_max_dist = PARAM_GNAV_EXIT_MAX_DIST,
|
||||
.exit_timeout_ms = PARAM_GNAV_EXIT_TIMEOUT,
|
||||
.dock_v = PARAM_GNAV_DOCK_V,
|
||||
.dock_distance = PARAM_GNAV_DOCK_DISTANCE,
|
||||
.heading_kp = PARAM_GNAV_HEADING_KP,
|
||||
.corridor_width = PARAM_CORRIDOR_WIDTH,
|
||||
};
|
||||
GlobalNav_Init(&gnav_cfg);
|
||||
|
||||
#else
|
||||
/* --- 初始化段脚本执行器 (单沟测试模式) --- */
|
||||
NavScriptConfig_t script_cfg = {
|
||||
.turn_target_angle = 3.14159265f, /* 固定：180度转向 */
|
||||
.turn_omega = PARAM_SCRIPT_TURN_OMEGA, /* 调优：转向角速度 */
|
||||
.corridor_length = 3.0f, /* 备用：垄沟长度估计 */
|
||||
.entry_align_timeout = PARAM_SCRIPT_ENTRY_TIMEOUT, /* 调优：入口对准超时 */
|
||||
.d_entry_exit_front = 0.12f, /* 调优：出入口距离阈值 */
|
||||
.entry_align_v = PARAM_SCRIPT_ENTRY_V, /* 调优：入口对准速度 */
|
||||
.exit_runout_m = PARAM_SCRIPT_EXIT_RUNOUT, /* 调优：退出后冲出距离 */
|
||||
.exit_v = PARAM_SCRIPT_EXIT_V, /* P1 修复：退出直线速度独立参数 */
|
||||
};
|
||||
NavScript_Init(&script_cfg);
|
||||
#endif
|
||||
}
|
||||
33
App/app_tasks.h
Normal file
33
App/app_tasks.h
Normal file
@@ -0,0 +1,33 @@
|
||||
#ifndef APP_TASKS_H
|
||||
#define APP_TASKS_H
|
||||
#include "cmsis_os2.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
/* =========================================================
|
||||
* 启动接力标志位定义 (按 Bit 划分)
|
||||
* ========================================================= */
|
||||
#define INIT_FLAG_CAN_READY (1UL << 0) // Bit 0: CAN 握手完成
|
||||
#define INIT_FLAG_IMU_READY (1UL << 1) // Bit 1: IMU 初始化完成
|
||||
#define INIT_FLAG_VL53_READY (1UL << 2) // Bit 2: 侧向雷达初始化完成
|
||||
#define INIT_FLAG_LASER_READY (1UL << 3) // Bit 3: 防撞雷达初始化完成
|
||||
|
||||
/* 全局事件组句柄声明 */
|
||||
extern osEventFlagsId_t g_init_events;
|
||||
void AppTasks_RunCanTxTask(void *argument);
|
||||
void AppTasks_RunMonitorTask(void *argument);
|
||||
|
||||
/* 三个核心传感器采集与黑板更新任务 */
|
||||
void AppTasks_RunImuTask_Impl(void *argument);
|
||||
void AppTasks_RunLaserTestTask_Impl(void *argument);
|
||||
void AppTasks_RunVl53Task_Impl(void *argument);
|
||||
void AppTasks_RunNavTask_Impl(void *argument);
|
||||
void AppTasks_Init(void);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* APP_TASKS_H */
|
||||
649
App/est/corridor_ekf.c
Normal file
649
App/est/corridor_ekf.c
Normal file
@@ -0,0 +1,649 @@
|
||||
/**
|
||||
* @file corridor_ekf.c
|
||||
* @brief 鲁棒 EKF 走廊相对定位滤波器实现
|
||||
*
|
||||
* 完整算法流程：
|
||||
*
|
||||
* 【预测步】Predict(vx, wz, dt)
|
||||
* x_pred = f(x, u) -- 非线性状态转移
|
||||
* P_pred = F * P * F^T + Q -- 协方差预测
|
||||
*
|
||||
* 【更新步】Update(obs)
|
||||
* z = h(x_pred) -- 观测预测
|
||||
* y = z_meas - z -- 新息 (Innovation)
|
||||
* S = H * P_pred * H^T + R -- 新息协方差
|
||||
* d² = y^T * S^(-1) * y -- 马氏距离平方
|
||||
* if d² > χ²_threshold: 拒绝观测 (鲁棒)
|
||||
* K = P_pred * H^T * S^(-1) -- 卡尔曼增益
|
||||
* x = x_pred + K * y -- 状态更新
|
||||
* P = (I - K * H) * P_pred -- 协方差更新
|
||||
*/
|
||||
|
||||
#include "corridor_ekf.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/* =========================================================
|
||||
* 内部静态状态
|
||||
* ========================================================= */
|
||||
static CorridorEKFConfig_t s_cfg;
|
||||
static CorridorEKFState_t s_state;
|
||||
static bool s_initialized = false;
|
||||
static uint32_t s_last_update_ms = 0U;
|
||||
|
||||
/* 协方差上界保护阈值 */
|
||||
#define P_MAX_DIAG 100.0f
|
||||
|
||||
/* =========================================================
|
||||
* 内部辅助函数
|
||||
* ========================================================= */
|
||||
|
||||
/** 限幅 */
|
||||
static inline float clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
/** 对称矩阵拷贝 + 双向取平均 (减少舍入误差传播) */
|
||||
static void symmetrize(float M[3][3])
|
||||
{
|
||||
float avg;
|
||||
avg = (M[0][1] + M[1][0]) * 0.5f;
|
||||
M[0][1] = M[1][0] = avg;
|
||||
|
||||
avg = (M[0][2] + M[2][0]) * 0.5f;
|
||||
M[0][2] = M[2][0] = avg;
|
||||
|
||||
avg = (M[1][2] + M[2][1]) * 0.5f;
|
||||
M[1][2] = M[2][1] = avg;
|
||||
}
|
||||
|
||||
/** 角度归一化到 [-π, π]
|
||||
* 防止 IMU yaw 累积角度跨越 ±π 时导致新息突变
|
||||
*/
|
||||
static float wrap_angle(float angle)
|
||||
{
|
||||
const float PI = 3.14159265358979323846f;
|
||||
while (angle > PI) angle -= 2.0f * PI;
|
||||
while (angle < -PI) angle += 2.0f * PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
/** P 上界保护 */
|
||||
static void protect_P(float P[3][3])
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (P[i][i] > P_MAX_DIAG) P[i][i] = P_MAX_DIAG;
|
||||
if (P[i][i] < 0.0f) P[i][i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/** Joseph 形式协方差更新 (1DOF 标量观测)
|
||||
* P_new = (I - K*H) * P * (I - K*H)^T + K * R * K^T
|
||||
*
|
||||
* 参数:
|
||||
* P[3][3] - 先验协方差 (输入/输出)
|
||||
* K[3] - 卡尔曼增益向量
|
||||
* H[3] - 观测矩阵 (1x3 行向量)
|
||||
* R - 观测噪声方差 (标量)
|
||||
*
|
||||
* 优点: 保证数值稳定性，即使有舍入误差也能保持 P 正定
|
||||
*/
|
||||
static void joseph_update_P(float P[3][3], const float K[3], const float H[3], float R)
|
||||
{
|
||||
/* 计算 A = (I - K*H) */
|
||||
float A[3][3];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
A[i][j] = (i == j ? 1.0f : 0.0f) - K[i] * H[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* 计算 A * P */
|
||||
float AP[3][3];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
AP[i][j] = 0.0f;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
AP[i][j] += A[i][k] * P[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 计算 A * P * A^T */
|
||||
float APAT[3][3];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
APAT[i][j] = 0.0f;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
APAT[i][j] += AP[i][k] * A[j][k]; // A^T[k][j] = A[j][k]
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 计算 K * R * K^T 并加到 APAT */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
P[i][j] = APAT[i][j] + K[i] * R * K[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** 计算 2x2 对称矩阵的逆 (原地) */
|
||||
static bool invert_2x2_sym(float S[2][2])
|
||||
{
|
||||
float det = S[0][0] * S[1][1] - S[0][1] * S[1][0];
|
||||
if (fabsf(det) < 1e-8f) {
|
||||
return false; // 奇异矩阵
|
||||
}
|
||||
float inv_det = 1.0f / det;
|
||||
float S00 = S[0][0];
|
||||
S[0][0] = inv_det * S[1][1];
|
||||
S[1][1] = inv_det * S00;
|
||||
S[0][1] = -inv_det * S[0][1];
|
||||
S[1][0] = S[0][1];
|
||||
return true;
|
||||
}
|
||||
|
||||
/** 2x2 对称矩阵求逆 (原地) */
|
||||
static bool invert_3x3_cholesky(float S[3][3])
|
||||
{
|
||||
// 使用 Cholesky 分解求逆
|
||||
float L[3][3] = {0};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j <= i; j++) {
|
||||
float sum = S[i][j];
|
||||
for (int k = 0; k < j; k++) {
|
||||
sum -= L[i][k] * L[j][k];
|
||||
}
|
||||
if (i == j) {
|
||||
if (sum <= 0.0f) return false;
|
||||
L[i][j] = sqrtf(sum);
|
||||
} else {
|
||||
L[i][j] = sum / L[j][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 求逆: S_inv = L^(-T) * L^(-1)
|
||||
float Linv[3][3] = {0};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
Linv[i][i] = 1.0f / L[i][i];
|
||||
for (int j = i - 1; j >= 0; j--) {
|
||||
float sum = 0.0f;
|
||||
for (int k = j + 1; k <= i; k++) {
|
||||
sum += L[k][j] * Linv[k][i];
|
||||
}
|
||||
Linv[j][i] = -sum / L[j][j];
|
||||
}
|
||||
}
|
||||
|
||||
// S_inv = Linv^T * Linv
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
float sum = 0.0f;
|
||||
for (int k = 0; k < 3; k++) {
|
||||
sum += Linv[k][i] * Linv[k][j];
|
||||
}
|
||||
S[i][j] = sum;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/** 计算马氏距离平方 (新息向量 y, 新息协方差 S_inv) */
|
||||
static float mahalanobis_d2_3dof(const float y[3], const float S_inv[3][3])
|
||||
{
|
||||
// d² = y^T * S_inv * y
|
||||
float tmp[3] = {0};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
tmp[i] += S_inv[i][j] * y[j];
|
||||
}
|
||||
}
|
||||
float d2 = 0.0f;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
d2 += y[i] * tmp[i];
|
||||
}
|
||||
return d2;
|
||||
}
|
||||
|
||||
/** 计算马氏距离平方 (1DOF: 只用 e_y) */
|
||||
static float mahalanobis_d2_1dof(float y_ey, float S_inv_00)
|
||||
{
|
||||
return y_ey * y_ey * S_inv_00;
|
||||
}
|
||||
|
||||
/** 计算马氏距离平方 (2DOF: e_ey + e_th_avg) */
|
||||
static float mahalanobis_d2_2dof(const float y[2], const float S_inv[2][2])
|
||||
{
|
||||
float tmp[2] = {0};
|
||||
for (int i = 0; i < 2; i++) {
|
||||
for (int j = 0; j < 2; j++) {
|
||||
tmp[i] += S_inv[i][j] * y[j];
|
||||
}
|
||||
}
|
||||
float d2 = 0.0f;
|
||||
for (int i = 0; i < 2; i++) {
|
||||
d2 += y[i] * tmp[i];
|
||||
}
|
||||
return d2;
|
||||
}
|
||||
|
||||
/** 零矩阵 */
|
||||
static void zero_3x3(float M[3][3])
|
||||
{
|
||||
memset(M, 0, sizeof(float) * 9);
|
||||
}
|
||||
|
||||
/** 单位矩阵 */
|
||||
static void eye_3x3(float M[3][3])
|
||||
{
|
||||
zero_3x3(M);
|
||||
M[0][0] = M[1][1] = M[2][2] = 1.0f;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void CorridorEKF_Init(const CorridorEKFConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
memset(&s_state, 0, sizeof(s_state));
|
||||
|
||||
/* 初始化状态 */
|
||||
s_state.x[0] = 0.0f; // e_y
|
||||
s_state.x[1] = 0.0f; // e_th
|
||||
s_state.x[2] = 0.0f; // s
|
||||
|
||||
/* 初始化协方差 */
|
||||
eye_3x3(s_state.P);
|
||||
s_state.P[0][0] = config->P0_diag[0];
|
||||
s_state.P[1][1] = config->P0_diag[1];
|
||||
s_state.P[2][2] = config->P0_diag[2];
|
||||
|
||||
s_initialized = true;
|
||||
}
|
||||
|
||||
void CorridorEKF_Reset(void)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
s_state.x[0] = 0.0f;
|
||||
s_state.x[1] = 0.0f;
|
||||
s_state.x[2] = 0.0f;
|
||||
|
||||
eye_3x3(s_state.P);
|
||||
s_state.P[0][0] = s_cfg.P0_diag[0];
|
||||
s_state.P[1][1] = s_cfg.P0_diag[1];
|
||||
s_state.P[2][2] = s_cfg.P0_diag[2];
|
||||
|
||||
s_last_update_ms = 0U;
|
||||
}
|
||||
|
||||
void CorridorEKF_ResetHeading(void)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
s_state.x[1] = 0.0f;
|
||||
|
||||
/* 清理航向与其它状态的耦合，避免旧航向误差继续通过协方差传播。 */
|
||||
s_state.P[0][1] = 0.0f;
|
||||
s_state.P[1][0] = 0.0f;
|
||||
s_state.P[1][2] = 0.0f;
|
||||
s_state.P[2][1] = 0.0f;
|
||||
s_state.P[1][1] = s_cfg.P0_diag[1];
|
||||
}
|
||||
|
||||
void CorridorEKF_RebaseAfterTurnaround(void)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
/* 同一条走廊掉头后，新的前进方向相反，横向误差符号需要镜像。 */
|
||||
s_state.x[0] = -s_state.x[0];
|
||||
s_state.x[1] = 0.0f;
|
||||
|
||||
/* e_y 与 e_th 的相关项在掉头后不再可直接沿用，清零重新收敛。 */
|
||||
s_state.P[0][1] = 0.0f;
|
||||
s_state.P[1][0] = 0.0f;
|
||||
s_state.P[1][2] = 0.0f;
|
||||
s_state.P[2][1] = 0.0f;
|
||||
s_state.P[1][1] = s_cfg.P0_diag[1];
|
||||
}
|
||||
|
||||
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s)
|
||||
{
|
||||
s_cfg.q_ey = q_ey;
|
||||
s_cfg.q_eth = q_eth;
|
||||
s_cfg.q_s = q_s;
|
||||
}
|
||||
|
||||
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth)
|
||||
{
|
||||
s_cfg.r_ey = r_ey;
|
||||
s_cfg.r_eth = r_eth;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 预测步 (Predict)
|
||||
* ========================================================= */
|
||||
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt)
|
||||
{
|
||||
if (!s_initialized || dt <= 0.0f) return;
|
||||
|
||||
float e_y = s_state.x[0];
|
||||
float e_th = s_state.x[1];
|
||||
float s = s_state.x[2];
|
||||
float vx = odom_vx;
|
||||
float wz = imu_wz;
|
||||
|
||||
/* 状态预测: x_pred = f(x, u) */
|
||||
float cos_th = cosf(e_th);
|
||||
float sin_th = sinf(e_th);
|
||||
|
||||
/* 安全检查: cos_th 不能太小 (防止数值爆炸) */
|
||||
if (fabsf(cos_th) < 0.01f) cos_th = (cos_th >= 0.0f) ? 0.01f : -0.01f;
|
||||
|
||||
float e_y_pred = e_y + vx * sin_th * dt;
|
||||
float e_th_pred = e_th + wz * dt;
|
||||
float s_pred = s + vx * cos_th * dt;
|
||||
|
||||
s_state.x[0] = e_y_pred;
|
||||
s_state.x[1] = e_th_pred;
|
||||
s_state.x[2] = s_pred;
|
||||
|
||||
/* 雅可比矩阵 F (状态转移的 Jacobian) */
|
||||
float F[3][3] = {0};
|
||||
F[0][0] = 1.0f;
|
||||
F[0][1] = vx * cos_th * dt; // de_y/de_th
|
||||
F[0][2] = 0.0f;
|
||||
|
||||
F[1][0] = 0.0f;
|
||||
F[1][1] = 1.0f;
|
||||
F[1][2] = 0.0f;
|
||||
|
||||
F[2][0] = 0.0f;
|
||||
F[2][1] = -vx * sin_th * dt; // ds/de_th
|
||||
F[2][2] = 1.0f;
|
||||
|
||||
/* 过程噪声协方差 Q (含耦合项)
|
||||
* [改进] 添加 e_y 和 e_th 的耦合噪声，反映横向-航向动力学耦合 */
|
||||
float Q[3][3] = {0};
|
||||
Q[0][0] = s_cfg.q_ey * dt * dt;
|
||||
Q[1][1] = s_cfg.q_eth * dt * dt;
|
||||
Q[2][2] = s_cfg.q_s * dt * dt;
|
||||
Q[0][1] = Q[1][0] = s_cfg.q_ey_eth * dt * dt; // 横向-航向耦合
|
||||
|
||||
/* 协方差预测: P_pred = F * P * F^T + Q */
|
||||
float F_P[3][3] = {0};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
for (int k = 0; k < 3; k++) {
|
||||
F_P[i][j] += F[i][k] * s_state.P[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float P_pred[3][3] = {0};
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
for (int k = 0; k < 3; k++) {
|
||||
P_pred[i][j] += F_P[i][k] * F[j][k]; // F^T: F[j][k] = F[k][j]
|
||||
}
|
||||
P_pred[i][j] += Q[i][j]; // 加过程噪声 (含非对角项)
|
||||
}
|
||||
}
|
||||
|
||||
symmetrize(P_pred);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
s_state.P[i][j] = P_pred[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
protect_P(s_state.P);
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 观测步 (Update) - 鲁棒 EKF
|
||||
*
|
||||
* 设计决策 (方向 B — IMU 主导航向)：
|
||||
* 侧墙激光仅用于更新横向位置 e_y，不再构建航向观测 z_eth_L/z_eth_R。
|
||||
* 航向 e_th 完全由 IMU 主导：
|
||||
* - 预测步: imu_wz 驱动 e_th 积分
|
||||
* - 观测步: CorridorEKF_UpdateIMUYaw() 提供 yaw_continuous 标量约束
|
||||
* 侧墙前后差分 (d_lr-d_lf) 的噪声在 ±2cm 误差下过大，不适合做航向主观测。
|
||||
* ========================================================= */
|
||||
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state)
|
||||
{
|
||||
if (!s_initialized) return 0;
|
||||
|
||||
/* 维护最近一次 EKF 输出对应的观测时间戳，供 GetState() 返回一致结果。 */
|
||||
s_last_update_ms = obs->t_ms;
|
||||
|
||||
int updated_obs_count = 0;
|
||||
|
||||
/* 清除新息和拒绝掩码 */
|
||||
memset(s_state.K, 0, sizeof(s_state.K));
|
||||
uint8_t reject_mask = 0U;
|
||||
float max_maha_d2 = 0.0f;
|
||||
|
||||
/* ----------------------------------------------------
|
||||
* 提取有效观测
|
||||
* ---------------------------------------------------- */
|
||||
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 1)) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 3)) != 0U);
|
||||
|
||||
/* 左右侧横向平均距离 */
|
||||
float d_lf = obs->d_lf, d_lr = obs->d_lr;
|
||||
float d_rf = obs->d_rf, d_rr = obs->d_rr;
|
||||
float W = s_cfg.corridor_width;
|
||||
float yoff = s_cfg.y_offset;
|
||||
float Rw = s_cfg.robot_width;
|
||||
|
||||
/* [改进A] 分侧传感器内缩补偿 — 消除左右安装不对称引起的系统性偏置
|
||||
* 左右各自使用独立的 inset 值计算期望居中读数 d_center */
|
||||
float d_center_left = (W - Rw) / 2.0f + s_cfg.left_sensor_inset;
|
||||
float d_center_right = (W - Rw) / 2.0f + s_cfg.right_sensor_inset;
|
||||
|
||||
/* 观测值 (测量) — 仅横向位置，不含航向 */
|
||||
float z_ey = 0.0f;
|
||||
int valid_sides = 0;
|
||||
|
||||
if (left_ok) {
|
||||
z_ey += d_center_left - ((d_lf + d_lr) / 2.0f) - yoff;
|
||||
valid_sides++;
|
||||
}
|
||||
|
||||
if (right_ok) {
|
||||
z_ey += ((d_rf + d_rr) / 2.0f) - d_center_right - yoff;
|
||||
valid_sides++;
|
||||
}
|
||||
|
||||
if (valid_sides == 0) {
|
||||
out_state->t_ms = obs->t_ms;
|
||||
out_state->e_y = s_state.x[0];
|
||||
out_state->e_th = s_state.x[1];
|
||||
out_state->s = s_state.x[2];
|
||||
out_state->conf = clampf(1.0f - (s_state.P[0][0] + s_state.P[1][1]) * 0.1f, 0.0f, 1.0f);
|
||||
out_state->obs_reject_mask = 0xFF;
|
||||
out_state->mahalanobis_d2 = 0.0f;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
out_state->P[i][j] = s_state.P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
/* 协方差膨胀 (无观测时的信任衰减) — 仅膨胀 e_y */
|
||||
s_state.P[0][0] += s_cfg.q_ey * 5.0f;
|
||||
protect_P(s_state.P);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (valid_sides == 2) {
|
||||
z_ey /= 2.0f;
|
||||
}
|
||||
|
||||
/* ----------------------------------------------------
|
||||
* 1DOF 标量 EKF 更新 — 仅 e_y
|
||||
* ---------------------------------------------------- */
|
||||
float e_y = s_state.x[0];
|
||||
float y_ey = z_ey - e_y;
|
||||
|
||||
/* [改进F] 自适应观测噪声 R:
|
||||
* 双侧观测: R × 0.5 (噪声互相平均)
|
||||
* 单侧观测: R × 3.0 (缺少交叉验证, VL53 可信度低时尤需降低信任)
|
||||
*/
|
||||
float R_ey = s_cfg.r_ey;
|
||||
if (valid_sides == 2) {
|
||||
R_ey *= 0.5f;
|
||||
} else if (valid_sides == 1) {
|
||||
R_ey *= 3.0f;
|
||||
}
|
||||
float S_ey = s_state.P[0][0] + R_ey;
|
||||
|
||||
if (fabsf(S_ey) < 1e-8f) {
|
||||
goto output_result;
|
||||
}
|
||||
|
||||
float d2_ey = y_ey * y_ey / S_ey;
|
||||
max_maha_d2 = d2_ey;
|
||||
|
||||
if (d2_ey > s_cfg.chi2_1dof) {
|
||||
reject_mask |= (1U << 0);
|
||||
goto output_result;
|
||||
}
|
||||
|
||||
float S_inv_ey = 1.0f / S_ey;
|
||||
float K_ey[3];
|
||||
K_ey[0] = s_state.P[0][0] * S_inv_ey;
|
||||
K_ey[1] = s_state.P[1][0] * S_inv_ey;
|
||||
K_ey[2] = s_state.P[2][0] * S_inv_ey;
|
||||
|
||||
s_state.x[0] += K_ey[0] * y_ey;
|
||||
s_state.x[1] += K_ey[1] * y_ey;
|
||||
s_state.x[2] += K_ey[2] * y_ey;
|
||||
|
||||
/* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
|
||||
* H = [1, 0, 0] (仅观测 e_y) */
|
||||
float H_ey[3] = {1.0f, 0.0f, 0.0f};
|
||||
joseph_update_P(s_state.P, K_ey, H_ey, R_ey);
|
||||
|
||||
symmetrize(s_state.P);
|
||||
protect_P(s_state.P);
|
||||
|
||||
updated_obs_count = 1;
|
||||
|
||||
output_result:
|
||||
out_state->t_ms = obs->t_ms;
|
||||
out_state->e_y = s_state.x[0];
|
||||
out_state->e_th = s_state.x[1];
|
||||
out_state->s = s_state.x[2];
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
out_state->P[i][j] = s_state.P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
out_state->mahalanobis_d2 = max_maha_d2;
|
||||
out_state->obs_reject_mask = reject_mask;
|
||||
|
||||
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
|
||||
float conf_from_P = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
|
||||
float side_factor = (valid_sides == 2) ? 1.0f : 0.7f;
|
||||
float reject_penalty = (reject_mask & (1U << 0)) ? 0.5f : 1.0f;
|
||||
|
||||
out_state->conf = clampf(conf_from_P * side_factor * reject_penalty, 0.0f, 1.0f);
|
||||
|
||||
return updated_obs_count;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 辅助 API
|
||||
* ========================================================= */
|
||||
|
||||
/* =========================================================
|
||||
* IMU 航向观测更新 (独立 1DOF 标量 EKF 更新)
|
||||
*
|
||||
* 观测方程: z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
|
||||
* 对应状态: e_th (x[1])
|
||||
* H = [0, 1, 0] (只观测 e_th)
|
||||
*
|
||||
* 设计意图：
|
||||
* - IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定
|
||||
* - 在侧墙观测丢失 (转弯/单侧退化) 时提供航向约束
|
||||
* - 使用较大 R 值，让侧墙观测在有效时主导
|
||||
* ========================================================= */
|
||||
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid)
|
||||
{
|
||||
if (!s_initialized || !valid) return;
|
||||
|
||||
/* 观测值: IMU 相对于走廊参考方向的航向偏差
|
||||
* [改进] 角度归一化防止 ±π 跨越时新息突变 */
|
||||
float z_eth_imu = wrap_angle(imu_yaw_rad - imu_yaw_ref_rad);
|
||||
|
||||
/* 新息: y = z - h(x), h(x) = e_th = x[1]
|
||||
* [改进] 再次归一化，防止 e_th 和 z_eth_imu 符号不同时差值超出 [-π, π] */
|
||||
float y_imu = wrap_angle(z_eth_imu - s_state.x[1]);
|
||||
|
||||
/* H = [0, 1, 0] → S = P[1][1] + R_imu */
|
||||
float R_imu = s_cfg.r_eth_imu;
|
||||
float S_imu = s_state.P[1][1] + R_imu;
|
||||
|
||||
if (fabsf(S_imu) < 1e-8f) return;
|
||||
|
||||
/* χ² 1DOF 检验: d² = y² / S */
|
||||
float d2_imu = y_imu * y_imu / S_imu;
|
||||
if (d2_imu > s_cfg.chi2_1dof) return; /* 拒绝异常观测 */
|
||||
|
||||
/* 卡尔曼增益: K = P * H^T / S = P[:][1] / S */
|
||||
float K[3];
|
||||
float S_inv = 1.0f / S_imu;
|
||||
K[0] = s_state.P[0][1] * S_inv;
|
||||
K[1] = s_state.P[1][1] * S_inv;
|
||||
K[2] = s_state.P[2][1] * S_inv;
|
||||
|
||||
/* 状态更新: x += K * y */
|
||||
s_state.x[0] += K[0] * y_imu;
|
||||
s_state.x[1] += K[1] * y_imu;
|
||||
s_state.x[2] += K[2] * y_imu;
|
||||
|
||||
/* Joseph 形式协方差更新: P = (I-KH)*P*(I-KH)^T + K*R*K^T
|
||||
* H = [0, 1, 0] (仅观测 e_th) */
|
||||
float H_imu[3] = {0.0f, 1.0f, 0.0f};
|
||||
joseph_update_P(s_state.P, K, H_imu, R_imu);
|
||||
|
||||
symmetrize(s_state.P);
|
||||
protect_P(s_state.P);
|
||||
}
|
||||
|
||||
void CorridorEKF_GetState(CorridorState_t *out)
|
||||
{
|
||||
if (!s_initialized || out == NULL) return;
|
||||
|
||||
out->t_ms = s_last_update_ms;
|
||||
out->e_y = s_state.x[0];
|
||||
out->e_th = s_state.x[1];
|
||||
out->s = s_state.x[2];
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
out->P[i][j] = s_state.P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
out->mahalanobis_d2 = 0.0f;
|
||||
out->obs_reject_mask = 0U;
|
||||
|
||||
float P_trace = s_state.P[0][0] + s_state.P[1][1] + s_state.P[2][2];
|
||||
out->conf = clampf(1.0f - P_trace * 0.5f, 0.0f, 1.0f);
|
||||
}
|
||||
161
App/est/corridor_ekf.h
Normal file
161
App/est/corridor_ekf.h
Normal file
@@ -0,0 +1,161 @@
|
||||
/**
|
||||
* @file corridor_ekf.h
|
||||
* @brief 鲁棒 EKF 走廊相对定位滤波器
|
||||
*
|
||||
* 状态向量：x = [e_y, e_th, s]^T
|
||||
* - e_y : 横向偏差 (m)
|
||||
* - e_th : 航向偏差 (rad)
|
||||
* - s : 沿走廊进度 (m)
|
||||
*
|
||||
* 观测模型 (方向 B — IMU 主导航向):
|
||||
* - 侧墙观测: 1DOF 标量更新，仅更新 e_y (横向偏差)
|
||||
* z_ey = 左右侧 VL53 平均距离差 (支持分侧内缩补偿)
|
||||
* - IMU 航向观测: 独立 1DOF 标量更新，更新 e_th
|
||||
* z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
|
||||
* - 侧墙航向观测 (z_eth_L/z_eth_R) 已取消:
|
||||
* VL53 前后差分噪声过大 (±2cm → ~13° 航向噪声)，不适合做航向源
|
||||
*
|
||||
* 鲁棒机制：
|
||||
* - χ² 马氏距离检验拒绝异常观测
|
||||
* - 分侧 VL53 内缩补偿 (消除左右安装不对称引起的系统性偏置)
|
||||
* - 单侧观测时自适应增大 R (降低退化状态下的信任度)
|
||||
* - 协方差上界保护 (防止发散)
|
||||
*/
|
||||
|
||||
#ifndef CORRIDOR_EKF_H
|
||||
#define CORRIDOR_EKF_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* 先引入消息定义 (含 EKF_STATE_DIM/EKF_OBS_DIM 宏) */
|
||||
#include "preproc/corridor_msgs.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* =========================================================
|
||||
* EKF 配置参数
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
/* 过程噪声协方差 Q */
|
||||
float q_ey; // e_y 过程噪声方差
|
||||
float q_eth; // e_th 过程噪声方差
|
||||
float q_s; // s 过程噪声方差
|
||||
float q_ey_eth; // [改进] e_y 和 e_th 耦合噪声 (横向-航向动力学耦合)
|
||||
|
||||
/* 观测噪声协方差 R */
|
||||
float r_ey; // 横向观测噪声方差 (侧墙)
|
||||
float r_eth; // 航向观测噪声方差 (侧墙)
|
||||
float r_eth_imu; // 航向观测噪声方差 (IMU yaw)，IMU 准确度高时可设较小值
|
||||
|
||||
/* 初始协方差 */
|
||||
float P0_diag[3]; // 初始 P 对角线
|
||||
|
||||
/* χ² 检验门限 */
|
||||
float chi2_1dof; // 1 自由度门限 (默认 3.84)
|
||||
float chi2_2dof; // 2 自由度门限 (默认 5.99)
|
||||
|
||||
/* 走廊几何参数 */
|
||||
float sensor_base_length; // 同侧前后雷达间距 L_s
|
||||
float corridor_width; // 走廊标准宽度
|
||||
float y_offset; // 期望偏置
|
||||
float side_sensor_inset; // [兼容] 侧向传感器统一内缩距离 (当 left/right 未单独设置时使用)
|
||||
float left_sensor_inset; // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘, 实测)
|
||||
float right_sensor_inset; // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘, 实测)
|
||||
float robot_width; // 车体外轮廓宽度 (用于单侧退化时的精确定位)
|
||||
} CorridorEKFConfig_t;
|
||||
|
||||
/* =========================================================
|
||||
* EKF 内部状态
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
float x[EKF_STATE_DIM]; // 状态向量
|
||||
float P[EKF_STATE_DIM][EKF_STATE_DIM]; // 状态协方差
|
||||
float K[EKF_STATE_DIM][EKF_OBS_DIM]; // 卡尔曼增益
|
||||
float S[EKF_OBS_DIM][EKF_OBS_DIM]; // 新息协方差
|
||||
float S_inv[EKF_OBS_DIM][EKF_OBS_DIM]; // S 的逆
|
||||
} CorridorEKFState_t;
|
||||
|
||||
/* =========================================================
|
||||
* API 接口
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 初始化 EKF 滤波器
|
||||
* @param config 配置参数
|
||||
*/
|
||||
void CorridorEKF_Init(const CorridorEKFConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief EKF 预测步 (时间更新)
|
||||
* @param odom_vx 里程计线速度 (m/s)
|
||||
* @param imu_wz IMU 角速度 (rad/s)
|
||||
* @param dt 时间间隔 (s)
|
||||
*/
|
||||
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt);
|
||||
|
||||
/**
|
||||
* @brief EKF 观测步 (测量更新) - 鲁棒版本
|
||||
* @param obs 预处理后的观测快照
|
||||
* @param out_state 输出状态 (含马氏距离、拒绝掩码等)
|
||||
* @return 成功更新的观测数
|
||||
*/
|
||||
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state);
|
||||
|
||||
/**
|
||||
* @brief EKF IMU 航向观测更新 (独立于侧墙观测，在 Update 后调用)
|
||||
*
|
||||
* 将 IMU 连续 yaw 的变化量作为 e_th 的额外标量观测，执行 1DOF EKF 更新。
|
||||
* 在侧墙观测丢失时 (转弯/单侧退化) 提供航向约束，防止 e_th 漂移。
|
||||
*
|
||||
* @param imu_yaw_rad IMU 连续 yaw 当前值 (rad)
|
||||
* @param imu_yaw_ref_rad 进入走廊时锁定的 IMU yaw 参考值 (rad)
|
||||
* z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
|
||||
* @param valid IMU 数据是否有效
|
||||
*/
|
||||
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid);
|
||||
|
||||
/**
|
||||
* @brief 获取当前状态估计
|
||||
*/
|
||||
void CorridorEKF_GetState(CorridorState_t *out);
|
||||
|
||||
/**
|
||||
* @brief 重置 EKF 状态
|
||||
*/
|
||||
void CorridorEKF_Reset(void);
|
||||
|
||||
/**
|
||||
* @brief 仅重置航向相关状态，用于掉头后重新建立走廊朝向基准
|
||||
*
|
||||
* 保留横向位置 e_y 与进度 s，只将 e_th 清零并清理其相关协方差，
|
||||
* 避免上一趟积累的航向误差在返程首拍继续驱动控制器猛打方向。
|
||||
*/
|
||||
void CorridorEKF_ResetHeading(void);
|
||||
|
||||
/**
|
||||
* @brief 180° 掉头后重建走廊状态
|
||||
*
|
||||
* 对同一条走廊原地掉头后：
|
||||
* - 航向误差 e_th 应回到 0
|
||||
* - 横向误差 e_y 在新的前进方向下符号需要翻转
|
||||
*/
|
||||
void CorridorEKF_RebaseAfterTurnaround(void);
|
||||
|
||||
/**
|
||||
* @brief 设置过程噪声 (运行时可调)
|
||||
*/
|
||||
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s);
|
||||
|
||||
/**
|
||||
* @brief 设置观测噪声 (运行时可调，用于自适应)
|
||||
*/
|
||||
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* CORRIDOR_EKF_H */
|
||||
167
App/est/corridor_filter.c
Normal file
167
App/est/corridor_filter.c
Normal file
@@ -0,0 +1,167 @@
|
||||
/**
|
||||
* @file corridor_filter.c
|
||||
* @brief 走廊相对定位滤波器 - 鲁棒 EKF 实现
|
||||
*
|
||||
* 本文件是对外统一接口层，内部调用 corridor_ekf 模块
|
||||
* 保持与原有互补滤波接口兼容，方便无缝替换
|
||||
*/
|
||||
|
||||
#include "corridor_filter.h"
|
||||
#include "corridor_ekf.h"
|
||||
#include "preproc/corridor_msgs.h" /* 引入 CHI2_THRESHOLD 宏 */
|
||||
#include "robot_params.h" /* 引入 PARAM_EKF_* 宏 */
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/* =========================================================
|
||||
* 兼容层配置
|
||||
* ========================================================= */
|
||||
static CorridorFilterConfig_t s_cfg;
|
||||
static bool s_initialized = false;
|
||||
|
||||
/* IMU yaw 参考值管理：进入走廊时锁定，用于计算相对走廊方向的航向偏差 */
|
||||
static float s_imu_yaw_ref_rad = 0.0f; // 走廊参考 yaw (rad)
|
||||
static bool s_imu_yaw_ref_set = false; // 参考值是否已锁定
|
||||
|
||||
/* =========================================================
|
||||
* 初始化
|
||||
* ========================================================= */
|
||||
void CorridorFilter_Init(const CorridorFilterConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
|
||||
/* 转换为 EKF 配置 */
|
||||
CorridorEKFConfig_t ekf_cfg;
|
||||
memset(&ekf_cfg, 0, sizeof(ekf_cfg));
|
||||
|
||||
/* 走廊几何参数 */
|
||||
ekf_cfg.sensor_base_length = config->sensor_base_length;
|
||||
ekf_cfg.corridor_width = config->corridor_width;
|
||||
ekf_cfg.y_offset = config->y_offset;
|
||||
ekf_cfg.side_sensor_inset = config->side_sensor_inset;
|
||||
ekf_cfg.left_sensor_inset = config->left_sensor_inset; /* [改进A] 左侧独立内缩 */
|
||||
ekf_cfg.right_sensor_inset = config->right_sensor_inset; /* [改进A] 右侧独立内缩 */
|
||||
ekf_cfg.robot_width = config->robot_width;
|
||||
|
||||
/* 过程噪声 Q —— 统一从 robot_params.h 读取，改参数只改那一个文件 */
|
||||
ekf_cfg.q_ey = PARAM_EKF_Q_EY; /* 横向过程噪声 */
|
||||
ekf_cfg.q_eth = PARAM_EKF_Q_ETH; /* 航向过程噪声 */
|
||||
ekf_cfg.q_s = PARAM_EKF_Q_S; /* 里程过程噪声 */
|
||||
ekf_cfg.q_ey_eth = PARAM_EKF_Q_EY_ETH; /* [改进] 横向-航向耦合噪声 */
|
||||
|
||||
/* 观测噪声 R */
|
||||
ekf_cfg.r_ey = PARAM_EKF_R_EY; /* 横向观测噪声 (侧墙) */
|
||||
ekf_cfg.r_eth = PARAM_EKF_R_ETH; /* 航向观测噪声 (侧墙) */
|
||||
ekf_cfg.r_eth_imu = PARAM_EKF_R_ETH_IMU; /* 航向观测噪声 (IMU yaw)，较大 */
|
||||
|
||||
/* 初始协方差 */
|
||||
ekf_cfg.P0_diag[0] = PARAM_EKF_P0_EY; /* e_y 初始不确定度 */
|
||||
ekf_cfg.P0_diag[1] = PARAM_EKF_P0_ETH; /* e_th 初始不确定度 */
|
||||
ekf_cfg.P0_diag[2] = 0.0f; /* s 初始确定（已知从 0 出发） */
|
||||
|
||||
/* χ² 门限 */
|
||||
ekf_cfg.chi2_1dof = PARAM_CHI2_1DOF;
|
||||
ekf_cfg.chi2_2dof = PARAM_CHI2_2DOF;
|
||||
|
||||
CorridorEKF_Init(&ekf_cfg);
|
||||
s_initialized = true;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 核心更新函数
|
||||
* ========================================================= */
|
||||
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
|
||||
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
|
||||
CorridorState_t *out_state)
|
||||
{
|
||||
if (!s_initialized || dt_s <= 0.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* 预测步 */
|
||||
CorridorEKF_Predict(odom_vx, imu_wz, dt_s);
|
||||
|
||||
/* 侧墙观测更新步 (含马氏距离异常检测) */
|
||||
CorridorEKF_Update(obs, out_state);
|
||||
|
||||
/* ---- IMU yaw 航向观测更新 ---- *
|
||||
* 在侧墙观测之后独立执行 1DOF 标量更新。
|
||||
*
|
||||
* 方向 B 改造：IMU 现在是航向 e_th 的唯一观测来源。
|
||||
* 侧墙不再提供航向观测。
|
||||
*
|
||||
* [改进E] 参考值管理策略 (IMU 驱动即时锁定):
|
||||
* IMU yaw 准确度高，转向完成后 IMU 指向即走廊方向。
|
||||
* 不再等 VL53 收敛 (conf >= 0.3)，而是在 EKF reset 后
|
||||
* 首次收到有效 IMU yaw 即锁定参考值。
|
||||
* 此时 e_th = 0 (刚 reset)，ref = imu_yaw。
|
||||
*/
|
||||
if (imu_yaw_valid) {
|
||||
if (!s_imu_yaw_ref_set) {
|
||||
/* [改进E] 即时锁定: 不依赖 VL53 置信度，IMU 有效即锁定 */
|
||||
s_imu_yaw_ref_rad = imu_yaw_continuous_rad; /* e_th 刚 reset 为 0，ref = imu_yaw */
|
||||
s_imu_yaw_ref_set = true;
|
||||
}
|
||||
|
||||
if (s_imu_yaw_ref_set) {
|
||||
CorridorEKF_UpdateIMUYaw(imu_yaw_continuous_rad, s_imu_yaw_ref_rad, true);
|
||||
|
||||
/* [改进D] 只更新状态值 (e_y, e_th, s, P)，
|
||||
* 保留 CorridorEKF_Update() 计算的 conf, obs_reject_mask, mahalanobis_d2。
|
||||
* 原来直接调用 GetState() 会用纯 P_trace 重算 conf 并清零诊断字段。 */
|
||||
CorridorState_t imu_updated;
|
||||
CorridorEKF_GetState(&imu_updated);
|
||||
out_state->e_y = imu_updated.e_y;
|
||||
out_state->e_th = imu_updated.e_th;
|
||||
out_state->s = imu_updated.s;
|
||||
memcpy(out_state->P, imu_updated.P, sizeof(out_state->P));
|
||||
/* conf, obs_reject_mask, mahalanobis_d2 保留自 CorridorEKF_Update() 的值 */
|
||||
out_state->t_ms = obs->t_ms;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* 重置 (进入新垄沟时调用)
|
||||
* ========================================================= */
|
||||
void CorridorFilter_Reset(void)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
/* 重置 EKF 内核: 状态归零, 协方差恢复初始值 */
|
||||
CorridorEKF_Reset();
|
||||
|
||||
/* 解锁 IMU yaw 参考值, 等待在新沟中重新锁定 */
|
||||
s_imu_yaw_ref_rad = 0.0f;
|
||||
s_imu_yaw_ref_set = false;
|
||||
}
|
||||
|
||||
void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
/* 同一条沟原地掉头后：当前朝向成为新参考，
|
||||
* 同时横向误差符号要镜像，航向误差要回零。 */
|
||||
CorridorEKF_RebaseAfterTurnaround();
|
||||
s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
|
||||
s_imu_yaw_ref_set = true;
|
||||
}
|
||||
|
||||
void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad)
|
||||
{
|
||||
if (!s_initialized) return;
|
||||
|
||||
CorridorEKF_ResetHeading();
|
||||
s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
|
||||
s_imu_yaw_ref_set = true;
|
||||
}
|
||||
|
||||
void CorridorFilter_CorrectIMUReference(float heading_correction_rad)
|
||||
{
|
||||
if (!s_initialized || !s_imu_yaw_ref_set) return;
|
||||
|
||||
/* 修正IMU参考值，用于消除转向系统性偏差
|
||||
* 例如：VL53检测到车头偏右2度，heading_correction_rad = -0.035 rad
|
||||
* 修正后，EKF会认为当前IMU方向才是正确的走廊方向 */
|
||||
s_imu_yaw_ref_rad += heading_correction_rad;
|
||||
}
|
||||
86
App/est/corridor_filter.h
Normal file
86
App/est/corridor_filter.h
Normal file
@@ -0,0 +1,86 @@
|
||||
#ifndef CORRIDOR_FILTER_H
|
||||
#define CORRIDOR_FILTER_H
|
||||
#include "preproc/corridor_preproc.h"
|
||||
#include "preproc/corridor_msgs.h"
|
||||
|
||||
/* 滤波器配置参数结构体 */
|
||||
typedef struct {
|
||||
float sensor_base_length; // 同侧前后雷达的纵向安装间距 L_s (m)
|
||||
float corridor_width; // 走廊标准宽度 (m)，比赛规则为 0.4m
|
||||
float y_offset; // 期望的偏置行走量 (m)，0 表示绝对居中
|
||||
float side_sensor_inset; // [兼容] VL53L0X 传感器统一内缩距离 (m)，当分侧值未设置时使用
|
||||
float left_sensor_inset; // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘) (m)
|
||||
float right_sensor_inset; // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘) (m)
|
||||
float robot_width; // 车体外轮廓宽度 (m)
|
||||
float alpha_theta; // 航向互补滤波系数 (0~1)
|
||||
// 【注】因为您的IMU极好，此值建议设为 0.98~0.995
|
||||
float alpha_y; // 横向低通滤波系数 (0~1)，建议设为 0.6~0.8 防止地毯颠簸
|
||||
} CorridorFilterConfig_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 初始化走廊滤波器
|
||||
* @param config 滤波器调参配置
|
||||
*/
|
||||
void CorridorFilter_Init(const CorridorFilterConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 核心函数：执行一次走廊状态估计 (建议与 CAN 发送同频，如 50Hz/20ms运行)
|
||||
* @param obs 预处理模块输出的、清洗干净的雷达观测快照
|
||||
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，左转为正
|
||||
* @param odom_vx 底盘反馈的当前里程计线速度 (m/s)
|
||||
* @param dt_s 距离上一次调用的时间间隔 (s)
|
||||
* @param imu_yaw_continuous_rad IMU unwrap 后的连续偏航角 (rad)
|
||||
* @param imu_yaw_valid IMU 数据是否有效
|
||||
* @param out_state 输出平滑后的 e_y (横向误差), e_th (航向误差) 和健康度
|
||||
*/
|
||||
void CorridorFilter_Update(const CorridorObs_t *obs, float imu_wz, float odom_vx,
|
||||
float dt_s, float imu_yaw_continuous_rad, bool imu_yaw_valid,
|
||||
CorridorState_t *out_state);
|
||||
|
||||
/**
|
||||
* @brief 重置滤波器状态 (进入新垄沟时必须调用)
|
||||
*
|
||||
* 重置内容：
|
||||
* - EKF 状态向量清零 (e_y=0, e_th=0, s=0)
|
||||
* - 协方差恢复到初始值
|
||||
* - IMU yaw 参考值解锁，等待重新锁定
|
||||
*/
|
||||
void CorridorFilter_Reset(void);
|
||||
|
||||
/**
|
||||
* @brief 单沟 180° 掉头后重建滤波器参考
|
||||
*
|
||||
* 同一条沟掉头后需要同时：
|
||||
* - 将当前 IMU yaw 作为新的走廊航向参考
|
||||
* - 清零航向误差 e_th
|
||||
* - 镜像横向误差 e_y 的符号
|
||||
*/
|
||||
void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad);
|
||||
|
||||
/**
|
||||
* @brief 仅重建航向参考，不改横向位置
|
||||
*
|
||||
* 用于进入新垄沟后，已经通过侧墙把车头摆正，
|
||||
* 这时把当前 IMU yaw 设为新的走廊参考，同时仅清零 e_th。
|
||||
*/
|
||||
void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad);
|
||||
|
||||
/**
|
||||
* @brief 修正IMU航向参考值（用于消除转向系统性偏差）
|
||||
*
|
||||
* 用于赛道模式转向后，用VL53检测到的墙壁航向误差修正IMU参考值
|
||||
* 只修正参考值，不改变当前状态估计
|
||||
*
|
||||
* @param heading_correction_rad 航向修正量 (rad)，正值表示车头需要左转
|
||||
*/
|
||||
void CorridorFilter_CorrectIMUReference(float heading_correction_rad);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // CORRIDOR_FILTER_H
|
||||
175
App/laser/Laser Manager.md
Normal file
175
App/laser/Laser Manager.md
Normal file
@@ -0,0 +1,175 @@
|
||||
激光雷达管理模块 (Laser Manager) 🚀
|
||||
===========================
|
||||
|
||||
本模块专为 **STM32H7 + FreeRTOS** 环境打造，用于同时驱动和解析 4 路单点激光雷达（2 路连续流 STP-23L，2 路文本流 ATK-MS53L1M）。
|
||||
|
||||
采用 **DMA 环形缓冲 + 串口空闲中断 (IDLE) + RTOS 事件驱动** 架构，彻底消灭单字节中断引起的 CPU 拥堵，是适用于机器人底盘避障与路径规划的“企业级”底层驱动。
|
||||
✨ 核心特性
|
||||
------
|
||||
|
||||
1. **零中断阻塞**：所有数据均由硬件 DMA 搬运，CPU 仅在数据接收过半/完成或触发 IDLE 空闲时被唤醒。
|
||||
|
||||
2. **免 Cache 物理隔离**：基于 MPU 划分专用 `.dma_buffer` 特区，彻底杜绝 Cortex-M7 的 Cache 一致性导致的数据乱码或 USB 掉线问题。
|
||||
|
||||
3. **极速解析算法**：
|
||||
|
||||
* **STP 协议**：采用 O(1) 环形指针流式提取，**0 内存搬运 (`memmove`)**。
|
||||
|
||||
* **ATK 协议**：采用 O(1) 字符特征匹配，彻底抛弃耗时的 `strstr` 和 `strtol` 标准库函数。
|
||||
|
||||
4. **高质量数据输出**：内置 **滑动中值滤波 (Median Filter)**，有效剔除玻璃反光、灰尘引起的突发测距尖峰，提供平滑稳定的控制级数据。
|
||||
|
||||
5. **安全可靠**：
|
||||
|
||||
* 采用 `taskENTER_CRITICAL()` 保护快照读取，杜绝多线程数据撕裂。
|
||||
|
||||
* 严格区分中断上下文 (`FROM_ISR`) 与任务上下文的临界区调用。
|
||||
|
||||
* 内置 `HAL_UART_ErrorCallback`，在遭受电磁干扰触发 ORE/FE 错误时自动重启 DMA，永久防死机。
|
||||
|
||||
* * *
|
||||
|
||||
⚙️ 移植与配置指南
|
||||
----------
|
||||
|
||||
### 第一步：STM32CubeMX 硬件配置
|
||||
|
||||
1. **MPU 设置 (极其重要)**
|
||||
|
||||
* 进入 `Cortex-M7` -> `MPU Control`
|
||||
|
||||
* Enable MPU
|
||||
|
||||
* **Region 1**: Base Address = **`0x30000000`**, Size = **`32KB`**
|
||||
|
||||
* Access Permission: **`ALL ACCESS PERMITTED`**
|
||||
|
||||
* Cacheable: **`Disable`** (彻底关闭该区域的 D-Cache)
|
||||
|
||||
2. **串口与 DMA 设置**
|
||||
|
||||
* **STP 雷达** (如 USART2, UART4 - 230400bps)：添加 RX DMA，模式选 **`Circular`** (数据宽度 Byte)。优先级建议设为 **`High`**。
|
||||
|
||||
* **ATK 雷达** (如 USART3, USART6 - 115200bps)：添加 RX DMA，模式选 **`Circular`**。优先级设为 **`Medium`**。
|
||||
|
||||
* **NVIC**：必须开启这 4 个串口的**全局中断 (Global Interrupt)**。
|
||||
|
||||
### 第二步：链接脚本 (.ld) 配置
|
||||
|
||||
打开工程的链接脚本（如 `STM32H743XX_FLASH.ld`），在 `RAM_D2` 区域添加 `.dma_buffer` 段定义：
|
||||
|
||||
代码段
|
||||
/* 给雷达 DMA 专门划分的免 Cache 区域 */
|
||||
.dma_buffer (NOLOAD) :
|
||||
{
|
||||
. = ALIGN(32);
|
||||
*(.dma_buffer)
|
||||
*(.dma_buffer*)
|
||||
. = ALIGN(32);
|
||||
} >RAM_D2
|
||||
|
||||
### 第三步：中断回调接管
|
||||
|
||||
确保在 `usart.c` 或 `main.c` 中 **删除或注释掉** 旧的串口接收回调，因为 `laser_manager.c` 底部已经重写了以下函数并接管了整个链路：
|
||||
|
||||
* `HAL_UART_RxCpltCallback`
|
||||
|
||||
* `HAL_UART_RxHalfCpltCallback`
|
||||
|
||||
* `HAL_UARTEx_RxEventCallback` (用于处理 IDLE)
|
||||
|
||||
* `HAL_UART_ErrorCallback` (用于自动恢复错误)
|
||||
|
||||
* * *
|
||||
|
||||
💻 API 使用说明
|
||||
-----------
|
||||
|
||||
### 1. 数据结构概览
|
||||
|
||||
C
|
||||
typedef struct {
|
||||
uint16_t distance_mm; // 经过中值滤波处理后平滑的距离值 (推荐用于避障控制)
|
||||
uint16_t raw_distance_mm; // 传感器吐出的原始跳动距离值 (仅供调试观察)
|
||||
uint8_t valid; // 数据是否有效 (1:有效, 0:无效)
|
||||
uint8_t online; // 雷达是否在线 (未超时)
|
||||
uint8_t fault_code; // 故障码 (0x00:正常, 0x80:掉线超时)
|
||||
uint32_t update_tick_ms; // 最后一次数据刷新的系统 tick
|
||||
} laser_simple_data_t;
|
||||
|
||||
### 2. 初始化与任务启动
|
||||
|
||||
在 FreeRTOS 调度器启动前（或统一的应用初始化任务中）调用初始化接口。它会自动配置并开启 DMA，同时创建一个高优先级的解析守护任务。
|
||||
|
||||
C
|
||||
#include "laser/laser_manager.h"
|
||||
|
||||
// 在 main.c 或 app_tasks.c 初始化阶段调用
|
||||
LASER_SIMPLE_Init();
|
||||
|
||||
### 3. 数据轮询与读取 (应用层)
|
||||
|
||||
在你的应用周期任务中（如 `100ms` 测试任务或控制任务），调用 `Poll` 并获取快照指针：
|
||||
|
||||
C
|
||||
void AppTasks_RunLaserTestTask_Impl(void *argument)
|
||||
{
|
||||
const laser_simple_snapshot_t *snap;
|
||||
uint32_t print_divider = 0;
|
||||
|
||||
LASER_SIMPLE_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
// 1. 触发状态机巡检 (更新在线状态/超时)
|
||||
LASER_SIMPLE_Poll(HAL_GetTick());
|
||||
|
||||
// 2. 获取绝对安全、无撕裂的全局数据快照
|
||||
snap = LASER_SIMPLE_GetSnapshot();
|
||||
|
||||
// 3. 降频打印 (例如每 500ms 打印一次)
|
||||
if (++print_divider >= 5)
|
||||
{
|
||||
print_divider = 0;
|
||||
printf("F_STP [Filter:%4umm, Raw:%4umm] Valid:%d Fault:%02X\r\n",
|
||||
snap->ch[LASER_CH_FRONT_STP].distance_mm,
|
||||
snap->ch[LASER_CH_FRONT_STP].raw_distance_mm,
|
||||
snap->ch[LASER_CH_FRONT_STP].valid,
|
||||
snap->ch[LASER_CH_FRONT_STP].fault_code);
|
||||
}
|
||||
|
||||
osDelay(100); // 维持高频巡检节拍 (切勿大于雷达设定的 Timeout 时间)
|
||||
}
|
||||
}
|
||||
|
||||
### 4. 便捷测距接口
|
||||
|
||||
如果你的小车只需要知道“前方/后方离障碍物最近是多少”，可以直接调用辅助接口：
|
||||
|
||||
C
|
||||
// 获取前/后两路雷达中，最近且有效的平滑距离值 (毫米)
|
||||
uint16_t front_min_dist = LASER_SIMPLE_GetFrontNearest();
|
||||
uint16_t rear_min_dist = LASER_SIMPLE_GetRearNearest();
|
||||
|
||||
* * *
|
||||
|
||||
❓ 常见问题 (Troubleshooting)
|
||||
------------------------
|
||||
|
||||
**Q: 烧录程序后，USB 虚拟串口无法识别 (或电脑提示无法识别的 USB 设备)？**
|
||||
|
||||
> **A:** 请检查 MPU 配置是否生效，以及链接脚本中是否正确加入了 `.dma_buffer`。如果 DMA 数组被编译器分配到了带有 D-Cache 的内存中，USB 外设会因为内存一致性冲突而死机。
|
||||
|
||||
**Q: 雷达数据一直是 0，且终端不停输出 FaultCode `0x80`？**
|
||||
|
||||
> **A:** `0x80` 代表超时掉线。请检查：
|
||||
>
|
||||
> 1. 雷达硬件连线（RX/TX 是否接反）。
|
||||
>
|
||||
> 2. `CubeMX` 中 4 个串口是否正确开启了**全局中断**（未开启中断将无法唤醒解析任务）。
|
||||
>
|
||||
> 3. 波特率是否匹配（STP: `230400`，ATK: `115200`）。
|
||||
|
||||
**Q: 想要调整中值滤波的平滑程度？**
|
||||
|
||||
> **A:** 在 `laser_manager.c` 顶部修改 `#define FILTER_WIN_SZ 3U`。改为 `5U` 会更平滑，但会增加几毫秒的系统响应延迟。通常 `3` 是最佳平衡点。
|
||||
317
App/laser/laser_manager.c
Normal file
317
App/laser/laser_manager.c
Normal file
@@ -0,0 +1,317 @@
|
||||
#include "laser_manager.h"
|
||||
#include <string.h>
|
||||
#include "cmsis_os2.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
|
||||
/* --- 配置区 --- */
|
||||
#define STP_TIMEOUT_MS 200U
|
||||
#define ATK_TIMEOUT_MS 500U
|
||||
|
||||
#define FILTER_WIN_SZ 3U // 中值滤波窗口大小（推荐 3 或 5）
|
||||
|
||||
#define FAULT_NONE 0x00
|
||||
#define FAULT_STP_CHECKSUM 0x41
|
||||
#define FAULT_STP_FRAME 0x42
|
||||
#define FAULT_TIMEOUT 0x80
|
||||
|
||||
/* MPU 无 Cache 段宏定义 */
|
||||
#if defined(__GNUC__)
|
||||
#define LASER_DMA_RAM __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)))
|
||||
#else
|
||||
#define LASER_DMA_RAM
|
||||
#endif
|
||||
|
||||
/* 缓冲区大小 */
|
||||
#define STP_DMA_SZ 512U
|
||||
#define ATK_DMA_SZ 256U
|
||||
|
||||
/* 分配在无 Cache 区域的 DMA 内存 */
|
||||
static LASER_DMA_RAM uint8_t g_stp_front_dma[STP_DMA_SZ];
|
||||
static LASER_DMA_RAM uint8_t g_atk_front_dma[ATK_DMA_SZ];
|
||||
static LASER_DMA_RAM uint8_t g_stp_rear_dma[STP_DMA_SZ];
|
||||
static LASER_DMA_RAM uint8_t g_atk_rear_dma[ATK_DMA_SZ];
|
||||
|
||||
/* 数据节点与快照 */
|
||||
typedef struct {
|
||||
UART_HandleTypeDef *huart;
|
||||
uint8_t *dma_buf;
|
||||
uint16_t dma_sz;
|
||||
uint16_t read_ptr;
|
||||
uint32_t last_tick;
|
||||
|
||||
// 滤波队列缓存
|
||||
uint16_t dist_history[FILTER_WIN_SZ];
|
||||
uint8_t hist_idx;
|
||||
uint8_t hist_cnt;
|
||||
|
||||
laser_simple_data_t data;
|
||||
} laser_node_t;
|
||||
|
||||
static laser_node_t g_nodes[LASER_CH_MAX] = {
|
||||
[LASER_CH_FRONT_STP] = { &huart2, g_stp_front_dma, STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
|
||||
[LASER_CH_FRONT_ATK] = { &huart3, g_atk_front_dma, ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
|
||||
[LASER_CH_REAR_STP] = { &huart4, g_stp_rear_dma, STP_DMA_SZ, 0, 0, {0}, 0, 0, {0} },
|
||||
[LASER_CH_REAR_ATK] = { &huart6, g_atk_rear_dma, ATK_DMA_SZ, 0, 0, {0}, 0, 0, {0} }
|
||||
};
|
||||
|
||||
static laser_simple_snapshot_t g_snapshot;
|
||||
static osThreadId_t laserTaskHandle;
|
||||
|
||||
/* --- 辅助宏与函数 --- */
|
||||
#define READ_RING(buf, sz, pos, offset) (buf[((pos) + (offset)) % (sz)])
|
||||
|
||||
static uint16_t le16_read(const uint8_t *p) { return (uint16_t)(p[0] | (p[1] << 8)); }
|
||||
|
||||
static uint16_t apply_median_filter(laser_node_t *n, uint16_t new_val) {
|
||||
n->dist_history[n->hist_idx] = new_val;
|
||||
n->hist_idx = (n->hist_idx + 1) % FILTER_WIN_SZ;
|
||||
if (n->hist_cnt < FILTER_WIN_SZ) n->hist_cnt++;
|
||||
|
||||
uint16_t temp[FILTER_WIN_SZ];
|
||||
for (int i = 0; i < n->hist_cnt; i++) temp[i] = n->dist_history[i];
|
||||
|
||||
for (int i = 0; i < n->hist_cnt - 1; i++) {
|
||||
for (int j = i + 1; j < n->hist_cnt; j++) {
|
||||
if (temp[i] > temp[j]) {
|
||||
uint16_t t = temp[i]; temp[i] = temp[j]; temp[j] = t;
|
||||
}
|
||||
}
|
||||
}
|
||||
return temp[n->hist_cnt / 2];
|
||||
}
|
||||
|
||||
/* --- STP 环形无 memmove 解析 (保持不变) --- */
|
||||
static void process_stp(laser_channel_t ch) {
|
||||
laser_node_t *n = &g_nodes[ch];
|
||||
uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
|
||||
uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;
|
||||
|
||||
while (available >= 11U) {
|
||||
uint16_t head = 0xFFFF;
|
||||
for (uint16_t i = 0; i <= available - 4; i++) {
|
||||
if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == 0xAA &&
|
||||
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+1) == 0xAA &&
|
||||
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+2) == 0xAA &&
|
||||
READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i+3) == 0xAA) {
|
||||
head = i; break;
|
||||
}
|
||||
}
|
||||
|
||||
if (head == 0xFFFF) {
|
||||
n->read_ptr = (n->read_ptr + available - 3) % n->dma_sz;
|
||||
break;
|
||||
}
|
||||
|
||||
n->read_ptr = (n->read_ptr + head) % n->dma_sz;
|
||||
available -= head;
|
||||
if (available < 11) break;
|
||||
|
||||
uint8_t data_len = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 8);
|
||||
uint16_t frame_len = 10 + data_len + 1;
|
||||
|
||||
if (data_len > 200) { n->read_ptr = (n->read_ptr + 1) % n->dma_sz; available--; continue; }
|
||||
if (available < frame_len) break;
|
||||
|
||||
uint8_t frame[256];
|
||||
uint32_t sum = 0;
|
||||
for (uint16_t i = 0; i < frame_len; i++) {
|
||||
frame[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
|
||||
if (i >= 4 && i < frame_len - 1) sum += frame[i];
|
||||
}
|
||||
|
||||
if ((sum & 0xFF) == frame[frame_len - 1] && frame[5] == 0x02 && data_len == 0xB8) {
|
||||
uint32_t p = 10;
|
||||
uint16_t best = 0xFFFF;
|
||||
|
||||
for (int i = 0; i < 12; i++) {
|
||||
int16_t dist = le16_read(&frame[p]); p += 8;
|
||||
uint8_t conf = frame[p]; p += 7;
|
||||
if (dist >= 70 && dist <= 7500 && conf >= 1 && dist < best) best = dist;
|
||||
}
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
if (best != 0xFFFF) {
|
||||
n->data.raw_distance_mm = best;
|
||||
n->data.distance_mm = apply_median_filter(n, best);
|
||||
n->data.valid = 1;
|
||||
n->data.fault_code = 0;
|
||||
} else {
|
||||
n->data.valid = 0;
|
||||
}
|
||||
n->last_tick = HAL_GetTick();
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
|
||||
n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
|
||||
available -= frame_len;
|
||||
}
|
||||
}
|
||||
|
||||
/* --- 【全新】ATK 环形滑动窗口解析 (无视断帧与粘包) --- */
|
||||
static void process_atk(laser_channel_t ch) {
|
||||
laser_node_t *n = &g_nodes[ch];
|
||||
uint16_t write_ptr = n->dma_sz - __HAL_DMA_GET_COUNTER(n->huart->hdmarx);
|
||||
uint16_t available = (write_ptr - n->read_ptr + n->dma_sz) % n->dma_sz;
|
||||
|
||||
// ATK 哪怕报故障，最小长度也有类似 "State:1\r\n" 约 9 字节
|
||||
while (available >= 5) {
|
||||
char c0 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 0);
|
||||
char c1 = (char)READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, 1);
|
||||
|
||||
// 匹配帧头 "d:" 或 "St" (State的开头)
|
||||
if ((c0 == 'd' && c1 == ':') || (c0 == 'S' && c1 == 't')) {
|
||||
// 寻找帧尾 '\n'
|
||||
uint16_t lf_pos = 0xFFFF;
|
||||
for (uint16_t i = 0; i < available; i++) {
|
||||
if (READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i) == '\n') {
|
||||
lf_pos = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果没找到换行符，说明这帧 DMA 还没搬运完，立刻退出等下次任务周期
|
||||
if (lf_pos == 0xFFFF) {
|
||||
break;
|
||||
}
|
||||
|
||||
uint16_t frame_len = lf_pos + 1;
|
||||
char line[64] = {0};
|
||||
for (uint16_t i = 0; i < frame_len && i < sizeof(line) - 1; i++) {
|
||||
line[i] = READ_RING(n->dma_buf, n->dma_sz, n->read_ptr, i);
|
||||
}
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
if (line[0] == 'S' && line[1] == 't') {
|
||||
int state_val = line[6] - '0';
|
||||
if (state_val >= 0 && state_val <= 9) {
|
||||
n->data.fault_code = state_val;
|
||||
if (state_val != 0) n->data.valid = 0;
|
||||
n->last_tick = HAL_GetTick();
|
||||
}
|
||||
}
|
||||
else if (line[0] == 'd' && line[1] == ':') {
|
||||
int dist_val = 0;
|
||||
const char *p = &line[2];
|
||||
while (*p == ' ') p++;
|
||||
while (*p >= '0' && *p <= '9') {
|
||||
dist_val = dist_val * 10 + (*p - '0');
|
||||
p++;
|
||||
}
|
||||
|
||||
if (dist_val > 0 && n->data.fault_code == 0) {
|
||||
n->data.raw_distance_mm = dist_val;
|
||||
n->data.distance_mm = apply_median_filter(n, dist_val);
|
||||
n->data.valid = 1;
|
||||
} else {
|
||||
n->data.valid = 0;
|
||||
}
|
||||
n->last_tick = HAL_GetTick();
|
||||
}
|
||||
taskEXIT_CRITICAL();
|
||||
|
||||
// 成功解析一帧，滑动读取指针
|
||||
n->read_ptr = (n->read_ptr + frame_len) % n->dma_sz;
|
||||
available -= frame_len;
|
||||
} else {
|
||||
// 没找到帧头，说明有垃圾数据，把指针往后挪 1 字节继续找
|
||||
n->read_ptr = (n->read_ptr + 1) % n->dma_sz;
|
||||
available--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* --- 核心 RTOS 任务 (全轮询模式) --- */
|
||||
static void LaserTask(void *arg) {
|
||||
(void)arg;
|
||||
for(;;) {
|
||||
// 【架构升级】：不再死等中断标志，而是以 10ms 的心跳主动去 DMA 缓冲区提货。
|
||||
// 50Hz 等于 20ms 来一帧，10ms 巡视一次性能富裕且绝不漏包。
|
||||
osDelay(10);
|
||||
uint32_t now = HAL_GetTick();
|
||||
|
||||
// 纯 DMA 内存操作，极快
|
||||
process_stp(LASER_CH_FRONT_STP);
|
||||
process_stp(LASER_CH_REAR_STP);
|
||||
process_atk(LASER_CH_FRONT_ATK);
|
||||
process_atk(LASER_CH_REAR_ATK);
|
||||
|
||||
// 统一心跳/超时/快照检查
|
||||
for (int i = 0; i < LASER_CH_MAX; i++) {
|
||||
uint32_t timeout = (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP) ? STP_TIMEOUT_MS : ATK_TIMEOUT_MS;
|
||||
|
||||
taskENTER_CRITICAL();
|
||||
if (now - g_nodes[i].last_tick > timeout) {
|
||||
g_nodes[i].data.online = 0;
|
||||
g_nodes[i].data.valid = 0;
|
||||
g_nodes[i].data.fault_code = FAULT_TIMEOUT;
|
||||
g_nodes[i].hist_cnt = 0; // 掉线时清空历史滤波数组
|
||||
} else {
|
||||
g_nodes[i].data.online = 1;
|
||||
g_nodes[i].data.update_tick_ms = g_nodes[i].last_tick;
|
||||
}
|
||||
g_snapshot.ch[i] = g_nodes[i].data;
|
||||
taskEXIT_CRITICAL();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* --- API 接口 --- */
|
||||
void LASER_SIMPLE_Init(void) {
|
||||
memset(&g_snapshot, 0, sizeof(g_snapshot));
|
||||
|
||||
// 【重要变更】：全员使用普通循环 DMA，彻底抛弃 IDLE 接收
|
||||
HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_STP].huart, g_nodes[LASER_CH_FRONT_STP].dma_buf, STP_DMA_SZ);
|
||||
HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_STP].huart, g_nodes[LASER_CH_REAR_STP].dma_buf, STP_DMA_SZ);
|
||||
HAL_UART_Receive_DMA(g_nodes[LASER_CH_FRONT_ATK].huart, g_nodes[LASER_CH_FRONT_ATK].dma_buf, ATK_DMA_SZ);
|
||||
HAL_UART_Receive_DMA(g_nodes[LASER_CH_REAR_ATK].huart, g_nodes[LASER_CH_REAR_ATK].dma_buf, ATK_DMA_SZ);
|
||||
|
||||
osThreadAttr_t attr = { .name = "LaserTsk", .stack_size = 1024 * 4, .priority = osPriorityAboveNormal };
|
||||
laserTaskHandle = osThreadNew(LaserTask, NULL, &attr);
|
||||
}
|
||||
|
||||
void LASER_SIMPLE_Poll(uint32_t tick_ms) {
|
||||
(void)tick_ms;
|
||||
}
|
||||
|
||||
const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void) {
|
||||
return &g_snapshot;
|
||||
}
|
||||
|
||||
static uint16_t nearest_of_two(const laser_simple_data_t *a, const laser_simple_data_t *b) {
|
||||
if (a->online && a->valid && b->online && b->valid) {
|
||||
return (a->distance_mm < b->distance_mm) ? a->distance_mm : b->distance_mm;
|
||||
}
|
||||
if (a->online && a->valid) return a->distance_mm;
|
||||
if (b->online && b->valid) return b->distance_mm;
|
||||
return 0U;
|
||||
}
|
||||
|
||||
uint16_t LASER_SIMPLE_GetFrontNearest(void) {
|
||||
return nearest_of_two(&g_snapshot.ch[LASER_CH_FRONT_STP], &g_snapshot.ch[LASER_CH_FRONT_ATK]);
|
||||
}
|
||||
|
||||
uint16_t LASER_SIMPLE_GetRearNearest(void) {
|
||||
return nearest_of_two(&g_snapshot.ch[LASER_CH_REAR_STP], &g_snapshot.ch[LASER_CH_REAR_ATK]);
|
||||
}
|
||||
|
||||
/* --- 中断桥接 (已大幅瘦身，仅保留错误恢复) --- */
|
||||
|
||||
// 因为改为轮询模式，不再需要依赖这几个正常接收的中断了。保留空函数防止外部调用报错。
|
||||
void LASER_UART_RxCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
|
||||
void LASER_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { (void)huart; }
|
||||
void LASER_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { (void)huart; (void)Size; }
|
||||
|
||||
// 错误自动恢复保留，这是底层硬件 ORE 报错时重启 DMA 必需的
|
||||
void LASER_UART_ErrorRecovery(UART_HandleTypeDef *huart) {
|
||||
for (int i=0; i<LASER_CH_MAX; i++) {
|
||||
if (g_nodes[i].huart == huart) {
|
||||
HAL_UART_DMAStop(huart);
|
||||
// 统一恢复为普通 DMA 接收
|
||||
if (i == LASER_CH_FRONT_STP || i == LASER_CH_REAR_STP)
|
||||
HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, STP_DMA_SZ);
|
||||
else
|
||||
HAL_UART_Receive_DMA(huart, g_nodes[i].dma_buf, ATK_DMA_SZ);
|
||||
}
|
||||
}
|
||||
}
|
||||
53
App/laser/laser_manager.h
Normal file
53
App/laser/laser_manager.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef __LASER_SIMPLE_H
|
||||
#define __LASER_SIMPLE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "usart.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
LASER_CH_FRONT_STP = 0,
|
||||
LASER_CH_FRONT_ATK,
|
||||
LASER_CH_REAR_STP,
|
||||
LASER_CH_REAR_ATK,
|
||||
LASER_CH_MAX
|
||||
} laser_channel_t;
|
||||
|
||||
typedef struct {
|
||||
uint16_t distance_mm; // [新增] 经过中值滤波处理后平滑的距离值 (推荐用于避障控制)
|
||||
uint16_t raw_distance_mm; // [新增] 传感器吐出的原始距离值 (供调试或特殊算法读取)
|
||||
uint8_t valid; // 数据是否有效
|
||||
uint8_t online; // 雷达是否在线(未超时)
|
||||
uint8_t fault_code; // 故障码 (0为正常，0x80为超时)
|
||||
uint32_t update_tick_ms; // 最后一次数据刷新的系统节拍
|
||||
} laser_simple_data_t;
|
||||
|
||||
typedef struct {
|
||||
laser_simple_data_t ch[LASER_CH_MAX];
|
||||
} laser_simple_snapshot_t;
|
||||
|
||||
/* 初始化并启动 DMA 及解析任务 */
|
||||
void LASER_SIMPLE_Init(void);
|
||||
|
||||
/* 周期调用 (实际为空，保留接口兼容) */
|
||||
void LASER_SIMPLE_Poll(uint32_t tick_ms);
|
||||
|
||||
/* 获取统一快照 (无数据撕裂风险) */
|
||||
const laser_simple_snapshot_t *LASER_SIMPLE_GetSnapshot(void);
|
||||
|
||||
/* 获取前后最小有效距离 (使用滤波后的值) */
|
||||
uint16_t LASER_SIMPLE_GetFrontNearest(void);
|
||||
uint16_t LASER_SIMPLE_GetRearNearest(void);
|
||||
/* --- 新增：暴露给全局回调分发器的专用接口 --- */
|
||||
void LASER_UART_RxCpltCallback(UART_HandleTypeDef *huart);
|
||||
void LASER_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart);
|
||||
void LASER_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size);
|
||||
void LASER_UART_ErrorRecovery(UART_HandleTypeDef *huart);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
169
App/nav/corridor_ctrl.c
Normal file
169
App/nav/corridor_ctrl.c
Normal file
@@ -0,0 +1,169 @@
|
||||
#include "corridor_ctrl.h"
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/* ====================== 内部状态 ====================== */
|
||||
static CorridorCtrlConfig_t s_cfg;
|
||||
static bool s_initialized = false;
|
||||
|
||||
/* 辅助：浮点数限幅 */
|
||||
static inline float clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
s_initialized = true;
|
||||
}
|
||||
|
||||
void CorridorCtrl_Compute(const CorridorState_t *state,
|
||||
const CorridorObs_t *obs,
|
||||
float imu_wz,
|
||||
RawCmd_t *out_cmd)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
out_cmd->v = 0.0f;
|
||||
out_cmd->w = 0.0f;
|
||||
out_cmd->flags = 0U;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 出沟保护: 当前激光检测到接近出口时，停止使用左右激光控制
|
||||
* 避免出沟时左右激光数据突变导致车身大幅度转向
|
||||
* ======================================================== */
|
||||
bool near_exit = false;
|
||||
if ((obs->valid_mask & (1U << 4)) != 0U) { /* 前激光有效 (bit 4) */
|
||||
if (obs->d_front <= s_cfg.exit_front_dist) {
|
||||
near_exit = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 核心控制律:
|
||||
* w_cmd = kp_theta * e_th + kd_theta * (-imu_wz) + kp_y * e_y
|
||||
*
|
||||
* - kp_theta * e_th : 比例项，车头偏了就回转
|
||||
* - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
|
||||
* 用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
|
||||
* - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
|
||||
*
|
||||
* 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏
|
||||
* ======================================================== */
|
||||
|
||||
float w_cmd;
|
||||
bool escape_active = false;
|
||||
if (near_exit) {
|
||||
/* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
|
||||
w_cmd = -(s_cfg.kd_theta * imu_wz);
|
||||
} else {
|
||||
/*
|
||||
* 简化原则：
|
||||
* - IMU 只管航向 (e_th + wz)
|
||||
* - 左右激光只管居中
|
||||
*
|
||||
* 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
|
||||
* e_y_direct = (d_right - d_left)/2
|
||||
* 车在正中时 d_left ~= d_right，因此误差应接近 0。
|
||||
*
|
||||
* 若当前帧双侧不完整，再退回滤波器给出的 e_y。
|
||||
*/
|
||||
float e_y_ctrl = state->e_y;
|
||||
bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 1)) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
|
||||
((obs->valid_mask & (1U << 3)) != 0U);
|
||||
|
||||
if (left_ok && right_ok) {
|
||||
float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
|
||||
float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
|
||||
e_y_ctrl = 0.5f * (d_right - d_left);
|
||||
}
|
||||
|
||||
/* 正常控制: IMU 管航向, 左右激光管居中 */
|
||||
w_cmd = -(s_cfg.kp_theta * state->e_th
|
||||
+ s_cfg.kd_theta * imu_wz
|
||||
+ s_cfg.kp_y * e_y_ctrl);
|
||||
|
||||
/* ========================================================
|
||||
* 近墙脱离保护:
|
||||
* 当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。
|
||||
* 此时不能只等 EKF 慢慢回中，直接叠加一个远离墙面的转向保护项。
|
||||
* ======================================================== */
|
||||
{
|
||||
bool left_front_ok = ((obs->valid_mask & (1U << 0)) != 0U);
|
||||
bool left_rear_ok = ((obs->valid_mask & (1U << 1)) != 0U);
|
||||
bool right_front_ok = ((obs->valid_mask & (1U << 2)) != 0U);
|
||||
bool right_rear_ok = ((obs->valid_mask & (1U << 3)) != 0U);
|
||||
float w_escape = 0.0f;
|
||||
float left_min = 10.0f;
|
||||
float right_min = 10.0f;
|
||||
|
||||
if (left_front_ok && obs->d_lf < left_min) left_min = obs->d_lf;
|
||||
if (left_rear_ok && obs->d_lr < left_min) left_min = obs->d_lr;
|
||||
if (right_front_ok && obs->d_rf < right_min) right_min = obs->d_rf;
|
||||
if (right_rear_ok && obs->d_rr < right_min) right_min = obs->d_rr;
|
||||
|
||||
if (left_min < s_cfg.wall_escape_dist) {
|
||||
float err = s_cfg.wall_escape_dist - left_min;
|
||||
w_escape -= s_cfg.wall_escape_kp * err; /* 左侧很近 -> 轻微右转 */
|
||||
escape_active = true;
|
||||
}
|
||||
|
||||
if (right_min < s_cfg.wall_escape_dist) {
|
||||
float err = s_cfg.wall_escape_dist - right_min;
|
||||
w_escape += s_cfg.wall_escape_kp * err; /* 右侧很近 -> 轻微左转 */
|
||||
escape_active = true;
|
||||
}
|
||||
|
||||
w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max);
|
||||
w_cmd += w_escape;
|
||||
}
|
||||
}
|
||||
|
||||
/* 角速度限幅：防止 PD 溢出导致原地打转 */
|
||||
w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
|
||||
|
||||
/* ========================================================
|
||||
* 线速度策略:
|
||||
* 基础巡航速度 v_cruise，
|
||||
* 当角速度偏大时适当降速（弯道减速），保持运动学协调
|
||||
* 公式: v = v_cruise * (1 - k * |w/w_max|)
|
||||
* k 取 0.3~0.5 较保守
|
||||
* ======================================================== */
|
||||
float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
|
||||
float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
|
||||
|
||||
/* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */
|
||||
if (escape_active) {
|
||||
v_cmd *= 0.80f;
|
||||
}
|
||||
|
||||
/* 线速度限幅：不允许倒车，不允许超速 */
|
||||
v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
|
||||
|
||||
/* ========================================================
|
||||
* 置信度降级保护:
|
||||
* 当滤波器健康度 conf 过低（两边雷达全瞎），
|
||||
* 说明走廊参照完全丢失，降低线速度防止盲飞
|
||||
*
|
||||
* 注意：阈值不宜过高，否则会过度降级导致控制器失效
|
||||
* ======================================================== */
|
||||
if (state->conf < 0.2f) {
|
||||
/* 健康度极低：速度打三折，保持航向惯性滑行 */
|
||||
v_cmd *= 0.3f;
|
||||
} else if (state->conf < 0.4f) {
|
||||
/* 健康度较低（单侧退化）：速度打七折 */
|
||||
v_cmd *= 0.7f;
|
||||
}
|
||||
|
||||
/* 输出赋值 */
|
||||
out_cmd->t_ms = state->t_ms;
|
||||
out_cmd->v = v_cmd;
|
||||
out_cmd->w = w_cmd;
|
||||
out_cmd->flags = 0U;
|
||||
}
|
||||
52
App/nav/corridor_ctrl.h
Normal file
52
App/nav/corridor_ctrl.h
Normal file
@@ -0,0 +1,52 @@
|
||||
#ifndef CORRIDOR_CTRL_H
|
||||
#define CORRIDOR_CTRL_H
|
||||
|
||||
#include "preproc/corridor_msgs.h"
|
||||
|
||||
/**
|
||||
* @brief 走廊控制器调参配置
|
||||
* @note 所有增益参数都应在调试阶段"从小往大调"，防止震荡
|
||||
*/
|
||||
typedef struct {
|
||||
float kp_theta; // 航向偏差比例增益 (rad/s per rad)
|
||||
float kd_theta; // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
|
||||
float kp_y; // 横向偏差比例增益 (rad/s per m)
|
||||
|
||||
float v_cruise; // 走廊内巡航线速度 (m/s)
|
||||
float w_max; // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
|
||||
float v_max; // 线速度输出硬限幅 (m/s)
|
||||
float speed_reduction_k; // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
|
||||
|
||||
float exit_front_dist; // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制
|
||||
float wall_escape_dist; // 近墙脱离阈值 (m)，小于此值触发直接远离墙面
|
||||
float wall_escape_kp; // 近墙脱离增益 (rad/s per m)
|
||||
float wall_escape_w_max; // 近墙脱离角速度限幅 (rad/s)
|
||||
} CorridorCtrlConfig_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 初始化走廊控制器
|
||||
* @param config 控制器增益与限幅配置
|
||||
*/
|
||||
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 核心函数：根据滤波器输出的走廊状态计算控制量
|
||||
* @param state 滤波器输出的走廊状态 (e_y, e_th, conf)
|
||||
* @param obs 预处理层的观测快照 (用于获取 d_front 等辅助信息)
|
||||
* @param imu_wz 当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
|
||||
* @param out_cmd 输出的控制指令 (v, w)
|
||||
*/
|
||||
void CorridorCtrl_Compute(const CorridorState_t *state,
|
||||
const CorridorObs_t *obs,
|
||||
float imu_wz,
|
||||
RawCmd_t *out_cmd);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // CORRIDOR_CTRL_H
|
||||
1051
App/nav/global_nav.c
Normal file
1051
App/nav/global_nav.c
Normal file
File diff suppressed because it is too large
Load Diff
156
App/nav/global_nav.h
Normal file
156
App/nav/global_nav.h
Normal file
@@ -0,0 +1,156 @@
|
||||
/**
|
||||
* @file global_nav.h
|
||||
* @brief 赛道级总控 — 混合导航状态机
|
||||
*
|
||||
* 三层架构中的最上层：
|
||||
* 上层 (本模块): 知道"当前第几沟、下一沟是谁、该往哪转"
|
||||
* 中层 (内嵌): 转向/连接段/重捕获动作执行
|
||||
* 下层 (已有): 沟内 corridor_ctrl 闭环
|
||||
*
|
||||
* 替代原 nav_script.c 的赛道编排职能。
|
||||
*/
|
||||
|
||||
#ifndef GLOBAL_NAV_H
|
||||
#define GLOBAL_NAV_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "preproc/corridor_msgs.h"
|
||||
#include "Contract/robot_blackboard.h"
|
||||
#include "nav/track_map.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* =========================================================
|
||||
* 赛道级阶段枚举
|
||||
* ========================================================= */
|
||||
typedef enum {
|
||||
GNAV_IDLE = 0, /* 启动区等待 */
|
||||
GNAV_ENTRY_STRAIGHT, /* 入场直线 */
|
||||
GNAV_TURN_INTO_CORRIDOR, /* 转向进入垄沟 (原地转 90°) */
|
||||
GNAV_REACQUIRE, /* 重捕获走廊 */
|
||||
GNAV_ALIGN, /* 捕获成功后停车对齐航线 */
|
||||
GNAV_CORRIDOR_TRACK, /* 沟内闭环跟踪 */
|
||||
GNAV_TURN_OUT_OF_CORRIDOR, /* 到端后出沟转向 (原地转 90°) */
|
||||
GNAV_LINK_STRAIGHT, /* 连接段直行 */
|
||||
GNAV_TURN_INTO_NEXT, /* 转向进入下一条沟 (原地转 90°) */
|
||||
GNAV_EXIT_STRAIGHT, /* 出场直行 */
|
||||
GNAV_DOCK, /* 回停启动区 */
|
||||
GNAV_FINISHED, /* 终态 */
|
||||
GNAV_ERROR /* 异常态 (超时兜底) */
|
||||
} GlobalNavStage_t;
|
||||
|
||||
/* =========================================================
|
||||
* 配置参数
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
/* 入场段 */
|
||||
float entry_v; /* 入场速度 m/s */
|
||||
float entry_distance; /* 入场里程上限 m */
|
||||
uint32_t entry_timeout_ms; /* 入场超时 ms */
|
||||
|
||||
/* 转向 */
|
||||
float turn_omega; /* 转向角速度 rad/s */
|
||||
float turn_tolerance_rad; /* 转向完成容差 rad */
|
||||
float turn_decel_zone_rad; /* 接近目标角时的减速区 rad */
|
||||
float turn_min_omega; /* 减速区最低角速度 rad/s */
|
||||
uint32_t turn_timeout_ms; /* 单次转向超时 ms */
|
||||
|
||||
/* 重捕获 */
|
||||
float reacquire_v; /* 重捕获速度 m/s */
|
||||
float reacquire_conf_thresh; /* 重捕获置信度阈值 */
|
||||
float reacquire_width_tol; /* 走廊宽度容差 m */
|
||||
float reacquire_min_odom; /* 重捕获最小入沟里程 m */
|
||||
uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
|
||||
uint32_t reacquire_timeout_ms;
|
||||
|
||||
/* 对齐 */
|
||||
float align_kp_th; /* 对齐航向P增益 (rad/s per rad) */
|
||||
float align_kp_y; /* 对齐横向P增益 (rad/s per m) */
|
||||
float align_th_tol_rad; /* 对齐航向容差 (rad) */
|
||||
float align_y_tol_m; /* 对齐横向容差 (m) */
|
||||
uint8_t align_confirm_ticks; /* 对齐确认拍数 */
|
||||
uint32_t align_timeout_ms; /* 对齐超时 ms */
|
||||
float reacquire_min_back_dist; /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */
|
||||
|
||||
/* 沟内 */
|
||||
float corridor_end_detect_dist; /* 到端检测距离 m */
|
||||
float corridor_length_max; /* 沟内里程保护上限 m */
|
||||
|
||||
/* 连接段 */
|
||||
float link_v;
|
||||
float link_distance;
|
||||
uint32_t link_timeout_ms;
|
||||
float link_gap_runout; /* 检测到沟口后继续前冲距离 (m) */
|
||||
float link_wall_target; /* 墙壁跟随目标距离 (m) */
|
||||
float link_wall_kp; /* 墙壁跟随P增益 */
|
||||
float link_wall_heading_kp; /* 单边墙平行修正增益 */
|
||||
float link_wall_blend; /* 墙壁跟随权重 (0~1) */
|
||||
|
||||
/* 出场 */
|
||||
float exit_v; /* 出场速度 m/s */
|
||||
float exit_runout; /* 出场冲刺距离 m */
|
||||
float exit_max_dist; /* 出场里程保护 m */
|
||||
uint32_t exit_timeout_ms;
|
||||
|
||||
/* 回停 */
|
||||
float dock_v; /* 回停速度 m/s */
|
||||
float dock_distance; /* 回停距离 m */
|
||||
|
||||
/* 航向保持 */
|
||||
float heading_kp; /* 航向保持P增益 */
|
||||
|
||||
/* 走廊宽度 (用于重捕获判定) */
|
||||
float corridor_width; /* 走廊宽度 m */
|
||||
} GlobalNavConfig_t;
|
||||
|
||||
/* =========================================================
|
||||
* 输出结构
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
GlobalNavStage_t stage; /* 当前阶段 */
|
||||
const char* stage_name; /* 阶段名字符串 (调试) */
|
||||
uint8_t corridor_id; /* 当前/目标垄沟 ID */
|
||||
uint8_t corridors_done; /* 已完成垄沟数 */
|
||||
|
||||
bool request_corridor; /* 是否请求沟内闭环 */
|
||||
bool use_override; /* 是否用 override 指令 */
|
||||
float override_v;
|
||||
float override_w;
|
||||
|
||||
SafetyMode_t safety_mode; /* 当前安全模式 */
|
||||
bool active; /* 导航是否在运行 */
|
||||
} GlobalNavOutput_t;
|
||||
|
||||
/* =========================================================
|
||||
* API
|
||||
* ========================================================= */
|
||||
void GlobalNav_Init(const GlobalNavConfig_t* cfg);
|
||||
void GlobalNav_Start(void);
|
||||
void GlobalNav_Stop(void);
|
||||
void GlobalNav_Reset(void);
|
||||
|
||||
/**
|
||||
* @brief 核心函数：每个导航周期调用一次
|
||||
* @param obs 预处理层的观测快照
|
||||
* @param state EKF 走廊状态
|
||||
* @param board 黑板快照 (读 IMU yaw_continuous, odom)
|
||||
* @param now_ms 当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
|
||||
* @param out 导航输出
|
||||
*/
|
||||
void GlobalNav_Update(const CorridorObs_t* obs,
|
||||
const CorridorState_t* state,
|
||||
const RobotBlackboard_t* board,
|
||||
uint32_t now_ms,
|
||||
GlobalNavOutput_t* out);
|
||||
|
||||
GlobalNavStage_t GlobalNav_GetStage(void);
|
||||
const char* GlobalNav_GetStageName(GlobalNavStage_t stage);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* GLOBAL_NAV_H */
|
||||
424
App/nav/nav_script.c
Normal file
424
App/nav/nav_script.c
Normal file
@@ -0,0 +1,424 @@
|
||||
/**
|
||||
* @file nav_script.c
|
||||
* @brief 段脚本执行器实现：编排完整比赛流程
|
||||
*/
|
||||
|
||||
#include "nav_script.h"
|
||||
#include "est/corridor_filter.h"
|
||||
#include "robot_params.h"
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
/* =========================================================
|
||||
* 内部状态
|
||||
* ========================================================= */
|
||||
static NavScriptConfig_t s_cfg;
|
||||
static NavScriptStage_t s_stage = SCRIPT_STAGE_IDLE;
|
||||
static bool s_initialized = false;
|
||||
static bool s_running = false;
|
||||
|
||||
/* 阶段内部状态变量 */
|
||||
static struct {
|
||||
uint32_t entry_align_start_ms; // 入口对准开始时间
|
||||
float turn_start_e_th; // 转向开始时的 EKF 航向 (保留作后备)
|
||||
float turn_start_imu_yaw_deg; // 转向开始时的 IMU 连续偏航角 (deg)
|
||||
bool turn_started; // 转向是否已开始
|
||||
bool turn_coarse_done; // 180° IMU 粗转是否已完成
|
||||
uint32_t turn_settle_start_ms; // 进入侧墙精调阶段的时刻
|
||||
float corridor_s_entry; // 进入垄沟时的 s 里程
|
||||
float end_rearm_s; // 掉头后到端检测重新使能的起始里程
|
||||
bool end_armed; // 到端检测是否已重新使能
|
||||
NavScriptStage_t post_turn_stage; // 本次转向完成后要进入的走廊阶段
|
||||
int pass_count; // 已走过的垄沟数
|
||||
float exit_start_s; // 离开垄沟瞬间的 s 里程 (0=未触发)
|
||||
} s_internal;
|
||||
|
||||
#define SCRIPT_END_REARM_DIST_M 0.12f
|
||||
#define SCRIPT_TURN_SETTLE_KP 1.4f
|
||||
#define SCRIPT_TURN_SETTLE_W_MAX 0.6f
|
||||
#define SCRIPT_TURN_SETTLE_TOL_RAD 0.05f
|
||||
#define SCRIPT_TURN_SETTLE_TIMEOUT_MS 1200u
|
||||
|
||||
/* =========================================================
|
||||
* 内部辅助函数
|
||||
* ========================================================= */
|
||||
static inline float clampf(float val, float lo, float hi)
|
||||
{
|
||||
if (val < lo) return lo;
|
||||
if (val > hi) return hi;
|
||||
return val;
|
||||
}
|
||||
|
||||
static bool compute_wall_heading_error(const CorridorObs_t *obs, float *out_heading_rad)
|
||||
{
|
||||
float heading_sum = 0.0f;
|
||||
int valid_count = 0;
|
||||
const float sensor_base = PARAM_SENSOR_BASE_LENGTH;
|
||||
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (left_ok) {
|
||||
heading_sum += atan2f(obs->d_lr - obs->d_lf, sensor_base);
|
||||
valid_count++;
|
||||
}
|
||||
|
||||
if (right_ok) {
|
||||
heading_sum += atan2f(obs->d_rf - obs->d_rr, sensor_base);
|
||||
valid_count++;
|
||||
}
|
||||
|
||||
if (valid_count == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
*out_heading_rad = heading_sum / (float)valid_count;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* =========================================================
|
||||
* API 实现
|
||||
* ========================================================= */
|
||||
|
||||
void NavScript_Init(const NavScriptConfig_t *config)
|
||||
{
|
||||
s_cfg = *config;
|
||||
s_stage = SCRIPT_STAGE_IDLE;
|
||||
s_running = false;
|
||||
s_initialized = true;
|
||||
|
||||
/* 初始化内部状态 */
|
||||
memset(&s_internal, 0, sizeof(s_internal));
|
||||
}
|
||||
|
||||
void NavScript_Start(void)
|
||||
{
|
||||
if (s_initialized) {
|
||||
s_stage = SCRIPT_STAGE_ENTRY_ALIGN;
|
||||
s_running = true;
|
||||
s_internal.entry_align_start_ms = 0;
|
||||
s_internal.pass_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
NavScriptStage_t NavScript_GetStage(void)
|
||||
{
|
||||
return s_stage;
|
||||
}
|
||||
|
||||
const char* NavScript_GetStageName(NavScriptStage_t stage)
|
||||
{
|
||||
switch (stage) {
|
||||
case SCRIPT_STAGE_IDLE: return "IDLE";
|
||||
case SCRIPT_STAGE_ENTRY_ALIGN: return "ENTRY_ALIGN";
|
||||
case SCRIPT_STAGE_CORRIDOR_FORWARD: return "CORRIDOR_FWD";
|
||||
case SCRIPT_STAGE_TURN_AT_END: return "TURN_AT_END";
|
||||
case SCRIPT_STAGE_CORRIDOR_BACKWARD: return "CORRIDOR_BWD";
|
||||
case SCRIPT_STAGE_EXIT: return "EXIT";
|
||||
case SCRIPT_STAGE_FINISHED: return "FINISHED";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
void NavScript_Reset(void)
|
||||
{
|
||||
s_stage = SCRIPT_STAGE_IDLE;
|
||||
s_running = false;
|
||||
memset(&s_internal, 0, sizeof(s_internal));
|
||||
}
|
||||
|
||||
void NavScript_Update(const CorridorObs_t *obs,
|
||||
const CorridorState_t *state,
|
||||
float imu_yaw_continuous_deg,
|
||||
NavScriptOutput_t *out)
|
||||
{
|
||||
if (!s_initialized) {
|
||||
out->stage = SCRIPT_STAGE_IDLE;
|
||||
out->stage_name = "IDLE";
|
||||
out->active = false;
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
return;
|
||||
}
|
||||
|
||||
/* 默认输出 */
|
||||
out->stage = s_stage;
|
||||
out->stage_name = NavScript_GetStageName(s_stage);
|
||||
out->active = s_running;
|
||||
out->request_corridor = true; // 默认请求走廊控制器
|
||||
out->use_override = false; // 默认不覆盖
|
||||
|
||||
if (!s_running) {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
return;
|
||||
}
|
||||
|
||||
/* ========================================================
|
||||
* 核心状态机：按比赛流程执行
|
||||
* ======================================================== */
|
||||
switch (s_stage) {
|
||||
/* ==============================================
|
||||
* STAGE 1: 入口对准
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_ENTRY_ALIGN: {
|
||||
if (s_internal.entry_align_start_ms == 0) {
|
||||
s_internal.entry_align_start_ms = state->t_ms;
|
||||
}
|
||||
|
||||
/* 检查是否已进入垄沟：两侧都有距离数据 */
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (left_ok && right_ok && state->conf >= 0.8f) {
|
||||
/* 两侧雷达都有数据，且置信度高 -> 进入垄沟，开始跟踪 */
|
||||
s_internal.corridor_s_entry = state->s;
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = true;
|
||||
s_internal.pass_count = 1;
|
||||
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
out->request_corridor = true;
|
||||
} else {
|
||||
/* 入口对准策略：慢速前进，让侧向雷达找墙 */
|
||||
out->override_v = s_cfg.entry_align_v; // 慢速前进 (由 PARAM_SCRIPT_ENTRY_V 配置)
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
out->request_corridor = false;
|
||||
|
||||
/* P1 修复: 超时保护使用配置参数 s_cfg.entry_align_timeout，
|
||||
* 而非硬编码 30000 ms */
|
||||
uint32_t elapsed = state->t_ms - s_internal.entry_align_start_ms;
|
||||
if (elapsed > (uint32_t)s_cfg.entry_align_timeout) {
|
||||
/* 超时强制进入走廊跟踪：必须补齐与正常入沟一致的内部状态，
|
||||
* 否则 pass_count 停留在 0，导致后续 TURN_AT_END 判定时
|
||||
* 多跑一趟走廊（三趟而非文档描述的两趟）。 */
|
||||
s_internal.corridor_s_entry = state->s;
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = true;
|
||||
s_internal.pass_count = 1;
|
||||
s_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 2: 向前走垄沟
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_CORRIDOR_FORWARD: {
|
||||
/* 使用走廊控制器 */
|
||||
out->request_corridor = true;
|
||||
|
||||
if (!s_internal.end_armed) {
|
||||
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
|
||||
s_internal.end_armed = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* 检查是否到端 */
|
||||
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||||
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
|
||||
/* 前向距离足够近 -> 到达垄沟末端，递增趟数并准备转向 */
|
||||
s_internal.pass_count++;
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = false;
|
||||
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_BACKWARD;
|
||||
s_stage = SCRIPT_STAGE_TURN_AT_END;
|
||||
out->request_corridor = false;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 3: 到端原地转向
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_TURN_AT_END: {
|
||||
out->request_corridor = false;
|
||||
|
||||
if (!s_internal.turn_started) {
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = true;
|
||||
s_internal.turn_coarse_done = false;
|
||||
s_internal.turn_settle_start_ms = 0U;
|
||||
}
|
||||
|
||||
/* 原地 180 度转向策略 */
|
||||
float target_delta = s_cfg.turn_target_angle; // 默认 PI (180度)
|
||||
|
||||
/* ---- 使用 IMU 连续 yaw 判定转角 (deg → rad) ---- *
|
||||
* 优点：IMU 内部维护的 yaw 经过模块校准，比外部积分 wz 更稳定，
|
||||
* 且 unwrap 后不存在 ±180° 跳变问题。
|
||||
* 后备：如果 IMU 离线，退化回 EKF e_th 差值判定。 */
|
||||
float delta_turned;
|
||||
float imu_delta_deg = imu_yaw_continuous_deg - s_internal.turn_start_imu_yaw_deg;
|
||||
delta_turned = imu_delta_deg * 0.01745329252f; // deg → rad
|
||||
|
||||
float remaining = target_delta - fabsf(delta_turned);
|
||||
|
||||
if (!s_internal.turn_coarse_done && remaining <= 0.1f) {
|
||||
s_internal.turn_coarse_done = true;
|
||||
s_internal.turn_settle_start_ms = obs->t_ms;
|
||||
}
|
||||
|
||||
if (s_internal.turn_coarse_done) {
|
||||
float wall_heading = 0.0f;
|
||||
bool wall_heading_ok = compute_wall_heading_error(obs, &wall_heading);
|
||||
bool settle_timeout = (obs->t_ms - s_internal.turn_settle_start_ms) >= SCRIPT_TURN_SETTLE_TIMEOUT_MS;
|
||||
|
||||
if (wall_heading_ok) {
|
||||
if (fabsf(wall_heading) <= SCRIPT_TURN_SETTLE_TOL_RAD) {
|
||||
CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f);
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = false;
|
||||
s_stage = s_internal.post_turn_stage;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
} else {
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = clampf(-SCRIPT_TURN_SETTLE_KP * wall_heading,
|
||||
-SCRIPT_TURN_SETTLE_W_MAX,
|
||||
SCRIPT_TURN_SETTLE_W_MAX);
|
||||
out->use_override = true;
|
||||
}
|
||||
} else if (settle_timeout) {
|
||||
/* 极端情况下侧墙暂时不可用，超时后接受 IMU 粗转结果，避免卡死。 */
|
||||
CorridorFilter_RebaseAfterTurnaround(imu_yaw_continuous_deg * 0.01745329252f);
|
||||
s_internal.end_rearm_s = state->s;
|
||||
s_internal.end_armed = false;
|
||||
s_stage = s_internal.post_turn_stage;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
} else {
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
}
|
||||
} else {
|
||||
/* 继续转向：根据目标方向选择角速度 */
|
||||
float turn_dir = (target_delta > 0.0f) ? 1.0f : -1.0f;
|
||||
|
||||
/* 接近目标时减速 */
|
||||
float turn_speed = s_cfg.turn_omega;
|
||||
if (remaining < 0.5f) {
|
||||
turn_speed *= (remaining / 0.5f); // 线性减速
|
||||
turn_speed = clampf(turn_speed, 0.3f, s_cfg.turn_omega);
|
||||
}
|
||||
|
||||
out->override_v = 0.0f; // 线速度为 0，原地转
|
||||
out->override_w = turn_dir * turn_speed;
|
||||
out->use_override = true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 4: 返程走垄沟 (原地转 180° 后继续正向行驶)
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_CORRIDOR_BACKWARD: {
|
||||
/* 使用走廊控制器，正向行驶 */
|
||||
out->request_corridor = true;
|
||||
|
||||
/* P1 修复: 原地转 180° 后车头已调转，返回方向即"向前"，
|
||||
* 因此到端检测应使用前向雷达 d_front，而非后向雷达 d_back */
|
||||
bool front_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
|
||||
|
||||
/* 掉头回来时，前向雷达可能还残留近端读数。
|
||||
* 必须先离开端墙一小段距离，再允许重新触发到端检测。 */
|
||||
if (!s_internal.end_armed) {
|
||||
if ((state->s - s_internal.end_rearm_s) >= SCRIPT_END_REARM_DIST_M) {
|
||||
s_internal.end_armed = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (s_internal.end_armed && front_ok && obs->d_front <= s_cfg.d_entry_exit_front) {
|
||||
/* [改进H] 到达另一端: 递增趟数并判断是否退出
|
||||
* pass_count=1(入沟首趟), =2(返回), =3(第三趟) → EXIT */
|
||||
s_internal.pass_count++;
|
||||
if (s_internal.pass_count >= 3) {
|
||||
/* 走了 3 趟 (去-回-去)，进入退出阶段 */
|
||||
s_stage = SCRIPT_STAGE_EXIT;
|
||||
s_internal.exit_start_s = 0.0f;
|
||||
} else {
|
||||
/* 继续往返: 转向后进入 FORWARD */
|
||||
s_internal.turn_start_e_th = state->e_th;
|
||||
s_internal.turn_start_imu_yaw_deg = imu_yaw_continuous_deg;
|
||||
s_internal.turn_started = false;
|
||||
s_internal.post_turn_stage = SCRIPT_STAGE_CORRIDOR_FORWARD;
|
||||
s_stage = SCRIPT_STAGE_TURN_AT_END;
|
||||
}
|
||||
out->request_corridor = false;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 5: 退出场地
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_EXIT: {
|
||||
/* P1 修复: 退出速度使用独立的 exit_v 参数，
|
||||
* 而非误用角速度参数 turn_omega * 0.5f */
|
||||
out->request_corridor = false;
|
||||
out->override_v = s_cfg.exit_v; // 直线冲出速度 (由 PARAM_SCRIPT_EXIT_V 配置)
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
|
||||
/* 检测是否已离开：两侧雷达都丢失 */
|
||||
bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
|
||||
bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
|
||||
((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
|
||||
|
||||
if (!left_ok && !right_ok) {
|
||||
/* 两侧都丢了 -> 已离开垄沟，再往前冲 exit_runout_m 后停车 */
|
||||
if (s_internal.exit_start_s == 0.0f) {
|
||||
s_internal.exit_start_s = state->s;
|
||||
}
|
||||
|
||||
if (state->s - s_internal.exit_start_s >= s_cfg.exit_runout_m) {
|
||||
/* 冲够了，停车 */
|
||||
s_stage = SCRIPT_STAGE_FINISHED;
|
||||
s_internal.exit_start_s = 0.0f;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* STAGE 6: 比赛完成，安全停车
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_FINISHED: {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
s_running = false;
|
||||
break;
|
||||
}
|
||||
|
||||
/* ==============================================
|
||||
* DEFAULT/IDLE: 安全停车
|
||||
* ============================================== */
|
||||
case SCRIPT_STAGE_IDLE:
|
||||
default: {
|
||||
out->request_corridor = false;
|
||||
out->override_v = 0.0f;
|
||||
out->override_w = 0.0f;
|
||||
out->use_override = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
133
App/nav/nav_script.h
Normal file
133
App/nav/nav_script.h
Normal file
@@ -0,0 +1,133 @@
|
||||
/**
|
||||
* @file nav_script.h
|
||||
* @brief 段脚本执行器：编排完整比赛流程
|
||||
*
|
||||
* 比赛流程模型：
|
||||
* ┌─────────────┐
|
||||
* │ ENTRY_ALIGN │ 入口对准：启动区 -> 第一垄沟入口
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ CORRIDOR_A │ 第一垄沟跟踪
|
||||
* └──────┬──────┘
|
||||
* │ (到端)
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ TURN_180 │ 原地 180 度转向
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ CORRIDOR_B │ 返回垄沟跟踪
|
||||
* └──────┬──────┘
|
||||
* │ (到端)
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ EXIT │ 退出比赛场地
|
||||
* └──────┬──────┘
|
||||
* │
|
||||
* v
|
||||
* ┌─────────────┐
|
||||
* │ FINISH │ 停在启动区
|
||||
* └─────────────┘
|
||||
*/
|
||||
|
||||
#ifndef NAV_SCRIPT_H
|
||||
#define NAV_SCRIPT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "preproc/corridor_msgs.h"
|
||||
#include "preproc/corridor_preproc.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本阶段枚举
|
||||
* ========================================================= */
|
||||
typedef enum {
|
||||
SCRIPT_STAGE_IDLE = 0, // 待命阶段，不输出任何指令
|
||||
SCRIPT_STAGE_ENTRY_ALIGN, // 入口对准：从启动区到第一垄沟
|
||||
SCRIPT_STAGE_CORRIDOR_FORWARD, // 向前走垄沟
|
||||
SCRIPT_STAGE_TURN_AT_END, // 到端原地转向
|
||||
SCRIPT_STAGE_CORRIDOR_BACKWARD,// 向后走垄沟
|
||||
SCRIPT_STAGE_EXIT, // 退出场地
|
||||
SCRIPT_STAGE_FINISHED, // 比赛完成，安全停车
|
||||
} NavScriptStage_t;
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本配置参数
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
float turn_target_angle; // 原地转向目标角度 (rad)，默认 PI (180度)
|
||||
float turn_omega; // 原地转向角速度 (rad/s)
|
||||
float corridor_length; // 垄沟长度估计 (m)，用于触发到端（备用）
|
||||
float entry_align_timeout; // 入口对准超时 (ms)
|
||||
float d_entry_exit_front; // 出入口前向距离阈值 (m)
|
||||
float entry_align_v; // 入口对准慢速前进速度 (m/s)
|
||||
float exit_runout_m; // 离开走廊后继续冲出的距离 (m)
|
||||
float exit_v; // 退出场地直线冲出速度 (m/s)
|
||||
} NavScriptConfig_t;
|
||||
|
||||
/* =========================================================
|
||||
* 段脚本输出
|
||||
* ========================================================= */
|
||||
typedef struct {
|
||||
NavScriptStage_t stage; // 当前执行阶段
|
||||
const char *stage_name; // 阶段名称字符串
|
||||
bool active; // 脚本是否正在运行
|
||||
bool request_corridor; // 是否请求走廊控制器
|
||||
float override_v; // 覆盖输出的 v (m/s)
|
||||
float override_w; // 覆盖输出的 w (rad/s)
|
||||
bool use_override; // 是否使用覆盖值
|
||||
} NavScriptOutput_t;
|
||||
|
||||
/* =========================================================
|
||||
* API 接口
|
||||
* ========================================================= */
|
||||
|
||||
/**
|
||||
* @brief 初始化段脚本执行器
|
||||
*/
|
||||
void NavScript_Init(const NavScriptConfig_t *config);
|
||||
|
||||
/**
|
||||
* @brief 开始运行比赛脚本
|
||||
*/
|
||||
void NavScript_Start(void);
|
||||
|
||||
/**
|
||||
* @brief 核心执行函数：每个导航周期调用一次
|
||||
* @param obs 预处理层的观测快照
|
||||
* @param state 滤波器的走廊状态
|
||||
* @param imu_yaw_continuous_deg IMU unwrap 后的连续偏航角 (度)，用于转弯判定
|
||||
* @param out 段脚本输出
|
||||
*/
|
||||
void NavScript_Update(const CorridorObs_t *obs,
|
||||
const CorridorState_t *state,
|
||||
float imu_yaw_continuous_deg,
|
||||
NavScriptOutput_t *out);
|
||||
|
||||
/**
|
||||
* @brief 获取当前执行阶段
|
||||
*/
|
||||
NavScriptStage_t NavScript_GetStage(void);
|
||||
|
||||
/**
|
||||
* @brief 获取阶段名称
|
||||
*/
|
||||
const char* NavScript_GetStageName(NavScriptStage_t stage);
|
||||
|
||||
/**
|
||||
* @brief 强制重置脚本到 IDLE
|
||||
*/
|
||||
void NavScript_Reset(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // NAV_SCRIPT_H
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user