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