Toshihisa T
LSM303DLH Test Program for get angle.
LSM303DLH/vector.cpp@3:f3796683b4c9, 2012-05-06 (annotated)
- Committer:
- tosihisa
- Date:
- Sun May 06 05:14:30 2012 +0000
- Revision:
- 3:f3796683b4c9
An angle of inclination is obtained using an accelerometer.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| tosihisa | 3:f3796683b4c9 | 1 | #include <vector.h> |
| tosihisa | 3:f3796683b4c9 | 2 | #include <math.h> |
| tosihisa | 3:f3796683b4c9 | 3 | |
| tosihisa | 3:f3796683b4c9 | 4 | void vector_cross(const vector *a,const vector *b, vector *out) |
| tosihisa | 3:f3796683b4c9 | 5 | { |
| tosihisa | 3:f3796683b4c9 | 6 | out->x = a->y*b->z - a->z*b->y; |
| tosihisa | 3:f3796683b4c9 | 7 | out->y = a->z*b->x - a->x*b->z; |
| tosihisa | 3:f3796683b4c9 | 8 | out->z = a->x*b->y - a->y*b->x; |
| tosihisa | 3:f3796683b4c9 | 9 | } |
| tosihisa | 3:f3796683b4c9 | 10 | |
| tosihisa | 3:f3796683b4c9 | 11 | float vector_dot(const vector *a,const vector *b) |
| tosihisa | 3:f3796683b4c9 | 12 | { |
| tosihisa | 3:f3796683b4c9 | 13 | return a->x*b->x+a->y*b->y+a->z*b->z; |
| tosihisa | 3:f3796683b4c9 | 14 | } |
| tosihisa | 3:f3796683b4c9 | 15 | |
| tosihisa | 3:f3796683b4c9 | 16 | void vector_normalize(vector *a) |
| tosihisa | 3:f3796683b4c9 | 17 | { |
| tosihisa | 3:f3796683b4c9 | 18 | float mag = sqrt(vector_dot(a,a)); |
| tosihisa | 3:f3796683b4c9 | 19 | a->x /= mag; |
| tosihisa | 3:f3796683b4c9 | 20 | a->y /= mag; |
| tosihisa | 3:f3796683b4c9 | 21 | a->z /= mag; |
| tosihisa | 3:f3796683b4c9 | 22 | } |