main.cpp

00001 #include "mbed.h"
00002 #include "MPU9250.h"
00003 #include "math.h"
00004 #include "Eigen/Core.h"     // 線形代数のライブラリ
00005 #include "Eigen/Geometry.h" // 同上
00006 #define  PI 3.141592654     // 円周率
00007 
00008 Timer       timer;
00009 Ticker      ticker;
00010 DigitalOut  led1(LED1);
00011 DigitalOut  led2(LED2);
00012 DigitalOut  led3(LED3);
00013 InterruptIn flt_pin(p30);
00014 Serial      pc (USBTX, USBRX);
00015 MPU9250     mpu(p28,p27);
00016 
00017 const float t_calibrate = 25.f;
00018 const float dt = 0.05;
00019 
00020 // 変数用意
00021 int   N = 0;                // 較正に用いるサンプル数
00022 
00023 float gyro[3];              // ジャイロセンサの出力
00024 float b_gyro[3] = {};       // ジャイロセンサのバイアス誤差．
00025 float acc[3];               // 加速度センサの出力
00026 // float acc_norm;             // 加速度ベクトルの2-ノルム（長さ）．
00027 float acc_normalized[3];    // 正規化した（長さ1の）加速度ベクトル．
00028 
00029 
00030 float pitch = 0.f;          // ピッチ角.
00031 float roll  = 0.f;          // バンク（ロール）角.
00032 float yaw;                  // 方位角（ヨー角）．
00033 Eigen::Vector4f q;          // 姿勢クォータニオン
00034 Eigen::Matrix4f Omg;
00035 Eigen::Vector4f q_dot;
00036 float omg[3];
00037 float q00_22, q11_33;
00038 
00039 void calibrate();
00040 void set_initial_quaternion(float roll, float pitch);
00041 void flt_pin_rise();
00042 void flt_pin_fall();
00043 void update();
00044 void update_correction();
00045 
00046 int main()
00047 {
00048     mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
00049     mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
00050     
00051     pc.printf("=\r\n\n");
00052     pc.printf("Calibration (%3.1f s) will start in\r\n  5\r\n", t_calibrate);
00053     wait(1.0);
00054     pc.printf("  4\r\n");
00055     wait(1.0);
00056     pc.printf("  3\r\n");
00057     wait(1.0);
00058     pc.printf("  2\r\n");
00059     wait(1.0);
00060     pc.printf("  1\r\n\n");
00061     wait(1.0);
00062     
00063     pc.printf("Calibrating...  ");
00064     ticker.attach(&calibrate, 0.01);
00065     timer.start();
00066     while (timer.read() < t_calibrate);
00067     ticker.detach();
00068     pc.printf("Done. (%d samples)\r\n\n", N);
00069 //    timer.stop();
00070     
00071     b_gyro[0] /= N;
00072     b_gyro[1] /= N;
00073     b_gyro[2] /= N;
00074     roll  /= N;
00075     pitch /= N;
00076     
00077     printf("Initial Attitude\r\n");
00078     printf("  bank:%6.1f deg,   pitch:%6.1f deg\r\n\n",
00079               roll*180/PI ,     pitch*180/PI);
00080     
00081     set_initial_quaternion(roll, pitch);
00082     
00083     pc.printf("waiting for launch...\r\n");
00084     flt_pin.mode(PullUp);
00085 //    flt_pin.fall(&flt_pin_fall);
00086     flt_pin.rise(&flt_pin_rise);
00087 //    ticker.attach(&update_correction, dt);
00088     while(1);
00089 }
00090 
00091 void calibrate()
00092 {
00093     //////////////////////////////////////
00094     //                                  //
00095     // ジャイロセンサの較正・初期姿勢角の取得 //
00096     //                                  //
00097     //////////////////////////////////////
00098     
00099     // b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
00100     mpu.getGyro(gyro);
00101     b_gyro[0] += gyro[0];
00102     b_gyro[1] += gyro[1];
00103     b_gyro[2] += gyro[2];
00104     
00105     // 重力加速度ベクトルを使って，姿勢角を算出
00106     mpu.getAccelero(acc);
00107 //    acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
00108 
00109 //    acc_normalized[0] = acc[0] / acc_norm;
00110 //    acc_normalized[1] = acc[1] / acc_norm;
00111 //    acc_normalized[2] = acc[2] / acc_norm;
00112     acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
00113     
00114     // N 回の姿勢角の平均をとって初期姿勢角とする
00115     pitch += asin (acc_normalized[2]);
00116     roll  += atan2(acc[1], -acc[0]);
00117     
00118     // count up
00119     N++;
00120 }
00121 
00122 void set_initial_quaternion(float roll, float pitch)
00123 {
00124     /////////////////////////////////////////
00125     //                                     //
00126     // 初期姿勢角から初期姿勢クォータニオンを計算 //
00127     //                                     //
00128     /////////////////////////////////////////
00129     float c_roll2 = cos(roll/2);      // 繰り返しの計算を避けるための一時変数
00130     float s_roll2 = sin(roll/2);      // 同上
00131     
00132     Eigen::Matrix4f qx0_otimes;
00133     qx0_otimes << c_roll2,-s_roll2,    0   ,    0   ,
00134                   s_roll2, c_roll2,    0   ,    0   ,
00135                      0   ,    0   , c_roll2, s_roll2,
00136                      0   ,    0   ,-s_roll2, c_roll2;
00137     
00138     Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
00139     
00140     // 初期姿勢クォータニオン q
00141     q = qx0_otimes * q_y0;
00142 }
00143 
00144 void flt_pin_rise()
00145 {
00146     pc.printf("Launch!\r\n");
00147     ticker.attach(&update, dt);
00148 }
00149 
00150 void flt_pin_fall()
00151 {
00152     pc.printf("fall.\r\n");
00153     ticker.detach();
00154 }
00155 
00156  /**
00157   * 観測値を用いて姿勢角を更新します．
00158   * ジャイロセンサの出力を単純に積算します．
00159   */
00160 void update()
00161 {
00162     mpu.getGyro(gyro);
00163     
00164     omg[0] =  gyro[2] - b_gyro[2];
00165     omg[1] = -gyro[1] + b_gyro[1];
00166     omg[2] =  gyro[0] - b_gyro[0];
00167     
00168     // 姿勢クォータニオンの時間微分を計算
00169     Omg << 0.f   ,-omg[0],-omg[1],-omg[2],
00170            omg[0], 0.f   , omg[2],-omg[1],
00171            omg[1],-omg[2], 0.f   , omg[0],
00172            omg[2], omg[1],-omg[0], 0.f   ;
00173 //    q_dot = 0.5f * Omg * q;
00174     
00175     // 現在時刻の姿勢クォータニオン
00176     q += Omg * q * 0.5f * dt;
00177     q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
00178     
00179     // 姿勢クォータニオンからオイラー角への変換
00180     q00_22 = (q[0] + q[2])*(q[0] - q[2]);
00181     q11_33 = (q[1] + q[3])*(q[1] - q[3]);
00182     roll  = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
00183     pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
00184 //    yaw   = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
00185     
00186     pc.printf("%11.7f   ",timer.read());
00187 //    pc.printf("%6.1f %6.1f %6.1f\r\n",
00188 //               roll*180/PI,  pitch*180/PI,  yaw*180/PI);
00189     pc.printf("%6.1f %6.1f\r\n",
00190                roll*180/PI,  pitch*180/PI);
00191 }
00192 
00193 Eigen::Vector3f bB_tilde;
00194 Eigen::Vector3f bB_hat;
00195 Eigen::Vector3f bG(0.f, 0.f, -1.f);
00196 Eigen::Vector3f rot_vec;
00197 float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
00198 Eigen::Matrix3f dcm;
00199 Eigen::Matrix4f rot_q_otimes;
00200 const float eps = 0.01;
00201 float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
00202 float a_norm_div;
00203 
00204  /**
00205   * 観測値を用いて姿勢角を更新します．
00206   * ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
00207   */
00208 void update_correction()
00209 {
00210     mpu.getGyro(gyro);
00211     
00212     omg[0] =  gyro[2] - b_gyro[2];
00213     omg[1] = -gyro[1] + b_gyro[1];
00214     omg[2] =  gyro[0] - b_gyro[0];
00215     
00216     // 姿勢クォータニオンの時間微分を計算
00217     Omg << 0.f   ,-omg[0],-omg[1],-omg[2],
00218            omg[0], 0.f   , omg[2],-omg[1],
00219            omg[1],-omg[2], 0.f   , omg[0],
00220            omg[2], omg[1],-omg[0], 0.f   ;
00221 //    q_dot = 0.5f * Omg * q;
00222     
00223     // 現在時刻の姿勢クォータニオン
00224     q += Omg * q * 0.5f * dt;
00225     q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
00226     
00227     // 姿勢クォータニオンからオイラー角への変換
00228     q00 = q[0]*q[0];
00229     q01 = q[0]*q[1];
00230     q02 = q[0]*q[2];
00231     q03 = q[0]*q[3];
00232     q11 = q[1]*q[1];
00233     q12 = q[1]*q[2];
00234     q13 = q[1]*q[3];
00235     q22 = q[2]*q[2];
00236     q23 = q[2]*q[3];
00237     q33 = q[3]*q[3];
00238     dcm <<  q00+q11-q22-q33,   2*(q12+q03)  ,   2*(q13-q02)  ,
00239               2*(q12-q03)  , q00-q11+q22-q33,   2*(q23+q01)  ,
00240               2*(q13+q02)  ,   2*(q23-q01)  , q00-q11-q22+q33;
00241     bB_hat = dcm*bG;
00242     mpu.getAccelero(acc);
00243     a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
00244     bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
00245     rot_vec = bB_hat.cross(bB_hat - bB_tilde);
00246     rot_q[1] = 0.5f*eps*rot_vec[0];
00247     rot_q[2] = 0.5f*eps*rot_vec[1];
00248     rot_q[3] = 0.5f*eps*rot_vec[2];
00249     rot_q_otimes <<   1.f   ,-rot_q[1],-rot_q[2],-rot_q[3],
00250                     rot_q[1],   1.f   , rot_q[3],-rot_q[2],
00251                     rot_q[2],-rot_q[3],   1.f   , rot_q[1],
00252                     rot_q[3], rot_q[2],-rot_q[1],   1.f   ;
00253     q = rot_q_otimes * q;
00254     q /= sqrtf(q00+q11+q22+q33);
00255     
00256     q00_22 = q00 - q22;
00257     q11_33 = q11 - q33;
00258     roll  = atan2( 2*(q23+q01), q00_22 - q11_33);
00259     pitch = asin (-2*(q13-q02));
00260     yaw   = atan2( 2*(q12+q03), q00_22 + q11_33);
00261     
00262 //    pc.printf("%11.7f   ",timer.read());
00263     pc.printf("%6.1f %6.1f %6.1f\r\n",
00264                roll*180/PI,  pitch*180/PI,  yaw*180/PI);
00265 //    pc.printf("%6.1f %6.1f\r\n",
00266 //               roll*180/PI,  pitch*180/PI);
00267 }