MPU6050のサンプルプログラム2
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Revision 23:79cdc1432160, committed 2015-06-24
- Comitter:
- ojan
- Date:
- Wed Jun 24 16:16:14 2015 +0000
- Parent:
- 22:3caa2983bf1d
- Child:
- 24:8838be99cec3
- Commit message:
- bug in SD and xbee
Changed in this revision
--- a/ErrorLogger.cpp Tue Jun 23 15:23:38 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,12 +0,0 @@ -#include "ErrorLogger.h" - -void InitLogger(Serial* dp) { - if(debugPort == dp) return; - debugPort = dp; -} - -void AbortWithMsg(const char* msg) { - while(!debugPort->writeable()); - debugPort->printf(msg); - abort(); -} \ No newline at end of file
--- a/ErrorLogger.h Tue Jun 23 15:23:38 2015 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,13 +0,0 @@ -#pragma once -#include "mbed.h" - -static Serial* debugPort = 0; - -void InitLogger(Serial* dp); - -/* - エラーメッセージを送信してプログラムを中断する - 引数: -*/ - -void AbortWithMsg(const char* msg); \ No newline at end of file
--- a/Matrix/Matrix.cpp Tue Jun 23 15:23:38 2015 +0000 +++ b/Matrix/Matrix.cpp Wed Jun 24 16:16:14 2015 +0000 @@ -1,4 +1,3 @@ -#include "mbed.h" #include "myConstants.h" #include "Matrix.h" @@ -6,7 +5,7 @@ Matrix::Matrix(int row, int col) : row(row), col(col), components(0) { components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + 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++) { @@ -17,7 +16,7 @@ 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"); + if (!components) error("Memory Allocation Error"); memcpy(components, comps, sizeof(float)*row*col); } @@ -28,7 +27,7 @@ Matrix::Matrix(const Matrix& m) : row(m.row), col(m.col), components(0) { components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, m.GetpComponents(), sizeof(float)*row*col); } @@ -48,14 +47,14 @@ col = m.col; delete[] components; components = new float[row*col]; - if (!components) AbortWithMsg("Memory Allocation Error"); + 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()) AbortWithMsg("Irregular Dimention"); + if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { @@ -69,7 +68,7 @@ } Matrix& Matrix::operator-=(const Matrix& m) { - if (row != m.GetRow() || col != m.GetCol()) AbortWithMsg("Irregular Dimention"); + if (row != m.GetRow() || col != m.GetCol()) error("Irregular Dimention"); for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { @@ -83,7 +82,7 @@ } /* Matrix& Matrix::operator*=(const Matrix& m) { - if (col != m.GetRow()) AbortWithMsg("Irregular Dimention"); + if (col != m.GetRow()) error("Irregular Dimention"); Matrix temp = Matrix(*this); col = m.GetCol(); @@ -116,7 +115,7 @@ } Matrix& Matrix::operator/=(float c) { - if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero"); + 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; @@ -127,7 +126,7 @@ } void Matrix::SetComp(int rowNo, int colNo, float val) { - if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error"); + if (rowNo > row || colNo > col) error("Index Out of Bounds Error"); components[(rowNo-1)*col + (colNo-1)] = val; } @@ -136,7 +135,7 @@ } float Matrix::Determinant() const{ - if (row != col) AbortWithMsg("failed to calculate det. : matrix is not square"); + if (row != col) error("failed to calculate det. : matrix is not square"); int decSign = 0; float retVal = 1.0f; @@ -151,7 +150,7 @@ } float Matrix::det() const { - if (row != col) AbortWithMsg("failed to calculate det : matrix is not square"); + if (row != col) error("failed to calculate det : matrix is not square"); Matrix temp(*this); int decSign = 1; @@ -180,7 +179,7 @@ // 列内、行内の最大要素を選んでも小さすぎる場合はエラー if (fabs(temp.components[j * col + j]) < NEARLY_ZERO) { - if (row != col) AbortWithMsg("failed to calculate det : Division by Zero"); + if (row != col) error("failed to calculate det : Division by Zero"); } } @@ -206,10 +205,10 @@ } Matrix Matrix::LU_Decompose(int* sign, Matrix* p) const{ - if (row != col) AbortWithMsg("failed to LU decomposition: matrix is not square"); + 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) AbortWithMsg("failed to LU decomposition: permitation matrix is incorrect"); + 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++) { @@ -232,7 +231,7 @@ } } float c = retVal.components[d * col + d]; - if (fabs(c) < NEARLY_ZERO) AbortWithMsg("failed to LU decomposition: Division by Zero"); + if (fabs(c) < NEARLY_ZERO) error("failed to LU decomposition: Division by Zero"); // d行d列目以降の行列について計算 for (int i = d+1; i < row; i++) { @@ -249,7 +248,7 @@ } float Matrix::Inverse(Matrix& invm) const{ - if (row != col) AbortWithMsg("failed to get Inv. : matrix is not square"); + if (row != col) error("failed to get Inv. : matrix is not square"); Matrix P(*this); Matrix LU(LU_Decompose(0, &P)); @@ -309,7 +308,7 @@ } Matrix Matrix::Transpose() const{ - //if (row != col) AbortWithMsg("failed to get Trans. : matrix is not square"); + //if (row != col) error("failed to get Trans. : matrix is not square"); Matrix retVal(col, row); for (int i = 0; i < row; i++) { @@ -334,7 +333,7 @@ } Matrix operator*(const Matrix& lhm, const Matrix& rhm) { - if(lhm.GetCol() != rhm.GetRow()) AbortWithMsg("Matrix product Error: Irregular Dimention."); + if(lhm.GetCol() != rhm.GetRow()) error("Matrix product Error: Irregular Dimention."); int row = lhm.GetRow(); int col = rhm.GetCol(); int sum = lhm.GetCol(); @@ -367,7 +366,7 @@ } void Matrix::SwapRow(int rowNo1, int rowNo2) { - if (rowNo1 > row || rowNo2 > row) AbortWithMsg("Index Out of Bounds Error !!"); + 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); @@ -378,7 +377,7 @@ } void Matrix::SwapCol(int colNo1, int colNo2) { - if (colNo1 > col || colNo2 > col) AbortWithMsg("Index Out of Bounds Error !!"); + if (colNo1 > col || colNo2 > col) error("Index Out of Bounds Error !!"); float temp = 0.0f; for (int i = 0; i < row; i++) {
--- a/Matrix/Matrix.h Tue Jun 23 15:23:38 2015 +0000 +++ b/Matrix/Matrix.h Wed Jun 24 16:16:14 2015 +0000 @@ -1,5 +1,5 @@ #pragma once -#include "ErrorLogger.h" +#include "mbed.h" class Matrix { @@ -92,7 +92,7 @@ } inline float GetComp(int rowNo, int colNo) const { - if (rowNo > row || colNo > col) AbortWithMsg("Index Out of Bounds Error !!"); + if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!"); return components[(rowNo-1)*col + (colNo-1)]; }
--- a/Vector/Vector.cpp Tue Jun 23 15:23:38 2015 +0000 +++ b/Vector/Vector.cpp Wed Jun 24 16:16:14 2015 +0000 @@ -1,11 +1,10 @@ -#include "mbed.h" #include "myConstants.h" #include "Vector.h" Vector::Vector(int dim) : dim(dim), components(0){ components = new float[dim]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); for(int i=0; i<dim; i++) components[i] = 0.0f; } @@ -16,7 +15,7 @@ Vector::Vector(const Vector& v) : dim(v.dim), components(0) { components = new float[dim]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, v.GetpComponents(), sizeof(float)*dim); } @@ -25,7 +24,7 @@ dim = v.dim; delete[] components; components = new float[dim]; - if (!components) AbortWithMsg("Memory Allocation Error"); + if (!components) error("Memory Allocation Error"); memcpy(components, v.GetpComponents(), sizeof(float)*dim); return *this; @@ -50,7 +49,7 @@ } Vector& Vector::operator/=(float c) { - if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero"); + if (fabs(c) < NEARLY_ZERO) error("Division by Zero"); for (int i = 0; i < dim; i++) { components[i] /= c; } @@ -59,7 +58,7 @@ } Vector& Vector::operator+=(const Vector& v) { - if (dim != v.dim) AbortWithMsg("failed to add: Irregular Dimention"); + if (dim != v.dim) error("failed to add: Irregular Dimention"); for (int i = 0; i < dim; i++) { components[i] += v.components[i]; } @@ -70,7 +69,7 @@ } Vector& Vector::operator-=(const Vector& v) { - if (dim != v.dim) AbortWithMsg("failed to subtract: Irregular Dimention"); + if (dim != v.dim) error("failed to subtract: Irregular Dimention"); for (int i = 0; i < dim; i++) { components[i] -= v.components[i]; } @@ -81,7 +80,7 @@ } void Vector::SetComp(int dimNo, float val) { - if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error"); + if (dimNo > dim) error("Index Out of Bounds Error"); components[dimNo-1] = val; } @@ -141,8 +140,8 @@ } Vector Cross(const Vector& lhv, const Vector& rhv) { - if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3"); - if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention"); + 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()); @@ -167,7 +166,7 @@ } float operator*(const Vector& lhv, const Vector& rhv) { - if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention"); + if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention"); float retVal = 0.0f; for (int i = 1; i <= lhv.GetDim(); i++) {
--- a/Vector/Vector.h Tue Jun 23 15:23:38 2015 +0000 +++ b/Vector/Vector.h Wed Jun 24 16:16:14 2015 +0000 @@ -1,5 +1,5 @@ #pragma once -#include "ErrorLogger.h" +#include "mbed.h" class Matrix; @@ -33,7 +33,7 @@ } inline float GetComp(int dimNo) const { - if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error !!"); + if (dimNo > dim) error("Index Out of Bounds Error !!"); return components[dimNo-1]; }
--- a/Vector/Vector_Matrix_operator.cpp Tue Jun 23 15:23:38 2015 +0000 +++ b/Vector/Vector_Matrix_operator.cpp Wed Jun 24 16:16:14 2015 +0000 @@ -1,7 +1,7 @@ #include "Vector_Matrix_operator.h" Vector operator*(const Matrix& lhm, const Vector& rhv) { - if (lhm.GetCol() != rhv.GetDim()) AbortWithMsg("Irregular Dimention"); + if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention"); Vector retVec(lhm.GetRow()); for (int i = 1; i <= lhm.GetRow(); i++) { @@ -18,7 +18,7 @@ } Vector operator*(const Vector& lhv, const Matrix& rhm) { - if (lhv.GetDim() != rhm.GetRow()) AbortWithMsg("Irregular Dimention"); + if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention"); Vector retVec(rhm.GetCol()); for (int i = 1; i <= rhm.GetCol(); i++) {
--- a/main.cpp Tue Jun 23 15:23:38 2015 +0000 +++ b/main.cpp Wed Jun 24 16:16:14 2015 +0000 @@ -3,7 +3,6 @@ #include "HMC5883L.h" #include "LPS25H.h" #include "GMS6_CR6.h" -#include "ErrorLogger.h" #include "Vector.h" #include "Matrix.h" #include "Vector_Matrix_operator.h" @@ -51,8 +50,8 @@ ConfigFile cfg; // ConfigFile PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 -AnalogIn servoVcc(PA_0); // バッテリー電圧監視用アナログ入力(サーボ用) -AnalogIn logicVcc(PA_1); // バッテリー電圧監視用アナログ入力(ロジック用) +AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用) +AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用) DigitalIn paraSensor(PB_0); // パラフォイルに繋がる(予定)の物理スイッチ Ticker ticker; // 割り込みタイマー Timer timer; // 時間計測用タイマー @@ -64,11 +63,13 @@ Vector raw_gyro(3); // 角速度(deg/s) 生 Vector raw_geomag(3); // 地磁気(?) 生 float raw_press; // 気圧(hPa) 生 +float raw_temp; // 温度(℃) 生 /* メインループ内ではこちらを参照する */ Vector acc(3); // 加速度(m/s^2) Vector gyro(3); // 角速度(deg/s) Vector geomag(3); // 地磁気(?) float press; // 気圧(hPa) +float temp; // 温度(℃) Vector raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル @@ -107,7 +108,7 @@ * target_x=111.222 * target_y=33.444 */ -float target_x = 0.0f, target_y = 0.0f; +float target_x, target_y; /* ---------- Kalman Filter ---------- */ // 地磁気ベクトル用 @@ -138,7 +139,7 @@ /****************************** private functions ******************************/ bool SD_Init(); // SDカード初期化 -void VectorsInit(); // 各種ベクトルの初期化 +void SensorsInit(); // 各種センサーの初期化 void KalmanInit(); // カルマンフィルタ初期化 bool LoadConfig(); // config読み取り int Find_last(); // SDカード初期化用関数 @@ -162,10 +163,9 @@ { i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 - - if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 - if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - + + // センサー関連の初期化 + SensorsInit(); //Config読み取り while(!LoadConfig()); @@ -177,9 +177,6 @@ //カルマンフィルタ初期化 KalmanInit(); - // 各種ベクトルの初期化 - VectorsInit(); - ticker.attach(&DataUpdate, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) NVIC_SetPriority(TIM5_IRQn, 5); @@ -217,9 +214,9 @@ sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d, %d,%d\r\n", UTC_t, yaw, pitch, roll, press, gms.longitude, gms.latitude, - lv, vrt_acc, loopTime, - Distance(target_p, p), optSensor.read_u16() - , pull_R, pull_L); + vrt_acc, temp, (int)paraSensor, + Distance(target_p, p), optSensor.read_u16(), + pull_R, pull_L); INT_flag = TRUE; // 割り込み許可 @@ -329,7 +326,7 @@ /* 北から西回りで目標方向の角度を出力 */ float targetY = cos( target_lat ) * sin( target_lng - p.GetComp(1) ); float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) ); - float theta = -atan2f( targetY, targetX ); + float theta = -atan2f( targetY, targetX ) * RAD_TO_DEG; float delta_theta = 0.0f; if(yaw >= 0.0f) { // ヨー角が正 @@ -402,11 +399,15 @@ } } -/** 各種ベクトルの初期化を行う関数 +/** 各種センサーの初期化を行う関数 * */ -void VectorsInit() +void SensorsInit() { + + if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化 + if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化 + //重力ベクトルの初期化 raw_g.SetComp(1, 0.0f); raw_g.SetComp(2, 0.0f); @@ -435,7 +436,7 @@ char filename[15]; int n = Find_last(); if(n < 0) { - pc.printf("Could not read a SD Card.\n"); + xbee.printf("Could not read a SD Card.\n"); return false; } sprintf(filename, "/sd/log%03d.csv", n+1); @@ -454,18 +455,18 @@ char value[20]; //Read a configuration file from a mbed. if (!cfg.read("/sd/config.txt")) { - pc.printf("Config file does not exist\n"); + xbee.printf("Config file does not exist\n"); return false; } else { //Get values if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value); else { - pc.printf("Failed to get value for target_x\n"); + xbee.printf("Failed to get value for target_x\n"); return false; } if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value); else { - pc.printf("Failed to get value for target_y\n"); + xbee.printf("Failed to get value for target_y\n"); return false; } } @@ -684,6 +685,8 @@ } +/* ------------------------------ 割り込み関数 ------------------------------ */ + /** データ取得&更新関数 * */ @@ -709,6 +712,7 @@ lps_cnt = (lps_cnt+1)%10; if(lps_cnt == 0) { raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA; + //raw_temp = TempLsbToDeg(lps.temperature()); } if(INT_flag != FALSE) { @@ -719,14 +723,13 @@ acc = raw_acc; gyro = raw_gyro; press = raw_press; + temp = raw_temp; vrt_acc = raw_acc * raw_g; } } -/* ------------------------------ 割り込み関数 ------------------------------ */ - /* ------------------------------ デバッグ用関数 ------------------------------ */ void toString(Matrix& m)
--- a/myConstants.h Tue Jun 23 15:23:38 2015 +0000 +++ b/myConstants.h Wed Jun 24 16:16:14 2015 +0000 @@ -18,6 +18,10 @@ /* Pressure Sensor */ #define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 +inline float TempLsbToDeg(short int temp) { + return (42.5f + (float)temp * 0.00208333333f); // degree_C = 42.5 + temp / 480; +} + /* GPS */ #define GPS_SQ_E 0.00669437999f // (第一離心率)^2 #define GPS_A 6378137.0f // 長半径(赤道半径)(m)