Revision 4:21a356ae0747, committed 2019-08-19

Comitter:

Joeatsumi

Date:

Mon Aug 19 02:50:13 2019 +0000

Parent:

3:3fa7882a5fd0

Commit message:

test program for auto pilot

Changed in this revision

--- a/HMC5883L.lib	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/Joeatsumi/code/HMC5883L/#52ffe160f17f

--- a/Matrix/Matrix.cpp	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,388 +0,0 @@
-#include "myConstants.h"
-#include "Matrix.h"
-
-
-
-Matrix::Matrix(int row, int col) : row(row), col(col), components(0) {
-    components = new float[row*col];
-    if (!components) error("Memory Allocation Error");
-    for(int i=0; i<row*col; i++) components[i] = 0.0f;
-    if (row == col) {
-        for (int i = 0; i < row; i++) {
-            components[i * col + i] = 1.0f;
-        }
-    }
-}
-
-Matrix::Matrix(int row, int col, float* comps) : row(row), col(col), components(0) {
-    components = new float[row*col];
-    if (!components) error("Memory Allocation Error");
-    memcpy(components, comps, sizeof(float)*row*col);
-}
-
-
-Matrix::~Matrix() {
-    delete[] components;
-}
-
-Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) {
-    components = new float[row*col];
-    if (!components) error("Memory Allocation Error");
-    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
-}
-
-Matrix Matrix::operator-() const{
-    Matrix retMat(*this);
-
-    for (int i = 0; i < row * col; i++) {
-        retMat.components[i] = - this->components[i];
-    }
-
-    return retMat;
-}
-
-Matrix& Matrix::operator=(const Matrix& m) {
-    if (this == &m) return *this;
-    row = m.row;
-    col = m.col;
-    delete[] components;
-    components = new float[row*col];
-    if (!components) error("Memory Allocation Error");
-    memcpy(components, m.GetpComponents(), sizeof(float)*row*col);
-
-    return *this;
-}
-
-Matrix& Matrix::operator+=(const Matrix& m) {
-    if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
-    
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            components[i * col + j] += m.components[i * col + j];
-        }
-    }
-
-    this->CleanUp();
-
-    return *this;
-}
-
-Matrix& Matrix::operator-=(const Matrix& m) {
-    if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention");
-
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            components[i * col + j] -= m.components[i * col + j];
-        }
-    }
-
-    this->CleanUp();
-
-    return *this;
-}
-/*
-Matrix& Matrix::operator*=(const Matrix& m) {
-    if (col != m.GetRow()) error("Irregular Dimention");
-    Matrix temp = Matrix(*this);
-    
-    col = m.GetCol();
-    delete[] components;
-    components = new float[row*col];
-
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            components[i*col + j] = 0.0f;
-            for (int k = 0; k < m.GetRow(); k++) {
-                components[i * col + j] += temp.components[i * col + k] * m.components[k * col + j];
-            }
-        }
-    }
-
-    this->CleanUp();
-
-    return *this;
-}
-*/
-
-Matrix& Matrix::operator*=(float c) {
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            components[i*col + j] *= c;
-        }
-    }
-
-    return *this;
-}
-
-Matrix& Matrix::operator/=(float c) {
-    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            components[i*col + j] /= c;
-        }
-    }
-
-    return *this;
-}
-
-void Matrix::SetComp(int rowNo, int colNo, float val) {
-    if (rowNo > row || colNo > col) error("Index Out of Bounds Error");
-    components[(rowNo-1)*col + (colNo-1)] = val;
-}
-
-void Matrix::SetComps(float* pComps) {
-    memcpy(components, pComps, sizeof(float) * row * col);
-}
-
-float Matrix::Determinant() const{
-    if (row != col) error("failed to calculate det. : matrix is not square");
-    int decSign = 0;
-    float retVal = 1.0f;
-
-    // 行列のLU分解
-    Matrix LU(this->LU_Decompose(&decSign));
-
-    for (int i = 0; i < LU.row; i++) {
-        retVal *= LU.components[i * LU.col + i];
-    }
-
-    return retVal*decSign;
-}
-
-float Matrix::det() const {
-    if (row != col) error("failed to calculate det : matrix is not square");
-    
-    Matrix temp(*this);
-    int decSign = 1;
-
-    for (int j = 0; j < col - 1; j++) {
-
-        // 列内のみで最大の要素を探す
-        int maxNo = j;
-        for (int k = j; k < row; k++) {
-            if (temp.components[maxNo * col + j] < temp.components[k * col + j]) maxNo = k;
-        }
-        if (maxNo != j) {
-            temp.SwapRow(j + 1, maxNo + 1);
-            decSign *= -1;
-        }
-        // 列内の最大要素が小さ過ぎる場合、行内の最大要素も探す
-        if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
-            maxNo = j;
-            for (int k = j; k < col; k++) {
-                if (temp.components[j * col + maxNo] < temp.components[j * col + k])maxNo = k;
-            }
-            if (maxNo != j) {
-                temp.SwapCol(j + 1, maxNo + 1);
-                decSign *= -1;
-            }
-
-            // 列内、行内の最大要素を選んでも小さすぎる場合はエラー
-            if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) {
-                if (row != col) error("failed to calculate det : Division by Zero");
-            }
-        }
-
-        float c1 = 1.0f / temp.components[j * col + j];
-
-        for (int i = j + 1; i < row; i++) {
-            float c2 = temp.components[i * col + j] * c1;
-            for (int k = j; k < col; k++) {
-                temp.components[i * col + k] = temp.components[i * col + k] - c2 * temp.components[j * col + k];
-            }
-        }
-        
-    }
-
-    if (fabs(temp.components[(row - 1) * col + (col - 1)]) < NEARLY_ZERO) return 0.0f;
-
-    float retVal = 1.0f;
-    for (int i = 0; i < row; i++) {
-        retVal *= temp.components[i * col + i];
-    }
-
-    return retVal * decSign;
-}
-
-Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{
-    if (row != col) error("failed to LU decomposition: matrix is not square");
-    if (sign != 0) *sign = 1;
-    if (p != 0) {
-        if (p->row != row || p->row != p->col) error("failed to LU decomposition: permitation matrix is incorrect");
-        // 置換行列は最初に単位行列にしておく
-        memset(p->components, 0, sizeof(float) * row * col);
-        for (int i = 0; i < row; i++) {
-            p->components[i * col + i] = 1.0f;
-        }
-    }
-    Matrix retVal(*this);
-
-    for (int d = 0; d < row - 1; d++) { // 1行1列ずつ分解を行う
-        // d列目の最大の要素を探索し、見つけた要素の行とd行目を交換する
-        int maxNo = d;
-        for (int i = d; i < row; i++) {
-            if (retVal.components[i * col + d] > retVal.components[maxNo * col + d]) maxNo = i;
-        }
-        if (maxNo != d) {
-            retVal.SwapRow(d + 1, maxNo + 1);
-            if (sign != 0) *sign *= -1;
-            if (p != 0) {
-                p->SwapRow(d + 1, maxNo + 1);
-            }
-        }
-        float c = retVal.components[d * col + d];
-        if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero");
-
-        // d行d列目以降の行列について計算
-        for (int i = d+1; i < row; i++) {
-            retVal.components[i * col + d] /= c;
-            for (int j = d+1; j < col; j++) {
-                retVal.components[i * col + j] -= retVal.components[d * col + j] * retVal.components[i * col + d];
-            }
-        }
-    }
-
-    retVal.CleanUp();
-
-    return retVal;
-}
-
-float Matrix::Inverse(Matrix& invm) const{
-    if (row != col) error("failed to get Inv. : matrix is not square");
-
-    Matrix P(*this);
-    Matrix LU(LU_Decompose(0, &P));
-
-    // 分解した行列の対角成分の積から行列式を求める
-    // det = 0　ならfalse
-    float det = 1.0f;
-    for (int i = 0; i < row; i++) {
-        det *= LU.components[i * col + i];
-    }
-    if (fabs(det) < NEARLY_ZERO) {
-        return fabs(det);
-    }
-
-    // U、Lそれぞれの逆行列を計算する
-    Matrix U_inv = Matrix(row, col);
-    Matrix L_inv = Matrix(row, col);
-
-    for (int j = 0; j < col; j++) {
-        for (int i = 0; i <= j; i++) {
-            int i_U = j - i;        // U行列の逆行列は対角成分から上へ向かって
-                                    // 左から順番に値を計算する
-
-            int j_L = col - 1 - j;  // L行列の逆行列は右から順番に
-            int i_L = j_L + i;      // 対角成分から下へ向かって計算する
-
-            if (i_U != j) { // 非対角成分
-                float temp_U = 0.0f;
-                float temp_L = 0.0f;
-
-                for (int k = 0; k < i; k++) {
-
-                    temp_U -= U_inv.components[(j - k) * col + j] * LU.components[i_U * col + (j - k)];
-                    
-                    if (k == 0) {
-                        temp_L -= LU.components[i_L * col + j_L];
-                    } else {
-                        temp_L -= L_inv.components[(j_L + k) * col + j_L] * LU.components[i_L * col + j_L + k];
-                    }
-                    
-                }
-
-                U_inv.components[i_U * col + j] = temp_U / LU.components[i_U * col + i_U];
-                L_inv.components[i_L * col + j_L] = temp_L;
-
-            } else {    // 対角成分
-                if (fabs(LU.components[i_U * col + i_U]) >= NEARLY_ZERO) {
-                    U_inv.components[i_U * col + i_U] = 1.0f / LU.components[i_U * col + i_U];
-                }
-            }
-        }
-    }
-
-    invm = U_inv * L_inv * P;
-
-    return -1.0f;
-}
-
-Matrix Matrix::Transpose() const{
-    //if (row != col) error("failed to get Trans. : matrix is not square");
-    Matrix retVal(col, row);
-
-    for (int i = 0; i < row; i++) {
-        for (int j = 0; j < col; j++) {
-            retVal.components[j * row + i] = this->components[i * col + j];        
-        }
-    }
-
-    return retVal;
-}
-
-Matrix operator+(const Matrix& lhm, const Matrix& rhm) {
-    Matrix temp = Matrix(lhm);
-    temp += rhm;
-    return temp;
-}
-
-Matrix operator-(const Matrix& lhm, const Matrix& rhm) {
-    Matrix temp = Matrix(lhm);
-    temp -= rhm;
-    return temp;
-}
-
-Matrix operator*(const Matrix& lhm, const Matrix& rhm) {
-    if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention.");
-    int row = lhm.GetRow();
-    int col = rhm.GetCol();
-    int sum = lhm.GetCol();
-    Matrix temp(row, col);
-    
-    for (int i = 1; i <= row; i++) {
-        for (int j = 1; j <= col; j++) {
-            float temp_c = 0.0f;
-            for (int k = 1; k <= sum; k++) {
-                temp_c += lhm.GetComp(i, k) * rhm.GetComp(k, j);
-            }
-            temp.SetComp(i, j, temp_c);
-        }
-    }
-    
-    return temp;
-}
-
-void Matrix::CleanUp() {
-    int num = row*col;
-    float maxComp = 0.0f;
-    for (int i = 0; i < num; i++) {
-        if (maxComp < fabs(components[i])) maxComp = fabs(components[i]);
-    }
-    if (maxComp > NEARLY_ZERO) {
-        for (int i = 0; i < num; i++) {
-            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
-        }
-    }
-}
-
-void Matrix::SwapRow(int rowNo1, int rowNo2) {
-    if (rowNo1 > row || rowNo2 > row) error("Index Out of Bounds Error !!");
-    float* temp = new float[col];
-
-    memcpy(temp, components + (rowNo1 - 1) * col, sizeof(float) * col);
-    memcpy(components + (rowNo1 - 1) * col, components + (rowNo2 - 1) * col, sizeof(float) * col);
-    memcpy(components + (rowNo2 - 1) * col, temp, sizeof(float) * col);
-
-    delete[] temp;
-}
-
-void Matrix::SwapCol(int colNo1, int colNo2) {
-    if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!");
-    float temp = 0.0f;
-
-    for (int i = 0; i < row; i++) {
-        temp = components[i * col + colNo1];
-        components[i * col + colNo1] = components[i * col + colNo2];
-        components[i * col + colNo2] = temp;
-    }
-}
\ No newline at end of file

--- a/Matrix/Matrix.h	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,115 +0,0 @@
-#pragma once
-#include "mbed.h"
-
-class Matrix
-{
-public:
-    /********** コンストラクタ　デストラクタ **********/
-    Matrix(int row, int col);
-    Matrix(int row, int col, float* comps);
-    ~Matrix();
-    Matrix(const Matrix& m);
-
-    /********** メンバ演算子 **********/
-    Matrix operator-() const;
-    Matrix& operator=(const Matrix& m);
-    Matrix& operator+=(const Matrix& m);
-    Matrix& operator-=(const Matrix& m);
-    //Matrix& operator*=(const Matrix& m);
-    Matrix& operator*=(float c);
-    Matrix& operator/=(float c);
-
-    /********** その他関数 **********/
-    /*
-        行列の成分を設定
-        引数：rowNo 行番号
-              colNo 列番号
-              val 設定値
-    */
-    void SetComp(int rowNo, int colNo, float val);
-
-    /*
-        行列の成分を全て設定。全成分を一度に指定する必要がある。
-        引数：pComps 設定値の入ったfloat配列。
-    */
-    void SetComps(float* pComps);
-    /*
-        行列式を計算する。行列が正方行列で無い場合にはエラー。
-    */
-    float Determinant() const;
-    
-    /*
-        行列式を計算する。行列が正方行列で無い場合にはエラー。
-    */
-    float det() const;
-
-    /*
-        行列をLU分解する
-        引数：sign (省略可)置換操作の符号を格納するポインタ
-             p (省略可)置換行列を格納する行列のポインタ。（分解する行列と同じ列数の正方行列)
-        返り値：LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
-    */
-    Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
-    
-    /*
-        逆行列を生成する
-        返り値で逆行列が存在するか否かを判断
-        引数：逆行列を格納する行列
-        返り値：逆行列が存在するか否か
-    */
-    float Inverse(Matrix& invm) const;
-    
-    /*
-        転置行列を生成する
-        返り値：転置行列
-    */
-    Matrix Transpose() const;
-    
-    /*
-        行列の行の入れ替えを行う
-        引数：rowNo1　行番号１
-             rowNo2　行番号２
-    */
-    void SwapRow(int rowNo1, int rowNo2);
-    /*
-        行列の列の入れ替えを行う
-        引数：colNo1　列番号１
-             colNo2　列番号２
-    */
-    void SwapCol(int colNo1, int colNo2);
-    
-    /********** インライン関数 **********/
-    inline int GetRow() const {
-        return row;
-    }
-
-    inline int GetCol() const {
-        return col;
-    }
-
-    inline const float* GetpComponents() const {
-        return (const float*)components;
-    }
-
-    inline float GetComp(int rowNo, int colNo) const {
-        if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!");
-        return components[(rowNo-1)*col + (colNo-1)];
-    }
-
-private:
-    int row;
-    int col;
-    float* components;
-
-    /*
-        行列の成分の中で無視できるほど小さい値を0と置き換える（掃除する）
-    */
-    void CleanUp();
-
-};
-
-// グローバル演算子
-Matrix operator+(const Matrix& lhm, const Matrix& rhm);
-Matrix operator-(const Matrix& lhm, const Matrix& rhm);
-Matrix operator*(const Matrix& lhm, const Matrix& rhm);
-

