Atsumi Toda
/
UAV_Logger1
UAVの姿勢推定に使用するプログラム。
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
diff -r 3fa7882a5fd0 -r 21a356ae0747 HMC5883L.lib --- 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
diff -r 3fa7882a5fd0 -r 21a356ae0747 Matrix/Matrix.cpp --- 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
diff -r 3fa7882a5fd0 -r 21a356ae0747 Matrix/Matrix.h --- 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 行番号1 - rowNo2 行番号2 - */ - void SwapRow(int rowNo1, int rowNo2); - /* - 行列の列の入れ替えを行う - 引数:colNo1 列番号1 - colNo2 列番号2 - */ - 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); -
diff -r 3fa7882a5fd0 -r 21a356ae0747 Vector/Vector.cpp --- 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; -}
diff -r 3fa7882a5fd0 -r 21a356ae0747 Vector/Vector.h --- 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); -
diff -r 3fa7882a5fd0 -r 21a356ae0747 Vector/Vector_Matrix_operator.cpp --- 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
diff -r 3fa7882a5fd0 -r 21a356ae0747 Vector/Vector_Matrix_operator.h --- 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
diff -r 3fa7882a5fd0 -r 21a356ae0747 main.cpp --- 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);//9軸の値の取得 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);//9軸の値の取得 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(); +}
diff -r 3fa7882a5fd0 -r 21a356ae0747 myConstants.h --- 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