forked from DigilentInc/vivado-library
-
Notifications
You must be signed in to change notification settings - Fork 302
/
main.c
508 lines (469 loc) · 16.1 KB
/
main.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
/******************************************************************************/
/* */
/* main.c -- Demo for the PmodNAV using SPI and UART */
/* */
/******************************************************************************/
/* Author: Samuel Lowe, Arthur Brown */
/* Copyright 2016, Digilent Inc. */
/******************************************************************************/
/*
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/******************************************************************************/
/* File Description: */
/* */
/* This file contains a demo to be used with the PmodNAV. Accelerometer, */
/* Magnetometer, Temperature, and Barometric Pressure data is captured over a */
/* SPI interface and displayed over UART. */
/* */
/* This application configures UART 16550 to baud rate 9600. */
/* PS7 UART (Zynq) is not initialized by this application, since bootrom/bsp */
/* configures it to baud rate 115200 */
/* */
/* ------------------------------------------------ */
/* | UART TYPE BAUD RATE | */
/* ------------------------------------------------ */
/* Uartns550 9600 */
/* Uartlite Configurable only in HW design */
/* ps7_uart 115200 (configured by bootrom/bsp) */
/* */
/******************************************************************************/
/* Revision History: */
/* */
/* 11/16/2016(SamL): Created */
/* 03/16/2017(ArtVVB): Completed & Validated */
/* 11/01/2017(ArtVVB): 2016.4 Maintenance */
/* 02/20/2018(atangzwj): Validated for Vivado 2017.4 */
/* */
/******************************************************************************/
/* Problems: */
/* */
/* In order to include "math.h" you must include m in compiler settings. */
/* See https://www.xilinx.com/support/answers/52971.html */
/* */
/******************************************************************************/
/***************************** Include Files *******************************/
#include <stdio.h>
#include "math.h"
#include "PmodNAV.h"
#include "sleep.h"
#include "xil_cache.h"
#include "xparameters.h"
/*************************** Global Variables ******************************/
PmodNAV nav;
/********************* Function Forward Declarations ***********************/
void NavDemo_Initialize(void);
void NavDemo_Run(void);
void NavDemo_Cleanup(void);
float NavDemo_ComputePref(float hPa, float altitudeMeters);
float NavDemo_ConvPresToAltF(float Pref, float hPa);
float NavDemo_ConvPresToAltM(float Pref, float hPa);
float NavDemo_ConvTempCToTempF(float tempC);
float NavDemo_ConvFeetToMeters(float feet);
float NavDemo_ConvMetersToFeet(float meters);
float NavDemo_AngleInXY(NAV_RectCoord r);
float NavDemo_DegreesFromVertical(NAV_RectCoord r);
float NavDemo_ScalarProjection(NAV_RectCoord orient, NAV_RectCoord r);
void NavDemo_EnableCaches(void);
void NavDemo_DisableCaches(void);
/***************************** Function Definitions ************************/
int main(void) {
NavDemo_Initialize();
NavDemo_Run();
NavDemo_Cleanup();
return 0;
}
/*** void NavDemo_Initialize()
**
** Parameters:
** None
**
** Return Values:
** None
**
** Errors:
** None
**
** Description:
** This function initializes the hardware used in the demo and starts a
** PmodNAV driver device
*/
void NavDemo_Initialize(void) {
NavDemo_EnableCaches();
xil_printf("Pmod Nav Demo Initializing...\n\r");
NAV_begin ( // intialize the PmodNAV driver device
&nav,
XPAR_PMODNAV_0_AXI_LITE_GPIO_BASEADDR,
XPAR_PMODNAV_0_AXI_LITE_SPI_BASEADDR
);
xil_printf("Pmod Nav Demo Initialized\n\r");
NAV_Init(&nav); // initialize the connection with each spi slave
}
/*** void NavDemo_Run(void)
**
** Parameters:
** None
**
** Return Values:
** None
**
** Errors:
** None
**
** Description:
** This function encapsulates the PmodNAV demo, capturing data from each
** part on the PmodNAV and displaying it over a UART connection
*/
void NavDemo_Run(void) {
float Pref,
tempF,
dps,
magXYd,
alt;
char *compass[8] = {"North", "North-East", "East", "South-East", "South",
"South-West", "West", "North-West"};
char *str;
// Set the known altitude as reference for future measurements of altitude
// Uncomment the below line to improve accuracy of the altimeter device by
// specifying the known altitude for your current location
printf("Pmod Nav Demo Started\n\r");
NAV_ReadPressurehPa(&nav);
alt = NavDemo_ConvFeetToMeters(2352); // altitude for Pullman, WA in meters
Pref = NavDemo_ComputePref(nav.hPa, alt);
usleep(100000);
while(1){
NAV_GetDeviceID(&nav);
printf("Device IDs\n\r");
printf(
"A/G ID:%d MAG ID:%d ALT ID:%d\n\r\n\r",
nav.idData.ag,
nav.idData.mag,
nav.idData.alt
);
NAV_GetData(&nav);
printf(
"Accelerometer G values X:%.2f G, Y:%.2f G, Z:%.2f G\n\r",
nav.acclData.X,
nav.acclData.Y,
nav.acclData.Z
);
printf(
" %.2f degrees from vertical\n\r",
NavDemo_DegreesFromVertical(nav.acclData)
);
// Format and serial print the data read from accelerometer instrument,
// stored in acclData variables and expressed in G
printf(
"Gyro dps values X:%.2f dps, Y:%.2f dps, Z:%.2f dps\n\r",
nav.gyroData.X,
nav.gyroData.Y,
nav.acclData.Z
);
dps = NavDemo_ScalarProjection(nav.acclData, nav.gyroData);
if (dps < -4) // Remove some noise around 0
str = "Clockwise";
else if (dps > 4)
str = "Counter-Clockwise";
else
str = "Marginal";
printf(" rotating about vertical at %6.2f dps (%s)\n\r", dps, str);
// Format and serial print the data read from Gyro instrument, stored in
// gyroData variables and expressed in degrees per second
printf(
"Magnetometer Gauss values X:%.2f Gauss, Y:%.2f Gauss, Z:%.2f Gauss \
\n\r",
nav.magData.X,
nav.magData.Y,
nav.magData.Z
);
// Format and serial print the data read from magnetometer instrument,
// stored in magData variables and expressed in Gauss
magXYd = NavDemo_AngleInXY(nav.magData);
str = compass[(int)((magXYd + 22.5) / 45.0) % 8];
// Get appropriate compass string - North is centered on 0 degrees
printf(" Heading in Degrees: %.2f Y Direction: %s\n\r", magXYd, str);
printf("Pressure: %f\n\r", nav.hPa);
alt = NavDemo_ConvPresToAltF(Pref, nav.hPa);
printf("Altitude: %.1f feet\n\r", alt);
tempF = NavDemo_ConvTempCToTempF(nav.tempC);
printf("Temperature: %.2f degrees Celsius\n\r", nav.tempC);
printf(" %.2f degrees Fahrenheit\n\r", tempF);
xil_printf("\n\r\n\r");
usleep(500000);
}
}
/*** void NavDemo_Cleanup(void)
**
** Parameters:
** None
**
** Return Values:
** None
**
** Errors:
** None
**
** Description:
** This function closes the PmodNAV device and ends the demo
*/
void NavDemo_Cleanup(void) {
NAV_end(&nav);
NavDemo_DisableCaches();
}
/*** float NavDemo_ComputePref(float hPa, float altitudeMeters)
**
** Parameters:
** hPa - a reading of the current pressure
** altitudeMeters - the parameter used to calibrate the altitude computing,
** is considered known for the wanted location
**
** Return Values:
** Pref - the reference pressure in hPa
**
** Errors:
** None
**
** Description:
** This function provides the reference pressure computed with a known
** altitude for the given location it performs a pressure reading, then
** computes the reference pressure using the altitude parameter.
**
** It needs to be called once for the correct operation of the altitude
** function, all the following pressure readings are affected by it. This
** This is needed because the current altitude is also affected by the
** current sea level air pressure, while the barometric pressure formula
** used to compute altitude is considering the sea level pressure constant
** at all times.
*/
float NavDemo_ComputePref(float hPa, float altitudeMeters) {
float altitudeFeet = NavDemo_ConvMetersToFeet(altitudeMeters);
float temp = 1 - (altitudeFeet / 145366.45);
return hPa / (powf(temp, 1 / 0.190284));
}
/*** float NavDemo_ConvPresToAltM(float hPa)
**
** Parameters:
** float hPa - parameter representing the value of pressure in hPa
**
** Return Values:
** float altMeters - it returns the current altitude based on the measured
** pressure and the previously computed reference
** pressure
**
** Errors:
** None
**
** Description:
** This function calls the ConvPresToAltF function to obtain the altitude
** in feet. Then it converts it to meters.
** The Pref is computed once and used for further calculations of the
** altitude.
*/
float NavDemo_ConvPresToAltM(float Pref, float hPa) {
return NavDemo_ConvPresToAltF(Pref, hPa) * 0.3048;
}
/*** float NavDemo_ConvPresToAltF(float Pref, float hPa)
**
** Parameters:
** float Pref - pressure reference
** float hPa - parameter representing the value of pressure in hPa
**
** Return Values:
** float altFeet - returns the value of the altitude in feet
**
** Errors:
** None
**
** Description:
** This function converts the provided pressure to altitude (in feet) using
** the previously computed Pref as reference pressure.
**
** The calculation of altitude is based on the following formula:
** Altitude_ft = (1-pow(*Pressure_mb/1013.25,0.190284))*145366.45
*/
float NavDemo_ConvPresToAltF(float Pref, float hPa) {
return ((1 - pow(hPa / Pref, 0.190284)) * 145366.45);
}
/*** float NavDemo_ConvTempCToTempF(float tempC)
**
** Parameters:
** tempC - parameter representing the value of temperature expressed in
** degrees Celsius
**
** Return Values:
** float - returns the value of the temperature in degrees Fahrenheit
**
** Errors:
** None
**
** Description:
** This function performs the conversion from Celsius to Fahrenheit degrees
** and returns the value of temperature in F
**
*/
float NavDemo_ConvTempCToTempF(float tempC) {
return 32 + (tempC * 1.8);
}
/*** float NavDemo_ConvFeetToMeters(float feet)
**
** Parameters:
** feet - a distance in feet
**
** Return Values:
** float - returns the parameter feet converted to meters
**
** Errors:
**
** Description:
** This function performs the conversion from units of feet to meters
*/
float NavDemo_ConvFeetToMeters(float feet) {
return feet / 0.3048;
}
/*** float NavDemo_ConvMetersToFeet(float meters)
**
** Parameters:
** meters - a distance in meters
**
** Return Values:
** float - returns the parameter meters converted to feet
**
** Errors:
**
** Description:
** This function performs the conversion from units of meters to feet
*/
float NavDemo_ConvMetersToFeet(float meters) {
return meters * 0.3048;
}
/*** float NavDemo_AngleInXY(NAV_RectCoord r)
**
** Parameters:
** r - the vector in rectangular coordinates to be converted to polar
**
** Return Value:
** p - returns the polar coordinate representation of the vector r
** projected onto the XY plane
**
** Errors:
** None
**
** Description:
** The function computes the degrees the vector r is rotated about the
** Z-axis from the vector (X=1,0,0)
*/
float NavDemo_AngleInXY(NAV_RectCoord r) {
float d;
if (r.X == 0)
d = (r.Y < 0) ? 90 : 0;
else
d = atan2f(r.Y, r.X) * 180 / M_PI;
if (d > 360)
d -= 360;
else if (d < 0)
d += 360;
return d;
}
/*** float NavDemo_DegreesFromVertical(NAV_RectCoord r)
**
** Parameters:
** r - the xyz vector to be operated upon
**
** Return Value:
** float - the angle in degrees between the vector r and the unit Z vector.
**
** Errors:
** None
**
** Description:
** The function computes the angle in degrees between the vector r and the
** vector (0,0,Z=1)
*/
float NavDemo_DegreesFromVertical(NAV_RectCoord r) {
// Determine the magnitude of the vector r.
float rM = sqrtf(powf(r.X, 2) + powf(r.Y, 2) + powf(r.Z, 2));
if (rM == 0)
return 0.0;
return acosf(r.Z / rM) * (180.0 / M_PI);
}
/*** float NavDemo_ScalarProjection(NAV_RectCoord orient, NAV_RectCoord r)
**
** Parameters:
** orient - the xyz vector
** rotation - the xyz vector
**
** Return Value:
** float - the angle in degrees between the vector r and the unit Z vector.
**
** Errors:
** None
**
** Description:
** This function returns the scalar projection of the r vector onto the
** orient vector. This can be used with gyroscope and accelerometer data to
** determine rotation of the PmodNAV about true vertical.
*/
float NavDemo_ScalarProjection(NAV_RectCoord orient, NAV_RectCoord r) {
float oM = sqrtf(powf(orient.X, 2) + powf(orient.Y, 2) + powf(orient.Z, 2));
return (r.X * orient.X + r.Y * orient.Y + r.Z * orient.Z) / oM;
}
/*** void NavDemo_EnableCaches(void)
**
** Parameters:
** None
**
** Return Values:
** None
**
** Errors:
** None
**
** Description:
** This function enables the instruction and/or data caches on
** architectures that require them
*/
void NavDemo_EnableCaches(void) {
#ifdef __MICROBLAZE__
#ifdef XPAR_MICROBLAZE_USE_ICACHE
Xil_ICacheEnable();
#endif
#ifdef XPAR_MICROBLAZE_USE_DCACHE
Xil_DCacheEnable();
#endif
#endif
}
/*** void NavDemo_DisableCaches(void)
**
** Parameters:
** None
**
** Return Values:
** None
**
** Errors:
** None
**
** Description:
** This function disables the instruction and/or data caches on
** architectures that require them
*/
void NavDemo_DisableCaches(void) {
#ifdef __MICROBLAZE__
#ifdef XPAR_MICROBLAZE_USE_ICACHE
Xil_ICacheDisable();
#endif
#ifdef XPAR_MICROBLAZE_USE_DCACHE
Xil_DCacheDisable();
#endif
#endif
}