main.cpp

00001 #include "mbed.h"
00002 #include "Vector.h"
00003 #include "Matrix.h"
00004 #include "Vector_Matrix_operator.h"
00005 
00006 /*---------------------------定数---------------------------*/
00007 #define PI 3.141592
00008 #define OMEGA 7.2921159/100000//地球の自転角速度 rad/s
00009 /*---------------------------------------------------------*/
00010 DigitalOut myled(LED1);
00011 
00012 int second;//軌道からの経過時間[s]
00013 double theta;//ECIとECEFのずれ[rad]
00014 
00015 double lon,lat;              // lon 経度 lat 緯度 [rad]
00016 double lon_degree,lat_degree;// lon_degree 経度 lat_degree 緯度 [degree]
00017 double el,roll,az;           //ピッチ,ロール,アジマス（rad）
00018 double pitch_degree,roll_degree,azimaith_degree;
00019                              //ピッチ,ロール,アジマス（10進数表記）
00020 
00021 Serial pc(USBTX, USBRX);     // Teratermとの接続 tx, rx
00022 
00023 /*--------------------------行列、ベクトル-----------------------------*/
00024 Matrix Q1(3, 3), Q2(3, 3),Q3(3,3);                        //検算の行列
00025 Matrix ECI_to_ECEF(3,3),ECEF_to_NED(3,3),NED_to_BODY(3,3);//変換行列
00026 Vector ECI(3),ECEF(3), NED(3),BODY(3);                    //座標系
00027 /*-------------------------------------------------------------------*/
00028 
00029 /*--------------------プロトタイプ宣言----------------------*/
00030 void    toString(Matrix& m);     // 行列デバッグ用
00031 void    toString_V(Vector& v);   // ベクトルデバッグ用
00032 double deg_to_rad(double);       // 度数単位からrad単位への変換
00033 /*--------------------------------------------------------*/
00034 
00035 /* ------------------------------ デバッグ用関数 ------------------------------ */
00036 
00037 void toString(Matrix& m)
00038 {
00039 
00040     for(int i=0; i<m.GetRow(); i++) {
00041         for(int j=0; j<m.GetCol(); j++) {
00042             pc.printf("%.6f\t", m.GetComp(i+1, j+1));
00043         }
00044         pc.printf("\r\n");
00045     }
00046 
00047 }
00048 
00049 void toString_V(Vector& v)
00050 {
00051 
00052     for(int i=0; i<v.GetDim(); i++) {
00053         pc.printf("%.6f\t", v.GetComp(i+1));
00054     }
00055     pc.printf("\r\n");
00056 
00057 }
00058 /*-------------------------------------------------------------------------*/
00059 /*---------------------度数単位からラジアン単位に変換する------------*/
00060 double deg_to_rad(double degree)
00061 {   double rad;
00062     rad=degree*PI/180;
00063     return rad;     
00064     }
00065 /*---------------------------------------------------------------*/
00066 
00067 int main() {
00068 
00069  /* ----------------Q1の要素値を設定----------------------*/
00070         float q1[9] = {
00071             1.0f, 2.0f, 3.0f,  
00072             4.0f, 5.0f, 6.0f, 
00073             7.0f, 8.0f, 9.0f
00074         };
00075             
00076          Q1.SetComps(q1);
00077 /*-----------------------------------------------------*/
00078 /* ----------------Q2の要素値を設定----------------------*/
00079         float q2[9] = {
00080             1.0f, 2.0f, 3.0f,  
00081             4.0f, 5.0f, 6.0f, 
00082             7.0f, 8.0f, 9.0f
00083         };
00084             
00085          Q2.SetComps(q2);
00086 /*-----------------------------------------------------*/
00087 /* ----------------ECIの要素値を設定----------------------*/
00088         float eci[3] = {
00089             4.0f,   
00090             1.0f, 
00091             0.0f
00092         };
00093             
00094          ECI.SetComps(eci);
00095 /*-----------------------------------------------------*/
00096 
00097 /*=================ECI座標系からECEF座標系への変換行列=====================*/
00098 
00099 second=3600*6;//１時間は3600秒
00100 theta=OMEGA*second;
00101 float eci_to_ecef[9]={
00102             cos(theta),sin(theta),0.0,
00103             -sin(theta),cos(theta),0.0,
00104             0.0,       0.0        ,0.0    
00105     }; 
00106     
00107         ECI_to_ECEF.SetComps(eci_to_ecef);
00108 /*====================================================================*/
00109 
00110 /*=================ECEF座標系からNED座標系への変換行列=====================*/    
00111 
00112 lon_degree=0.0;  //経度
00113 lat_degree=0.0;  //緯度
00114 lon=deg_to_rad(lon_degree);
00115 lat=deg_to_rad(lat_degree);
00116 
00117 float ecef_to_ned[9]={
00118             -sin(lat)*cos(lon),-sin(lat)*sin(lon)    ,cos(lat),
00119                -sin(lon)             ,cos(lon)     ,0.0,
00120             -cos(lat)*cos(lon)    ,  -cos(lat)*sin(lon) ,-sin(lat)
00121     }; 
00122     
00123         ECEF_to_NED.SetComps(ecef_to_ned);
00124 /*====================================================================*/
00125 
00126 /*=================NED座標系から機体座標系への変換行列=====================*/  
00127 
00128 pitch_degree=0;//ピッチ（10進数表記）
00129 roll_degree=0;//ロール（10進数表記）
00130 azimaith_degree=0;//アジマス（10進数表記）
00131 
00132 el=deg_to_rad(pitch_degree);//ピッチ（rad）
00133 roll=deg_to_rad(roll_degree);//ロール（rad）
00134 az=deg_to_rad(azimaith_degree);//アジマス（rad）
00135 
00136 float ned_to_body[9]={-cos(el)*cos(az) , cos(el)*sin(az)    ,-sin(el),
00137                       
00138                       cos(roll)*sin(az)+sin(roll)*sin(el)*cos(az),
00139                       cos(az)*cos(roll)+sin(roll)*sin(el)*sin(az),                  
00140                       sin(roll)*cos(el),
00141                       
00142                       -sin(roll)*sin(az)+cos(roll)*sin(el)*cos(az),
00143                       -sin(roll)*cos(az)+cos(roll)*sin(el)*sin(az),                  
00144                         cos(roll)*cos(el)
00145         };
00146         
00147         NED_to_BODY.SetComps(ned_to_body);    
00148 /*--------------------------------------------------------------------*/
00149 /*-------------------計算を行う-----------------------*/
00150         Q3 = Q1 * Q2;    
00151         ECEF = ECI_to_ECEF*ECI;
00152         NED  = ECEF_to_NED*ECEF;
00153         BODY = NED_to_BODY*NED;
00154 /*-----------------------------------------------------*/
00155 /*------------Tera Termで計算結果を表示する--------------------*/
00156     toString(Q3);
00157     toString_V(ECEF);
00158     toString_V(NED);
00159     toString_V(BODY);
00160     //pc.printf("%.3f\r\n",sin(deg_to_rad(30)));
00161 /*-----------------------------------------------------*/
00162 }