CIA
エラー判定用 '\0' + クォータニオンのfloat型4成分，合計17byteのデータをひたすらバイナリ形式で送り続ける．最初のバイトが '\0' でなかったらズレているので，適当なバイト数見送って先頭が '\0' になるよう合わせること．
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 21:52:15 by
1.7.2