diff --git a/main/main.ino b/main/main.ino index a6c85d2..5f4e9ae 100644 --- a/main/main.ino +++ b/main/main.ino @@ -54,7 +54,7 @@ void send_tvc(Matrix u, Matrix * last_u, double yaw) //rotate input are body z axis float gamma=yaw+BETA; - float rotation_values [4]={cos(gamma), sin(gamma), -sin(gamma), cos(gamma)}; + vector rotation_values {cos(gamma), sin(gamma), -sin(gamma), cos(gamma)}; Matrix R=Matrix(2, 2, rotation_values); u=R*u; @@ -153,13 +153,13 @@ void loop() else { ws.construct_y(); - //ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors } break; } case(FINAL_COUNTDOWN): { - if (change_mode_to_prep_tvc(millis()>ws.next_mode_time)) + if (change_mode_to_prep_tvc(millis() > ws.next_mode_time)) { transition_to_prep_tvc(); ws.next_mode_time=millis()+PREP_TVC_PERIOD*KILO_I; @@ -168,7 +168,7 @@ void loop() else { ws.construct_y(); - //ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors + ws.x=ws.x+(L*(ws.y-(C*ws.x))).scale(ws.dt); //state estimation only using sensors } break; } diff --git a/main/matrix.cpp b/main/matrix.cpp index 8bec628..8472180 100644 --- a/main/matrix.cpp +++ b/main/matrix.cpp @@ -1,71 +1,32 @@ #include "matrix.hh" #include +#include using namespace std; -//matrix subtraction helper function -float * m_sub_helper(float * A, float * B, int elements) { - float * C; - C = new float[elements]; - for (int i = 0; i < elements; i++) { - C[i] = A[i] - B[i]; - } - //delete C; - return C; -} - -//scalar multiplication helper function -float * s_mult_helper(float * A, float rho, int elements) { - float * C; - C = new float[elements]; - for (int i = 0; i < elements; i++) { - C[i] = A[i] * rho; - } - //delete C; - return C; -} - -//matrix multiplication helper function -float * m_mult_helper(float * A, float * B, int m, int n, int p) { - float * C; - C = new float[m*p]; - for (int i = 0; i < m; i++) - { - for (int j = 0; j < p; j++) - { - C[i*p + j] = 0; - for (int k = 0; k < n; k++) - { - C[i*p + j] += A[i*n + k] * B[j + p * k]; - } - } - } - //delete C; - return C; -} - -//matrix addition helper function -float * m_add_helper(float * A, float * B, int elements) { - float * C; - C = new float[elements]; - for (int i = 0; i < elements; i++) { - C[i] = A[i] + B[i]; - } - //delete C; - return C; -} - //matrix multiplication Matrix Matrix::operator*(Matrix B) { - const int m=rows; - const int p=B.columns; - const int n=columns; - float * new_values; + int m=rows; + int p=B.columns; + int n=columns; + + vector new_values (m*p, 0); + if (columns==B.rows) { - new_values=m_mult_helper(values, B.values, m, n, p); + for (int i = 0; i < m; i++) + { + for (int j = 0; j < p; j++) + { + for (int k = 0; k < n; k++) + { + new_values[i*p + j] += values[i*n + k] * B.values[j + p * k]; + } + } + } } + return Matrix(m, p, new_values); } @@ -76,19 +37,26 @@ Matrix Matrix::operator+(Matrix B) const int n_b=B.columns; const int n_a=columns; const int m_b=B.rows; - float * new_values; + + vector new_values (m_a*n_a, 0); + if ((m_a==m_b)&&(n_a==n_b)) { - new_values=m_add_helper(values, B.values, m_a*n_a); + for (int i = 0; i < m_a*n_a; i++) { + new_values[i] = values[i] + B.values[i]; + } } + return Matrix(m_a, n_a, new_values); } //scalar multiplication function Matrix Matrix :: scale(float k) { - float * new_values; - new_values=s_mult_helper(values, k, rows*columns); + vector new_values (rows*columns, 0); + for (int i = 0; i < rows*columns; i++) { + new_values[i] = values[i] * k; + } return Matrix(rows, columns, new_values); } @@ -99,30 +67,20 @@ Matrix Matrix::operator-(Matrix B) const int n_b=B.columns; const int n_a=columns; const int m_b=B.rows; - float * new_values; - if ((m_a==m_b)&&(n_a==n_b)) - { - new_values=m_sub_helper(values, B.values, m_a*n_a); - } - return Matrix(m_a, n_a, new_values); -} -string display_matrix(Matrix A) -{ - std::string result; - for (int i=0; i new_values (m_a*n_a, 0); + + if ((m_a==m_b)&&(n_a==n_b)) { - for (int j = 0; j < A.columns; j++) - { - result+= A.select(i+1,j+1); - result+= " "; + for (int i = 0; i < m_a*n_a; i++) { + new_values[i] = values[i] - B.values[i]; } - result+="\n"; } - return result; + + return Matrix(m_a, n_a, new_values); } -void Matrix:: redefine(float* update_values) +void Matrix:: redefine(vector update_values) { values=update_values; } diff --git a/main/matrix.hh b/main/matrix.hh index 0124176..80fc910 100644 --- a/main/matrix.hh +++ b/main/matrix.hh @@ -12,9 +12,9 @@ class Matrix public: int rows; int columns; - float * values; + vector values; - Matrix(int row_number, int column_number, float * mat_values) + Matrix(int row_number, int column_number, vector mat_values) { rows=row_number; columns=column_number; @@ -26,13 +26,11 @@ class Matrix return values[(row-1)*columns+(column-1)]; } - void redefine(float*); + void redefine(vector); Matrix operator+(Matrix); Matrix operator-(Matrix); Matrix scale (float); Matrix operator*(Matrix); }; -string display_matrix(Matrix); - #endif diff --git a/main/mission_constants.hh b/main/mission_constants.hh index 0a26b45..4d7f117 100644 --- a/main/mission_constants.hh +++ b/main/mission_constants.hh @@ -2,6 +2,9 @@ #define __MISSION_CONSTANTS_HH__ #include "matrix.hh" +#include + +using namespace std; //***************************************************************************** // CONVERSION FACTORS @@ -86,38 +89,38 @@ const float MOMENT_INERTIA_YY=1;//TODO: make exact const float MASS=1; //control constants TODO: fill these out -const float A_VALUES [36] = {0,THRUST/MASS,0,0,0,0, - 0,0,1,0,0,0, - 0,0,0,0,0,0, - 0,0,0,-THRUST/MASS,0,0, - 0,0,0,0,0,1, - 0,0,0,0,0,0}; //dynamics matrix -const Matrix A=Matrix(6, 6, A_VALUES); +vector A_VALUES {0,THRUST/MASS,0,0,0,0, + 0,0,1,0,0,0, + 0,0,0,0,0,0, + 0,0,0,-THRUST/MASS,0,0, + 0,0,0,0,0,1, + 0,0,0,0,0,0}; //dynamics matrix +Matrix A=Matrix(6, 6, A_VALUES); -const float B_VALUES [12] = {THRUST/MASS, 0, +vector B_VALUES {THRUST/MASS, 0, 0, 0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_YY, 0, 0, -THRUST/MASS, 0, 0, 0, -THRUST*MOMENT_ARM/MOMENT_INERTIA_XX}; //input matrix -const Matrix B=Matrix(6, 2, B_VALUES); +Matrix B=Matrix(6, 2, B_VALUES); -const float K_VALUES [12] = {0,0,0,0,0,0, +vector K_VALUES {0,0,0,0,0,0, 0,0,0,0,0,0}; //controller gain -const Matrix KC=Matrix(2, 6, K_VALUES); +Matrix KC=Matrix(2, 6, K_VALUES); -const float C_VALUES [24] = {1,0,0,0,0,0, +vector C_VALUES {1,0,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,0,1}; //sensor matrix -const Matrix C=Matrix(4, 6, C_VALUES); +Matrix C=Matrix(4, 6, C_VALUES); -const float L_VALUES [24] = {0, 0, 0, 0, +vector L_VALUES {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; //kalman gain -const Matrix L=Matrix(6, 4, L_VALUES); +Matrix L=Matrix(6, 4, L_VALUES); #endif diff --git a/main/workspace.hh b/main/workspace.hh index 5e96fcd..0280be4 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -58,7 +58,7 @@ class Workspace Matrix y=Matrix(4, 1, initialize_4); Matrix u=Matrix(2, 1, initialize_2); Matrix last_u=Matrix(2, 1, initialize_2); - float y_values [4]= {0, 0, 0, 0}; + vector y_values {0, 0, 0, 0}; float yaw; // state