Atsumi Toda / Mbed 2 deprecated Auto_pilot_prototype_2

ドローン用計測制御基板の作り方vol.2で使用したピッチ制御プログラムです。

Dependencies:   mbed MPU6050_alter SDFileSystem

Matrix/Matrix.h@0:e647f6de3d26, 2020-03-06 (annotated)

Committer:
Joeatsumi
Date:
Fri Mar 06 15:03:57 2020 +0000
Revision:
0:e647f6de3d26
An software for autopilot(ver2)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:e647f6de3d26 1 #pragma once
Joeatsumi 0:e647f6de3d26 2 #include "mbed.h"
Joeatsumi 0:e647f6de3d26 3
Joeatsumi 0:e647f6de3d26 4 class Matrix
Joeatsumi 0:e647f6de3d26 5 {
Joeatsumi 0:e647f6de3d26 6 public:
Joeatsumi 0:e647f6de3d26 7 /********** コンストラクタ　デストラクタ **********/
Joeatsumi 0:e647f6de3d26 8 Matrix(int row, int col);
Joeatsumi 0:e647f6de3d26 9 Matrix(int row, int col, float* comps);
Joeatsumi 0:e647f6de3d26 10 ~Matrix();
Joeatsumi 0:e647f6de3d26 11 Matrix(const Matrix& m);
Joeatsumi 0:e647f6de3d26 12
Joeatsumi 0:e647f6de3d26 13 /********** メンバ演算子 **********/
Joeatsumi 0:e647f6de3d26 14 Matrix operator-() const;
Joeatsumi 0:e647f6de3d26 15 Matrix& operator=(const Matrix& m);
Joeatsumi 0:e647f6de3d26 16 Matrix& operator+=(const Matrix& m);
Joeatsumi 0:e647f6de3d26 17 Matrix& operator-=(const Matrix& m);
Joeatsumi 0:e647f6de3d26 18 //Matrix& operator*=(const Matrix& m);
Joeatsumi 0:e647f6de3d26 19 Matrix& operator*=(float c);
Joeatsumi 0:e647f6de3d26 20 Matrix& operator/=(float c);
Joeatsumi 0:e647f6de3d26 21
Joeatsumi 0:e647f6de3d26 22 /********** その他関数 **********/
Joeatsumi 0:e647f6de3d26 23 /*
Joeatsumi 0:e647f6de3d26 24 行列の成分を設定
Joeatsumi 0:e647f6de3d26 25 引数：rowNo 行番号
Joeatsumi 0:e647f6de3d26 26 colNo 列番号
Joeatsumi 0:e647f6de3d26 27 val 設定値
Joeatsumi 0:e647f6de3d26 28 */
Joeatsumi 0:e647f6de3d26 29 void SetComp(int rowNo, int colNo, float val);
Joeatsumi 0:e647f6de3d26 30
Joeatsumi 0:e647f6de3d26 31 /*
Joeatsumi 0:e647f6de3d26 32 行列の成分を全て設定。全成分を一度に指定する必要がある。
Joeatsumi 0:e647f6de3d26 33 引数：pComps 設定値の入ったfloat配列。
Joeatsumi 0:e647f6de3d26 34 */
Joeatsumi 0:e647f6de3d26 35 void SetComps(float* pComps);
Joeatsumi 0:e647f6de3d26 36 /*
Joeatsumi 0:e647f6de3d26 37 行列式を計算する。行列が正方行列で無い場合にはエラー。
Joeatsumi 0:e647f6de3d26 38 */
Joeatsumi 0:e647f6de3d26 39 float Determinant() const;
Joeatsumi 0:e647f6de3d26 40
Joeatsumi 0:e647f6de3d26 41 /*
Joeatsumi 0:e647f6de3d26 42 行列式を計算する。行列が正方行列で無い場合にはエラー。
Joeatsumi 0:e647f6de3d26 43 */
Joeatsumi 0:e647f6de3d26 44 float det() const;
Joeatsumi 0:e647f6de3d26 45
Joeatsumi 0:e647f6de3d26 46 /*
Joeatsumi 0:e647f6de3d26 47 行列をLU分解する
Joeatsumi 0:e647f6de3d26 48 引数：sign (省略可)置換操作の符号を格納するポインタ
Joeatsumi 0:e647f6de3d26 49 p (省略可)置換行列を格納する行列のポインタ。（分解する行列と同じ列数の正方行列)
Joeatsumi 0:e647f6de3d26 50 返り値：LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
Joeatsumi 0:e647f6de3d26 51 */
Joeatsumi 0:e647f6de3d26 52 Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
Joeatsumi 0:e647f6de3d26 53
Joeatsumi 0:e647f6de3d26 54 /*
Joeatsumi 0:e647f6de3d26 55 逆行列を生成する
Joeatsumi 0:e647f6de3d26 56 返り値で逆行列が存在するか否かを判断
Joeatsumi 0:e647f6de3d26 57 引数：逆行列を格納する行列
Joeatsumi 0:e647f6de3d26 58 返り値：逆行列が存在するか否か
Joeatsumi 0:e647f6de3d26 59 */
Joeatsumi 0:e647f6de3d26 60 float Inverse(Matrix& invm) const;
Joeatsumi 0:e647f6de3d26 61
Joeatsumi 0:e647f6de3d26 62 /*
Joeatsumi 0:e647f6de3d26 63 転置行列を生成する
Joeatsumi 0:e647f6de3d26 64 返り値：転置行列
Joeatsumi 0:e647f6de3d26 65 */
Joeatsumi 0:e647f6de3d26 66 Matrix Transpose() const;
Joeatsumi 0:e647f6de3d26 67
Joeatsumi 0:e647f6de3d26 68 /*
Joeatsumi 0:e647f6de3d26 69 行列の行の入れ替えを行う
Joeatsumi 0:e647f6de3d26 70 引数：rowNo1　行番号１
Joeatsumi 0:e647f6de3d26 71 rowNo2　行番号２
Joeatsumi 0:e647f6de3d26 72 */
Joeatsumi 0:e647f6de3d26 73 void SwapRow(int rowNo1, int rowNo2);
Joeatsumi 0:e647f6de3d26 74 /*
Joeatsumi 0:e647f6de3d26 75 行列の列の入れ替えを行う
Joeatsumi 0:e647f6de3d26 76 引数：colNo1　列番号１
Joeatsumi 0:e647f6de3d26 77 colNo2　列番号２
Joeatsumi 0:e647f6de3d26 78 */
Joeatsumi 0:e647f6de3d26 79 void SwapCol(int colNo1, int colNo2);
Joeatsumi 0:e647f6de3d26 80
Joeatsumi 0:e647f6de3d26 81 /********** インライン関数 **********/
Joeatsumi 0:e647f6de3d26 82 inline int GetRow() const {
Joeatsumi 0:e647f6de3d26 83 return row;
Joeatsumi 0:e647f6de3d26 84 }
Joeatsumi 0:e647f6de3d26 85
Joeatsumi 0:e647f6de3d26 86 inline int GetCol() const {
Joeatsumi 0:e647f6de3d26 87 return col;
Joeatsumi 0:e647f6de3d26 88 }
Joeatsumi 0:e647f6de3d26 89
Joeatsumi 0:e647f6de3d26 90 inline const float* GetpComponents() const {
Joeatsumi 0:e647f6de3d26 91 return (const float*)components;
Joeatsumi 0:e647f6de3d26 92 }
Joeatsumi 0:e647f6de3d26 93
Joeatsumi 0:e647f6de3d26 94 inline float GetComp(int rowNo, int colNo) const {
Joeatsumi 0:e647f6de3d26 95 if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!");
Joeatsumi 0:e647f6de3d26 96 return components[(rowNo-1)*col + (colNo-1)];
Joeatsumi 0:e647f6de3d26 97 }
Joeatsumi 0:e647f6de3d26 98
Joeatsumi 0:e647f6de3d26 99 private:
Joeatsumi 0:e647f6de3d26 100 int row;
Joeatsumi 0:e647f6de3d26 101 int col;
Joeatsumi 0:e647f6de3d26 102 float* components;
Joeatsumi 0:e647f6de3d26 103
Joeatsumi 0:e647f6de3d26 104 /*
Joeatsumi 0:e647f6de3d26 105 行列の成分の中で無視できるほど小さい値を0と置き換える（掃除する）
Joeatsumi 0:e647f6de3d26 106 */
Joeatsumi 0:e647f6de3d26 107 void CleanUp();
Joeatsumi 0:e647f6de3d26 108
Joeatsumi 0:e647f6de3d26 109 };
Joeatsumi 0:e647f6de3d26 110
Joeatsumi 0:e647f6de3d26 111 // グローバル演算子
Joeatsumi 0:e647f6de3d26 112 Matrix operator+(const Matrix& lhm, const Matrix& rhm);
Joeatsumi 0:e647f6de3d26 113 Matrix operator-(const Matrix& lhm, const Matrix& rhm);
Joeatsumi 0:e647f6de3d26 114 Matrix operator*(const Matrix& lhm, const Matrix& rhm);
Joeatsumi 0:e647f6de3d26 115