hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Revision 3:5358a691a100, committed 2015-05-30
- Comitter:
- ojan
- Date:
- Sat May 30 18:08:34 2015 +0000
- Parent:
- 2:d2b60a1d0cd9
- Child:
- 4:45dc5590abc0
- Commit message:
- estimate geomagnetism vector with Kalman Filter
Changed in this revision
--- a/LPS25H/LPS25H.h Sun May 24 17:44:11 2015 +0000
+++ b/LPS25H/LPS25H.h Sat May 30 18:08:34 2015 +0000
@@ -36,6 +36,8 @@
#include "mbed.h"
#include "typedef.h"
+
+
class LPS25H{
public:
LPS25H (PinName sda, PinName scl);
@@ -51,8 +53,8 @@
I2C* _i2c;
- DWORD_VAL press;
- WORD_VAL temp;
+ LPS_Typedef::DWORD_VAL press;
+ LPS_Typedef::WORD_VAL temp;
char buf[2];
};
--- a/LPS25H/typedef.h Sun May 24 17:44:11 2015 +0000
+++ b/LPS25H/typedef.h Sat May 30 18:08:34 2015 +0000
@@ -9,47 +9,51 @@
#ifndef TYPEDEF_H
#define TYPEDEF_H
-typedef unsigned char BYTE; /* 8-bit unsigned */
-typedef unsigned short int WORD; /* 16-bit unsigned */
-typedef unsigned int DWORD; /* 32-bit unsigned */
-typedef unsigned long long QWORD; /* 64-bit unsigned */
-
-
-typedef union
-{
- WORD Val;
- BYTE v[2];
- short S;
- struct
- {
- BYTE LB;
- BYTE HB;
- } byte;
-} WORD_VAL;
+namespace LPS_Typedef {
-typedef union
-{
- DWORD Val;
- WORD w[2];
- BYTE v[4];
- struct
+ typedef unsigned char BYTE; /* 8-bit unsigned */
+ typedef unsigned short int WORD; /* 16-bit unsigned */
+ typedef unsigned int DWORD; /* 32-bit unsigned */
+ typedef unsigned long long QWORD; /* 64-bit unsigned */
+
+
+ typedef union
{
- WORD LW;
- WORD HW;
- } word;
- struct
+ WORD Val;
+ BYTE v[2];
+ short S;
+ struct
+ {
+ BYTE LB;
+ BYTE HB;
+ } byte;
+ } WORD_VAL;
+
+ typedef union
{
- BYTE LB;
- BYTE HB;
- BYTE UB;
- BYTE MB;
- } byte;
- struct
- {
- WORD_VAL low;
- WORD_VAL high;
- }wordUnion;
+ DWORD Val;
+ WORD w[2];
+ BYTE v[4];
+ struct
+ {
+ WORD LW;
+ WORD HW;
+ } word;
+ struct
+ {
+ BYTE LB;
+ BYTE HB;
+ BYTE UB;
+ BYTE MB;
+ } byte;
+ struct
+ {
+ WORD_VAL low;
+ WORD_VAL high;
+ }wordUnion;
+
+ } DWORD_VAL;
-} DWORD_VAL;
+}
#endif /* TYPEDEF_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Log/Log.cpp Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,114 @@
+#include "Log.h"
+
+Log::Log(PinName rx, PinName tx, PinName mosi, PinName miso, PinName sck, PinName cs, const char* name) :
+ _device(rx, tx), _sd(mosi, miso, sck, cs, name){
+ buf_send.initialize_buffer();
+ _device.attach(this, &Log::int_serial_tx, Serial::TxIrq);
+}
+
+int Log::initialize_sdlog(const char* str){
+ char filename[15];
+ int n = find_last();
+ if(n < 0) return 0;
+
+ //ログ番号を+1してファイルを新規作成
+ //ファイル名は"logXXX.csv"
+ sprintf(filename, "/sd/log%03d.csv", n+1);
+ fp = fopen(filename, "w");
+
+ _device.printf(str);
+ fprintf(fp, str);
+ return 1;
+}
+
+void Log::close(){
+ wait(0.5);
+ fclose(fp);
+}
+
+//SDカード内からログ番号の最大値を取得する関数
+int Log::find_last() {
+ int i, n = 0;
+ char c;
+ DIR *dp;
+ struct dirent *dirst;
+ dp = opendir("/sd/");
+ if (!dp){
+ printf("Could not open directry\n");
+ return -1;
+ }
+ while((dirst = readdir(dp)) != NULL) {
+ if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
+ n = i;
+ }
+ }
+ closedir(dp);
+ return n;
+}
+
+void Log::puts(const char* str){
+ int16_t len=strlen(str);
+ int16_t capa=buf_send.buffer_capacity();
+ bool empty=buf_send.is_buffer_empty();
+ char ch;
+ if(len>capa){
+ len=capa;
+ }
+ buf_send.write_buffer((const uint8_t*)str, 0, len);
+ if((_device.writeable()) && (empty)){
+ ch=buf_send.read_buffer_byte();
+ _device.putc(ch);
+ fputc(ch, fp);
+ }
+}
+
+void Log::putc(char ch){
+ int16_t capa=buf_send.buffer_capacity();
+ if(capa==0) return;
+ buf_send.write_buffer_byte(ch);
+}
+
+void Log::write_data(uint8_t* buf, int16_t size){
+ int capa=buf_send.buffer_capacity();
+ if(size>capa){
+ size=capa;
+ }
+ buf_send.write_buffer(buf, 0, size);
+}
+
+bool Log::is_empty(){
+ return buf_send.is_buffer_empty();
+}
+
+int16_t Log::recieve_buffer_size(){
+ return buf_recieve.buffer_size();
+}
+
+int16_t Log::getc(){
+ if(buf_recieve.is_buffer_empty()){
+ return -1;
+ }
+ return buf_recieve.read_buffer_byte();
+}
+
+int16_t Log::read_data(uint8_t* buf, int16_t size){
+ int len=buf_recieve.buffer_size();
+ if(size>len){
+ size=len;
+ }
+ buf_recieve.read_buffer(buf, 0, size);
+ return size;
+}
+
+char Log::int_tx(){
+ return buf_send.read_buffer_byte();
+}
+
+void Log::int_serial_tx(){
+ char ch;
+ while((_device.writeable()) && (buf_send.is_buffer_empty()==false)){
+ ch=buf_send.read_buffer_byte();
+ _device.putc(ch);
+ fputc(ch, fp);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Log/Log.h Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,37 @@
+//**** Log.h ****
+//ログをリングバッファを用いて、
+//シリアル送信、SDカード出力を行う
+//***************
+
+#ifndef LOG_H_
+#define LOG_H_
+
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "RingBuffer.h"
+
+class Log{
+public:
+ Log(PinName rx, PinName tx, PinName mosi, PinName miso, PinName sck, PinName cs, const char* name);
+ int initialize_sdlog(const char* str);
+ void close();
+ int find_last();
+ void puts(const char *str);
+ void putc(char ch);
+ void write_data(uint8_t* buf, int16_t size);
+ bool is_empty();
+ int16_t recieve_buffer_size();
+ int16_t getc();
+ int16_t read_data(uint8_t* buf, int16_t size);
+ char int_tx();
+ void int_serial_tx();
+
+protected:
+ SDFileSystem _sd;
+ FILE *fp;
+ Serial _device;
+ RingBuffer buf_send;
+ RingBuffer buf_recieve;
+};
+
+#endif /* LOG_H_ */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.cpp Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,389 @@
+#include "mbed.h"
+#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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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()) AbortWithMsg("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()) AbortWithMsg("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()) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("failed to LU decomposition: matrix is not square");
+ if (sign != 0) *sign = 1;
+ if (p != 0) {
+ if (p->row != row || p->row != p->col) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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()) AbortWithMsg("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) AbortWithMsg("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) AbortWithMsg("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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix/Matrix.h Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,115 @@
+#pragma once
+#include "ErrorLogger.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) AbortWithMsg("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);
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RingBuffer/RingBuffer.cpp Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,63 @@
+#include "RingBuffer.h"
+
+void RingBuffer::initialize_buffer(){
+ start=0;
+ end=0;
+}
+
+int16_t RingBuffer::buffer_size(){
+ if(end >= start){
+ return end - start;
+ }
+ return RING_BUFFER_SIZE - (start - end);
+}
+
+uint8_t RingBuffer::read_buffer_byte(){
+ uint8_t a;
+ a=buffer[start];
+ start=(start+1)%RING_BUFFER_SIZE;
+ return a;
+}
+
+int16_t RingBuffer::read_buffer_short(){
+ int16_t a, b;
+ a=buffer[start];
+ b=buffer[(start+1)%RING_BUFFER_SIZE];
+ start=(start+2)%RING_BUFFER_SIZE;
+ return (a&0xff)|(b<<8);
+}
+
+void RingBuffer::read_buffer(uint8_t buf[], int16_t offset, int16_t size){
+ int16_t i;
+ uint8_t* p=buf+offset;
+
+ for(i=0; i<size; i++){
+ *(p++)=buffer[start];
+ start=(start+1)%RING_BUFFER_SIZE;
+ }
+}
+
+int16_t RingBuffer::buffer_capacity(){
+ if(end >= start){
+ return RING_BUFFER_SIZE-(end-start)-1;
+ }
+ return (start-end)-1;
+}
+
+void RingBuffer::write_buffer_byte(uint8_t ch){
+ buffer[end]=ch;
+ end=(end+1)%RING_BUFFER_SIZE;
+}
+
+void RingBuffer::write_buffer(const uint8_t buf[], int16_t offset, int16_t size){
+ int16_t i;
+ const uint8_t* p=buf+offset;
+ for(i=0; i<size; i++){
+ buffer[end]=*(p++);
+ end=(end+1)%RING_BUFFER_SIZE;
+ }
+}
+
+bool RingBuffer::is_buffer_empty(){
+ return (start==end);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/RingBuffer/RingBuffer.h Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,26 @@
+#ifndef RING_BUFFER_H_
+#define RING_BUFFER_H_
+
+#include "mbed.h"
+
+#define RING_BUFFER_SIZE 512
+
+class RingBuffer{
+public:
+ void initialize_buffer();
+ int16_t buffer_size();
+ uint8_t read_buffer_byte();
+ int16_t read_buffer_short();
+ void read_buffer(uint8_t buf[], int16_t offset, int16_t size);
+ int16_t buffer_capacity();
+ void write_buffer_byte(uint8_t ch);
+ void write_buffer(const uint8_t buf[], int16_t offset, int16_t size);
+ bool is_buffer_empty();
+
+protected:
+ uint8_t buffer[RING_BUFFER_SIZE];
+ int16_t start;
+ int16_t end;
+};
+
+#endif /* RING_BUFFER_H_ */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Sat May 30 18:08:34 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/mbed/code/SDFileSystem/#7b35d1709458
--- a/Vector/Vector.cpp Sun May 24 17:44:11 2015 +0000
+++ b/Vector/Vector.cpp Sat May 30 18:08:34 2015 +0000
@@ -6,9 +6,7 @@
Vector::Vector(int dim) : dim(dim), components(0){
components = new float[dim];
if (!components) AbortWithMsg("Memory Allocation Error");
- for(int i=0; i<dim; i++) {
- components[i] = 0.0f;
- }
+ for(int i=0; i<dim; i++) components[i] = 0.0f;
}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector_Matrix_operator.cpp Sat May 30 18:08:34 2015 +0000
@@ -0,0 +1,35 @@
+#include "Vector_Matrix_operator.h"
+
+Vector operator*(const Matrix& lhm, const Vector& rhv) {
+ if (lhm.GetCol() != rhv.GetDim()) AbortWithMsg("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()) AbortWithMsg("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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Vector/Vector_Matrix_operator.h Sat May 30 18:08:34 2015 +0000 @@ -0,0 +1,5 @@ +#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 Sun May 24 17:44:11 2015 +0000
+++ b/main.cpp Sat May 30 18:08:34 2015 +0000
@@ -5,7 +5,10 @@
#include "GMS6_CR6.h"
#include "ErrorLogger.h"
#include "Vector.h"
+#include "Matrix.h"
+#include "Vector_Matrix_operator.h"
#include "myConstants.h"
+#include "Log.h"
/********** private define **********/
#define TRUE 1
@@ -15,30 +18,50 @@
/********** private typedef **********/
/********** private variables **********/
-I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート
+I2C i2c(PB_9, PB_8); // I2Cポート
MPU6050 mpu(&i2c); // 加速度・角速度センサ
HMC5883L hmc(&i2c); // 地磁気センサ
LPS25H lps(&i2c); // 気圧センサ
-Serial gps(D8, D2); // GPS通信用シリアルポート
+Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
GMS6_CR6 gms(&gps, &pc); // GPS
-Ticker timer; // 割り込みタイマー
+Ticker INT_timer; // 割り込みタイマー
+//Log logger(PA_9, PA_10, PB_5, PB_4, PB_3, PB_10, "sd"); // ロガー（microSD、XBee）
-const float Freq = 0.01f; // 割り込み周期（s）
+const float dt = 0.1f; // 割り込み周期（s）
int lps_cnt = 0; // 気圧センサ読み取りカウント
-uint8_t INT_flag = FALSE; // 割り込み可否フラグ
+uint8_t INT_flag = TRUE; // 割り込み可否フラグ
Vector acc(3); // 加速度(m/s^2)
Vector gyro(3); // 角速度(deg/s)
Vector geomag(3); // 地磁気(?)
float press; // 気圧(hPa)
Vector g(3); // 重力ベクトル
-Vector n(3); // 地磁気ベクトル
-Vector bias(3); // 地磁気センサのバイアスベクトル
+//Vector n(3); // 地磁気ベクトル
+
+/* ----- Kalman Filter ----- */
+Vector pri_x(6);
+Matrix pri_P(6, 6);
+Vector post_x(6);
+Matrix post_P(6, 6);
+Matrix F(6, 6), H(3, 6);
+Matrix R(6, 6), Q(3, 3);
+Matrix I(6, 6);
+Matrix K(6, 3);
+Matrix S(3, 3), inv(3, 3);
+/* ----- ------------- ----- */
+
+Timer timer;
+
+char data[1024] = {};
/********** private functions **********/
+void KalmanInit();
+void KalmanUpdate();
void INT_func(); // 割り込み用関数
+void toString(Matrix& m);
+void toString(Vector& v);
/********** main function **********/
@@ -49,7 +72,12 @@
if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化
if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化
- timer.attach(&INT_func, Freq); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
+ //char* title = "log data\r\n"; // ログのタイトル行
+ //if(!logger.initialize_sdlog(title)) return 0; // ログファイル初期設定
+
+ KalmanInit();
+
+ INT_timer.attach(&INT_func, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
//重力ベクトルの初期化
g.SetComp(1, 0.0f);
@@ -59,23 +87,40 @@
/* ---------- ↓↓↓ ここからメインループ ↓↓↓ ---------- */
while(1) {
- // 1秒おきにセンサーの出力をpcへ出力
- wait(1.0f);
+ // 0.1秒おきにセンサーの出力をpcへ出力
+ wait(0.1f);
INT_flag = FALSE; // 割り込みによる変数書き換えを阻止
// センサ類の全出力値をPCに送信
- pc.printf("%f,", acc.GetComp(1));
- pc.printf("%f,", acc.GetComp(2));
- pc.printf("%f,", acc.GetComp(3));
- pc.printf("%f,", gyro.GetComp(1));
- pc.printf("%f,", gyro.GetComp(2));
- pc.printf("%f,", gyro.GetComp(3));
- pc.printf("%f,", geomag.GetComp(1));
- pc.printf("%f,", geomag.GetComp(2));
- pc.printf("%f,", geomag.GetComp(3));
- pc.printf("%f\r\n", press);
+ /*
+ pc.printf("%.3f,", acc.GetComp(1));
+ pc.printf("%.3f,", acc.GetComp(2));
+ pc.printf("%.3f,", acc.GetComp(3));
+ pc.printf("%.3f,", gyro.GetComp(1));
+ pc.printf("%.3f,", gyro.GetComp(2));
+ pc.printf("%.3f,", gyro.GetComp(3));
+ pc.printf("%.3f,", geomag.GetComp(1));
+ pc.printf("%.3f,", geomag.GetComp(2));
+ pc.printf("%.3f\r\n", geomag.GetComp(3));
+ */
+ //pc.printf("%.3f\r\n", press);
+
+ pc.printf("%.3f,", geomag.GetComp(1));
+ pc.printf("%.3f,", geomag.GetComp(2));
+ pc.printf("%.3f,", geomag.GetComp(3));
+ pc.printf("%.3f,", post_x.GetComp(1));
+ pc.printf("%.3f,", post_x.GetComp(2));
+ pc.printf("%.3f\r\n", post_x.GetComp(3));
+
+ /*
+ sprintf(data, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
+ acc.GetComp(1), acc.GetComp(2), acc.GetComp(3),
+ gyro.GetComp(1), gyro.GetComp(2), gyro.GetComp(3),
+ geomag.GetComp(1), geomag.GetComp(2), geomag.GetComp(3), press);
+ logger.puts(data);
+ */
INT_flag = TRUE; // 割り込み許可
}
@@ -83,9 +128,74 @@
/* ---------- ↑↑↑ ここまでメインループ ↑↑↑ ---------- */
}
+void KalmanInit() {
+
+ // 誤差共分散行列の値を決める（対角成分のみ）
+ float alpha_R = 60.0f;
+ float alpha_Q = 100.0f;
+ R *= alpha_R;
+ Q *= alpha_Q;
+
+ // 状態方程式のヤコビアンの初期値を代入（時間変化あり）
+ float f[36] = {
+ 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
+ -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
+ gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+ };
+
+ F.SetComps(f);
+
+ // 観測方程式のヤコビアンの値を設定（時間変化無し）
+ float h[18] = {
+ 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f
+ };
+
+ H.SetComps(h);
+}
+
+void KalmanUpdate() {
+ // ヤコビアンの更新
+ float f[36] = {
+ 1.0f, gyro.GetComp(3)*dt, -gyro.GetComp(2)*dt, 0.0f, 0.0f, 0.0f,
+ -gyro.GetComp(3)*dt, 1.0f, gyro.GetComp(1)*dt, 0.0f, 0.0f, 0.0f,
+ gyro.GetComp(2)*dt, -gyro.GetComp(1)*dt, 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f
+ };
+
+ F.SetComps(f);
+
+ // 事前推定値の更新
+ pri_x = F * post_x;
+ // 事前誤差分散行列の更新
+ pri_P = F * post_P * F.Transpose() + R;
+
+ // カルマンゲインの計算
+ S = Q + H * pri_P * H.Transpose();
+ float det;
+ if((det = S.Inverse(inv)) >= 0.0f) {
+ pc.printf("E:%.3f\r\n", det);
+ return; // 万が一、逆行列が見つからなかった場合は前回の推定値を保持して終了
+ }
+ K = pri_P * H.Transpose() * inv;
+
+ // 事後推定値の更新
+ post_x = pri_x + K * (geomag - H * pri_x);
+ // 事後誤差分散行列の更新
+ post_P = (I - K * H) * pri_P;
+}
+
void INT_func() {
- if(INT_flag == FALSE) {
+ if(INT_flag != FALSE) {
+ timer.reset();
+ timer.start();
// センサーの値を更新
mpu.read();
hmc.read();
@@ -97,17 +207,40 @@
}
Vector delta_g = Cross(gyro, g); // Δg = ω × g
- g = 0.9f * (g - delta_g * Freq) + 0.1f * acc.Normalize(); // 相補フィルタ
+ g = 0.9f * (g - delta_g * dt) + 0.1f * acc.Normalize(); // 相補フィルタ
g = g.Normalize();
- Vector delta_n = Cross(gyro,n);
- n = 0.9f * (n - delta_n * Freq) + 0.1f * geomag.Normalize();
- n = n.Normalize();
+
+ KalmanUpdate();
+
// LPS25Hによる気圧の取得は10Hz
lps_cnt = (lps_cnt+1)%10;
if(lps_cnt == 0) {
press = (float)lps.pressure() * PRES_LSB_TO_HPA;
}
+
+ timer.stop();
+ //pc.printf("%d(us)\r\n", timer.read_us());
}
+}
+
+void toString(Matrix& m) {
+
+ for(int i=0; i<m.GetRow(); i++) {
+ for(int j=0; j<m.GetCol(); j++) {
+ pc.printf("%.6f\t", m.GetComp(i+1, j+1));
+ }
+ pc.printf("\r\n");
+ }
+
+}
+
+void toString(Vector& v) {
+
+ for(int i=0; i<v.GetDim(); i++) {
+ pc.printf("%.6f\t", v.GetComp(i+1));
+ }
+ pc.printf("\r\n");
+
}
\ No newline at end of file
--- a/myConstants.h Sun May 24 17:44:11 2015 +0000 +++ b/myConstants.h Sat May 30 18:08:34 2015 +0000 @@ -1,7 +1,7 @@ #pragma once -#define NEARLY_ZERO 0.000001f -#define ZERO_TOLERANCE 0.001f +#define NEARLY_ZERO 0.000000001f +#define ZERO_TOLERANCE 0.000001f #define RAD_TO_DEG 57.2957795f // 180 / π #define DEG_TO_RAD 0.0174532925f // π / 180 #define ACC_LSB_TO_G 0.0000610351562f // g/LSB (1/2^14