-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
vl53l5cx_platform.c
118 lines (94 loc) · 2.6 KB
/
vl53l5cx_platform.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
/*
* Copyright (C) 2024 Fabien-B <name.surname@gmail.com>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*/
#include "lidar/vl53l5cx_platform.h"
#include "hal.h"
#define VL53L5_I2C_TIMEOUT chTimeMS2I(100)
uint8_t RdByte(
VL53L5CX_Platform *dev,
uint16_t index,
uint8_t *p_data)
{
return RdMulti(dev, index, p_data, 1);
}
uint8_t WrByte(
VL53L5CX_Platform *dev,
uint16_t index,
uint8_t data)
{
return WrMulti(dev, index, &data, 1);
}
uint8_t WrMulti(
VL53L5CX_Platform *dev,
uint16_t index,
uint8_t *pdata,
uint32_t count)
{
I2CDriver * i2cd = (I2CDriver *)dev->i2cdev->reg_addr;
i2cAcquireBus(i2cd);
msg_t ret;
for(size_t offset = 0; offset<count; offset += VL53L5CX_I2C_BUF_SIZE) {
uint16_t reg_addr = index + offset;
dev->buf[0] = (reg_addr & 0xFF00) >> 8; // MSB first
dev->buf[1] = (reg_addr & 0x00FF);
size_t size = Min(count-offset, VL53L5CX_I2C_BUF_SIZE);
memcpy((uint8_t *) dev->buf + 2, (pdata + offset), size);
cacheBufferFlush(dev->buf, size+2);
ret = i2cMasterTransmitTimeout(i2cd, dev->address, dev->buf, size+2, NULL, 0, VL53L5_I2C_TIMEOUT);
}
i2cReleaseBus(i2cd);
return ret;
}
uint8_t RdMulti(
VL53L5CX_Platform *dev,
uint16_t index,
uint8_t *pdata,
uint32_t count)
{
I2CDriver * i2cd = (I2CDriver *)dev->i2cdev->reg_addr;
i2cAcquireBus(i2cd);
msg_t ret = 0;
for(size_t offset = 0; offset<count; offset += VL53L5CX_I2C_BUF_SIZE) {
uint16_t reg_addr = index + offset;
dev->buf[0] = (reg_addr & 0xFF00) >> 8; // MSB first
dev->buf[1] = (reg_addr & 0x00FF);
size_t size = Min(count-offset, VL53L5CX_I2C_BUF_SIZE);
cacheBufferFlush(dev->buf, 2);
ret = i2cMasterTransmitTimeout(i2cd, dev->address, dev->buf, 2, dev->buf, size, VL53L5_I2C_TIMEOUT);
cacheBufferInvalidate(dev->buf, count);
memcpy(pdata+offset, dev->buf, count);
}
i2cReleaseBus(i2cd);
return ret;
}
void SwapBuffer(
uint8_t *buffer,
uint16_t size)
{
uint32_t i, tmp;
/* Example of possible implementation using <string.h> */
for(i = 0; i < size; i = i + 4)
{
tmp = (
buffer[i]<<24)
|(buffer[i+1]<<16)
|(buffer[i+2]<<8)
|(buffer[i+3]);
memcpy(&(buffer[i]), &tmp, 4);
}
}
uint8_t WaitMs(
VL53L5CX_Platform *p_platform,
uint32_t TimeMs)
{
(void)p_platform;
chThdSleepMilliseconds(TimeMs);
return 0;
}