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