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)