Statistics
| Branch: | Tag: | Revision:

amiro-os / unittests / periphery-lld / src / ut_alld_DW1000_v1.c @ 0ecf4119

History | View | Annotate | Download (14.645 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

    
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
#include <v1/alld_dw1000_regs_v1.h>
31
#include <string.h>
32

    
33

    
34
/******************************************************************************/
35
/* LOCAL DEFINITIONS                                                          */
36
/******************************************************************************/
37

    
38
//#define UNIT_TEST_SNIPPETS_DW1000   // switch between unit test and demo apps
39

    
40
#define SWS1_SHF_MODE 0x02  //short frame mode (6.81M)
41
#define SWS1_CH5_MODE 0x04  //channel 5 mode
42
#define SWS1_ANC_MODE 0x08  //anchor mode
43
#define SWS1_A1A_MODE 0x10  //anchor/tag address A1
44
#define SWS1_A2A_MODE 0x20  //anchor/tag address A2
45
#define SWS1_A3A_MODE 0x40  //anchor/tag address A3
46

    
47
#define S1_SWITCH_ON  (1)
48
#define S1_SWITCH_OFF (0)
49

    
50
/******************************************************************************/
51
/* EXPORTED VARIABLES                                                         */
52
/******************************************************************************/
53

    
54
/******************************************************************************/
55
/* LOCAL TYPES                                                                */
56
/******************************************************************************/
57

    
58
/******************************************************************************/
59
/* LOCAL VARIABLES                                                            */
60
/******************************************************************************/
61
uint8_t s1switch = 0;
62
int instance_anchaddr = 0;
63
int dr_mode = 0;
64
int chan, tagaddr, ancaddr;
65
int instance_mode = ANCHOR;
66

    
67

    
68
/******************************************************************************/
69
/* LOCAL FUNCTIONS                                                            */
70
/******************************************************************************/
71

    
72
/*! @brief Change the SPI speed configuration on the fly */
73
void setHighSpeed_SPI(bool speedValue, DW1000Driver* drv){
74
  spiStop(drv->spid);
75
  if (speedValue == FALSE){
76
    spiStart(drv->spid, &moduleHalSpiUwbLsConfig);  // low speed spi configuration
77
  }
78
  else{
79
    spiStart(drv->spid, &moduleHalSpiUwbHsConfig); // high speed spi configuration
80
  }
81
}
82

    
83
/*! @brief entry point to the IRQn event in DW1000 module */
84
void process_deca_irq(void){
85
  do{
86
    dwt_isr();
87
    //while IRS line active (ARM can only do edge sensitive interrupts)
88
  }while(port_CheckEXT_IRQ() == 1);
89
}
90

    
91
/*! @brief Manually set the chip select pin of the SPI */
92
void set_SPI_chip_select(void){
93
  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_HIGH);
94
}
95

    
96
/*! @brief Manually reset the chip select pin of the SPI */
97
void clear_SPI_chip_select(void){
98
  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_LOW);
99
}
100

    
101
/*! @brief Manually reset the DW1000 module  */
102
void reset_DW1000(void){
103
  // Set the pin as output
104
  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_OUTPUT);
105
  //drive the RSTn pin low
106
  apalGpioWrite(moduleGpioDw1000Reset.gpio, APAL_GPIO_LOW);
107
  //put the pin back to tri-state ... as input
108
//  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_INPUT); // TODO:
109
  aosThdMSleep(7);
110
}
111

    
112
/*! @brief waking up the DW1000 module using Chip Select pin */
113
void wakeup_DW1000(void){
114
  clear_SPI_chip_select();
115
  aosThdMSleep(1);
116
  set_SPI_chip_select();
117
  aosThdMSleep(7);  // wait for XTAL to stabilize
118

    
119
  // set wakeup pin directly high
120
//   apalGpioWrite(moduleGpioDw1000WakeUp.gpio, APAL_GPIO_HIGH);
121
   aosThdMSleep(10);
122
}
123

    
124
/*! @brief Configure instance tag/anchor/etc... addresses */
125
void addressconfigure(uint8_t s1switch, uint8_t mode){
126
  uint16_t instAddress ;
127
  instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4;
128
  if(mode == ANCHOR) {
129
    if(instance_anchaddr > 3) {
130
      instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener
131
    }
132
    else {
133
      instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr;
134
    }
135
  }
136
  else{
137
    instAddress = (uint16_t)instance_anchaddr;
138
  }
139
  instancesetaddresses(instAddress);
140
}
141

    
142
/*! @brief returns the use case / operational mode */
143
int decarangingmode(uint8_t s1switch){
144
  int mode = 0;
145
  if(s1switch & SWS1_SHF_MODE)  {
146
    mode = 1;
147
  }
148
  if(s1switch & SWS1_CH5_MODE) {
149
    mode = mode + 2;
150
  }
151
  return mode;
152
}
153

    
154
/*! @brief Check connection setting and initialize DW1000 module */
155
int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){
156
  uint32_t devID ;
157
  int result;
158

    
159
  setHighSpeed_SPI(FALSE, drv);          //low speed spi max. ~4M
160
  devID = instancereaddeviceid() ;
161

    
162
  if(DWT_DEVICE_ID != devID) {
163
    clear_SPI_chip_select();
164
    Sleep(1);
165
    set_SPI_chip_select();
166
    Sleep(7);
167
    devID = instancereaddeviceid() ;
168
    if(DWT_DEVICE_ID != devID){
169
      return(-1) ;
170
    }    
171
    dwt_softreset();
172
  }
173
    reset_DW1000();  //reset the DW1000 by driving the RSTn line low
174
  if((s1switch & SWS1_ANC_MODE) == 0){
175
    instance_mode = TAG;
176
  }
177
  else{
178
    instance_mode = ANCHOR;
179
  }
180
  result = instance_init(drv);
181
  if (0 > result){
182
    return(-1) ;
183
  }
184
  setHighSpeed_SPI(TRUE, drv);       // high speed spi max. ~ 20M
185
  devID = instancereaddeviceid() ;
186
  if (DWT_DEVICE_ID != devID){
187
    return(-1) ;
188
  }
189
  addressconfigure(s1switch, (uint8_t)instance_mode) ;
190
  if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){
191
    instance_mode = LISTENER;
192
  }
193
  instancesetrole(instance_mode) ;       // Set this instance role
194
  dr_mode = decarangingmode(s1switch);
195
  chan = chConfig[dr_mode].channelNumber ;
196
  instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ;
197

    
198
  return (int32_t)devID;
199
}
200

    
201
/*! @brief Main Entry point to Initialization of UWB DW1000 configuration  */
202
#pragma GCC optimize ("O3")
203
int UWB_Init(DW1000Driver* drv){
204

    
205
  /*! Software defined Configurartion for TAG, ANC, and other settings as needed */
206
  s1switch = S1_SWITCH_OFF << 1  // (on = 6.8 Mbps, off = 110 kbps)
207
           | S1_SWITCH_OFF << 2  // (on = CH5, off = CH2)
208
           | S1_SWITCH_OFF << 3  // (on = Anchor, off = TAG)
209
           | S1_SWITCH_OFF << 4  // (configure Tag or anchor ID no.)
210
           | S1_SWITCH_OFF << 5  // (configure Tag or anchor ID no.)
211
           | S1_SWITCH_OFF << 6  // (configure Tag or anchor ID no.)
212
           | S1_SWITCH_OFF << 7; // Not use in this demo
213

    
214
  port_DisableEXT_IRQ();           //disable ScenSor IRQ until we configure the device
215
  if(inittestapplication(s1switch, drv) == -1) {
216
    return (-1); //error
217
  }
218
  aosThdMSleep(5);
219
  port_EnableEXT_IRQ();  //enable DW1000 IRQ before starting
220

    
221
  return 0;
222
}
223

    
224

    
225
/******************************************************************************/
226
/* EXPORTED FUNCTIONS                                                         */
227
/******************************************************************************/
228
aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut) {
229

    
230
  aosDbgCheck(ut->data != NULL);
231
  aos_utresult_t result = {0, 0};
232

    
233
#if defined (AMIROLLD_CFG_MIC9404x)
234
  // Enable 3.3 and 1.8 supply voltages for powering up the DW1000 module in AMiRo Light Ring
235
  if ((ut->data != NULL) && (((ut_dw1000data_t*)(ut->data))->mic9404xd != NULL)){
236
    mic9404x_lld_state_t state;
237
    uint32_t status = APAL_STATUS_OK;
238

    
239
    chprintf(stream, "reading current status of the Power..\n");
240
    status = mic9404x_lld_get(((ut_dw1000data_t*)(ut->data))->mic9404xd, &state);
241
    if (status == APAL_STATUS_OK) {
242
      aosUtPassedMsg(stream, &result, "power %s\n", (state == MIC9404x_LLD_STATE_ON) ? "enabled" : "disabled");
243
    } else {
244
      aosUtFailed(stream, &result);
245
    }
246
    if (state == MIC9404x_LLD_STATE_OFF) {
247
      chprintf(stream, "enabling the power ...\n");
248
      status = mic9404x_lld_set(((ut_dw1000data_t*)(ut->data))->mic9404xd, MIC9404x_LLD_STATE_ON);
249
      status |= mic9404x_lld_get(((ut_dw1000data_t*)(ut->data))->mic9404xd, &state);
250
      if (state == MIC9404x_LLD_STATE_ON) {
251
        aosThdSSleep(2);
252
        status |= mic9404x_lld_get(((ut_dw1000data_t*)(ut->data))->mic9404xd, &state);
253
      }
254
      if ((status == APAL_STATUS_OK) && (state == MIC9404x_LLD_STATE_ON)) {
255
        aosUtPassed(stream, &result);
256
      } else {
257
        aosUtFailed(stream, &result);
258
      }
259
    }
260
    aosThdSleep(1);
261
    return result;
262
  }
263
#endif  /* defined (AMIROLLD_CFG_MIC9404x) */
264

    
265

    
266
  // Start the DW1000 module UT after powering up
267
  chprintf(stream, "init DW1000...\n");
268
  aosThdSleep(1);
269
  ut_dw1000data_t* h_dw1000data = NULL;
270
  if((ut->data != NULL) && (((ut_dw1000data_t*)(ut->data))->dw1000d != NULL)){
271
    h_dw1000data = ut->data;
272
    chprintf(stream, "assign handle for DW1000Driver struct \n");
273
  }
274
  else {
275
    chprintf(stream, "unsupported data type \n");
276
    return result;
277
  }
278
  aosThdSleep(1);
279

    
280
  reset_DW1000();     // hard reset
281
//  wakeup_DW1000();
282
  aosThdMSleep(5);
283
  int init = dwt_initialise(DWT_LOADUCODE, h_dw1000data->dw1000d);
284
  if (init == 0){
285
    chprintf(stream, "DW1000 is initialized \n");
286
  }
287
  else {
288
    chprintf(stream, "init error with return value: %d \n", init);
289
  }
290
  aosThdMSleep(5);
291

    
292
/*! Unit Test snippets for DW1000.
293
 * @Note: Event IRQ for DW1000 should be tested separately
294
 */
295
#if defined(UNIT_TEST_SNIPPETS_DW1000)
296
  uint32_t actual_devID;
297
  port_DisableEXT_IRQ();
298

    
299
  /*! UT1: Low speed SPI result */
300
  setHighSpeed_SPI(false, h_dw1000data->dw1000d);
301
  chprintf(stream, "expected ID (LS SPI): 0xDECA0130 \n");
302
  aosThdMSleep(5);
303
  actual_devID = instancereaddeviceid();
304
  chprintf(stream, "read ID (LS SPI): 0x%x\n", actual_devID);
305
  aosThdMSleep(5);
306

    
307
  //if the read of device ID fails, the DW1000 could be asleep
308
  if(DWT_DEVICE_ID != actual_devID){
309
    clear_SPI_chip_select();
310
    aosThdMSleep(1);
311
    set_SPI_chip_select();
312
    aosThdMSleep(7);
313
    actual_devID = instancereaddeviceid() ;
314
    if(DWT_DEVICE_ID != actual_devID){
315
      chprintf(stream, "SPI is not working or Unsupported Device ID\n");
316
      chprintf(stream, "actual device ID is: 0x%x\n", actual_devID);
317
      chprintf(stream, "expected device ID: 0xDECA0130 \n");
318
      aosThdMSleep(5);
319
    }   
320
    dwt_softreset();
321
  }
322

    
323
  if (actual_devID == DWT_DEVICE_ID){
324
    aosUtPassed(stream, &result);
325
  } else {
326
    aosUtFailed(stream, &result);
327
  }
328

    
329
  /*! Blinking TX and RX LED simultenously for 5 times */
330
  chprintf(stream, "blinking TX and RX LEDs simultenously for 5 times \n");
331
  for(int i =0; i< 5; i++){
332
    uint8_t mode = 3;
333
    uint32_t reg ;
334

    
335
    // Set up for LED output.
336
    reg = dwt_read32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET); // Hardcoded = 0xDE001400
337
    reg &= ~(GPIO_MSGP2_MASK | GPIO_MSGP3_MASK);
338
    reg |= (GPIO_PIN2_RXLED | GPIO_PIN3_TXLED);
339
    dwt_write32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET, reg);
