Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/Matrix/Matrix.h@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Working version with priorities set and update time display

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #ifndef __DCM_MATRIX_H
shimniok 0:a6a169de725f 2 #define __DCM_MATRIX_H
shimniok 0:a6a169de725f 3
shimniok 0:a6a169de725f 4 /** Matrix math library */
shimniok 0:a6a169de725f 5
shimniok 0:a6a169de725f 6 #define SINGULAR_MATRIX (1<<0)
shimniok 0:a6a169de725f 7 #define ERR1 (1<<1)
shimniok 0:a6a169de725f 8 #define ERR2 (1<<2)
shimniok 0:a6a169de725f 9 #define ERR3 (1<<3)
shimniok 0:a6a169de725f 10
shimniok 0:a6a169de725f 11 /** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut
shimniok 0:a6a169de725f 12 * @param v1 is the first vector
shimniok 0:a6a169de725f 13 * @param v2 is the second vector
shimniok 0:a6a169de725f 14 * @returns vectorOut = v1 x v2
shimniok 0:a6a169de725f 15 */
shimniok 0:a6a169de725f 16 void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
shimniok 0:a6a169de725f 17
shimniok 0:a6a169de725f 18 /** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2
shimniok 0:a6a169de725f 19 * @param vectorIn the vector
shimniok 0:a6a169de725f 20 * @param scale2 is the scalar
shimniok 0:a6a169de725f 21 * @returns vectorOut the result
shimniok 0:a6a169de725f 22 */
shimniok 0:a6a169de725f 23 void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
shimniok 0:a6a169de725f 24
shimniok 0:a6a169de725f 25 /** TDot product of two 3x1 vectors vector1 . vector2
shimniok 0:a6a169de725f 26 * @param vector1 is the first vector
shimniok 0:a6a169de725f 27 * @param vector2 is the second vector
shimniok 0:a6a169de725f 28 * @returns float result of dot product
shimniok 0:a6a169de725f 29 */
shimniok 0:a6a169de725f 30 float Vector_Dot_Product(float vector1[3], float vector2[3]);
shimniok 0:a6a169de725f 31
shimniok 0:a6a169de725f 32 /** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2
shimniok 0:a6a169de725f 33 * @param vectorIn1 is the first vector
shimniok 0:a6a169de725f 34 * @param vectorIn2 is the second vector
shimniok 0:a6a169de725f 35 * @returns vectorOut is the result of the addition
shimniok 0:a6a169de725f 36 */
shimniok 0:a6a169de725f 37 void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
shimniok 0:a6a169de725f 38
shimniok 0:a6a169de725f 39 /** Adds two 3x3 matrices: C = A + B
shimniok 0:a6a169de725f 40 * @param A is the first vector
shimniok 0:a6a169de725f 41 * @param B is the second vector
shimniok 0:a6a169de725f 42 * @returns C is the result of the addition
shimniok 0:a6a169de725f 43 */
shimniok 0:a6a169de725f 44 void Matrix_Add(float C[3][3], float A[3][3], float B[3][3]);
shimniok 0:a6a169de725f 45
shimniok 0:a6a169de725f 46 /** Adds two n x m matrices: C = A + B
shimniok 0:a6a169de725f 47 * @param A is the first vector
shimniok 0:a6a169de725f 48 * @param B is the second vector
shimniok 0:a6a169de725f 49 * @param n is the rows of A, B, C
shimniok 0:a6a169de725f 50 * @param m is the columns of A, B, C
shimniok 0:a6a169de725f 51 * @returns C is the result of the addition
shimniok 0:a6a169de725f 52 */
shimniok 0:a6a169de725f 53 void Matrix_Add(int n, int m, float *C, float *A, float *B);
shimniok 0:a6a169de725f 54
shimniok 0:a6a169de725f 55 void Matrix_Subtract(int n, int m, float *C, float *A, float *B);
shimniok 0:a6a169de725f 56
shimniok 0:a6a169de725f 57 /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab
shimniok 0:a6a169de725f 58 * @param a is the first vector
shimniok 0:a6a169de725f 59 * @param b is the second vector
shimniok 0:a6a169de725f 60 * @returns c as the result of the mutliplication
shimniok 0:a6a169de725f 61 */
shimniok 0:a6a169de725f 62 void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]);
shimniok 0:a6a169de725f 63
shimniok 0:a6a169de725f 64 /** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab
shimniok 0:a6a169de725f 65 * @param A is the first matrix
shimniok 0:a6a169de725f 66 * @param B is the second matrix
shimniok 0:a6a169de725f 67 * @param n is the rows of A
shimniok 0:a6a169de725f 68 * @param p is the columns of A and rows of B
shimniok 0:a6a169de725f 69 * @param m is the columns of B
shimniok 0:a6a169de725f 70 * @returns C as the result of the mutliplication
shimniok 0:a6a169de725f 71 */
shimniok 0:a6a169de725f 72 void Matrix_Multiply(int n, int m, int p, float *C, float *A, float *B);
shimniok 0:a6a169de725f 73
shimniok 0:a6a169de725f 74 void Matrix_Transpose(int n, int m, float *C, float *A);
shimniok 0:a6a169de725f 75
shimniok 0:a6a169de725f 76 void Matrix_Inverse(int n, float *A);
shimniok 0:a6a169de725f 77
shimniok 0:a6a169de725f 78 void Matrix_Inverse(int n, float *C, float *A);
shimniok 0:a6a169de725f 79
shimniok 0:a6a169de725f 80 void Matrix_Copy(int n, int m, float *C, float *A);
shimniok 0:a6a169de725f 81
shimniok 0:a6a169de725f 82 /** Prints out an m x m matrix
shimniok 0:a6a169de725f 83 * @param A is the matrix to print
shimniok 0:a6a169de725f 84 * @param name is the name to print out along with the vector
shimniok 0:a6a169de725f 85 */
shimniok 0:a6a169de725f 86 void Matrix_print(int n, int m, float *A, const char *name);
shimniok 0:a6a169de725f 87
shimniok 0:a6a169de725f 88 /** Prints out a 1 x 3 vector
shimniok 0:a6a169de725f 89 * @param A is the 1 x 3 vector to print
shimniok 0:a6a169de725f 90 * @param name is the name to print out along with the vector
shimniok 0:a6a169de725f 91 */
shimniok 0:a6a169de725f 92 void Vector_Print(float A[3], const char *name);
shimniok 0:a6a169de725f 93
shimniok 0:a6a169de725f 94 #endif