DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
Matrix.cpp@0:dc35364e2291, 2012-04-12 (annotated)
- Committer:
- krmreynolds
- Date:
- Thu Apr 12 13:47:23 2012 +0000
- Revision:
- 0:dc35364e2291
- Child:
- 1:3272ece36ce1
Added GNU Agreement
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 0:dc35364e2291 | 1 | /* mbed L3G4200D Library version 0.1 |
krmreynolds | 0:dc35364e2291 | 2 | * Copyright (c) 2012 Prediluted |
krmreynolds | 0:dc35364e2291 | 3 | * |
krmreynolds | 0:dc35364e2291 | 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
krmreynolds | 0:dc35364e2291 | 5 | * of this software and associated documentation files (the "Software"), to deal |
krmreynolds | 0:dc35364e2291 | 6 | * in the Software without restriction, including without limitation the rights |
krmreynolds | 0:dc35364e2291 | 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
krmreynolds | 0:dc35364e2291 | 8 | * copies of the Software, and to permit persons to whom the Software is |
krmreynolds | 0:dc35364e2291 | 9 | * furnished to do so, subject to the following conditions: |
krmreynolds | 0:dc35364e2291 | 10 | * |
krmreynolds | 0:dc35364e2291 | 11 | * The above copyright notice and this permission notice shall be included in |
krmreynolds | 0:dc35364e2291 | 12 | * all copies or substantial portions of the Software. |
krmreynolds | 0:dc35364e2291 | 13 | * |
krmreynolds | 0:dc35364e2291 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
krmreynolds | 0:dc35364e2291 | 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
krmreynolds | 0:dc35364e2291 | 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
krmreynolds | 0:dc35364e2291 | 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
krmreynolds | 0:dc35364e2291 | 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
krmreynolds | 0:dc35364e2291 | 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
krmreynolds | 0:dc35364e2291 | 20 | * THE SOFTWARE. |
krmreynolds | 0:dc35364e2291 | 21 | */ |
krmreynolds | 0:dc35364e2291 | 22 | |
krmreynolds | 0:dc35364e2291 | 23 | //Computes the dot product of two vectors |
krmreynolds | 0:dc35364e2291 | 24 | float Vector_Dot_Product(float vector1[3],float vector2[3]) { |
krmreynolds | 0:dc35364e2291 | 25 | float op=0; |
krmreynolds | 0:dc35364e2291 | 26 | |
krmreynolds | 0:dc35364e2291 | 27 | for (int c=0; c<3; c++) { |
krmreynolds | 0:dc35364e2291 | 28 | op+=vector1[c]*vector2[c]; |
krmreynolds | 0:dc35364e2291 | 29 | } |
krmreynolds | 0:dc35364e2291 | 30 | |
krmreynolds | 0:dc35364e2291 | 31 | return op; |
krmreynolds | 0:dc35364e2291 | 32 | } |
krmreynolds | 0:dc35364e2291 | 33 | |
krmreynolds | 0:dc35364e2291 | 34 | //Computes the cross product of two vectors |
krmreynolds | 0:dc35364e2291 | 35 | void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) { |
krmreynolds | 0:dc35364e2291 | 36 | vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); |
krmreynolds | 0:dc35364e2291 | 37 | vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); |
krmreynolds | 0:dc35364e2291 | 38 | vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); |
krmreynolds | 0:dc35364e2291 | 39 | } |
krmreynolds | 0:dc35364e2291 | 40 | |
krmreynolds | 0:dc35364e2291 | 41 | //Multiply the vector by a scalar. |
krmreynolds | 0:dc35364e2291 | 42 | void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) { |
krmreynolds | 0:dc35364e2291 | 43 | for (int c=0; c<3; c++) { |
krmreynolds | 0:dc35364e2291 | 44 | vectorOut[c]=vectorIn[c]*scale2; |
krmreynolds | 0:dc35364e2291 | 45 | } |
krmreynolds | 0:dc35364e2291 | 46 | } |
krmreynolds | 0:dc35364e2291 | 47 | |
krmreynolds | 0:dc35364e2291 | 48 | void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) { |
krmreynolds | 0:dc35364e2291 | 49 | for (int c=0; c<3; c++) { |
krmreynolds | 0:dc35364e2291 | 50 | vectorOut[c]=vectorIn1[c]+vectorIn2[c]; |
krmreynolds | 0:dc35364e2291 | 51 | } |
krmreynolds | 0:dc35364e2291 | 52 | } |
krmreynolds | 0:dc35364e2291 | 53 | |
krmreynolds | 0:dc35364e2291 | 54 | //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). |
krmreynolds | 0:dc35364e2291 | 55 | void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) { |
krmreynolds | 0:dc35364e2291 | 56 | float op[3]; |
krmreynolds | 0:dc35364e2291 | 57 | for (int x=0; x<3; x++) { |
krmreynolds | 0:dc35364e2291 | 58 | for (int y=0; y<3; y++) { |
krmreynolds | 0:dc35364e2291 | 59 | for (int w=0; w<3; w++) { |
krmreynolds | 0:dc35364e2291 | 60 | op[w]=a[x][w]*b[w][y]; |
krmreynolds | 0:dc35364e2291 | 61 | } |
krmreynolds | 0:dc35364e2291 | 62 | mat[x][y]=0; |
krmreynolds | 0:dc35364e2291 | 63 | mat[x][y]=op[0]+op[1]+op[2]; |
krmreynolds | 0:dc35364e2291 | 64 | } |
krmreynolds | 0:dc35364e2291 | 65 | } |
krmreynolds | 0:dc35364e2291 | 66 | } |