Statistics
| Branch: | Tag: | Revision:

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

History | View | Annotate | Download (11.5 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
21 4cb40108 Thomas Schöpping
#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 e05848a6 Robin Ewers
32 f3ac1c96 Thomas Schöpping
/******************************************************************************/
33
/* LOCAL DEFINITIONS                                                          */
34
/******************************************************************************/
35 e05848a6 Robin Ewers
36 4cb40108 Thomas Schöpping
//#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 f3ac1c96 Thomas Schöpping
/******************************************************************************/
49
/* EXPORTED VARIABLES                                                         */
50
/******************************************************************************/
51
52
/******************************************************************************/
53
/* LOCAL TYPES                                                                */
54
/******************************************************************************/
55
56
/******************************************************************************/
57
/* LOCAL VARIABLES                                                            */
58
/******************************************************************************/
59 4cb40108 Thomas Schöpping
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 f3ac1c96 Thomas Schöpping
66
/******************************************************************************/
67
/* LOCAL FUNCTIONS                                                            */
68
/******************************************************************************/
69
70 4cb40108 Thomas Schöpping
/*! @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 3106e8cc Thomas Schöpping
  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_OUTPUT);
115 4cb40108 Thomas Schöpping
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 3106e8cc Thomas Schöpping
//  palSetLineMode(moduleGpioDw1000Reset.gpio->line, APAL_GPIO_DIRECTION_INPUT); // TODO:
121 4cb40108 Thomas Schöpping
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 f3ac1c96 Thomas Schöpping
/******************************************************************************/
246
/* EXPORTED FUNCTIONS                                                         */
247
/******************************************************************************/
248 e05848a6 Robin Ewers
249 4cb40108 Thomas Schöpping
250 e05848a6 Robin Ewers
aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut) {
251
252 4cb40108 Thomas Schöpping
  aosDbgCheck(ut->data != NULL);
253 e05848a6 Robin Ewers
254 4cb40108 Thomas Schöpping
  aos_utresult_t result = {0, 0};
255 e05848a6 Robin Ewers
256 4cb40108 Thomas Schöpping
  chprintf(stream, "init DW1000...\n");
257
  dwt_initialise(DWT_LOADUCODE, (DW1000Driver*) ut->data);
258
  aosThdMSleep(5);
259 e05848a6 Robin Ewers
260
261 4cb40108 Thomas Schöpping
/*! Unit Test snippets for DW1000.
262
 * @Note: Event IRQ for DW1000 should be tested separately
263
 */
264
#if defined(UNIT_TEST_SNIPPETS_DW1000)
265 e05848a6 Robin Ewers
266 4cb40108 Thomas Schöpping
  uint32_t actual_deviceId;
267 e05848a6 Robin Ewers
268 4cb40108 Thomas Schöpping
  port_DisableEXT_IRQ();
269 e05848a6 Robin Ewers
270 4cb40108 Thomas Schöpping
  setHighSpeed_SPI(false, (DW1000Driver*) ut->data);
271 a193bcf1 Thomas Schöpping
  chprintf(stream, "expected device ID (LS SPI): 0xDECA0130\n");
272 4cb40108 Thomas Schöpping
  aosThdMSleep(5);
273
  actual_deviceId = instancereaddeviceid();
274
  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);
275
  aosThdMSleep(5);
276 e05848a6 Robin Ewers
277 4cb40108 Thomas Schöpping
  //if the read of device ID fails, the DW1000 could be asleep
278
  if(DWT_DEVICE_ID != actual_deviceId){
279 e05848a6 Robin Ewers
280 4cb40108 Thomas Schöpping
    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 a193bcf1 Thomas Schöpping
      chprintf(stream, "expected device ID: 0xDECA0130\n");
290 4cb40108 Thomas Schöpping
      aosThdMSleep(5);
291
    }
292 e05848a6 Robin Ewers
293 4cb40108 Thomas Schöpping
    //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 a193bcf1 Thomas Schöpping
  chprintf(stream, "initialise instance for DW1000\n");
307 4cb40108 Thomas Schöpping
  aosThdSleep(1);
308
309
  int x_init = instance_init((DW1000Driver*) ut->data) ;
310
311
  if (0 != x_init){
312 a193bcf1 Thomas Schöpping
    chprintf(stream, "init error with return value: %d\n", x_init);
313 4cb40108 Thomas Schöpping
    aosThdSleep(1);
314
  }
315
  else {
316 a193bcf1 Thomas Schöpping
    chprintf(stream, "init success with return value: %d\n", x_init);
317 4cb40108 Thomas Schöpping
    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 a193bcf1 Thomas Schöpping
  chprintf(stream, "initialise the configuration for UWB application\n");
345 4cb40108 Thomas Schöpping
  aosThdSleep(1);
346
347
  int uwb_init = UWB_Init((DW1000Driver*) ut->data);
348
349
  if (0 != uwb_init){
350 a193bcf1 Thomas Schöpping
    chprintf(stream, "UWB config error with return value: %d\n", uwb_init);
351 4cb40108 Thomas Schöpping
    aosThdSleep(1);
352
  }
353
  else {
354 a193bcf1 Thomas Schöpping
    chprintf(stream, "UWB config success with return value: %d\n", uwb_init);
355 4cb40108 Thomas Schöpping
    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 /* defined(UNIT_TEST_SNIPPETS_DW1000) */
371
372
373
  /*! RUN THE STATE MACHINE DEMO APP (RTLS) */
374
375 a193bcf1 Thomas Schöpping
  chprintf(stream, "initialise the State Machine\n");
376 4cb40108 Thomas Schöpping
  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 a193bcf1 Thomas Schöpping
    chprintf(stream, "error in UWB config with return value: %d\n", uwb_init);
383 4cb40108 Thomas Schöpping
  }
384
  else {
385 a193bcf1 Thomas Schöpping
    chprintf(stream, "succeed the init of UWB config\n");
386 4cb40108 Thomas Schöpping
  }
387
  aosThdSleep(1);
388
389 a193bcf1 Thomas Schöpping
  chprintf(stream, "running the RTLS demo application ...\n");
390 4cb40108 Thomas Schöpping
  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  /* defined(UNIT_TEST_SNIPPETS_DW1000) */
399
400
  return result;
401 e05848a6 Robin Ewers
}
402
403 4cb40108 Thomas Schöpping
404
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */