-
Notifications
You must be signed in to change notification settings - Fork 0
/
uart_read_line_sensor_ld2410v3.h
286 lines (265 loc) · 7.99 KB
/
uart_read_line_sensor_ld2410v3.h
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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
#include "esphome.h"
class LD2410 : public Component, public UARTDevice
{
public:
LD2410(UARTComponent *parent) : UARTDevice(parent) {}
std::vector<uint8_t> bytes;
const std::vector<uint8_t> config_header = {0xFD, 0xFC, 0xFB, 0xFA, 0x1C, 0x00};
const std::vector<uint8_t> target_header = {0xF4, 0xF3, 0xF2, 0xF1, 0x0D, 0x00};
const std::vector<uint8_t> ld2410_end_conf = {0x04, 0x03, 0x02, 0x01};
void ESP_LOGD_HEX(std::vector<uint8_t> bytes, uint8_t separator) {
std::string res;
size_t len = bytes.size();
char buf[5];
for (size_t i = 0; i < len; i++) {
if (i > 0) {
res += separator;
}
sprintf(buf, "%02X", bytes[i]);
res += buf;
}
ESP_LOGD("custom", "%s", res.c_str());
}
void sendCommand(char *commandStr, char *commandValue, int commandValueLen)
{
uint16_t len = 2;
if (commandValue != nullptr) {
len += commandValueLen;
}
std::vector<uint8_t> ld2410_conf = {0xFD, 0xFC, 0xFB, 0xFA, lowByte(len), highByte(len), commandStr[0], commandStr[1]};
if (commandValue != nullptr)
{
for (int i = 0; i < commandValueLen; i++)
{
ld2410_conf.push_back(commandValue[i]);
}
}
for (int i = 0; i < ld2410_end_conf.size(); i++)
{
ld2410_conf.push_back(ld2410_end_conf[i]);
}
// ESP_LOGD_HEX(ld2410_conf,':');
write_array(std::vector<uint8_t>(ld2410_conf.begin(), ld2410_conf.end()));
}
int twoByteToInt(char firstByte, char secondByte)
{
return (int16_t)(secondByte << 8) + firstByte;
}
void handleTargetData(std::vector<uint8_t> buffer)
{
TARGETUnion targetUnion;
std::copy(buffer.begin(), buffer.end(), targetUnion.bytes);
if (id(show_stats).state == 1 && targetUnion.target.type == 0x02 && targetUnion.target.state != 0x00)
{
int movdist = twoByteToInt(targetUnion.target.movdist, targetUnion.target.movdist2);
if (id(movingTargetDistance).state != movdist)
{
id(movingTargetDistance).publish_state(movdist);
}
if (id(movingTargetEnergy).state != targetUnion.target.movval)
{
id(movingTargetEnergy).publish_state(targetUnion.target.movval);
}
int stadist = twoByteToInt(targetUnion.target.stadist, targetUnion.target.stadist2);
if (id(stillTargetDistance).state != stadist)
{
id(stillTargetDistance).publish_state(stadist);
}
if (id(stillTargetEnergy).state != targetUnion.target.staval)
{
id(stillTargetEnergy).publish_state(targetUnion.target.staval);
}
int decdist = twoByteToInt(targetUnion.target.decdist, targetUnion.target.decdist2);
if (id(detectDistance).state != decdist)
{
id(detectDistance).publish_state(decdist);
}
}
else
{
return;
}
// Engineering data - datasheet is horrible
// if (targetUnion.target.type == 0x01)
// }
}
void handleConfData(std::vector<uint8_t> buffer)
{
CONFUnion confUnion;
std::copy(buffer.begin(), buffer.end(), confUnion.bytes);
if (confUnion.conf.cmd == 0x61 && confUnion.conf.cmd_val == 0x01 && confUnion.conf.ack_stat == 0x00 && confUnion.conf.head == 0xAA)
{
id(maxconfigDistance).publish_state(float(confUnion.conf.max_sta_dist * 0.75));
id(allSensitivity).publish_state(confUnion.conf.mov0sen);
id(noneDuration).publish_state(confUnion.conf.none);
}
}
void setConfigMode(bool confenable)
{
char cmd[2] = {confenable ? 0xFF : 0xFE,0x00};
char value[2] = {0x01, 0x00};
sendCommand(cmd, confenable ? value : nullptr, 2);
}
void queryParameters()
{
char cmd_query[2] = {0x61, 0x00};
sendCommand(cmd_query, nullptr, 0);
}
void setup() override
{ }
void loop() override
{
while (available())
{
bytes.push_back(read());
if (bytes.size() < 6)
{
continue;
}
if (doesHeaderMatch(bytes, config_header))
{
if (bytes.size() < sizeof(CONF))
{
continue;
}
handleConfData(bytes);
bytes.clear();
}
else if (doesHeaderMatch(bytes, target_header)) {
if (bytes.size() < sizeof(TARGET))
{
continue;
}
handleTargetData(bytes);
bytes.clear();
}
else
{
bytes.erase(bytes.begin());
continue;
}
}
}
void setEngineeringMode(bool engenable)
{
char cmd[2] = {engenable ? 0x62 : 0x63,0x00};
sendCommand(cmd, nullptr, 0);
}
void setMaxDistancesAndNoneDuration(int maxMovingDistanceRange, int maxStillDistanceRange, int noneDuration)
{
char cmd[2] = {0x60, 0x00};
char value[18] = {0x00, 0x00, lowByte(maxMovingDistanceRange), highByte(maxMovingDistanceRange), 0x00, 0x00, 0x01, 0x00, lowByte(maxStillDistanceRange), highByte(maxStillDistanceRange), 0x00, 0x00, 0x02, 0x00, lowByte(noneDuration), highByte(noneDuration), 0x00, 0x00};
sendCommand(cmd, value, sizeof(value));
}
void setAllSensitivity(int senval)
{
// 64 00 00 00 FF FF 00 00 01 00 28 00 00 00 02 00 28 00 00 00 04 03 02 01
// {cmd }{dword}{ dgate }{mword} { mval }{sword}{ sval }{ MFR }
char cmd[2] = {0x64, 0x00};
char value[18] = {0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x01, 0x00, lowByte(senval), highByte(senval), 0x00, 0x00, 0x02, 0x00, lowByte(senval), highByte(senval), 0x00, 0x00};
sendCommand(cmd, value, sizeof(value));
}
void factoryReset()
{
char cmd[2] = {0xA2, 0x00};
sendCommand(cmd, nullptr, 0);
}
void reboot()
{
char cmd[2] = {0xA3, 0x00};
sendCommand(cmd, nullptr, 0);
// not need to exit config mode because the ld2410 will reboot automatically
}
void setBaudrate(int index)
{
char cmd[2] = {0xA1, 0x00};
char value[2] = {index, 0x00};
sendCommand(cmd, value, sizeof(value));
}
bool doesHeaderMatch(std::vector<uint8_t> bytes, std::vector<uint8_t> header)
{
bool is_equal = std::equal(header.begin(), header.end(), bytes.begin());
return (is_equal == true ? true : false);
}
/*
TARGET EXAMPLE DATA
{F4:F3:F2:F1}:{0D:00}:{02}:{AA}: 02 : 4B:00: 4F : 00:00 : 64 : 29:00 :{55}: {00} :{F8:F7:F6:F5}
{ header } {len} {typ}{hd}{state}{mdist}{mval}{stadis}{staval}{decdis} {tl} {chck} { MFR }
*/
typedef struct
{
uint32_t MFR;
uint16_t len;
uint8_t type; // target or engineering
uint8_t head; // fixed head
uint8_t state; // state
uint8_t movdist; // movement distance
uint8_t movdist2; // movement distance
uint8_t movval; // movement energy value
uint8_t stadist; // stationary distance
uint8_t stadist2; // stationary distance
uint8_t staval; // stationary energy value
uint8_t decdist; // detection distance
uint8_t decdist2; // detection distance
uint8_t tail; // tail
uint8_t chk; // unused
uint32_t MFR_end ; // end
} TARGET;
typedef union
{
TARGET target;
uint8_t bytes[sizeof(TARGET)];
} TARGETUnion;
/*
CONF EXAMPLE DATA
FD:FC:FB:FA MFR[0-3]
1C:00 len[4-5]
61:01 CMD[6-7]
00:00 ACKstat[8-9]
AA Head [10]
08 maxDist [11]
06 maxMovDist[12]
06 maxStaDist[13]
1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E:1E (9mov & 9sta sensitivities)
5A:00 none[32-33]
04:03:02:01[34-37]
*/
typedef struct
{
uint32_t MFR;
uint16_t len;
uint8_t cmd;
uint8_t cmd_val;
uint8_t ack_stat;
uint8_t ack_stathigh;
uint8_t head;
uint8_t max_dist;
uint8_t max_mov_dist;
uint8_t max_sta_dist;
uint8_t mov0sen;
uint8_t mov1sen;
uint8_t mov2sen;
uint8_t mov3sen;
uint8_t mov4sen;
uint8_t mov5sen;
uint8_t mov6sen;
uint8_t mov7sen;
uint8_t mov8sen;
uint8_t sta0sen;
uint8_t sta1sen;
uint8_t sta2sen;
uint8_t sta3sen;
uint8_t sta4sen;
uint8_t sta5sen;
uint8_t sta6sen;
uint8_t sta7sen;
uint8_t sta8sen;
uint16_t none;
uint32_t MFR_end;
} CONF;
typedef union
{
CONF conf;
uint8_t bytes[sizeof(CONF)];
} CONFUnion;
};