amiro-os / unittests / periphery-lld / src / ut_alld_dw1000_v1.c @ ed19a68b
History | View | Annotate | Download (11.626 KB)
1 | d3710331 | Cung Sang | /*
|
---|---|---|---|
2 | AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
|
||
3 | Copyright (C) 2016..2019 Thomas Schöpping et al.
|
||
4 | |||
5 | This program is free software: you can redistribute it and/or modify
|
||
6 | it under the terms of the GNU General Public License as published by
|
||
7 | the Free Software Foundation, either version 3 of the License, or
|
||
8 | (at your option) any later version.
|
||
9 | |||
10 | This program is distributed in the hope that it will be useful,
|
||
11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
13 | GNU General Public License for more details.
|
||
14 | |||
15 | You should have received a copy of the GNU General Public License
|
||
16 | along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
17 | */
|
||
18 | |||
19 | #include <amiroos.h> |
||
20 | |||
21 | #if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1)) || defined(__DOXYGEN__) |
||
22 | |||
23 | #include <aos_debug.h> |
||
24 | #include <chprintf.h> |
||
25 | #include <aos_thread.h> |
||
26 | #include <math.h> |
||
27 | #include <module.h> |
||
28 | #include <alld_DW1000.h> |
||
29 | #include <v1/deca_instance_v1.h> |
||
30 | |||
31 | |||
32 | /******************************************************************************/
|
||
33 | /* LOCAL DEFINITIONS */
|
||
34 | /******************************************************************************/
|
||
35 | |||
36 | //#define UNIT_TEST_SNIPPETS_DW1000 // switch between unit test and demo apps
|
||
37 | |||
38 | #define SWS1_SHF_MODE 0x02 //short frame mode (6.81M) |
||
39 | #define SWS1_CH5_MODE 0x04 //channel 5 mode |
||
40 | #define SWS1_ANC_MODE 0x08 //anchor mode |
||
41 | #define SWS1_A1A_MODE 0x10 //anchor/tag address A1 |
||
42 | #define SWS1_A2A_MODE 0x20 //anchor/tag address A2 |
||
43 | #define SWS1_A3A_MODE 0x40 //anchor/tag address A3 |
||
44 | #define SWS1_USB2SPI_MODE 0x78 //USB to SPI mode |
||
45 | |||
46 | #define S1_SWITCH_ON (1) |
||
47 | #define S1_SWITCH_OFF (0) |
||
48 | |||
49 | /******************************************************************************/
|
||
50 | /* EXPORTED VARIABLES */
|
||
51 | /******************************************************************************/
|
||
52 | |||
53 | /******************************************************************************/
|
||
54 | /* LOCAL TYPES */
|
||
55 | /******************************************************************************/
|
||
56 | |||
57 | /******************************************************************************/
|
||
58 | /* LOCAL VARIABLES */
|
||
59 | /******************************************************************************/
|
||
60 | uint8_t s1switch = 0;
|
||
61 | int instance_anchaddr = 0; |
||
62 | int dr_mode = 0; |
||
63 | int chan, tagaddr, ancaddr;
|
||
64 | int instance_mode = ANCHOR;
|
||
65 | |||
66 | |||
67 | /******************************************************************************/
|
||
68 | /* LOCAL FUNCTIONS */
|
||
69 | /******************************************************************************/
|
||
70 | |||
71 | 7b6ab4bf | Cung Sang | /*! @brief Change the SPI speed configuration on the fly */
|
72 | void setHighSpeed_SPI(bool speedValue, DW1000Driver* drv){ |
||
73 | d3710331 | Cung Sang | |
74 | 7b6ab4bf | Cung Sang | spiStop(drv->spid); |
75 | d3710331 | Cung Sang | // spiAcquireBus(&MODULE_HAL_SPI_UWB);
|
76 | |||
77 | if (speedValue == FALSE){
|
||
78 | 7b6ab4bf | Cung Sang | spiStart(drv->spid, &moduleHalSpiUwbLsConfig); // low speed spi configuration
|
79 | d3710331 | Cung Sang | } |
80 | else{
|
||
81 | 7b6ab4bf | Cung Sang | spiStart(drv->spid, &moduleHalSpiUwbHsConfig); // high speed spi configuration
|
82 | d3710331 | Cung Sang | } |
83 | } |
||
84 | |||
85 | /*! @brief Manually set the chip select pin of the SPI */
|
||
86 | 7b6ab4bf | Cung Sang | void set_SPI_chip_select(void){ |
87 | d3710331 | Cung Sang | apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_HIGH); |
88 | } |
||
89 | |||
90 | /*! @brief Manually reset the chip select pin of the SPI */
|
||
91 | 7b6ab4bf | Cung Sang | void clear_SPI_chip_select(void){ |
92 | d3710331 | Cung Sang | apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_LOW); |
93 | } |
||
94 | |||
95 | /*! @brief Manually reset the DW1000 module */
|
||
96 | void reset_DW1000(void){ |
||
97 | |||
98 | // Set the pin as output
|
||
99 | palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_OUTPUT); |
||
100 | |||
101 | //drive the RSTn pin low
|
||
102 | apalGpioWrite(moduleGpioDw1000Reset.gpio, APAL_GPIO_LOW); |
||
103 | |||
104 | //put the pin back to tri-state ... as input
|
||
105 | // palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_INPUT); // TODO:
|
||
106 | |||
107 | aosThdMSleep(2);
|
||
108 | } |
||
109 | |||
110 | |||
111 | 7b6ab4bf | Cung Sang | /*! @brief Configure instance tag/anchor/etc... addresses */
|
112 | d3710331 | Cung Sang | void addressconfigure(uint8_t s1switch, uint8_t mode){
|
113 | uint16_t instAddress ; |
||
114 | |||
115 | instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4; |
||
116 | |||
117 | if(mode == ANCHOR) {
|
||
118 | if(instance_anchaddr > 3) { |
||
119 | instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener |
||
120 | } |
||
121 | else {
|
||
122 | instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr; |
||
123 | } |
||
124 | } |
||
125 | else{
|
||
126 | instAddress = (uint16_t)instance_anchaddr; |
||
127 | } |
||
128 | |||
129 | instancesetaddresses(instAddress); |
||
130 | } |
||
131 | |||
132 | |||
133 | 7b6ab4bf | Cung Sang | /*! @brief returns the use case / operational mode */
|
134 | d3710331 | Cung Sang | int decarangingmode(uint8_t s1switch){
|
135 | int mode = 0; |
||
136 | |||
137 | if(s1switch & SWS1_SHF_MODE) {
|
||
138 | mode = 1;
|
||
139 | } |
||
140 | |||
141 | if(s1switch & SWS1_CH5_MODE) {
|
||
142 | mode = mode + 2;
|
||
143 | } |
||
144 | |||
145 | return mode;
|
||
146 | } |
||
147 | |||
148 | 7b6ab4bf | Cung Sang | /*! @brief Check connection setting and initialize DW1000 module */
|
149 | uint32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){ |
||
150 | d3710331 | Cung Sang | uint32_t devID ; |
151 | int result;
|
||
152 | |||
153 | 7b6ab4bf | Cung Sang | setHighSpeed_SPI(FALSE, drv); //low speed spi max. ~4M
|
154 | d3710331 | Cung Sang | devID = instancereaddeviceid() ; |
155 | |||
156 | if(DWT_DEVICE_ID != devID) {
|
||
157 | 7b6ab4bf | Cung Sang | clear_SPI_chip_select(); |
158 | d3710331 | Cung Sang | Sleep(1);
|
159 | 7b6ab4bf | Cung Sang | set_SPI_chip_select(); |
160 | d3710331 | Cung Sang | Sleep(7);
|
161 | devID = instancereaddeviceid() ; |
||
162 | if(DWT_DEVICE_ID != devID){
|
||
163 | 7b6ab4bf | Cung Sang | return(-1) ; |
164 | d3710331 | Cung Sang | } |
165 | 7b6ab4bf | Cung Sang | dwt_softreset(); |
166 | d3710331 | Cung Sang | } |
167 | |||
168 | reset_DW1000(); //reset the DW1000 by driving the RSTn line low
|
||
169 | |||
170 | if((s1switch & SWS1_ANC_MODE) == 0){ |
||
171 | instance_mode = TAG; |
||
172 | } |
||
173 | else{
|
||
174 | instance_mode = ANCHOR; |
||
175 | } |
||
176 | |||
177 | 7b6ab4bf | Cung Sang | result = instance_init(drv) ; // TODO
|
178 | d3710331 | Cung Sang | // result = instance_init() ;
|
179 | |||
180 | if (0 > result){ |
||
181 | return(-1) ; |
||
182 | } |
||
183 | |||
184 | 7b6ab4bf | Cung Sang | setHighSpeed_SPI(TRUE, drv); // high speed spi max. ~ 20M
|
185 | d3710331 | Cung Sang | devID = instancereaddeviceid() ; |
186 | |||
187 | if (DWT_DEVICE_ID != devID){
|
||
188 | return(-1) ; |
||
189 | } |
||
190 | |||
191 | addressconfigure(s1switch, (uint8_t)instance_mode) ; |
||
192 | |||
193 | if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){ |
||
194 | instance_mode = LISTENER; |
||
195 | } |
||
196 | |||
197 | instancesetrole(instance_mode) ; // Set this instance role
|
||
198 | dr_mode = decarangingmode(s1switch); |
||
199 | chan = chConfig[dr_mode].channelNumber ; |
||
200 | 7b6ab4bf | Cung Sang | instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ; |
201 | d3710331 | Cung Sang | |
202 | return devID;
|
||
203 | } |
||
204 | |||
205 | 7b6ab4bf | Cung Sang | |
206 | /*! @brief Main Entry point to Initialization of UWB DW1000 configuration */
|
||
207 | d3710331 | Cung Sang | #pragma GCC optimize ("O3") |
208 | 7b6ab4bf | Cung Sang | int UWB_Init(DW1000Driver* drv){
|
209 | d3710331 | Cung Sang | |
210 | /*! Software defined Configurartion for TAG, ANC, and other settings as needed */
|
||
211 | 7b6ab4bf | Cung Sang | s1switch = S1_SWITCH_OFF << 1 // (on = 6.8 Mbps, off = 110 kbps) |
212 | | S1_SWITCH_OFF << 2 // (on = CH5, off = CH2) |
||
213 | | S1_SWITCH_OFF << 3 // (on = Anchor, off = TAG) |
||
214 | | S1_SWITCH_OFF << 4 // (configure Tag or anchor ID no.) |
||
215 | | S1_SWITCH_OFF << 5 // (configure Tag or anchor ID no.) |
||
216 | | S1_SWITCH_OFF << 6 // (configure Tag or anchor ID no.) |
||
217 | | S1_SWITCH_OFF << 7; // Not use in this demo |
||
218 | d3710331 | Cung Sang | |
219 | |||
220 | port_DisableEXT_IRQ(); //disable ScenSor IRQ until we configure the device
|
||
221 | |||
222 | if((s1switch & SWS1_USB2SPI_MODE) == SWS1_USB2SPI_MODE){
|
||
223 | return 1; |
||
224 | } |
||
225 | else{
|
||
226 | //run RTLS application
|
||
227 | 7b6ab4bf | Cung Sang | if(inittestapplication(s1switch, drv) == (uint32_t)-1) { |
228 | d3710331 | Cung Sang | return 0; //error |
229 | } |
||
230 | |||
231 | aosThdMSleep(5);
|
||
232 | } |
||
233 | |||
234 | port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
|
||
235 | |||
236 | return 0; |
||
237 | } |
||
238 | |||
239 | |||
240 | /******************************************************************************/
|
||
241 | /* EXPORTED FUNCTIONS */
|
||
242 | /******************************************************************************/
|
||
243 | |||
244 | |||
245 | aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut) { |
||
246 | |||
247 | aosDbgCheck(ut->data != NULL);
|
||
248 | |||
249 | aos_utresult_t result = {0, 0}; |
||
250 | |||
251 | 7b6ab4bf | Cung Sang | chprintf(stream, "init DW1000...\n");
|
252 | dwt_initialise(DWT_LOADUCODE, (DW1000Driver*) ut->data); |
||
253 | aosThdMSleep(5);
|
||
254 | d3710331 | Cung Sang | |
255 | 7b6ab4bf | Cung Sang | |
256 | /*! Unit Test snippets for DW1000.
|
||
257 | * @Note: Passed all 4 unit tests. Event IRQ should be tested separately
|
||
258 | */
|
||
259 | d3710331 | Cung Sang | #ifdef UNIT_TEST_SNIPPETS_DW1000
|
260 | |||
261 | uint32_t actual_deviceId; |
||
262 | |||
263 | port_DisableEXT_IRQ(); |
||
264 | |||
265 | 7b6ab4bf | Cung Sang | setHighSpeed_SPI(false, (DW1000Driver*) ut->data);
|
266 | d3710331 | Cung Sang | chprintf(stream, "expected device ID (LS SPI): 0xDECA0130 \n");
|
267 | aosThdMSleep(5);
|
||
268 | actual_deviceId = instancereaddeviceid(); |
||
269 | chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
|
||
270 | aosThdMSleep(5);
|
||
271 | |||
272 | if(DWT_DEVICE_ID != actual_deviceId) //if the read of device ID fails, the DW1000 could be asleep |
||
273 | { |
||
274 | port_SPIx_clear_chip_select(); //CS low
|
||
275 | aosThdMSleep(1); //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise |
||
276 | port_SPIx_set_chip_select(); //CS high
|
||
277 | aosThdMSleep(7);
|
||
278 | actual_deviceId = instancereaddeviceid() ; |
||
279 | |||
280 | // SPI not working or Unsupported Device ID
|
||
281 | if(DWT_DEVICE_ID != actual_deviceId){
|
||
282 | chprintf(stream, "SPI not working or Unsupported Device ID\n");
|
||
283 | chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);
|
||
284 | chprintf(stream, "expected device ID: 0xDECA0130 \n");
|
||
285 | aosThdMSleep(5);
|
||
286 | // return(-1) ;
|
||
287 | } |
||
288 | |||
289 | //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
|
||
290 | dwt_softreset(); |
||
291 | } |
||
292 | |||
293 | 7b6ab4bf | Cung Sang | /*! UT1: Low speed SPI result */
|
294 | d3710331 | Cung Sang | if (actual_deviceId == DWT_DEVICE_ID){
|
295 | aosUtPassed(stream, &result); |
||
296 | } else {
|
||
297 | aosUtFailed(stream, &result); |
||
298 | } |
||
299 | reset_DW1000(); |
||
300 | |||
301 | |||
302 | chprintf(stream, " Initialise instance for DW1000 \n");
|
||
303 | aosThdSleep(5);
|
||
304 | |||
305 | int x_init = instance_init((DW1000Driver*) ut->data) ; // TODO |
||
306 | // int x_init = instance_init() ;
|
||
307 | |||
308 | if (0 != x_init){ |
||
309 | chprintf(stream, "Init error with return value: %d \n", x_init);
|
||
310 | aosThdSleep(5);
|
||
311 | } |
||
312 | else {
|
||
313 | chprintf(stream, "Init success with return value: %d \n", x_init);
|
||
314 | aosThdSleep(5);
|
||
315 | } |
||
316 | |||
317 | |||
318 | 7b6ab4bf | Cung Sang | /*! UT2: Initialization result*/
|
319 | d3710331 | Cung Sang | if (x_init == 0){ |
320 | aosUtPassed(stream, &result); |
||
321 | } else {
|
||
322 | aosUtFailed(stream, &result); |
||
323 | } |
||
324 | |||
325 | |||
326 | 7b6ab4bf | Cung Sang | setHighSpeed_SPI(true, (DW1000Driver*) ut->data);
|
327 | d3710331 | Cung Sang | |
328 | chprintf(stream, "expected device ID (HS SPI): 0xDECA0130\n");
|
||
329 | actual_deviceId = instancereaddeviceid(); |
||
330 | chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
|
||
331 | aosThdMSleep(5);
|
||
332 | |||
333 | 7b6ab4bf | Cung Sang | /*! UT3: High speed SPI result*/
|
334 | d3710331 | Cung Sang | if (actual_deviceId == DWT_DEVICE_ID){
|
335 | aosUtPassed(stream, &result); |
||
336 | } else {
|
||
337 | aosUtFailed(stream, &result); |
||
338 | } |
||
339 | |||
340 | port_EnableEXT_IRQ(); |
||
341 | reset_DW1000(); |
||
342 | |||
343 | |||
344 | chprintf(stream, " Initialise the configuration for UWB application \n");
|
||
345 | aosThdSleep(5);
|
||
346 | |||
347 | 7b6ab4bf | Cung Sang | int uwb_init = UWB_Init((DW1000Driver*) ut->data);
|
348 | d3710331 | Cung Sang | |
349 | if (0 != uwb_init){ |
||
350 | chprintf(stream, "UWB config error with return value: %d \n", uwb_init);
|
||
351 | aosThdSleep(5);
|
||
352 | } |
||
353 | else {
|
||
354 | chprintf(stream, "UWB config success with return value: %d \n", uwb_init);
|
||
355 | aosThdSleep(5);
|
||
356 | } |
||
357 | |||
358 | 7b6ab4bf | Cung Sang | /*! UT4: UWB configuration result
|
359 | d3710331 | Cung Sang | * If all the four unit tests are passed, the module is ready to run.
|
360 | * Note that the interrupt IRQn should be tested separately.
|
||
361 | */
|
||
362 | if (uwb_init == 0){ |
||
363 | aosUtPassed(stream, &result); |
||
364 | } else {
|
||
365 | aosUtFailed(stream, &result); |
||
366 | } |
||
367 | |||
368 | /************** End of UNIT_TEST_SNIPPETS_DW1000*****************/
|
||
369 | |||
370 | #else
|
||
371 | |||
372 | // RUN THE STATE MACHINE DEMO APP
|
||
373 | |||
374 | chprintf(stream, " Initialise the State Machine \n");
|
||
375 | aosThdSleep(2);
|
||
376 | |||
377 | 7b6ab4bf | Cung Sang | /* Initialize UWB system with user defined configuration */
|
378 | int uwb_init = UWB_Init((DW1000Driver*) ut->data);
|
||
379 | d3710331 | Cung Sang | |
380 | if (0 != uwb_init){ |
||
381 | chprintf(stream, "UWB config error with return value: %d \n", uwb_init);
|
||
382 | } |
||
383 | else {
|
||
384 | chprintf(stream, "UWB config success with return value: %d \n", uwb_init);
|
||
385 | } |
||
386 | aosThdSleep(1);
|
||
387 | |||
388 | chprintf(stream, " Running the RTLS demo application \n");
|
||
389 | aosThdSleep(1);
|
||
390 | |||
391 | |||
392 | /* Run the localization system demo app as a thread */
|
||
393 | while(1){ |
||
394 | instance_run(); |
||
395 | // aosThdUSleep(10);
|
||
396 | } |
||
397 | |||
398 | #endif /* UNIT_TEST_SNIPPETS_DW1000 */ |
||
399 | |||
400 | return result;
|
||
401 | } |
||
402 | |||
403 | |||
404 | #endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */ |