Statistics
| Branch: | Tag: | Revision:

amiro-os / test / periphery-lld / DW1000_v1 / aos_test_DW1000.c @ 65ebff5f

History | View | Annotate | Download (10.583 KB)

1
/*
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
#include <aos_test_DW1000.h>
21

    
22
#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
23

    
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
#include <deca_instance.h>
31

    
32

    
33
/******************************************************************************/
34
/* LOCAL DEFINITIONS                                                          */
35
/******************************************************************************/
36

    
37
#define TEST_SNIPPETS_DW1000   // switch between test and demo apps
38

    
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
/******************************************************************************/
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
/*! @brief Manually reset the DW1000 module  */
72
void reset_DW1000(void){
73

    
74
  // Set the pin as output
75
  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_OUTPUT);
76

    
77
  //drive the RSTn pin low
78
  apalGpioWrite(moduleGpioDw1000Reset.gpio, APAL_GPIO_LOW);
79

    
80
  //put the pin back to tri-state ... as input
81
//  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_INPUT); // TODO:
82

    
83
  aosThdMSleep(2);
84
}
85

    
86
/*! @brief Configure instance tag/anchor/etc... addresses */
87
void addressconfigure(uint8_t s1switch, uint8_t mode){
88
  uint16_t instAddress ;
89

    
90
  instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4;
91

    
92
  if(mode == ANCHOR) {
93
    if(instance_anchaddr > 3) {
94
      instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener
95
    }
96
    else {
97
      instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr;
98
    }
99
  }
100
  else{
101
    instAddress = (uint16_t)instance_anchaddr;
102
  }
103

    
104
  instancesetaddresses(instAddress);
105
}
106

    
107
/*! @brief returns the use case / operational mode */
108
int decarangingmode(uint8_t s1switch){
109
  int mode = 0;
110

    
111
  if(s1switch & SWS1_SHF_MODE)  {
112
    mode = 1;
113
  }
114

    
115
  if(s1switch & SWS1_CH5_MODE) {
116
    mode = mode + 2;
117
  }
118

    
119
  return mode;
120
}
121

    
122
/*! @brief Check connection setting and initialize DW1000 module */
123
int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){
124
  uint32_t devID ;
125
  int result;
126

    
127
  setHighSpeed_SPI(FALSE, drv);          //low speed spi max. ~4M
128
  devID = instancereaddeviceid() ;
129

    
130
  if(DWT_DEVICE_ID != devID) {
131
    clear_SPI_chip_select();
132
    Sleep(1);
133
    set_SPI_chip_select();
134
    Sleep(7);
135
    devID = instancereaddeviceid() ;
136
    if(DWT_DEVICE_ID != devID){
137
      return(-1) ;
138
    }    
139
    dwt_softreset();
140
  }
141

    
142
    reset_DW1000();  //reset the DW1000 by driving the RSTn line low
143

    
144
  if((s1switch & SWS1_ANC_MODE) == 0){
145
    instance_mode = TAG;
146
  }
147
  else{
148
    instance_mode = ANCHOR;
149
  }
150

    
151
  result = instance_init(drv) ;
152

    
153
  if (0 > result){
154
    return(-1) ;
155
  }
156

    
157
  setHighSpeed_SPI(TRUE, drv);       // high speed spi max. ~ 20M
158
  devID = instancereaddeviceid() ;
159

    
160
  if (DWT_DEVICE_ID != devID){
161
    return(-1) ;
162
  }
163

    
164
  addressconfigure(s1switch, (uint8_t)instance_mode) ;
165

    
166
  if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){
167
    instance_mode = LISTENER;
168
  }
169

    
170
  instancesetrole(instance_mode) ;       // Set this instance role
171
  dr_mode = decarangingmode(s1switch);
172
  chan = chConfig[dr_mode].channelNumber ;
173
  instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ;
174

    
175
  return (int32_t)devID;
176
}
177

    
178
/*! @brief Main Entry point to Initialization of UWB DW1000 configuration  */
179
#pragma GCC optimize ("O3")
180
int UWB_Init(DW1000Driver* drv){
181

    
182
  /*! Software defined Configurartion for TAG, ANC, and other settings as needed */
183
  s1switch = S1_SWITCH_OFF << 1  // (on = 6.8 Mbps, off = 110 kbps)
184
           | S1_SWITCH_OFF << 2  // (on = CH5, off = CH2)
185
           | S1_SWITCH_OFF << 3  // (on = Anchor, off = TAG)
186
           | S1_SWITCH_OFF << 4  // (configure Tag or anchor ID no.)
187
           | S1_SWITCH_OFF << 5  // (configure Tag or anchor ID no.)
188
           | S1_SWITCH_OFF << 6  // (configure Tag or anchor ID no.)
189
           | S1_SWITCH_OFF << 7; // Not use in this demo
190

    
191

    
192
  port_DisableEXT_IRQ();           //disable ScenSor IRQ until we configure the device
193

    
194
  if(inittestapplication(s1switch, drv) == -1) {
195
    return (-1); //error
196
  }
197

    
198
  aosThdMSleep(5);
199

    
200
  port_EnableEXT_IRQ();  //enable DW1000 IRQ before starting
201

    
202
  return 0;
203
}
204

    
205

    
206
/******************************************************************************/
207
/* EXPORTED FUNCTIONS                                                         */
208
/******************************************************************************/
209

    
210

    
211
aos_testresult_t aosTestDw1000Func(BaseSequentialStream* stream, const aos_test_t* test) {
212

    
213
  aosDbgCheck(test->data != NULL &&
214
      ((aos_test_dw1000data_t*)test->data)->driver != NULL &&
215
      ((aos_test_dw1000data_t*)test->data)->evtsource != NULL);
216

    
217

    
218
  aos_testresult_t result = {0, 0};
219

    
220
  chprintf(stream, "init DW1000...\n");
221
  dwt_initialise(DWT_LOADUCODE, ((aos_test_dw1000data_t*)test->data)->driver);
222
  aosThdMSleep(5);
223

    
224

    
225
/*! Test snippets for DW1000.
226
 * @Note: Event IRQ for DW1000 should be tested separately
227
 */
228
#if defined(TEST_SNIPPETS_DW1000)
229

    
230
  uint32_t actual_deviceId;
231

    
232
  port_DisableEXT_IRQ();
233

    
234
  setHighSpeed_SPI(false, ((aos_test_dw1000data_t*)test->data)->driver);
235
  chprintf(stream, "expected device ID (LS SPI): 0xDECA0130\n");
236
  aosThdMSleep(5);
237
  actual_deviceId = instancereaddeviceid();
238
  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
239
  aosThdMSleep(5);
240

    
241
  //if the read of device ID fails, the DW1000 could be asleep
242
  if(DWT_DEVICE_ID != actual_deviceId){
243

    
244
    clear_SPI_chip_select();
245
    aosThdMSleep(1);
246
    set_SPI_chip_select();
247
    aosThdMSleep(7);
248
    actual_deviceId = instancereaddeviceid() ;
249

    
250
    if(DWT_DEVICE_ID != actual_deviceId){
251
      chprintf(stream, "SPI is not working or Unsupported Device ID\n");
252
      chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);
253
      chprintf(stream, "expected device ID: 0xDECA0130\n");
254
      aosThdMSleep(5);
255
    }
256

    
257
    //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
258
    dwt_softreset();
259
  }
260

    
261
  /*! Test1: Low speed SPI result */
262
  if (actual_deviceId == DWT_DEVICE_ID){
263
    aosTestPassed(stream, &result);
264
  } else {
265
    aosTestFailed(stream, &result);
266
  }
267

    
268
  reset_DW1000();
269

    
270
  chprintf(stream, "initialise instance for DW1000\n");
271
  aosThdSleep(1);
272

    
273
  int x_init = instance_init(((aos_test_dw1000data_t*)test->data)->driver) ;
274

    
275
  if (0 != x_init){
276
    chprintf(stream, "init error with return value: %d\n", x_init);
277
    aosThdSleep(1);
278
  }
279
  else {
280
    chprintf(stream, "init success with return value: %d\n", x_init);
281
    aosThdSleep(1);
282
  }
283

    
284
  /*! Test2: Initialization  result*/
285
  if (x_init == 0){
286
    aosTestPassed(stream, &result);
287
  } else {
288
    aosTestFailed(stream, &result);
289
  }
290

    
291
  setHighSpeed_SPI(true, ((aos_test_dw1000data_t*)test->data)->driver);
292

    
293
  chprintf(stream, "expected device ID (HS SPI): 0xDECA0130\n");
294
  actual_deviceId = instancereaddeviceid();
295
  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
296
  aosThdMSleep(1);
297

    
298
  /*! Test3: High speed SPI result*/
299
  if (actual_deviceId == DWT_DEVICE_ID){
300
    aosTestPassed(stream, &result);
301
  } else {
302
    aosTestFailed(stream, &result);
303
  }
304

    
305
  port_EnableEXT_IRQ();
306
  reset_DW1000();
307

    
308
  chprintf(stream, "initialise the configuration for UWB application\n");
309
  aosThdSleep(1);
310

    
311
  int uwb_init = UWB_Init(((aos_test_dw1000data_t*)test->data)->driver);
312

    
313
  if (0 != uwb_init){
314
    chprintf(stream, "UWB config error with return value: %d\n", uwb_init);
315
    aosThdSleep(1);
316
  }
317
  else {
318
    chprintf(stream, "UWB config success with return value: %d\n", uwb_init);
319
    aosThdSleep(1);
320
  }
321

    
322
  /*! Test4: UWB configuration result
323
   * If all the four tests are passed, the module is ready to run.
324
   * Note that the interrupt IRQn should be tested separately.
325
   */
326
  if (uwb_init == 0){
327
    aosTestPassed(stream, &result);
328
  } else {
329
    aosTestFailed(stream, &result);
330
  }
331

    
332
  /************** End of TEST_SNIPPETS_DW1000*****************/
333

    
334
#else /* defined(TEST_SNIPPETS_DW1000) */
335

    
336

    
337
  /*! RUN THE STATE MACHINE DEMO APP (RTLS) */
338

    
339
  chprintf(stream, "initialise the State Machine\n");
340
  aosThdSleep(2);
341

    
342
  /* Initialize UWB system with user defined configuration  */
343
  int uwb_init = UWB_Init(((aos_test_dw1000data_t*)test->data)->driver);
344

    
345
  if (0 != uwb_init){
346
    chprintf(stream, "error in UWB config with return value: %d\n", uwb_init);
347
  }
348
  else {
349
    chprintf(stream, "succeed the init of UWB config\n");
350
  }
351
  aosThdSleep(1);
352

    
353
  chprintf(stream, "running the RTLS demo application ...\n");
354
  aosThdSleep(1);
355

    
356
  /*! Run the localization system demo app as a thread */
357
  while(1){
358
    instance_run();
359
//    aosThdUSleep(100);
360
  }
361

    
362
#endif  /* defined(TEST_SNIPPETS_DW1000) */
363

    
364
  return result;
365
}
366

    
367

    
368
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */