Statistics
| Branch: | Tag: | Revision:

amiro-os / unittests / periphery-lld / src / ut_alld_dw1000_v1.c @ 10b520a8

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

    
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

    
45
#define S1_SWITCH_ON  (1)
46
#define S1_SWITCH_OFF (0)
47

    
48
/******************************************************************************/
49
/* EXPORTED VARIABLES                                                         */
50
/******************************************************************************/
51

    
52
/******************************************************************************/
53
/* LOCAL TYPES                                                                */
54
/******************************************************************************/
55

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

    
65

    
66
/******************************************************************************/
67
/* LOCAL FUNCTIONS                                                            */
68
/******************************************************************************/
69

    
70
/*! @brief Change the SPI speed configuration on the fly */
71
void setHighSpeed_SPI(bool speedValue, DW1000Driver* drv){
72

    
73
  spiStop(drv->spid);
74

    
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
 *
85
 * */
86
void process_deca_irq(void){
87
  do{
88
    dwt_isr();
89
    //while IRS line active (ARM can only do edge sensitive interrupts)
90
  }while(port_CheckEXT_IRQ() == 1);
91
}
92

    
93
/*! @brief Check the current value of GPIO pin and return the value */
94
apalGpioState_t port_CheckEXT_IRQ(void) {
95
  apalGpioState_t  val;
96
  apalGpioRead(moduleGpioDw1000Irqn.gpio, &val);
97
  return val;
98
}
99

    
100
/*! @brief Manually set the chip select pin of the SPI */
101
void set_SPI_chip_select(void){
102
  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_HIGH);
103
}
104

    
105
/*! @brief Manually reset the chip select pin of the SPI */
106
void clear_SPI_chip_select(void){
107
  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_LOW);
108
}
109

    
110
/*! @brief Manually reset the DW1000 module  */
111
void reset_DW1000(void){
112

    
113
  // Set the pin as output
114
  palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_OUTPUT);
115

    
116
  //drive the RSTn pin low
117
  apalGpioWrite(moduleGpioDw1000Reset.gpio, APAL_GPIO_LOW);
118

    
119
  //put the pin back to tri-state ... as input
120
//  palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_INPUT); // TODO:
121

    
122
  aosThdMSleep(2);
123
}
124

    
125
/*! @brief Configure instance tag/anchor/etc... addresses */
126
void addressconfigure(uint8_t s1switch, uint8_t mode){
127
  uint16_t instAddress ;
128

    
129
  instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4;
130

    
131
  if(mode == ANCHOR) {
132
    if(instance_anchaddr > 3) {
133
      instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener
134
    }
135
    else {
136
      instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr;
137
    }
138
  }
139
  else{
140
    instAddress = (uint16_t)instance_anchaddr;
141
  }
142

    
143
  instancesetaddresses(instAddress);
144
}
145

    
146
/*! @brief returns the use case / operational mode */
147
int decarangingmode(uint8_t s1switch){
148
  int mode = 0;
149

    
150
  if(s1switch & SWS1_SHF_MODE)  {
151
    mode = 1;
152
  }
153

    
154
  if(s1switch & SWS1_CH5_MODE) {
155
    mode = mode + 2;
156
  }
157

    
158
  return mode;
159
}
160

    
161
/*! @brief Check connection setting and initialize DW1000 module */
162
int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){
163
  uint32_t devID ;
164
  int result;
165

    
166
  setHighSpeed_SPI(FALSE, drv);          //low speed spi max. ~4M
167
  devID = instancereaddeviceid() ;
168

    
169
  if(DWT_DEVICE_ID != devID) {
170
    clear_SPI_chip_select();
171
    Sleep(1);
172
    set_SPI_chip_select();
173
    Sleep(7);
174
    devID = instancereaddeviceid() ;
175
    if(DWT_DEVICE_ID != devID){
176
      return(-1) ;
177
    }    
178
    dwt_softreset();
179
  }
180

    
181
    reset_DW1000();  //reset the DW1000 by driving the RSTn line low
182

    
183
  if((s1switch & SWS1_ANC_MODE) == 0){
184
    instance_mode = TAG;
185
  }
186
  else{
187
    instance_mode = ANCHOR;
188
  }
189

    
190
  result = instance_init(drv) ;
191

    
192
  if (0 > result){
193
    return(-1) ;
194
  }
195

    
196
  setHighSpeed_SPI(TRUE, drv);       // high speed spi max. ~ 20M
197
  devID = instancereaddeviceid() ;
198

    
199
  if (DWT_DEVICE_ID != devID){
200
    return(-1) ;
201
  }
202

    
203
  addressconfigure(s1switch, (uint8_t)instance_mode) ;
204

    
205
  if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){
206
    instance_mode = LISTENER;
207
  }
208

    
209
  instancesetrole(instance_mode) ;       // Set this instance role
210
  dr_mode = decarangingmode(s1switch);
211
  chan = chConfig[dr_mode].channelNumber ;
212
  instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ;
213

    
214
  return (int32_t)devID;
215
}
216

    
217
/*! @brief Main Entry point to Initialization of UWB DW1000 configuration  */
218
#pragma GCC optimize ("O3")
219
int UWB_Init(DW1000Driver* drv){
220

    
221
  /*! Software defined Configurartion for TAG, ANC, and other settings as needed */
222
  s1switch = S1_SWITCH_OFF << 1  // (on = 6.8 Mbps, off = 110 kbps)
223
           | S1_SWITCH_OFF << 2  // (on = CH5, off = CH2)
224
           | S1_SWITCH_OFF << 3  // (on = Anchor, off = TAG)
225
           | S1_SWITCH_OFF << 4  // (configure Tag or anchor ID no.)
226
           | S1_SWITCH_OFF << 5  // (configure Tag or anchor ID no.)
227
           | S1_SWITCH_OFF << 6  // (configure Tag or anchor ID no.)
228
           | S1_SWITCH_OFF << 7; // Not use in this demo
229

    
230

    
231
  port_DisableEXT_IRQ();           //disable ScenSor IRQ until we configure the device
232

    
233
  if(inittestapplication(s1switch, drv) == -1) {
234
    return (-1); //error
235
  }
236

    
237
  aosThdMSleep(5);
238

    
239
  port_EnableEXT_IRQ();  //enable DW1000 IRQ before starting
240

    
241
  return 0;
242
}
243

    
244

    
245
/******************************************************************************/
246
/* EXPORTED FUNCTIONS                                                         */
247
/******************************************************************************/
248

    
249

    
250
aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut) {
251

    
252
  aosDbgCheck(ut->data != NULL);
253

    
254
  aos_utresult_t result = {0, 0};
255

    
256
  chprintf(stream, "init DW1000...\n");
257
  dwt_initialise(DWT_LOADUCODE, (DW1000Driver*) ut->data);
258
  aosThdMSleep(5);
259

    
260

    
261
/*! Unit Test snippets for DW1000.
262
 * @Note: Event IRQ for DW1000 should be tested separately
263
 */
264
#ifdef UNIT_TEST_SNIPPETS_DW1000
265

    
266
  uint32_t actual_deviceId;
267

    
268
  port_DisableEXT_IRQ();
269

    
270
  setHighSpeed_SPI(false, (DW1000Driver*) ut->data);
271
  chprintf(stream, "expected device ID (LS SPI): 0xDECA0130 \n");
272
  aosThdMSleep(5);
273
  actual_deviceId = instancereaddeviceid();
274
  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
275
  aosThdMSleep(5);
276

    
277
  //if the read of device ID fails, the DW1000 could be asleep
278
  if(DWT_DEVICE_ID != actual_deviceId){
279

    
280
    clear_SPI_chip_select();
281
    aosThdMSleep(1);
282
    set_SPI_chip_select();
283
    aosThdMSleep(7);
284
    actual_deviceId = instancereaddeviceid() ;
285

    
286
    if(DWT_DEVICE_ID != actual_deviceId){
287
      chprintf(stream, "SPI is not working or Unsupported Device ID\n");
288
      chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);
289
      chprintf(stream, "expected device ID: 0xDECA0130 \n");
290
      aosThdMSleep(5);
291
    }
292

    
293
    //clear the sleep bit - so that after the hard reset below the DW does not go into sleep
294
    dwt_softreset();
295
  }
296

    
297
  /*! UT1: Low speed SPI result */
298
  if (actual_deviceId == DWT_DEVICE_ID){
299
    aosUtPassed(stream, &result);
300
  } else {
301
    aosUtFailed(stream, &result);
302
  }
303

    
304
  reset_DW1000();
305

    
306
  chprintf(stream, "initialise instance for DW1000 \n");
307
  aosThdSleep(1);
308

    
309
  int x_init = instance_init((DW1000Driver*) ut->data) ;
310

    
311
  if (0 != x_init){
312
    chprintf(stream, "init error with return value: %d \n", x_init);
313
    aosThdSleep(1);
314
  }
315
  else {
316
    chprintf(stream, "init success with return value: %d \n", x_init);
317
    aosThdSleep(1);
318
  }
319

    
320
  /*! UT2: Initialization  result*/
321
  if (x_init == 0){
322
    aosUtPassed(stream, &result);
323
  } else {
324
    aosUtFailed(stream, &result);
325
  }
326

    
327
  setHighSpeed_SPI(true, (DW1000Driver*) ut->data);
328

    
329
  chprintf(stream, "expected device ID (HS SPI): 0xDECA0130\n");
330
  actual_deviceId = instancereaddeviceid();
331
  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
332
  aosThdMSleep(1);
333

    
334
  /*! UT3: High speed SPI result*/
335
  if (actual_deviceId == DWT_DEVICE_ID){
336
    aosUtPassed(stream, &result);
337
  } else {
338
    aosUtFailed(stream, &result);
339
  }
340

    
341
  port_EnableEXT_IRQ();
342
  reset_DW1000();
343

    
344
  chprintf(stream, "initialise the configuration for UWB application \n");
345
  aosThdSleep(1);
346

    
347
  int uwb_init = UWB_Init((DW1000Driver*) ut->data);
348

    
349
  if (0 != uwb_init){
350
    chprintf(stream, "UWB config error with return value: %d \n", uwb_init);
351
    aosThdSleep(1);
352
  }
353
  else {
354
    chprintf(stream, "UWB config success with return value: %d \n", uwb_init);
355
    aosThdSleep(1);
356
  }
357

    
358
  /*! UT4: UWB configuration result
359
   * 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

    
373
  /*! RUN THE STATE MACHINE DEMO APP (RTLS) */
374

    
375
  chprintf(stream, "initialise the State Machine \n");
376
  aosThdSleep(2);
377

    
378
  /* Initialize UWB system with user defined configuration  */
379
  int uwb_init = UWB_Init((DW1000Driver*) ut->data);
380

    
381
  if (0 != uwb_init){
382
    chprintf(stream, "error in UWB config with return value: %d \n", uwb_init);
383
  }
384
  else {
385
    chprintf(stream, "succeed the init of UWB config \n");
386
  }
387
  aosThdSleep(1);
388

    
389
  chprintf(stream, "running the RTLS demo application ... \n");
390
  aosThdSleep(1);
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) */