--- a/Vector/Vector.cpp	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,178 +0,0 @@
-#include "myConstants.h"
-#include "Vector.h"
-
-
-Vector::Vector(int dim) : dim(dim), components(0){
-    components = new float[dim];
-    if (!components) error("Memory Allocation Error");
-    for(int i=0; i<dim; i++) components[i] = 0.0f;
-}
-
-
-Vector::~Vector() {
-    delete[] components;
-}
-
-Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
-    components = new float[dim];
-    if (!components) error("Memory Allocation Error");
-    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
-}
-
-Vector& Vector::operator=(const Vector& v) {
-    if (this == &v) return *this;
-    dim = v.dim;
-    delete[] components;
-    components = new float[dim];
-    if (!components) error("Memory Allocation Error");
-    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
-
-    return *this;
-}
-
-Vector Vector::operator+() {
-    return *this;
-}
-
-Vector Vector::operator-() {
-    Vector retVec(*this);
-    retVec *= -1;
-    return retVec;
-}
-
-Vector& Vector::operator*=(float c) {
-    for (int i = 0; i < dim; i++) {
-        components[i] *= c;
-    }
-
-    return *this;
-}
-
-Vector& Vector::operator/=(float c) {
-    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
-    for (int i = 0; i < dim; i++) {
-        components[i] /= c;
-    }
-
-    return *this;
-}
-
-Vector& Vector::operator+=(const Vector& v) {
-    if (dim != v.dim) error("failed to add: Irregular Dimention");
-    for (int i = 0; i < dim; i++) {
-        components[i] += v.components[i];
-    }
-
-    this->CleanUp();
-
-    return *this;
-}
-
-Vector& Vector::operator-=(const Vector& v) {
-    if (dim != v.dim) error("failed to subtract: Irregular Dimention");
-    for (int i = 0; i < dim; i++) {
-        components[i] -= v.components[i];
-    }
-
-    this->CleanUp();
-
-    return *this;
-}
-
-void Vector::SetComp(int dimNo, float val) {
-    if (dimNo > dim) error("Index Out of Bounds Error");
-    components[dimNo-1] = val;
-}
-
-void Vector::SetComps(float* pComps) {
-    memcpy(components, pComps, sizeof(float) * dim);
-}
-
-float Vector::GetNorm() const {
-    float norm = 0.0f;
-    for (int i = 0; i < dim; i++) {
-        norm += components[i] * components[i];
-    }
-    return sqrt(norm);
-}
-
-//以下ではclaen up関数は無効化してある。
-Vector Vector::Normalize() const {
-    float norm = GetNorm();
-    Vector temp(*this);
-    for (int i = 0; i < dim; i++) {
-        temp.components[i] /= norm;
-    }
-    //temp.CleanUp();
-    return temp;
-}
-
-Vector Vector::GetParaCompTo(Vector v) {
-    Vector norm_v = v.Normalize();
-    return (*this * norm_v) * norm_v;
-}
-
-Vector Vector::GetPerpCompTo(Vector v) {
-    return (*this - this->GetParaCompTo(v));
-}
-
-void Vector::CleanUp() {
-    float maxComp = 0.0f;
-    for (int i = 0; i < dim; i++) {
-        if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
-    }
-    if (maxComp > NEARLY_ZERO) {
-        for (int i = 0; i < dim; i++) {
-            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
-        }
-    }
-}
-
-Vector operator+(const Vector& lhv, const Vector& rhv) {
-    Vector retVec(lhv);
-    retVec += rhv;
-    return retVec;
-}
-
-Vector operator-(const Vector& lhv, const Vector& rhv) {
-    Vector retVec(lhv);
-    retVec -= rhv;
-    return retVec;
-}
-
-Vector Cross(const Vector& lhv, const Vector& rhv) {
-    if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
-    if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");
-
-    Vector retVec(lhv.GetDim());
-
-    for (int i = 0; i < lhv.GetDim(); i++) {
-        retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
-            - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
-    }
-
-    return retVec;
-}
-
-Vector operator*(const float c, const Vector& rhv) {
-    Vector retVec(rhv);
-    retVec *= c;
-    return retVec;
-}
-
-Vector operator*(const Vector& lhv, const float c) {
-    Vector retVec(lhv);
-    retVec *= c;
-    return retVec;
-}
-
-float operator*(const Vector& lhv, const Vector& rhv) {
-    if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
-    float retVal = 0.0f;
-
-    for (int i = 1; i <= lhv.GetDim(); i++) {
-        retVal += lhv.GetComp(i) * rhv.GetComp(i);
-    }
-
-    return retVal;
-}

--- a/Vector/Vector.h	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#pragma once
-#include "mbed.h"
-
-class Matrix;
-
-class Vector {
-public:
-    Vector(int dim);
-    ~Vector();
-    Vector(const Vector& v);
-
-    Vector& operator=(const Vector& v);
-    Vector operator+();
-    Vector operator-();
-    Vector& operator*=(float c);
-    Vector& operator/=(float c);
-    Vector& operator+=(const Vector& v);
-    Vector& operator-=(const Vector& v);
-
-    void SetComp(int dimNo, float val);
-    void SetComps(float* vals);
-    float GetNorm() const;
-    Vector Normalize() const;
-    Vector GetParaCompTo(Vector v);
-    Vector GetPerpCompTo(Vector v);
-
-    inline int GetDim() const {
-        return dim;
-    }
-
-    inline const float* GetpComponents() const {
-        return (const float*)components;
-    }
-
-    inline float GetComp(int dimNo) const {
-        if (dimNo > dim) error("Index Out of Bounds Error !!");
-        return components[dimNo-1];
-    }
-
-    void CleanUp();
-
-private:
-    int dim;
-    float* components;
-
-    Vector& operator*=(const Matrix& m);
-    Vector& operator*=(const Vector& m);
-};
-
-Vector operator+(const Vector& lhv, const Vector& rhv);
-Vector operator-(const Vector& lhv, const Vector& rhv);
-Vector Cross(const Vector& lhv, const Vector& rhv);
-Vector operator*(const float c, const Vector& rhv);
-Vector operator*(const Vector& lhv, const float c);
-float operator*(const Vector& lhv, const Vector& rhv);
-

--- a/Vector/Vector_Matrix_operator.cpp	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,35 +0,0 @@
-#include "Vector_Matrix_operator.h"
-
-Vector operator*(const Matrix& lhm, const Vector& rhv) {
-    if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention");
-    Vector retVec(lhm.GetRow());
-
-    for (int i = 1; i <= lhm.GetRow(); i++) {
-        float temp = 0.0f;
-        for (int j = 1; j <= rhv.GetDim(); j++) {
-            temp += lhm.GetComp(i, j)*rhv.GetComp(j);
-        }
-        retVec.SetComp(i, temp);
-    }
-
-    retVec.CleanUp();
-
-    return retVec;
-}
-
-Vector operator*(const Vector& lhv, const Matrix& rhm) {
-    if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention");
-    Vector retVec(rhm.GetCol());
-
-    for (int i = 1; i <= rhm.GetCol(); i++) {
-        float temp = 0.0f;
-        for (int j = 1; j <= lhv.GetDim(); j++) {
-            temp += lhv.GetComp(j) * rhm.GetComp(j, i);
-        }
-        retVec.SetComp(i, temp);
-    }
-
-    retVec.CleanUp();
-
-    return retVec;
-}
\ No newline at end of file

--- a/Vector/Vector_Matrix_operator.h	Wed Jul 24 12:00:01 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,5 +0,0 @@
-#include "Matrix.h"
-#include "Vector.h"
-
-Vector operator*(const Matrix& lhm, const Vector& rhv);
-Vector operator*(const Vector& lhv, const Matrix& rhm);
\ No newline at end of file

--- a/main.cpp	Wed Jul 24 12:00:01 2019 +0000
+++ b/main.cpp	Mon Aug 19 02:50:13 2019 +0000
@@ -1,104 +1,84 @@
+/*
+This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos. 
+*/
+
 #include "mbed.h"
+#include "string.h"
+#include "MPU6050.h"
+
 #include <stdio.h>
 #include <errno.h>
-#include "MPU6050.h"
-#include "HMC5883L.h"
 
-#include "Vector.h"
-#include "Matrix.h"
-#include "Vector_Matrix_operator.h"
-
-// Block devices
-//#include "SPIFBlockDevice.h"
-//#include "DataFlashBlockDevice.h"
 #include "SDBlockDevice.h"
-//#include "HeapBlockDevice.h"
-
-// File systems
-
-//#include "LittleFileSystem.h"
 #include "FATFileSystem.h"
 
+#include "myConstants.h"
+
+#define READBUFFERSIZE  70
+#define DELIMITER   ","
 #define M_PI 3.141592
 #define PI2  (2*M_PI)
 
-/*地磁気センサの補正値（足し合わせる）*/
-#define MAG_FIX_X 338
-#define MAG_FIX_Y 20
-#define MAG_FIX_Z -50
-
 /*ジャイロセンサの補正値（引く）*/
 #define GYRO_FIX_X 290.5498
 #define GYRO_FIX_Y 99.29363
 #define GYRO_FIX_Z 61.41011
 
-// File system declaration
-FATFileSystem   fileSystem("fs");
+/*---------ピン指定-------------------*/
+DigitalOut myled(LED1);
+
+DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる
+
+InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る（立ち下がり割り込み）
 
-/*-----割り込み--------*/
-Ticker flipper;
-/*--------------------*/
-/*-----タイマー---------*/
-Timer passed_time;
-/*---------------------*/
-/*-------ピン指定------------*/
+InterruptIn input_from_Aileron_1(p11);
+InterruptIn input_from_elevator(p12);//
+InterruptIn input_from_throttle(p13);//
+InterruptIn input_from_rudder(p14);//
+InterruptIn input_from_gear(p15);//
+InterruptIn input_from_Aileron_2(p16);//
+InterruptIn input_from_pitch(p17);//
+InterruptIn input_from_AUX5(p18);//
+
+
+MPU6050 mpu(p28, p27);//sda scl
+
+Serial pc(USBTX, USBRX);    // tx, rx
+Serial gps(p9,p10);// gpsとの接続 tx, rx 
+SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs
+
 //declare PWM pins here
 PwmOut ESC (p21);
 PwmOut Elevator_servo(p22);
 PwmOut Rudder_servo(p23);
+PwmOut Aileron_R_servo(p25);
+PwmOut Aileron_L_servo(p26);
+
+/*-----------------------------------*/
+
+/*-----ファイルポインタ-----*/
+// File system declaration
+FATFileSystem   fileSystem("fs");
+/*-----------------------*/
+
+/*------------------------GPSの変数------------------------*/
+int GPS_flag_update=0;
+int GPS_interruptIn=0;
+
+float longtude_gps,latitude_gps,azimuth_gps,speed_gps;
+    
+//char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
+char NMEA_sentense[READBUFFERSIZE];
+
+/*--------------------------------------------------------*/
 
 
-SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs
-MPU6050 mpu(p28, p27);//sda scl
-HMC5883L compass(p28, p27);//sda scl
-
-RawSerial gps(p9,p10); //serial port for gps_module
-RawSerial pc(USBTX, USBRX);
-
-/*--------------------------*/
-
-DigitalIn switch_off(p30);
-DigitalOut led2(LED2);
-
-/*-----------グローバル変数-----------*/
-double Euler_angle[3];
-double Euler_angle_Initiated[3];
-
-float g_hokui,g_tokei;
-int fp_count=0;
-int gps_flag=0;
-int gps_flag_conversion=0;
-
-char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
-char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している
-
-unsigned int  tokei_int_receive;
-unsigned int  hokui_int_receive;
-
-float  tokei_float_receive;
-float  hokui_float_receive;
-
-float a[3];//加速度の値
-float g[3];//ジャイロの値[rad/s]
-float dpsX,dpsY,dpsZ;        
-float AccX,AccY,AccZ;
-
-int sensor_array[6];
-int16_t mag[3];
-
-double Roll_Acc,Pitch_Acc;
-double Yaw;
-double Azi_mag;//地磁気からの方位
-double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion
-
-/*----------------------------------*/
-/*--------------------------行列、ベクトル-----------------------------*/
-
-Vector Quaternion_atitude_from_omega(4);
-Matrix Omega_matrix(4,4),Half_matrix(4,4);
-Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);
-
-/*-------------------------------------------------------------------*/
+/*--------------------------角度とセンサ--------------------------*/
+float sensor_array[6];
+float Acc_Pitch_Roll[2];
+float Gyro_Pitch_Roll[2];//pitch and roll
+float omega_z;
+/*--------------------------------------------------------------*/
 
 /*----------PID コントロール-----------------*/
 #define Gain_Kp 0.8377
@@ -116,292 +96,81 @@
 #define UPPER_DUTY  0.0020
 #define LOWER_DUTY  0.0010
 
+#define THRESHOLD 0.0018
+
 double pitch_duty,roll_duty,yaw_duty;
 
+double first_period,second_period,third_period,fourth_period;
+double fifth_period,sixth_period,seventh_period,eighth_period;
+
+double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0;
+double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0;
+
+double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty;
+
 /*------------------------------------------*/
 
-
-/*プロトタイプ宣言*/
-void toString_V(Vector& v);   // ベクトルデバッグ用
-void gps_rx();
-void gps_convertion();
-/*--------------*/
-
-void toString_V(Vector& v)
-{
-
-    for(int i=0; i<v.GetDim(); i++) {
-        pc.printf("%.6f\t", v.GetComp(i+1));
-    }
-    pc.printf("\r\n");
-
-}
-
-void scan_update(int box_sensor[6],int16_t m[3]){
-    
-    int a[3];//加速度の値
-    int g[3];//角速度の値
+/*-----タイマー---------*/
+Timer passed_time;//経過時間の計算
+Timer ch_time;//
 
-    int16_t raw_compass[3];//地磁気センサの値
- 
-    mpu.setAcceleroRange(0);//acceleration range is +-4G
-    mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
-    
-    mpu.getAcceleroRaw(a);//   加速度を格納する配列[m/s2]  a[0] is x axis,a[1] is y axis,a[2] is z axis;
-    mpu.getGyroRaw(g);    //   角速度を格納する配列[rad/s] 
-    compass.getXYZ(raw_compass);//地磁気の値を格納する配列
-    
-    box_sensor[0]=-g[0];
-    box_sensor[1]=g[1];
-    box_sensor[2]=-g[2];
-    
-    box_sensor[3]=a[0];
-    box_sensor[4]=-a[1];
-    box_sensor[5]=a[2];
-    
-    m[0]=(raw_compass[0]);//x
-    m[1]=(raw_compass[2]);//y
-    m[2]=(raw_compass[1]);//z
-
-}
+float Time_new,Time_old=0.0;
+float Servo_command=0.002;
+/*---------------------*/
+/*---------PWM割り込みフラグ-------*/
+int PWM_flag_update=0;
+/*-------------------------------*/
 
-void gps_rx(){
-            
-            if((gps.getc()=='$')&&(gps_flag==0)){
-               for(int i=0;i<9;i++){
-                    gps_data[i]=gps.getc();
-                    //pc.putc(gps_data[i]);
-                    }
-                    
-                gps_data[9]='\0';
-                
-               for(int i=0;i<9;i++){
-                    gps_data_2[i]=gps.getc();
-                    //pc.putc(gps_data[i]);
-                    }
-                    
-                gps_data_2[9]='\0';
-                
-                }//if(twe.getc()==':')
-            
-            //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
-            
-            gps_flag=1;
-            
-    }//gps_rx ends
+/*----------プロトタイプ宣言----------*/
+void gps_rx();
+void gps_read();
 
-void gps_convertion(){
-    while(1){
-        if(gps_flag==1){
-            tokei_int_receive=atoi(gps_data);
-                hokui_int_receive=atoi(gps_data_2);
-                
-                //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
-                
-                tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
-                hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;   
-                
-                gps_flag_conversion=1;
-                
-            }else{gps_flag_conversion=0;}
-        }//while ends
-    
-    }//ends
+double Degree_to_PWM_duty(double degree);
+double PID_duty_pitch(double target,double vol,float dt);
+double PID_duty_rudder(double target,double vol,float dt,float yaw_rate);
+double PID_duty_roll(double target,double vol,float dt);
 
-void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
-    
-    double Roll_before = double(y_acc)/double(z_acc);
-    double Roll = atan(Roll_before);
-             
-    double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
-    double Pitch = -atan(Pitch_before);
-             
-    //Yaw=0.0;
-    
-    /*Quaternionの要素へオイラー角を変換する*/
-    qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
-    qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
-    qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0);
-    qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0);
-    
-             
-    }
-    
-void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
-    
-    double Roll_before = double(y_acc)/double(z_acc);
-    double Roll = atan(Roll_before);
-             
-    double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
-    double Pitch = -atan(Pitch_before);
+void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]);
+void scan_update(float box_sensor[6]);
+void sensor_update();
 
-    
-    qua[0]=Pitch;
-    qua[1]=Roll;
-         
-    }
+//gpsの1pps信号の割り込み
+void flip();
 
-void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
-    
-    //y_acc*=-1.0;
-    
-    n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
-    n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
-    n[2]=0.0;
-    
-    n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
-    
-    }
 
-double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
-    
-    Vector Mag_from_sensor(3),Mag_fixed_sensor(3);                    //座標系
-    Matrix Rotation(3, 3);                        //検算の行列
+void Initialize_ESC();
+void Activate_ESC();
 
-    double Correct_vector_n[4];//回転行列の成分
-    Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
-                                 
-    double n_x=Correct_vector_n[0];
-    double n_y=Correct_vector_n[1];
-    double n_z=Correct_vector_n[2];
-    double theta=-Correct_vector_n[3];
-                                 
-    float Rotate_element[9] = {
-        n_x*n_x*(1-cos(theta))+cos(theta)    , n_x*n_y*(1-cos(theta))       , n_y*sin(theta),  
-        n_x*n_y*(1-cos(theta))                ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta), 
-        n_y*sin(theta)                       , n_x*sin(theta)              , cos(theta)
-    };
-                
-    Rotation.SetComps(Rotate_element);
-                                                //x           y                    z
-    float Geomagnetism[3]={  float(magnet[1]+MAG_FIX_X) 
-                           , float(magnet[0]+MAG_FIX_Y)  
-                           , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
-                           
-    Mag_from_sensor.SetComps(Geomagnetism);
-                
-    Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
-                
-    float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
-    float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
-       
-    double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
-    
-    /*            
-    if(Azi < 0.0) // fix sign
-    Azi += PI2;
-            
-    if(Azi > PI2) // fix overflow
-    Azi -= PI2;
-    */
-    
-    return Azi;
-    
-    }
+void Input_Aileron_1();
+void Input_elevator();
+void Input_throttle();
+void Input_rudder();
+void Input_gear();
+void Input_Aileron_2();
+void Input_pitch();
+void Input_AUX5();
 
-//void led2_thread(void const *argument) {
-void imu_thread() {
-    while (true) {
-    }//while ends
-}
-
-void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){
-    
-    //float pitch,roll,yaw;
-    
-    Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) );
-    Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2)));
-    Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3));
+void Input_Aileron_1_fall();
+void Input_elevator_fall();
+void Input_throttle_fall();
+void Input_rudder_fall();
+void Input_gear_fall();
+void Input_Aileron_2_fall();
+void Input_pitch_fall();
+void Input_AUX5_fall();
+/*---------------------------------*/
 
-    }
-    
 
-double Degree_to_PWM_duty(double degree){
-    
-    double duty=0.0;
-    
-    duty=(0.2/1000)/20.0*degree;
-    
-    if((duty+NUTRAL)>=UPPER_DUTY){
-        duty=UPPER_DUTY-NUTRAL;
-    }else if((duty+NUTRAL)<=LOWER_DUTY){
-        duty=NUTRAL-LOWER_DUTY;
-    }else{}
-    
-    return duty;
+int main() {
     
-    }
-    
-double PID_duty(double target,double vol,float dt){
-            //ここで角度の単位で偏差は入力される
-            //出力はPWMで
-            double duty=0.0;
-            double add_duty=0.0;
-            
-            Prop_p=target-vol;
-            
-            pc.printf("%f/r/n",Prop_p);
-            
-            Int_p+=Prop_p*double(dt);
-            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
-            
-            Pre_Prop_p=Prop_p;
-            
-            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
-            
-            add_duty=1.0*Prop_p;
-            
-            duty+=add_duty;
-            duty=Degree_to_PWM_duty(duty);
-            
-            return duty;
-            
-}
-
-void Initialize_ESC(){
-    
-    ESC.pulsewidth(0.002);
     wait(3);
     
-    ESC.pulsewidth(0.001);
-    wait(5);
     
-    }
+    /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
     
     
-void Activate_ESC(){
     
-    ESC.pulsewidth(0.001);
-    wait(5);    
-    
-    }
-
-// Entry point for the example
-int main()
-{
-    gps.baud(115200);//GT-720Fボーレート
-    pc.baud(115200);
-    //imu_thread
-    
-    compass.init();//hmc5883の起動
-    
-    Thread thread1 (gps_convertion);
-    gps.attach(gps_rx, Serial::RxIrq);
-    
-        /*define period of serve and ESC*/
-    ESC.period(SERVO_PERIOD);    
-    Elevator_servo.period(SERVO_PERIOD);
-    Rudder_servo.period(SERVO_PERIOD);
-    /*------------------------------*/
-    Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
-    Rudder_servo.pulsewidth(NUTRAL);
-    
-    Activate_ESC();
-    
-    float Time_old=0.0;//時間初期化
-    passed_time.start();//タイマー開始
-    
-    pc.printf("--- Mbed OS filesystem example ---\n");
+       pc.printf("--- Mbed OS filesystem example ---\n");
 
     // Try to mount the filesystem
     pc.printf("Mounting the filesystem... ");
@@ -422,215 +191,539 @@
     }
 
     // Open the numbers file
-    pc.printf("Opening \"/fs/numbers.txt\"... ");
+    pc.printf("Opening \"/fs/log.csv\"... ");
     fflush(stdout);
 
-    FILE*   f = fopen("/fs/numbers.txt", "r+");
+    FILE*   f = fopen("/fs/log.csv", "r+");
+    FILE*   fp = fopen("/fs/log.txt", "r+");
+    
     pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
     
     if (!f) {
         // Create the numbers file if it doesn't exist
         pc.printf("No file found, creating a new file... ");
         fflush(stdout);
-        f = fopen("/fs/numbers.txt", "w+");
+        f = fopen("/fs/log.csv", "w+");
+        fp = fopen("/fs/log.txt", "w+");
+        
         pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
             }
-    
-    /*初期姿勢のQuaternionの設定*/
-    scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x  mag[2]=-Z
-    Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
-    
-    Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
+            
     
-    float Initialize_atitude[4];
-    Initialize_atitude[0]=Quaternion_from_acc[0];
-    Initialize_atitude[1]=Quaternion_from_acc[1];
-    Initialize_atitude[2]=Quaternion_from_acc[2];
-    Initialize_atitude[3]=Quaternion_from_acc[3];
-    
-    Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
     
     
-    while(1){
- 
-            /*gpsから来た文字列の処理
-            if((gps_flag==1)&&(gps_flag_conversion==1)){
-                
-                err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
-                 
-                gps_flag=0;
-                gps_flag_conversion=0;
-                
-                }else{}
-            */
-                
-            scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x  mag[2]=-Z
-            float Time_new=float(passed_time.read());//時間の取得
-            
-            //地磁気の補正用に使う。
-            //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
-            
-            //ジャイロの補正用に使う
-            //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
+    /*define period of serve and ESC*/
+    ESC.period(SERVO_PERIOD);    
+    Elevator_servo.period(SERVO_PERIOD);
+    Rudder_servo.period(SERVO_PERIOD);
+    Aileron_R_servo.period(SERVO_PERIOD);
+    Aileron_L_servo.period(SERVO_PERIOD);
+    /*------------------------------*/
+    Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
+    Rudder_servo.pulsewidth(NUTRAL);
+    Aileron_R_servo.pulsewidth(NUTRAL);
+    Aileron_L_servo.pulsewidth(NUTRAL);
+    
+    Activate_ESC();//Activate ESC
+    
+    /*--------------ピン変化割り込みに対応した関数の宣言--------------*/
+    button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。
+
+    input_from_throttle.rise(&Input_throttle);//
+
+    input_from_Aileron_1.fall(&Input_Aileron_1_fall);
+    input_from_elevator.fall(&Input_elevator_fall);//
+    input_from_throttle.fall(&Input_throttle_fall);//
+    input_from_rudder.fall(&Input_rudder_fall);//
+    input_from_gear.fall(&Input_gear_fall);
+    input_from_Aileron_2.fall(&Input_Aileron_2_fall);//
+    input_from_pitch.fall(&Input_pitch_fall);//
+    input_from_AUX5.fall(&Input_AUX5_fall);//    
+    
+    ch_time.start();//時間計測スタート
+    /*----------------------------------------------------------*/
+    
+    pc.baud(115200);
+    gps.baud(115200);
+    
+    //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
+    //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み
+    
+    passed_time.start();//タイマー開始
+    Time_old=float(passed_time.read());
+    
+    /*初期姿勢角の算出*/
+    scan_update(sensor_array);//角速度、加速度を求める
+    Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
+    Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch
+    Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll
+    
+    pc.printf("Start!!\r\n");
+    
+    while(1) {
+       
+        switch (GPS_interruptIn) {
+        case 1:
+           
+           gps_rx();
+           gps_read();    
+           GPS_interruptIn--;
+            break;
             
-            /*----------------ジャイロセンサからQuaternionを求める----------------*/
-            float omega_x=float((  float(sensor_array[0])-GYRO_FIX_X  )/ 7505.7);
-            float omega_y=float((  float(sensor_array[1])-GYRO_FIX_Y  )/ 7505.7);
-            float omega_z=float((  float(sensor_array[2])-GYRO_FIX_Z  )/ 7505.7);
-            
-            float omega[16] = {
+        default:
             
-            0.0         , -omega_x , -omega_y, -omega_z,
-            omega_x     ,0.0       ,omega_z  ,-omega_y ,
-            omega_y     , -omega_z , 0.0     , omega_x , 
-            omega_z     ,  omega_y ,-omega_x ,   0.0    
-                
-                 };
-
-            Omega_matrix.SetComps(omega);
+           sensor_update();       
+            break;
+        }//switch end
+        
+        /*
+        ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。
+        以下のpitch control等は自動操縦とみなしてよい。
+        */
+        
+        
+        if(switch_duty>THRESHOLD){//オートパイロット
             
-            float half[16] ={
-                (Time_new-Time_old)*0.5,0.0,0.0,0.0,
-                0.0,(Time_new-Time_old)*0.5,0.0,0.0,
-                0.0,0.0,(Time_new-Time_old)*0.5,0.0,
-                0.0,0.0,0.0,(Time_new-Time_old)*0.5
-                };
-                
+            //Pitch control
+            //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
+            elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old));
+            Elevator_servo.pulsewidth(elevator_duty); 
             
-            Half_matrix.SetComps(half);
-            //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
-            Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
+            //roll control
+            //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old))
+            aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old));
             
-            Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
+            //角速度だけフィードバック    
+            rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z);
+            ///pc.printf("%f\r\n",rudder_duty);
             
             
-            //Time_old=Time_new;//時間の更新
-            
-            /*
-            pc.printf("%f,%f,%f,%f\r\n"
-                ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
-                ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
-            */
-            
-            /*----------------------------------------------------------------*/
-            
-            /*
-            err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
-                ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
-                    ,sensor_array[6],sensor_array[7],sensor_array[8]);
-            */
-            
-            /*--------------------加速センサで地磁気の値を補正する --------------------*/
-            
-            if(9.8065*1.1  >  
-            sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
-                        / 16384.0 * 9.81)
-            {
+            //スロットルはマニュアル
+            //Servo_command
+            if( (float(passed_time.read())-Servo_command)>0.003 ){
                 
-                Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
-                    
-                //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);                
-                
-                double pitch_and_roll[2];
-                Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
-                
-                //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG);
+                ESC.pulsewidth(throttle_duty);            
+                Elevator_servo.pulsewidth(elevator_duty);
+                Rudder_servo.pulsewidth(rudder_duty); 
+                Aileron_R_servo.pulsewidth(aileron_duty);
+                Aileron_L_servo.pulsewidth(aileron_duty);
                 
-                Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
-                
-                /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
-                float Quaternion_from_acc_fixed[4];
-                Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
-                Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
-                Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
-                Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
-                 
-                Vector Qua_acc(4);
-                 
-                Qua_acc.SetComps(Quaternion_from_acc_fixed);                
-                /*---------------------------------------------*/
-                
-                /*---------------------相補フィルタのゲインに用いる---------------------*/
-                float comp_1[16]={0.95,0.0,0.0,0.0,
-                                 0.0,0.95,0.0,0.0,
-                                 0.0,0.0,0.95,0.0,
-                                 0.0,0.0,0.0,0.95
-                                };
-                /*--------------------------------------------------------------------*/
-                /*---------------------quaternionの時間微分に用いる---------------------*/
-                float comp_2[16]={0.05,0.0,0.0,0.0,
-                                 0.0,0.05,0.0,0.0,
-                                 0.0,0.0,0.05,0.0,
-                                 0.0,0.0,0.0,0.05
-                                };
-                                
-                Complement_matrix_1.SetComps(comp_1);
-                Complement_matrix_2.SetComps(comp_2);
-                
-                /*--------------------------------------------------------------------*/
+                Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
+                }else{}
+            
+            }else{//マニュアルモード
+            
+            if( (float(passed_time.read())-Servo_command)>0.003 ){
+                            
+                            ESC.pulsewidth(throttle_duty);
+                            Elevator_servo.pulsewidth(elevator_duty); 
+                            Rudder_servo.pulsewidth(rudder_duty);
+                            Aileron_R_servo.pulsewidth(aileron_duty);
+                            Aileron_L_servo.pulsewidth(aileron_duty);
+                            
+                            Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新
+                            
+                            }else{}
                 
-                                
-                /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
-                
-                Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
-                                    +Complement_matrix_2*Qua_acc;
-                
-                /*----------------------------------------------------------------------------*/
-                
-                
-                /*
-                pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
-                     ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
-                     ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
-                     ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
-                     ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
-                */
-                     
-            }else{}
+                }
+        
+        pc.printf("%f,%f,%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps,latitude_gps,longtude_gps);
+        err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps);
+        err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps);
+                    
+        Time_old=Time_new;//時間更新
+        
+        if(switch_off==1){
+            //fclose(fp);
+            //flipper.detach();
+            
+            err = fclose(f);
+            err = fclose(fp);
+            
+            pc.printf("Writing finish!");
+            myled=0;
+            break;
+                    
+        }
+                    
+        //wait(0.002);//この値は10倍されると考える
+        }//while ends
+    
+}//main ends
+
+void gps_rx(){
+       
+      // __disable_irq(); // 割り込み禁止
+       
+    int i=0;
+          
+      while(gps.getc()!='$'){
+      }
+      while( (NMEA_sentense[i]=gps.getc()) != '\r'){
+      //while( (NMEA_sentense[i]=pc.getc()) != '\r'){
+        //pc.putc(NMEA_sentense[i]);
+        i++;
+        if(i==READBUFFERSIZE){  
+           i=(READBUFFERSIZE-1);
+           break;
+         }
+      }
+      NMEA_sentense[i]='\0';
+      //pc.printf("\r\n");
+      
+      if(GPS_flag_update==0){
+              GPS_flag_update++;
+            }
+            
+    //__enable_irq(); // 割り込み許可
+    
+}//void gps_rx
+
+void gps_read(){
+    /*------gps のNMEAフォーマットの処理プログラム----------------*/
+        float temp=0.0, deg=0.0, min=0.0;
             
+        char *pszTime;//UTC時刻
+        char* pszLat;////緯度
+        char* pszLong;//経度
+        char* pszSp;//速度
+        char* pszSpd;//速度方向
+        char* pszDate;//日付
+        
+        //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない
+        //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
+        
+        //Time_old=float(passed_time.read());
+       
+       
+        
+        if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい
+        //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい
+        
+                   strtok( NMEA_sentense, DELIMITER );  // $GPRMC
+                   pszTime = strtok( NULL, DELIMITER );  // UTC時刻
+                   strtok( NULL, DELIMITER );  // ステータス
+                   pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm)
+                   strtok( NULL, DELIMITER );  // 北緯か南緯か
+                   pszLong = strtok( NULL, DELIMITER );  // 経度(dddmm.mmmm)
+                   strtok( NULL, DELIMITER );  // 東経か西経か
+                   
+                   pszSp = strtok( NULL, DELIMITER );  // 速度（vvv.v）
+                   pszSpd = strtok( NULL, DELIMITER );  // 速度方向（ddd.d）
+                   pszDate = strtok( NULL, DELIMITER );  // 日付
+                   
+                    if( NULL != pszLong ){//pszLongがnullでないのならば
+                          temp = atof(pszLat);
+                          deg = (int)(temp/100);
+                          min = temp - deg * 100;
+                          latitude_gps = deg + min / 60;//latitudeの算出
+                          
+                          temp = atof(pszLong);
+                          deg = (int)(temp/100);
+                          min = temp - deg * 100;
+                          longtude_gps = deg + min / 60;//longtudeの算出
+                          
+                          speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC;
+                          
+                          azimuth_gps=atof(pszSpd);
+                          
+                          GPS_flag_update--;
+                          
+                          //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps);
+                          
+                      }else{}//end of ( NULL != pszLong ) 
+                      
+         }else{}//end of  if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){
+    }
+
+
+void sensor_update(){
+           scan_update(sensor_array);//角速度、加速度を求める
+       
+       Time_new=float(passed_time.read());//時間の更新
+       
+       Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch
+       Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll
+       
+       
+       /*--------------------加速センサでジャイロの値を補正する --------------------*/
+            
+          
+            
+       if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])))
+                  {
+                            Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll);
+                            Gyro_Pitch_Roll[0]*=0.95;//
+                            Gyro_Pitch_Roll[1]*=0.95;//                         
+                            Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);//
+                            Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);//
+                            
+                            
+                    }else{}
+       
+      //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG);
+    //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]);
+
+       //Time_old=Time_new;
+       
+    }
+double Degree_to_PWM_duty(double degree){
+    
+    double duty=0.0;
+    
+    duty=(0.2/1000)/20.0*degree;
+    
+    if((duty+NUTRAL)>=UPPER_DUTY){
+        duty=UPPER_DUTY-NUTRAL;
+    }else if((duty+NUTRAL)<=LOWER_DUTY){
+        duty=NUTRAL-LOWER_DUTY;
+    }else{}
+    
+    return duty;
+    
+    }
+    
+double PID_duty_pitch(double target,double vol,float dt){
+            //ここで角度の単位で偏差は入力される
+            //出力はPWMで
+            double duty=0.0;
+            double add_duty=0.0;
+            
+            Prop_p=target-vol;
+            
+            //pc.printf("%f/r/n",Prop_p);
+            
+            Int_p+=Prop_p*double(dt);
+            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+            
+            Pre_Prop_p=Prop_p;
+            
+            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
+            
+            add_duty=1.0*Prop_p;
+            
+            duty+=add_duty;
+            duty=Degree_to_PWM_duty(duty);
+            
+            return duty;
+            
+}
+
+double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){
+            //ここで角度の単位で偏差は入力される
+            //出力はPWMで
+            double duty=0.0;
+            double add_duty=0.0;
+            
+            Prop_p=target-vol;
+            
+            //pc.printf("%f/r/n",Prop_p);
+            
+            Int_p+=Prop_p*double(dt);
+            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+            
+            Pre_Prop_p=Prop_p;
+            
+            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
             
-            /*
-            pc.printf("%f,%f,%f,%f,%f\r\n"
-                     ,Time_new
-                     ,Quaternion_atitude_from_omega.GetComp(1)
-                     ,Quaternion_atitude_from_omega.GetComp(2)
-                     ,Quaternion_atitude_from_omega.GetComp(3)
-                     ,Quaternion_atitude_from_omega.GetComp(4));
-            */
-            /*-----------Quaternionからオイラー角への変換-----------*/
-            Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1)
-                                ,Quaternion_atitude_from_omega.GetComp(2)
-                                ,Quaternion_atitude_from_omega.GetComp(3)
-                                ,Quaternion_atitude_from_omega.GetComp(4)
-                                ,Euler_angle);
-                                
-            /*-----------オイラー角の表示----------------------*/
+            //add_duty=1.0*Prop_p;
+            //今回は角速度に反応させてラダーを動かす
+            add_duty=double(yaw_rate)*0.1;
+            
+            duty+=add_duty;
+            duty=Degree_to_PWM_duty(duty);
+            
+            return duty;
+            
+}
+
+double PID_duty_roll(double target,double vol,float dt){
+            //ここで角度の単位で偏差は入力される
+            //出力はPWMで
+            double duty=0.0;
+            double add_duty=0.0;
+            
+            Prop_p=target-vol;
+            
+            //pc.printf("%f/r/n",Prop_p);
+            
+            Int_p+=Prop_p*double(dt);
+            Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
+            
+            Pre_Prop_p=Prop_p;
+            
+            //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
+            
+            add_duty=1.0*Prop_p;
+            
+            duty+=add_duty;
+            duty=Degree_to_PWM_duty(duty);
+            
+            return duty;
             
-            pc.printf("%f,%f,%f,%f,%f\r\n"
-                     ,Time_new
-                     ,Euler_angle[0]
-                     ,Euler_angle[1]
-                     ,Euler_angle[2]
-                     );
-            
-            /*--------------------------------------------------*/
-            //Pitch control
-            pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old));
-            Elevator_servo.pulsewidth(pitch_duty); 
-             
+}
+void Initialize_ESC(){
+    
+    ESC.pulsewidth(0.002);
+    wait(3);
+    
+    ESC.pulsewidth(0.001);
+    wait(5);
+    
+    }
+    
+    
+void Activate_ESC(){
+    
+    ESC.pulsewidth(0.001);
+    wait(5);    
+    
+    }
+
+void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){
+    
+    float Roll_before = (y_acc)/(z_acc);
+    float Roll = atan(Roll_before);
              
-            Time_old=Time_new;//時間の更新
-            
-            wait(0.001);
-            
-            /*----------------------------------------------------------------*/
+    float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) );
+    float Pitch = -atan(Pitch_before);
+    
+    qua[0]=Pitch*RAD_TO_DEG;
+    qua[1]=Roll*RAD_TO_DEG;
+    //float Acc_Pitch_Roll[2];
+    
+    }
+    
+void scan_update(float box_sensor[6]){
+    
+    float a[3];//加速度の値
+    float g[3];//ジャイロの値[rad/s]
+    
+    //int16_t raw_compass[3];//地磁気センサの値
+    
+    mpu.setAcceleroRange(0);//acceleration range is +-4G
+    mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
+    
+    mpu.getAccelero(a);//   加速度を格納する配列[m/s2]  a[0] is x axis,a[1] is y axis,a[2] is z axis;
+    mpu.getGyro(g);//   角速度を格納する配列[rad/s]
+    
+    //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]);
+    
+    //compass.getXYZ(raw_compass);//地磁気の値を格納する配列
+    
+    box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG;
+    box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG;
+    box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG;
+    
+    omega_z=box_sensor[2];
+    
+    box_sensor[3]=a[0];
+    box_sensor[4]=-a[1];
+    box_sensor[5]=a[2];
+    
+    //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]);
+}
+
+void flip(){
+    
+    GPS_interruptIn++;
+    
+};
+
+
+//ch1
+void Input_Aileron_1(){
+    
+    first_period_old=ch_time.read();
+    //pc.printf("ch1=%f",second_period);    
+    };
+
+void Input_Aileron_1_fall(){
+    first_period=ch_time.read();
+    aileron_duty=first_period-first_period_old;
+};
+    
+
+//ch2
+void Input_elevator(){
+    second_period_old=ch_time.read();
+};
 
-            if(switch_off==1){
-                // Close the file which also flushes any cached writes
-                    pc.printf("Closing \"/fs/numbers.txt\"... ");
-                    err = fclose(f);
-                    break;
-                }
-            
-            }//while(1) ends
-}//main ends
+void Input_elevator_fall(){
+    second_period=ch_time.read();    
+    elevator_duty=second_period-second_period_old;
+    };
+        
+    
+//ch3
+void Input_throttle(){
+    
+    first_period_old=ch_time.read();
+    second_period_old=ch_time.read();
+    third_period_old=ch_time.read();
+    fourth_period_old=ch_time.read();  
+    fifth_period_old=ch_time.read();
+    sixth_period_old=ch_time.read();
+    
+    
+}
+
+void Input_throttle_fall(){
+    third_period=ch_time.read();
+    
+    /*デコード*/
+    throttle_duty=third_period-third_period_old;
+    //rudder_duty=fourth_period-fourth_period_old;
+    //elevator_duty=second_period-second_period_old;
+    //aileron_duty=first_period-first_period_old;
+    
+    //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty);
+}
+
+//ch4
+void Input_rudder(){
+    fourth_period_old=ch_time.read();  
+}
+
+void Input_rudder_fall(){
+    fourth_period=ch_time.read();
+    rudder_duty=fourth_period-fourth_period_old;  
+}
+
+//ch5
+void Input_gear(){
+    fifth_period_old=ch_time.read();
+
+}
+
+void Input_gear_fall(){
+    fifth_period=ch_time.read();
+    switch_duty=fifth_period-fifth_period_old;
+}
+
+//ch6
+void Input_Aileron_2(){
+    sixth_period_old=ch_time.read();
+    }
+    
+void Input_Aileron_2_fall(){
+    sixth_period=ch_time.read();
+    };
+    
+//ch7
+void Input_pitch(){
+    seventh_period_old=ch_time.read();
+}
+
+void Input_pitch_fall(){
+    seventh_period=ch_time.read();
+    }
+        
+//ch8
+void Input_AUX5(){
+    eighth_period_old=ch_time.read();
+}
+
+void Input_AUX5_fall(){
+    eighth_period=ch_time.read();
+}

--- a/myConstants.h	Wed Jul 24 12:00:01 2019 +0000
+++ b/myConstants.h	Mon Aug 19 02:50:13 2019 +0000
@@ -34,4 +34,7 @@
 #define MAG_DECLINATION     7.5f                    // declination (deg)
 
 /* ADC */
-#define ADC_LSB_TO_V        0.000050354f            // 3.3(V)/65535(LSB)
\ No newline at end of file
+#define ADC_LSB_TO_V        0.000050354f            // 3.3(V)/65535(LSB)
+
+/*Speed convertion*/
+#define KNOT_TO_METER_PER_SEC 0.51444f
\ No newline at end of file