エラー判定用 '\0' + クォータニオンのfloat型4成分,合計17byteのデータをひたすらバイナリ形式で送り続ける.最初のバイトが '\0' でなかったらズレているので,適当なバイト数見送って先頭が '\0' になるよう合わせること.
Diff: main.cpp
- Revision:
- 0:2a7221ee9861
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 21 07:58:57 2018 +0000 @@ -0,0 +1,293 @@ +#include "mbed.h" +#include "MPU9250.h" +#include "math.h" +#include "Eigen/Core.h" // 線形代数のライブラリ +#include "Eigen/Geometry.h" // 同上 +#define PI 3.141592654 // 円周率 + +Timer timer; +Ticker ticker; +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); +//InterruptIn flt_pin(p30); +Serial pc (USBTX, USBRX); +MPU9250 mpu(p28,p27); + +const float t_calibrate = 25.f; +const float dt = 0.05; + +// 変数用意 +int N = 0; // 較正に用いるサンプル数 + +float gyro[3]; // ジャイロセンサの出力 +float b_gyro[3] = {}; // ジャイロセンサのバイアス誤差. +float acc[3]; // 加速度センサの出力 +// float acc_norm; // 加速度ベクトルの2-ノルム(長さ). +float acc_normalized[3]; // 正規化した(長さ1の)加速度ベクトル. + + +float pitch = 0.f; // ピッチ角. +float roll = 0.f; // バンク(ロール)角. +float yaw; // 方位角(ヨー角). +Eigen::Vector4f q; // 姿勢クォータニオン +Eigen::Matrix4f Omg; +Eigen::Vector4f q_dot; +float omg[3]; +float q00_22, q11_33; + +void calibrate(); +void set_initial_quaternion(float roll, float pitch); +void flt_pin_rise(); +void flt_pin_fall(); +void update(); +void update_correction(); + +int main() +{ + mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G); + mpu.setGyroRange(MPU9250_GYRO_RANGE_2000); + + led1 = 1; + led2 = 1; + led3 = 1; +// pc.printf("=\r\n\n"); +// pc.printf("Calibration (%3.1f s) will start in\r\n 5\r\n", t_calibrate); + wait(1.0); +// pc.printf(" 4\r\n"); + wait(1.0); +// pc.printf(" 3\r\n"); + wait(1.0); +// pc.printf(" 2\r\n"); + wait(1.0); +// pc.printf(" 1\r\n\n"); + wait(1.0); + led1 = 0; +// pc.printf("Calibrating... "); + ticker.attach(&calibrate, 0.01); + timer.start(); + while (timer.read() < t_calibrate); + ticker.detach(); +// pc.printf("Done. (%d samples)\r\n\n", N); +// timer.stop(); + led2 = 0; + + b_gyro[0] /= N; + b_gyro[1] /= N; + b_gyro[2] /= N; + roll /= N; + pitch /= N; + +// printf("Initial Attitude\r\n"); +// printf(" bank:%6.1f deg, pitch:%6.1f deg\r\n\n", +// roll*180/PI , pitch*180/PI); + + set_initial_quaternion(roll, pitch); + + led3 = 0; +// pc.printf("waiting for launch...\r\n"); +// flt_pin.mode(PullUp); +// flt_pin.fall(&flt_pin_fall); +// flt_pin.rise(&flt_pin_rise); + ticker.attach(&update_correction, dt); + led4 = 1; + while(1); +} + +void calibrate() +{ + ////////////////////////////////////// + // // + // ジャイロセンサの較正・初期姿勢角の取得 // + // // + ////////////////////////////////////// + + // b_gyro にはジャイロセンサの出力の平均(バイアス誤差)を格納. + mpu.getGyro(gyro); + b_gyro[0] += gyro[0]; + b_gyro[1] += gyro[1]; + b_gyro[2] += gyro[2]; + + // 重力加速度ベクトルを使って,姿勢角を算出 + mpu.getAccelero(acc); +// acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); + +// acc_normalized[0] = acc[0] / acc_norm; +// acc_normalized[1] = acc[1] / acc_norm; +// acc_normalized[2] = acc[2] / acc_norm; + acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); + + // N 回の姿勢角の平均をとって初期姿勢角とする + pitch += asin (acc_normalized[2]); + roll += atan2(acc[1], -acc[0]); + + // count up + N++; +} + +void set_initial_quaternion(float roll, float pitch) +{ + ///////////////////////////////////////// + // // + // 初期姿勢角から初期姿勢クォータニオンを計算 // + // // + ///////////////////////////////////////// + float c_roll2 = cos(roll/2); // 繰り返しの計算を避けるための一時変数 + float s_roll2 = sin(roll/2); // 同上 + + Eigen::Matrix4f qx0_otimes; + qx0_otimes << c_roll2,-s_roll2, 0 , 0 , + s_roll2, c_roll2, 0 , 0 , + 0 , 0 , c_roll2, s_roll2, + 0 , 0 ,-s_roll2, c_roll2; + + Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f); + + // 初期姿勢クォータニオン q + q = qx0_otimes * q_y0; +} + +void flt_pin_rise() +{ + pc.printf("Launch!\r\n"); + ticker.attach(&update, dt); +} + +void flt_pin_fall() +{ + pc.printf("fall.\r\n"); + ticker.detach(); +} + + /** + * 観測値を用いて姿勢角を更新します. + * ジャイロセンサの出力を単純に積算します. + */ +void update() +{ + mpu.getGyro(gyro); + + omg[0] = gyro[2] - b_gyro[2]; + omg[1] = -gyro[1] + b_gyro[1]; + omg[2] = gyro[0] - b_gyro[0]; + + // 姿勢クォータニオンの時間微分を計算 + Omg << 0.f ,-omg[0],-omg[1],-omg[2], + omg[0], 0.f , omg[2],-omg[1], + omg[1],-omg[2], 0.f , omg[0], + omg[2], omg[1],-omg[0], 0.f ; +// q_dot = 0.5f * Omg * q; + + // 現在時刻の姿勢クォータニオン + q += Omg * q * 0.5f * dt; + q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); + + // 姿勢クォータニオンからオイラー角への変換 + q00_22 = (q[0] + q[2])*(q[0] - q[2]); + q11_33 = (q[1] + q[3])*(q[1] - q[3]); + roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33); + pitch = asin (-2*(q[1]*q[3]-q[0]*q[2])); +// yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33); + +// pc.printf("%11.7f ",timer.read()); +// pc.printf("%6.1f %6.1f %6.1f\r\n", +// roll*180/PI, pitch*180/PI, yaw*180/PI); +// pc.printf("%6.1f %6.1f\r\n", +// roll*180/PI, pitch*180/PI); +} + +Eigen::Vector3f bB_tilde; +Eigen::Vector3f bB_hat; +Eigen::Vector3f bG(0.f, 0.f, -1.f); +Eigen::Vector3f rot_vec; +float rot_q[4] = {1.f, 0.f, 0.f, 0.f}; +Eigen::Matrix3f dcm; +Eigen::Matrix4f rot_q_otimes; +const float eps = 0.01; +float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; +float a_norm_div; +float send_buff[4]; +char* send_bufc = (char*)send_buff; + + /** + * 観測値を用いて姿勢角を更新します. + * ジャイロセンサの出力を積算した上で,加速度センサの出力を用いて補正します. + */ +void update_correction() +{ + mpu.getGyro(gyro); + + omg[0] = gyro[2] - b_gyro[2]; + omg[1] = -gyro[1] + b_gyro[1]; + omg[2] = gyro[0] - b_gyro[0]; + + // 姿勢クォータニオンの時間微分を計算 + Omg << 0.f ,-omg[0],-omg[1],-omg[2], + omg[0], 0.f , omg[2],-omg[1], + omg[1],-omg[2], 0.f , omg[0], + omg[2], omg[1],-omg[0], 0.f ; +// q_dot = 0.5f * Omg * q; + + // 現在時刻の姿勢クォータニオン + q += Omg * q * 0.5f * dt; + q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]); + + // 姿勢クォータニオンからオイラー角への変換 + q00 = q[0]*q[0]; + q01 = q[0]*q[1]; + q02 = q[0]*q[2]; + q03 = q[0]*q[3]; + q11 = q[1]*q[1]; + q12 = q[1]*q[2]; + q13 = q[1]*q[3]; + q22 = q[2]*q[2]; + q23 = q[2]*q[3]; + q33 = q[3]*q[3]; + dcm << q00+q11-q22-q33, 2*(q12+q03) , 2*(q13-q02) , + 2*(q12-q03) , q00-q11+q22-q33, 2*(q23+q01) , + 2*(q13+q02) , 2*(q23-q01) , q00-q11-q22+q33; + bB_hat = dcm*bG; + mpu.getAccelero(acc); + a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]); + bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div; + rot_vec = bB_hat.cross(bB_hat - bB_tilde); + rot_q[1] = 0.5f*eps*rot_vec[0]; + rot_q[2] = 0.5f*eps*rot_vec[1]; + rot_q[3] = 0.5f*eps*rot_vec[2]; + rot_q_otimes << 1.f ,-rot_q[1],-rot_q[2],-rot_q[3], + rot_q[1], 1.f , rot_q[3],-rot_q[2], + rot_q[2],-rot_q[3], 1.f , rot_q[1], + rot_q[3], rot_q[2],-rot_q[1], 1.f ; + q = rot_q_otimes * q; +// q00 = q[0]*q[0]; // Note that these values are based on +// q11 = q[1]*q[1]; // rotated quatenion (L253), so they are diferent from +// q22 = q[2]*q[2]; // the values calculated above (L228-237). +// q33 = q[3]*q[3]; +// q /= sqrtf(q00 + q11 + q22 + q33); + + q /= sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); + send_buff[0] = q[0]; + send_buff[1] = q[1]; + send_buff[2] = q[2]; + send_buff[3] = q[3]; +// send_buff[0] = 0.1f; +// send_buff[1] = 0.2f; +// send_buff[2] = 0.3f; +// send_buff[3] = 0.4f; +// pc.write(send_buf,4); + pc.putc('\0'); + for (int i = 0; i < 16; i++) + pc.putc(send_bufc[i]); +// q00_22 = q00 - q22; +// q11_33 = q11 - q33; +// roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33); +// pitch = asin (-2*(q[1]*q[3]-q[0]*q[2])); +// yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33); + +// pc.printf("%11.7f ",timer.read()); +// pc.printf("%6.1f %6.1f %6.1f\r\n", +// roll*180/PI, pitch*180/PI, yaw*180/PI); +// pc.printf("%6.1f %6.1f\r\n", +// roll*180/PI, pitch*180/PI); +} \ No newline at end of file