Statistics
| Branch: | Tag: | Revision:

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) */