340

    
341
    // Enable LP Oscillator to run from counter and turn on de-bounce clock.
342
    reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET);      // hard-coded = 0xF0B40200
343
    reg |= (PMSC_CTRL0_GPDCE | PMSC_CTRL0_KHZCLEN);
344
    dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET, reg);
345

    
346
    // Enable LEDs to blink and set default blink time.
347
    reg = PMSC_LEDC_BLNKEN | PMSC_LEDC_BLINK_TIME_DEF;
348
    // Make LEDs blink once if requested.
349
    if (mode & DWT_LEDS_INIT_BLINK)
350
    {
351
      reg |= PMSC_LEDC_BLINK_NOW_ALL;
352
    }
353
    dwt_write32bitoffsetreg(PMSC_ID, PMSC_LEDC_OFFSET, reg);
354
    // Clear force blink bits if needed.
355
    if(mode & DWT_LEDS_INIT_BLINK)
356
    {
357
      reg &= ~PMSC_LEDC_BLINK_NOW_ALL;
358
      dwt_write32bitoffsetreg(PMSC_ID, PMSC_LEDC_OFFSET, reg);
359
    }
360
    aosThdMSleep(500);
361
  }
362

    
363
  /*! UT2: Initialization  of the DW1000 module */
364
  reset_DW1000();
365
  chprintf(stream, "initialise the instance for DW1000 \n");
366
  aosThdMSleep(100);
367
  int x_init = instance_init(h_dw1000data->dw1000d) ;
368
  if (x_init != 0){
369
    chprintf(stream, "init error with return value: %d \n", x_init);
370
  }
371
  else {
372
    chprintf(stream, "succeed init! \n");
373
  }
374
  aosThdMSleep(100);
375
  if (x_init == 0){
376
    aosUtPassed(stream, &result);
377
  } else {
378
    aosUtFailed(stream, &result);
379
  }
380

    
381
  /*! UT3: High speed SPI Testing */
382
  setHighSpeed_SPI(true, h_dw1000data->dw1000d);
383
  chprintf(stream, "expected ID (HS SPI): 0xDECA0130\n");
384
  actual_devID = instancereaddeviceid();
385
  chprintf(stream, "Read ID (HS SPI): 0x%x\n", actual_devID);
386
  aosThdMSleep(100);
387
  if (actual_devID == DWT_DEVICE_ID){
388
    aosUtPassed(stream, &result);
389
  } else {
390
    aosUtFailed(stream, &result);
391
  }
392

    
393
  /*! UT4: Configuration of UWB module
394
   * If all the five unit tests are passed, the module is ready to run.
395
   * Note that the interrupt IRQn should be tested separately.
396
   */
397
  port_EnableEXT_IRQ();
398
  reset_DW1000();
399
  chprintf(stream, "initialise the configuration for UWB application \n");
400
  aosThdSleep(1);
401
  int uwb_init = UWB_Init(h_dw1000data->dw1000d);
402
  if (uwb_init != 0){
403
    chprintf(stream, "UWB config error with return value: %d \n", uwb_init);
404
  }
405
  else {
406
    chprintf(stream, "succeed UWB config process \n", uwb_init);
407
  }
408
  if (uwb_init == 0){
409
    aosUtPassed(stream, &result);
410
  } else {
411
    aosUtFailed(stream, &result);
412
  }
413

    
414
  /************** End of UNIT_TEST_SNIPPETS_DW1000 *****************/
415

    
416
#else /* defined(UNIT_TEST_SNIPPETS_DW1000) */
417

    
418
  /*! RUN THE STATE MACHINE DEMO APP (RTLS) */
419
  chprintf(stream, "initialise the State Machine \n");
420
  aosThdSleep(2);
421

    
422
  /* Initialize UWB system with user defined configuration  */
423
  int uwb_init = UWB_Init(h_dw1000data->dw1000d);
424

    
425
  if (uwb_init != 0){
426
    chprintf(stream, "error in UWB config with return value: %d \n", uwb_init);
427
  }
428
  else {
429
    chprintf(stream, "succeed the init of UWB config \n");
430
  }
431
  aosThdSleep(1);
432
  chprintf(stream, "running the RTLS demo application ... \n");
433

    
434
  /*! Run the localization system demo app as a thread */
435
  while(1){
436
    instance_run();
437
//    aosThdUSleep(10);
438
//    aosThdMSleep(1);
439
  }
440

    
441
#endif  /* defined(UNIT_TEST_SNIPPETS_DW1000) */
442

    
443
  return result;
444
}
445

    
446

    
447
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */