amiro-os / test / periphery-lld / DW1000_v1 / aos_test_DW1000.c @ a3ac2400
History | View | Annotate | Download (11.6 KB)
1 | e05848a6 | Robin Ewers | /*
|
---|---|---|---|
2 | AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
|
||
3 | 84f0ce9e | Thomas Schöpping | Copyright (C) 2016..2019 Thomas Schöpping et al.
|
4 | e05848a6 | Robin Ewers | |
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 | ddf34c3d | Thomas Schöpping | #include <amiroos.h> |
20 | 4c72a54c | Thomas Schöpping | #include <aos_test_DW1000.h> |
21 | ddf34c3d | Thomas Schöpping | |
22 | 4c72a54c | Thomas Schöpping | #if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__) |
23 | 4cb40108 | Thomas Schöpping | |
24 | #include <aos_debug.h> |
||
25 | #include <chprintf.h> |
||
26 | #include <aos_thread.h> |
||
27 | #include <math.h> |
||
28 | #include <module.h> |
||
29 | #include <alld_DW1000.h> |
||
30 | 4c72a54c | Thomas Schöpping | #include <deca_instance.h> |
31 | 4cb40108 | Thomas Schöpping | |
32 | e05848a6 | Robin Ewers | |
33 | f3ac1c96 | Thomas Schöpping | /******************************************************************************/
|
34 | /* LOCAL DEFINITIONS */
|
||
35 | /******************************************************************************/
|
||
36 | e05848a6 | Robin Ewers | |
37 | 8d4d058e | Cung Sang | #define TEST_SNIPPETS_DW1000 // switch between test and demo apps |
38 | 4cb40108 | Thomas Schöpping | |
39 | #define SWS1_SHF_MODE 0x02 //short frame mode (6.81M) |
||
40 | #define SWS1_CH5_MODE 0x04 //channel 5 mode |
||
41 | #define SWS1_ANC_MODE 0x08 //anchor mode |
||
42 | #define SWS1_A1A_MODE 0x10 //anchor/tag address A1 |
||
43 | #define SWS1_A2A_MODE 0x20 //anchor/tag address A2 |
||
44 | #define SWS1_A3A_MODE 0x40 //anchor/tag address A3 |
||
45 | |||
46 | #define S1_SWITCH_ON (1) |
||
47 | #define S1_SWITCH_OFF (0) |
||
48 | |||
49 | f3ac1c96 | Thomas Schöpping | /******************************************************************************/
|
50 | /* EXPORTED VARIABLES */
|
||
51 | /******************************************************************************/
|
||
52 | |||
53 | /******************************************************************************/
|
||
54 | /* LOCAL TYPES */
|
||
55 | /******************************************************************************/
|
||
56 | |||
57 | /******************************************************************************/
|
||
58 | /* LOCAL VARIABLES */
|
||
59 | /******************************************************************************/
|
||
60 | 4cb40108 | Thomas Schöpping | 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 | f3ac1c96 | Thomas Schöpping | |
67 | /******************************************************************************/
|
||
68 | /* LOCAL FUNCTIONS */
|
||
69 | /******************************************************************************/
|
||
70 | |||
71 | 4cb40108 | Thomas Schöpping | /*! @brief Configure instance tag/anchor/etc... addresses */
|
72 | void addressconfigure(uint8_t s1switch, uint8_t mode){
|
||
73 | uint16_t instAddress ; |
||
74 | |||
75 | instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4; |
||
76 | |||
77 | if(mode == ANCHOR) {
|
||
78 | if(instance_anchaddr > 3) { |
||
79 | instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener |
||
80 | } |
||
81 | else {
|
||
82 | instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr; |
||
83 | } |
||
84 | } |
||
85 | else{
|
||
86 | instAddress = (uint16_t)instance_anchaddr; |
||
87 | } |
||
88 | |||
89 | instancesetaddresses(instAddress); |
||
90 | } |
||
91 | |||
92 | /*! @brief returns the use case / operational mode */
|
||
93 | int decarangingmode(uint8_t s1switch){
|
||
94 | int mode = 0; |
||
95 | |||
96 | if(s1switch & SWS1_SHF_MODE) {
|
||
97 | mode = 1;
|
||
98 | } |
||
99 | |||
100 | if(s1switch & SWS1_CH5_MODE) {
|
||
101 | mode = mode + 2;
|
||
102 | } |
||
103 | |||
104 | return mode;
|
||
105 | } |
||
106 | |||
107 | /*! @brief Check connection setting and initialize DW1000 module */
|
||
108 | int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){ |
||
109 | uint32_t devID ; |
||
110 | int result;
|
||
111 | |||
112 | setHighSpeed_SPI(FALSE, drv); //low speed spi max. ~4M
|
||
113 | devID = instancereaddeviceid() ; |
||
114 | |||
115 | if(DWT_DEVICE_ID != devID) {
|
||
116 | clear_SPI_chip_select(); |
||
117 | Sleep(1);
|
||
118 | set_SPI_chip_select(); |
||
119 | Sleep(7);
|
||
120 | devID = instancereaddeviceid() ; |
||
121 | if(DWT_DEVICE_ID != devID){
|
||
122 | return(-1) ; |
||
123 | } |
||
124 | dwt_softreset(); |
||
125 | } |
||
126 | |||
127 | reset_DW1000(); //reset the DW1000 by driving the RSTn line low
|
||
128 | |||
129 | if((s1switch & SWS1_ANC_MODE) == 0){ |
||
130 | instance_mode = TAG; |
||
131 | } |
||
132 | else{
|
||
133 | instance_mode = ANCHOR; |
||
134 | } |
||
135 | |||
136 | result = instance_init(drv) ; |
||
137 | |||
138 | if (0 > result){ |
||
139 | return(-1) ; |
||
140 | } |
||
141 | |||
142 | setHighSpeed_SPI(TRUE, drv); // high speed spi max. ~ 20M
|
||
143 | devID = instancereaddeviceid() ; |
||
144 | |||
145 | if (DWT_DEVICE_ID != devID){
|
||
146 | return(-1) ; |
||
147 | } |
||
148 | |||
149 | addressconfigure(s1switch, (uint8_t)instance_mode) ; |
||
150 | |||
151 | if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){ |
||
152 | instance_mode = LISTENER; |
||
153 | } |
||
154 | |||
155 | instancesetrole(instance_mode) ; // Set this instance role
|
||
156 | dr_mode = decarangingmode(s1switch); |
||
157 | chan = chConfig[dr_mode].channelNumber ; |
||
158 | instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ; |
||
159 | |||
160 | return (int32_t)devID;
|
||
161 | } |
||
162 | |||
163 | /*! @brief Main Entry point to Initialization of UWB DW1000 configuration */
|
||
164 | #pragma GCC optimize ("O3") |
||
165 | int UWB_Init(DW1000Driver* drv){
|
||
166 | |||
167 | /*! Software defined Configurartion for TAG, ANC, and other settings as needed */
|
||
168 | s1switch = S1_SWITCH_OFF << 1 // (on = 6.8 Mbps, off = 110 kbps) |
||
169 | | S1_SWITCH_OFF << 2 // (on = CH5, off = CH2) |
||
170 | | S1_SWITCH_OFF << 3 // (on = Anchor, off = TAG) |
||
171 | | S1_SWITCH_OFF << 4 // (configure Tag or anchor ID no.) |
||
172 | | S1_SWITCH_OFF << 5 // (configure Tag or anchor ID no.) |
||
173 | | S1_SWITCH_OFF << 6 // (configure Tag or anchor ID no.) |
||
174 | | S1_SWITCH_OFF << 7; // Not use in this demo |
||
175 | |||
176 | |||
177 | port_DisableEXT_IRQ(); //disable ScenSor IRQ until we configure the device
|
||
178 | |||
179 | if(inittestapplication(s1switch, drv) == -1) { |
||
180 | return (-1); //error |
||
181 | } |
||
182 | |||
183 | aosThdMSleep(5);
|
||
184 | |||
185 | port_EnableEXT_IRQ(); //enable DW1000 IRQ before starting
|
||
186 | |||
187 | return 0; |
||
188 | } |
||
189 | |||
190 | |||
191 | f3ac1c96 | Thomas Schöpping | /******************************************************************************/
|
192 | /* EXPORTED FUNCTIONS */
|
||
193 | /******************************************************************************/
|
||
194 | e05848a6 | Robin Ewers | |
195 | 8d4d058e | Cung Sang | aos_testresult_t aosTestDw1000Func(BaseSequentialStream* stream, const aos_test_t* test) {
|
196 | |||
197 | aosDbgCheck(test->data != NULL &&
|
||
198 | ((aos_test_dw1000data_t*)test->data)->driver != NULL &&
|
||
199 | ((aos_test_dw1000data_t*)test->data)->evtsource != NULL);
|
||
200 | e05848a6 | Robin Ewers | |
201 | a3ac2400 | Cung Sang | aos_testresult_t result = {0, 0}; |
202 | |||
203 | |||
204 | #if (BOARD_MIC9404x_CONNECTED == true) |
||
205 | // Enable 3.3V and 1.8V supply voltages for powering up the DW1000 module in AMiRo Light Ring
|
||
206 | if ((test->data != NULL) && (((aos_test_dw1000data_t*)(test->data))->mic9404xd != NULL)){ |
||
207 | // && (((aos_test_dw1000data_t*)(test->data))->driver == NULL)){
|
||
208 | mic9404x_lld_state_t state; |
||
209 | uint32_t status = APAL_STATUS_OK; |
||
210 | |||
211 | chprintf(stream, "reading current status of the Power..\n");
|
||
212 | status = mic9404x_lld_get(((aos_test_dw1000data_t*)(test->data))->mic9404xd, &state); |
||
213 | if (status == APAL_STATUS_OK) {
|
||
214 | aosTestPassedMsg(stream, &result, "power %s\n", (state == MIC9404x_LLD_STATE_ON) ? "enabled" : "disabled"); |
||
215 | } else {
|
||
216 | aosTestFailed(stream, &result); |
||
217 | } |
||
218 | if (state == MIC9404x_LLD_STATE_OFF) {
|
||
219 | chprintf(stream, "enabling the power ...\n");
|
||
220 | status = mic9404x_lld_set(((aos_test_dw1000data_t*)(test->data))->mic9404xd, MIC9404x_LLD_STATE_ON); |
||
221 | status |= mic9404x_lld_get(((aos_test_dw1000data_t*)(test->data))->mic9404xd, &state); |
||
222 | if (state == MIC9404x_LLD_STATE_ON) {
|
||
223 | aosThdSSleep(2);
|
||
224 | status |= mic9404x_lld_get(((aos_test_dw1000data_t*)(test->data))->mic9404xd, &state); |
||
225 | } |
||
226 | if ((status == APAL_STATUS_OK) && (state == MIC9404x_LLD_STATE_ON)) {
|
||
227 | aosTestPassed(stream, &result); |
||
228 | } else {
|
||
229 | aosTestFailed(stream, &result); |
||
230 | } |
||
231 | } |
||
232 | aosThdSleep(1);
|
||
233 | return result;
|
||
234 | } |
||
235 | #endif /* BOARD_MIC9404x_CONNECTED == true */ |
||
236 | e05848a6 | Robin Ewers | |
237 | |||
238 | 4cb40108 | Thomas Schöpping | chprintf(stream, "init DW1000...\n");
|
239 | 4c72a54c | Thomas Schöpping | dwt_initialise(DWT_LOADUCODE, ((aos_test_dw1000data_t*)test->data)->driver); |
240 | 4cb40108 | Thomas Schöpping | aosThdMSleep(5);
|
241 | e05848a6 | Robin Ewers | |
242 | 4c72a54c | Thomas Schöpping | /*! Test snippets for DW1000.
|
243 | 4cb40108 | Thomas Schöpping | * @Note: Event IRQ for DW1000 should be tested separately
|
244 | */
|
||
245 | 4c72a54c | Thomas Schöpping | #if defined(TEST_SNIPPETS_DW1000)
|
246 | e05848a6 | Robin Ewers | |
247 | 4cb40108 | Thomas Schöpping | uint32_t actual_deviceId; |
248 | e05848a6 | Robin Ewers | |
249 | 4cb40108 | Thomas Schöpping | port_DisableEXT_IRQ(); |
250 | e05848a6 | Robin Ewers | |
251 | 4c72a54c | Thomas Schöpping | setHighSpeed_SPI(false, ((aos_test_dw1000data_t*)test->data)->driver);
|
252 | a193bcf1 | Thomas Schöpping | chprintf(stream, "expected device ID (LS SPI): 0xDECA0130\n");
|
253 | 4cb40108 | Thomas Schöpping | aosThdMSleep(5);
|
254 | actual_deviceId = instancereaddeviceid(); |
||
255 | chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
|
||
256 | aosThdMSleep(5);
|
||
257 | e05848a6 | Robin Ewers | |
258 | 4cb40108 | Thomas Schöpping | //if the read of device ID fails, the DW1000 could be asleep
|
259 | if(DWT_DEVICE_ID != actual_deviceId){
|
||
260 | e05848a6 | Robin Ewers | |
261 | 4cb40108 | Thomas Schöpping | clear_SPI_chip_select(); |
262 | aosThdMSleep(1);
|
||
263 | set_SPI_chip_select(); |
||
264 | aosThdMSleep(7);
|
||
265 | actual_deviceId = instancereaddeviceid() ; |
||
266 | |||
267 | if(DWT_DEVICE_ID != actual_deviceId){
|
||
268 | chprintf(stream, "SPI is not working or Unsupported Device ID\n");
|
||
269 | chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);
|
||
270 | a193bcf1 | Thomas Schöpping | chprintf(stream, "expected device ID: 0xDECA0130\n");
|
271 | 4cb40108 | Thomas Schöpping | aosThdMSleep(5);
|
272 | } |
||
273 | e05848a6 | Robin Ewers | |
274 | 4cb40108 | Thomas Schöpping | //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
|
275 | dwt_softreset(); |
||
276 | } |
||
277 | |||
278 | 4c72a54c | Thomas Schöpping | /*! Test1: Low speed SPI result */
|
279 | 4cb40108 | Thomas Schöpping | if (actual_deviceId == DWT_DEVICE_ID){
|
280 | 4c72a54c | Thomas Schöpping | aosTestPassed(stream, &result); |
281 | 4cb40108 | Thomas Schöpping | } else {
|
282 | 4c72a54c | Thomas Schöpping | aosTestFailed(stream, &result); |
283 | 4cb40108 | Thomas Schöpping | } |
284 | |||
285 | reset_DW1000(); |
||
286 | |||
287 | a193bcf1 | Thomas Schöpping | chprintf(stream, "initialise instance for DW1000\n");
|
288 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
289 | |||
290 | 4c72a54c | Thomas Schöpping | int x_init = instance_init(((aos_test_dw1000data_t*)test->data)->driver) ;
|
291 | 4cb40108 | Thomas Schöpping | |
292 | if (0 != x_init){ |
||
293 | a193bcf1 | Thomas Schöpping | chprintf(stream, "init error with return value: %d\n", x_init);
|
294 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
295 | } |
||
296 | else {
|
||
297 | a193bcf1 | Thomas Schöpping | chprintf(stream, "init success with return value: %d\n", x_init);
|
298 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
299 | } |
||
300 | |||
301 | 4c72a54c | Thomas Schöpping | /*! Test2: Initialization result*/
|
302 | 4cb40108 | Thomas Schöpping | if (x_init == 0){ |
303 | 4c72a54c | Thomas Schöpping | aosTestPassed(stream, &result); |
304 | 4cb40108 | Thomas Schöpping | } else {
|
305 | 4c72a54c | Thomas Schöpping | aosTestFailed(stream, &result); |
306 | 4cb40108 | Thomas Schöpping | } |
307 | |||
308 | 4c72a54c | Thomas Schöpping | setHighSpeed_SPI(true, ((aos_test_dw1000data_t*)test->data)->driver);
|
309 | 4cb40108 | Thomas Schöpping | |
310 | chprintf(stream, "expected device ID (HS SPI): 0xDECA0130\n");
|
||
311 | actual_deviceId = instancereaddeviceid(); |
||
312 | chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
|
||
313 | aosThdMSleep(1);
|
||
314 | |||
315 | 4c72a54c | Thomas Schöpping | /*! Test3: High speed SPI result*/
|
316 | 4cb40108 | Thomas Schöpping | if (actual_deviceId == DWT_DEVICE_ID){
|
317 | 4c72a54c | Thomas Schöpping | aosTestPassed(stream, &result); |
318 | 4cb40108 | Thomas Schöpping | } else {
|
319 | 4c72a54c | Thomas Schöpping | aosTestFailed(stream, &result); |
320 | 4cb40108 | Thomas Schöpping | } |
321 | |||
322 | port_EnableEXT_IRQ(); |
||
323 | reset_DW1000(); |
||
324 | |||
325 | a193bcf1 | Thomas Schöpping | chprintf(stream, "initialise the configuration for UWB application\n");
|
326 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
327 | |||
328 | 4c72a54c | Thomas Schöpping | int uwb_init = UWB_Init(((aos_test_dw1000data_t*)test->data)->driver);
|
329 | 4cb40108 | Thomas Schöpping | |
330 | if (0 != uwb_init){ |
||
331 | a193bcf1 | Thomas Schöpping | chprintf(stream, "UWB config error with return value: %d\n", uwb_init);
|
332 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
333 | } |
||
334 | else {
|
||
335 | a193bcf1 | Thomas Schöpping | chprintf(stream, "UWB config success with return value: %d\n", uwb_init);
|
336 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
337 | } |
||
338 | |||
339 | 4c72a54c | Thomas Schöpping | /*! Test4: UWB configuration result
|
340 | * If all the four tests are passed, the module is ready to run.
|
||
341 | 4cb40108 | Thomas Schöpping | * Note that the interrupt IRQn should be tested separately.
|
342 | */
|
||
343 | if (uwb_init == 0){ |
||
344 | 4c72a54c | Thomas Schöpping | aosTestPassed(stream, &result); |
345 | 4cb40108 | Thomas Schöpping | } else {
|
346 | 4c72a54c | Thomas Schöpping | aosTestFailed(stream, &result); |
347 | 4cb40108 | Thomas Schöpping | } |
348 | |||
349 | 4c72a54c | Thomas Schöpping | /************** End of TEST_SNIPPETS_DW1000*****************/
|
350 | 4cb40108 | Thomas Schöpping | |
351 | 4c72a54c | Thomas Schöpping | #else /* defined(TEST_SNIPPETS_DW1000) */ |
352 | 4cb40108 | Thomas Schöpping | |
353 | |||
354 | /*! RUN THE STATE MACHINE DEMO APP (RTLS) */
|
||
355 | |||
356 | a193bcf1 | Thomas Schöpping | chprintf(stream, "initialise the State Machine\n");
|
357 | 4cb40108 | Thomas Schöpping | aosThdSleep(2);
|
358 | |||
359 | /* Initialize UWB system with user defined configuration */
|
||
360 | 4c72a54c | Thomas Schöpping | int uwb_init = UWB_Init(((aos_test_dw1000data_t*)test->data)->driver);
|
361 | 4cb40108 | Thomas Schöpping | |
362 | if (0 != uwb_init){ |
||
363 | a193bcf1 | Thomas Schöpping | chprintf(stream, "error in UWB config with return value: %d\n", uwb_init);
|
364 | 4cb40108 | Thomas Schöpping | } |
365 | else {
|
||
366 | a193bcf1 | Thomas Schöpping | chprintf(stream, "succeed the init of UWB config\n");
|
367 | 4cb40108 | Thomas Schöpping | } |
368 | aosThdSleep(1);
|
||
369 | |||
370 | a193bcf1 | Thomas Schöpping | chprintf(stream, "running the RTLS demo application ...\n");
|
371 | 4cb40108 | Thomas Schöpping | aosThdSleep(1);
|
372 | |||
373 | /*! Run the localization system demo app as a thread */
|
||
374 | while(1){ |
||
375 | instance_run(); |
||
376 | 8d4d058e | Cung Sang | // aosThdUSleep(100);
|
377 | 4cb40108 | Thomas Schöpping | } |
378 | |||
379 | 4c72a54c | Thomas Schöpping | #endif /* defined(TEST_SNIPPETS_DW1000) */ |
380 | 4cb40108 | Thomas Schöpping | |
381 | return result;
|
||
382 | e05848a6 | Robin Ewers | } |
383 | |||
384 | 4cb40108 | Thomas Schöpping | |
385 | 4c72a54c | Thomas Schöpping | #endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */ |