Atsumi Toda
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
--- 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