-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrange_rx.ino
203 lines (168 loc) · 6.74 KB
/
range_rx.ino
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
#include "dw3000.h"
#include "SPI.h"
extern SPISettings _fastSPI;
#define PIN_RST 27
#define PIN_IRQ 34
#define PIN_SS 4
#define RNG_DELAY_MS 100
#define TX_ANT_DLY 16391
#define RX_ANT_DLY 16391
#define ALL_MSG_COMMON_LEN 10
#define ALL_MSG_SN_IDX 2
#define RESP_MSG_POLL_RX_TS_IDX 10
#define RESP_MSG_RESP_TX_TS_IDX 14
#define RESP_MSG_TS_LEN 4
#define POLL_TX_TO_RESP_RX_DLY_UUS 240
#define RESP_RX_TIMEOUT_UUS 400
/* Default communication configuration. We use default non-STS DW mode. */
static dwt_config_t config = {
5, /* Channel number. */
DWT_PLEN_64, /* Preamble length. Used in TX only. */
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */
1, /* 0 to use standard 8 symbol SFD, 1 to use non-standard 8 symbol, 2 for non-standard 16 symbol SFD and 3 for 4z 8 symbol SDF type */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
DWT_PHRRATE_STD, /* PHY header rate. */
(129 + 8 - 8), /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
DWT_STS_MODE_OFF, /* STS disabled */
DWT_STS_LEN_64, /* STS length see allowed values in Enum dwt_sts_lengths_e */
DWT_PDOA_M0 /* PDOA mode off */
};
static uint8_t frame_seq_nb = 0;
static uint8_t rx_buffer[20];
static uint32_t status_reg = 0;
static double tof;
static double distance;
extern dwt_txconfig_t txconfig_options;
int counter = 0;
void setup()
{
UART_init();
spiBegin(PIN_IRQ, PIN_RST);
spiSelect(PIN_SS);
_fastSPI = SPISettings(16000000L, MSBFIRST, SPI_MODE0);
delay(2); // Time needed for DW3000 to start up (transition from INIT_RC to IDLE_RC, or could wait for SPIRDY event)
while (!dwt_checkidlerc()) // Need to make sure DW IC is in IDLE_RC before proceeding
{
UART_puts("IDLE FAILED\r\n");
while (1)
;
}
if (dwt_initialise(DWT_DW_INIT) == DWT_ERROR)
{
UART_puts("INIT FAILED\r\n");
while (1)
;
}
// Enabling LEDs here for debug so that for each TX the D1 LED will flash on DW3000 red eval-shield boards.
dwt_setleds(DWT_LEDS_ENABLE | DWT_LEDS_INIT_BLINK);
/* Configure DW IC. See NOTE 6 below. */
if (dwt_configure(&config)) // if the dwt_configure returns DWT_ERROR either the PLL or RX calibration has failed the host should reset the device
{
UART_puts("CONFIG FAILED\r\n");
while (1)
;
}
/* Configure the TX spectrum parameters (power, PG delay and PG count) */
dwt_configuretxrf(&txconfig_options);
/* Apply default antenna delay value. See NOTE 2 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Set expected response's delay and timeout. See NOTE 1 and 5 below.
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
/* Next can enable TX/RX states output on GPIOs 5 and 6 to help debug, and also TX/RX LEDs
* Note, in real low power applications the LEDs should not be used. */
dwt_setlnapamode(DWT_LNA_ENABLE | DWT_PA_ENABLE);
Serial.println("Range RX");
Serial.println("Setup over........");
}
char tagID;
#define TAG_nb 4 //number of TAG used for Ranging
static double dist_result[TAG_nb];
void loop()
{
Serial.print(counter);
Serial.print(",");
/* Ranging process for Tag ID : 1 */
Multi_Ranging('1',0);
Serial.print(",");
//double dist_TAG1 = dist_result[0];
/* Ranging process for Tag ID : 2 */
Multi_Ranging('2',1);
Serial.print(",");
//double dist_TAG2 = dist_result[1];
/* Ranging process for Tag ID : 3 */
Multi_Ranging('3',2);
//double dist_TAG3 = dist_result[2];
Serial.println();
counter++;
/* A delay between ranging exchanges. */
Sleep(RNG_DELAY_MS);
}
void Multi_Ranging(char tagID, uint8_t dist){
double tof;
double distance;
uint8_t tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'T', 'A', 'G', tagID, 0xE0, 0, 0};
uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'G', tagID, 'T', 'A', 0xE1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS_BIT_MASK);
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0, 1);
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG_BIT_MASK | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG_BIT_MASK)
{
uint32_t frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG_BIT_MASK);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RXFLEN_MASK;
if (frame_len <= sizeof(rx_buffer))
{
dwt_readrxdata(rx_buffer, frame_len, 0);
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
{
uint32_t poll_tx_ts, resp_rx_ts, poll_rx_ts, resp_tx_ts;
int32_t rtd_init, rtd_resp;
float clockOffsetRatio ;
poll_tx_ts = dwt_readtxtimestamplo32();
resp_rx_ts = dwt_readrxtimestamplo32();
clockOffsetRatio = ((float)dwt_readclockoffset()) / (uint32_t)(1<<26);
resp_msg_get_ts(&rx_buffer[RESP_MSG_POLL_RX_TS_IDX], &poll_rx_ts);
resp_msg_get_ts(&rx_buffer[RESP_MSG_RESP_TX_TS_IDX], &resp_tx_ts);
rtd_init = resp_rx_ts - poll_tx_ts;
rtd_resp = resp_tx_ts - poll_rx_ts;
tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
dist_result[dist] = distance;
/* switch(dist){
case 0:
dist_result[dist] = distance;
break;
case 1:
dist_result[dist] = distance;
break;
case 2:
dist_result[dist] = distance;
break;
}*/
Serial.print("TAG ID : ");
Serial.print(dist + 1);
Serial.print(", distance : ");
Serial.print(dist_result[dist]);
}
}
}
else
{
/* Clear RX error/timeout events in the DW IC status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
}
//return dist_result;
/* Execute a delay between ranging exchanges. */
}