CIA / Mbed 2 deprecated MPU9250_Quaternion_Binary_Serial

エラー判定用 '\0' + クォータニオンのfloat型4成分，合計17byteのデータをひたすらバイナリ形式で送り続ける．最初のバイトが '\0' でなかったらズレているので，適当なバイト数見送って先頭が '\0' になるよう合わせること．

Dependencies:   Eigen mbed

main.cpp@0:2a7221ee9861, 2018-03-21 (annotated)

Committer:
daqn
Date:
Wed Mar 21 07:58:57 2018 +0000
Revision:
0:2a7221ee9861
i dont know what should i write here

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daqn 0:2a7221ee9861 1 #include "mbed.h"
daqn 0:2a7221ee9861 2 #include "MPU9250.h"
daqn 0:2a7221ee9861 3 #include "math.h"
daqn 0:2a7221ee9861 4 #include "Eigen/Core.h" // 線形代数のライブラリ
daqn 0:2a7221ee9861 5 #include "Eigen/Geometry.h" // 同上
daqn 0:2a7221ee9861 6 #define PI 3.141592654 // 円周率
daqn 0:2a7221ee9861 7
daqn 0:2a7221ee9861 8 Timer timer;
daqn 0:2a7221ee9861 9 Ticker ticker;
daqn 0:2a7221ee9861 10 DigitalOut led1(LED1);
daqn 0:2a7221ee9861 11 DigitalOut led2(LED2);
daqn 0:2a7221ee9861 12 DigitalOut led3(LED3);
daqn 0:2a7221ee9861 13 DigitalOut led4(LED4);
daqn 0:2a7221ee9861 14 //InterruptIn flt_pin(p30);
daqn 0:2a7221ee9861 15 Serial pc (USBTX, USBRX);
daqn 0:2a7221ee9861 16 MPU9250 mpu(p28,p27);
daqn 0:2a7221ee9861 17
daqn 0:2a7221ee9861 18 const float t_calibrate = 25.f;
daqn 0:2a7221ee9861 19 const float dt = 0.05;
daqn 0:2a7221ee9861 20
daqn 0:2a7221ee9861 21 // 変数用意
daqn 0:2a7221ee9861 22 int N = 0; // 較正に用いるサンプル数
daqn 0:2a7221ee9861 23
daqn 0:2a7221ee9861 24 float gyro[3]; // ジャイロセンサの出力
daqn 0:2a7221ee9861 25 float b_gyro[3] = {}; // ジャイロセンサのバイアス誤差．
daqn 0:2a7221ee9861 26 float acc[3]; // 加速度センサの出力
daqn 0:2a7221ee9861 27 // float acc_norm; // 加速度ベクトルの2-ノルム（長さ）．
daqn 0:2a7221ee9861 28 float acc_normalized[3]; // 正規化した（長さ1の）加速度ベクトル．
daqn 0:2a7221ee9861 29
daqn 0:2a7221ee9861 30
daqn 0:2a7221ee9861 31 float pitch = 0.f; // ピッチ角.
daqn 0:2a7221ee9861 32 float roll = 0.f; // バンク（ロール）角.
daqn 0:2a7221ee9861 33 float yaw; // 方位角（ヨー角）．
daqn 0:2a7221ee9861 34 Eigen::Vector4f q; // 姿勢クォータニオン
daqn 0:2a7221ee9861 35 Eigen::Matrix4f Omg;
daqn 0:2a7221ee9861 36 Eigen::Vector4f q_dot;
daqn 0:2a7221ee9861 37 float omg[3];
daqn 0:2a7221ee9861 38 float q00_22, q11_33;
daqn 0:2a7221ee9861 39
daqn 0:2a7221ee9861 40 void calibrate();
daqn 0:2a7221ee9861 41 void set_initial_quaternion(float roll, float pitch);
daqn 0:2a7221ee9861 42 void flt_pin_rise();
daqn 0:2a7221ee9861 43 void flt_pin_fall();
daqn 0:2a7221ee9861 44 void update();
daqn 0:2a7221ee9861 45 void update_correction();
daqn 0:2a7221ee9861 46
daqn 0:2a7221ee9861 47 int main()
daqn 0:2a7221ee9861 48 {
daqn 0:2a7221ee9861 49 mpu.setAcceleroRange(MPU9250_ACCELERO_RANGE_16G);
daqn 0:2a7221ee9861 50 mpu.setGyroRange(MPU9250_GYRO_RANGE_2000);
daqn 0:2a7221ee9861 51
daqn 0:2a7221ee9861 52 led1 = 1;
daqn 0:2a7221ee9861 53 led2 = 1;
daqn 0:2a7221ee9861 54 led3 = 1;
daqn 0:2a7221ee9861 55 // pc.printf("=\r\n\n");
daqn 0:2a7221ee9861 56 // pc.printf("Calibration (%3.1f s) will start in\r\n 5\r\n", t_calibrate);
daqn 0:2a7221ee9861 57 wait(1.0);
daqn 0:2a7221ee9861 58 // pc.printf(" 4\r\n");
daqn 0:2a7221ee9861 59 wait(1.0);
daqn 0:2a7221ee9861 60 // pc.printf(" 3\r\n");
daqn 0:2a7221ee9861 61 wait(1.0);
daqn 0:2a7221ee9861 62 // pc.printf(" 2\r\n");
daqn 0:2a7221ee9861 63 wait(1.0);
daqn 0:2a7221ee9861 64 // pc.printf(" 1\r\n\n");
daqn 0:2a7221ee9861 65 wait(1.0);
daqn 0:2a7221ee9861 66 led1 = 0;
daqn 0:2a7221ee9861 67 // pc.printf("Calibrating... ");
daqn 0:2a7221ee9861 68 ticker.attach(&calibrate, 0.01);
daqn 0:2a7221ee9861 69 timer.start();
daqn 0:2a7221ee9861 70 while (timer.read() < t_calibrate);
daqn 0:2a7221ee9861 71 ticker.detach();
daqn 0:2a7221ee9861 72 // pc.printf("Done. (%d samples)\r\n\n", N);
daqn 0:2a7221ee9861 73 // timer.stop();
daqn 0:2a7221ee9861 74 led2 = 0;
daqn 0:2a7221ee9861 75
daqn 0:2a7221ee9861 76 b_gyro[0] /= N;
daqn 0:2a7221ee9861 77 b_gyro[1] /= N;
daqn 0:2a7221ee9861 78 b_gyro[2] /= N;
daqn 0:2a7221ee9861 79 roll /= N;
daqn 0:2a7221ee9861 80 pitch /= N;
daqn 0:2a7221ee9861 81
daqn 0:2a7221ee9861 82 // printf("Initial Attitude\r\n");
daqn 0:2a7221ee9861 83 // printf(" bank:%6.1f deg, pitch:%6.1f deg\r\n\n",
daqn 0:2a7221ee9861 84 // roll*180/PI , pitch*180/PI);
daqn 0:2a7221ee9861 85
daqn 0:2a7221ee9861 86 set_initial_quaternion(roll, pitch);
daqn 0:2a7221ee9861 87
daqn 0:2a7221ee9861 88 led3 = 0;
daqn 0:2a7221ee9861 89 // pc.printf("waiting for launch...\r\n");
daqn 0:2a7221ee9861 90 // flt_pin.mode(PullUp);
daqn 0:2a7221ee9861 91 // flt_pin.fall(&flt_pin_fall);
daqn 0:2a7221ee9861 92 // flt_pin.rise(&flt_pin_rise);
daqn 0:2a7221ee9861 93 ticker.attach(&update_correction, dt);
daqn 0:2a7221ee9861 94 led4 = 1;
daqn 0:2a7221ee9861 95 while(1);
daqn 0:2a7221ee9861 96 }
daqn 0:2a7221ee9861 97
daqn 0:2a7221ee9861 98 void calibrate()
daqn 0:2a7221ee9861 99 {
daqn 0:2a7221ee9861 100 //////////////////////////////////////
daqn 0:2a7221ee9861 101 // //
daqn 0:2a7221ee9861 102 // ジャイロセンサの較正・初期姿勢角の取得 //
daqn 0:2a7221ee9861 103 // //
daqn 0:2a7221ee9861 104 //////////////////////////////////////
daqn 0:2a7221ee9861 105
daqn 0:2a7221ee9861 106 // b_gyro にはジャイロセンサの出力の平均（バイアス誤差）を格納．
daqn 0:2a7221ee9861 107 mpu.getGyro(gyro);
daqn 0:2a7221ee9861 108 b_gyro[0] += gyro[0];
daqn 0:2a7221ee9861 109 b_gyro[1] += gyro[1];
daqn 0:2a7221ee9861 110 b_gyro[2] += gyro[2];
daqn 0:2a7221ee9861 111
daqn 0:2a7221ee9861 112 // 重力加速度ベクトルを使って，姿勢角を算出
daqn 0:2a7221ee9861 113 mpu.getAccelero(acc);
daqn 0:2a7221ee9861 114 // acc_norm = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
daqn 0:2a7221ee9861 115
daqn 0:2a7221ee9861 116 // acc_normalized[0] = acc[0] / acc_norm;
daqn 0:2a7221ee9861 117 // acc_normalized[1] = acc[1] / acc_norm;
daqn 0:2a7221ee9861 118 // acc_normalized[2] = acc[2] / acc_norm;
daqn 0:2a7221ee9861 119 acc_normalized[2] = acc[2] / sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
daqn 0:2a7221ee9861 120
daqn 0:2a7221ee9861 121 // N 回の姿勢角の平均をとって初期姿勢角とする
daqn 0:2a7221ee9861 122 pitch += asin (acc_normalized[2]);
daqn 0:2a7221ee9861 123 roll += atan2(acc[1], -acc[0]);
daqn 0:2a7221ee9861 124
daqn 0:2a7221ee9861 125 // count up
daqn 0:2a7221ee9861 126 N++;
daqn 0:2a7221ee9861 127 }
daqn 0:2a7221ee9861 128
daqn 0:2a7221ee9861 129 void set_initial_quaternion(float roll, float pitch)
daqn 0:2a7221ee9861 130 {
daqn 0:2a7221ee9861 131 /////////////////////////////////////////
daqn 0:2a7221ee9861 132 // //
daqn 0:2a7221ee9861 133 // 初期姿勢角から初期姿勢クォータニオンを計算 //
daqn 0:2a7221ee9861 134 // //
daqn 0:2a7221ee9861 135 /////////////////////////////////////////
daqn 0:2a7221ee9861 136 float c_roll2 = cos(roll/2); // 繰り返しの計算を避けるための一時変数
daqn 0:2a7221ee9861 137 float s_roll2 = sin(roll/2); // 同上
daqn 0:2a7221ee9861 138
daqn 0:2a7221ee9861 139 Eigen::Matrix4f qx0_otimes;
daqn 0:2a7221ee9861 140 qx0_otimes << c_roll2,-s_roll2, 0 , 0 ,
daqn 0:2a7221ee9861 141 s_roll2, c_roll2, 0 , 0 ,
daqn 0:2a7221ee9861 142 0 , 0 , c_roll2, s_roll2,
daqn 0:2a7221ee9861 143 0 , 0 ,-s_roll2, c_roll2;
daqn 0:2a7221ee9861 144
daqn 0:2a7221ee9861 145 Eigen::Vector4f q_y0(cos(pitch/2), 0.f, sin(pitch/2), 0.f);
daqn 0:2a7221ee9861 146
daqn 0:2a7221ee9861 147 // 初期姿勢クォータニオン q
daqn 0:2a7221ee9861 148 q = qx0_otimes * q_y0;
daqn 0:2a7221ee9861 149 }
daqn 0:2a7221ee9861 150
daqn 0:2a7221ee9861 151 void flt_pin_rise()
daqn 0:2a7221ee9861 152 {
daqn 0:2a7221ee9861 153 pc.printf("Launch!\r\n");
daqn 0:2a7221ee9861 154 ticker.attach(&update, dt);
daqn 0:2a7221ee9861 155 }
daqn 0:2a7221ee9861 156
daqn 0:2a7221ee9861 157 void flt_pin_fall()
daqn 0:2a7221ee9861 158 {
daqn 0:2a7221ee9861 159 pc.printf("fall.\r\n");
daqn 0:2a7221ee9861 160 ticker.detach();
daqn 0:2a7221ee9861 161 }
daqn 0:2a7221ee9861 162
daqn 0:2a7221ee9861 163 /**
daqn 0:2a7221ee9861 164 * 観測値を用いて姿勢角を更新します．
daqn 0:2a7221ee9861 165 * ジャイロセンサの出力を単純に積算します．
daqn 0:2a7221ee9861 166 */
daqn 0:2a7221ee9861 167 void update()
daqn 0:2a7221ee9861 168 {
daqn 0:2a7221ee9861 169 mpu.getGyro(gyro);
daqn 0:2a7221ee9861 170
daqn 0:2a7221ee9861 171 omg[0] = gyro[2] - b_gyro[2];
daqn 0:2a7221ee9861 172 omg[1] = -gyro[1] + b_gyro[1];
daqn 0:2a7221ee9861 173 omg[2] = gyro[0] - b_gyro[0];
daqn 0:2a7221ee9861 174
daqn 0:2a7221ee9861 175 // 姿勢クォータニオンの時間微分を計算
daqn 0:2a7221ee9861 176 Omg << 0.f ,-omg[0],-omg[1],-omg[2],
daqn 0:2a7221ee9861 177 omg[0], 0.f , omg[2],-omg[1],
daqn 0:2a7221ee9861 178 omg[1],-omg[2], 0.f , omg[0],
daqn 0:2a7221ee9861 179 omg[2], omg[1],-omg[0], 0.f ;
daqn 0:2a7221ee9861 180 // q_dot = 0.5f * Omg * q;
daqn 0:2a7221ee9861 181
daqn 0:2a7221ee9861 182 // 現在時刻の姿勢クォータニオン
daqn 0:2a7221ee9861 183 q += Omg * q * 0.5f * dt;
daqn 0:2a7221ee9861 184 q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
daqn 0:2a7221ee9861 185
daqn 0:2a7221ee9861 186 // 姿勢クォータニオンからオイラー角への変換
daqn 0:2a7221ee9861 187 q00_22 = (q[0] + q[2])*(q[0] - q[2]);
daqn 0:2a7221ee9861 188 q11_33 = (q[1] + q[3])*(q[1] - q[3]);
daqn 0:2a7221ee9861 189 roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
daqn 0:2a7221ee9861 190 pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
daqn 0:2a7221ee9861 191 // yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
daqn 0:2a7221ee9861 192
daqn 0:2a7221ee9861 193 // pc.printf("%11.7f ",timer.read());
daqn 0:2a7221ee9861 194 // pc.printf("%6.1f %6.1f %6.1f\r\n",
daqn 0:2a7221ee9861 195 // roll*180/PI, pitch*180/PI, yaw*180/PI);
daqn 0:2a7221ee9861 196 // pc.printf("%6.1f %6.1f\r\n",
daqn 0:2a7221ee9861 197 // roll*180/PI, pitch*180/PI);
daqn 0:2a7221ee9861 198 }
daqn 0:2a7221ee9861 199
daqn 0:2a7221ee9861 200 Eigen::Vector3f bB_tilde;
daqn 0:2a7221ee9861 201 Eigen::Vector3f bB_hat;
daqn 0:2a7221ee9861 202 Eigen::Vector3f bG(0.f, 0.f, -1.f);
daqn 0:2a7221ee9861 203 Eigen::Vector3f rot_vec;
daqn 0:2a7221ee9861 204 float rot_q[4] = {1.f, 0.f, 0.f, 0.f};
daqn 0:2a7221ee9861 205 Eigen::Matrix3f dcm;
daqn 0:2a7221ee9861 206 Eigen::Matrix4f rot_q_otimes;
daqn 0:2a7221ee9861 207 const float eps = 0.01;
daqn 0:2a7221ee9861 208 float q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
daqn 0:2a7221ee9861 209 float a_norm_div;
daqn 0:2a7221ee9861 210 float send_buff[4];
daqn 0:2a7221ee9861 211 char* send_bufc = (char*)send_buff;
daqn 0:2a7221ee9861 212
daqn 0:2a7221ee9861 213 /**
daqn 0:2a7221ee9861 214 * 観測値を用いて姿勢角を更新します．
daqn 0:2a7221ee9861 215 * ジャイロセンサの出力を積算した上で，加速度センサの出力を用いて補正します．
daqn 0:2a7221ee9861 216 */
daqn 0:2a7221ee9861 217 void update_correction()
daqn 0:2a7221ee9861 218 {
daqn 0:2a7221ee9861 219 mpu.getGyro(gyro);
daqn 0:2a7221ee9861 220
daqn 0:2a7221ee9861 221 omg[0] = gyro[2] - b_gyro[2];
daqn 0:2a7221ee9861 222 omg[1] = -gyro[1] + b_gyro[1];
daqn 0:2a7221ee9861 223 omg[2] = gyro[0] - b_gyro[0];
daqn 0:2a7221ee9861 224
daqn 0:2a7221ee9861 225 // 姿勢クォータニオンの時間微分を計算
daqn 0:2a7221ee9861 226 Omg << 0.f ,-omg[0],-omg[1],-omg[2],
daqn 0:2a7221ee9861 227 omg[0], 0.f , omg[2],-omg[1],
daqn 0:2a7221ee9861 228 omg[1],-omg[2], 0.f , omg[0],
daqn 0:2a7221ee9861 229 omg[2], omg[1],-omg[0], 0.f ;
daqn 0:2a7221ee9861 230 // q_dot = 0.5f * Omg * q;
daqn 0:2a7221ee9861 231
daqn 0:2a7221ee9861 232 // 現在時刻の姿勢クォータニオン
daqn 0:2a7221ee9861 233 q += Omg * q * 0.5f * dt;
daqn 0:2a7221ee9861 234 q /= sqrtf(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
daqn 0:2a7221ee9861 235
daqn 0:2a7221ee9861 236 // 姿勢クォータニオンからオイラー角への変換
daqn 0:2a7221ee9861 237 q00 = q[0]*q[0];
daqn 0:2a7221ee9861 238 q01 = q[0]*q[1];
daqn 0:2a7221ee9861 239 q02 = q[0]*q[2];
daqn 0:2a7221ee9861 240 q03 = q[0]*q[3];
daqn 0:2a7221ee9861 241 q11 = q[1]*q[1];
daqn 0:2a7221ee9861 242 q12 = q[1]*q[2];
daqn 0:2a7221ee9861 243 q13 = q[1]*q[3];
daqn 0:2a7221ee9861 244 q22 = q[2]*q[2];
daqn 0:2a7221ee9861 245 q23 = q[2]*q[3];
daqn 0:2a7221ee9861 246 q33 = q[3]*q[3];
daqn 0:2a7221ee9861 247 dcm << q00+q11-q22-q33, 2*(q12+q03) , 2*(q13-q02) ,
daqn 0:2a7221ee9861 248 2*(q12-q03) , q00-q11+q22-q33, 2*(q23+q01) ,
daqn 0:2a7221ee9861 249 2*(q13+q02) , 2*(q23-q01) , q00-q11-q22+q33;
daqn 0:2a7221ee9861 250 bB_hat = dcm*bG;
daqn 0:2a7221ee9861 251 mpu.getAccelero(acc);
daqn 0:2a7221ee9861 252 a_norm_div = 1.f / sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
daqn 0:2a7221ee9861 253 bB_tilde << acc[2]*a_norm_div,-acc[1]*a_norm_div, acc[0]*a_norm_div;
daqn 0:2a7221ee9861 254 rot_vec = bB_hat.cross(bB_hat - bB_tilde);
daqn 0:2a7221ee9861 255 rot_q[1] = 0.5f*eps*rot_vec[0];
daqn 0:2a7221ee9861 256 rot_q[2] = 0.5f*eps*rot_vec[1];
daqn 0:2a7221ee9861 257 rot_q[3] = 0.5f*eps*rot_vec[2];
daqn 0:2a7221ee9861 258 rot_q_otimes << 1.f ,-rot_q[1],-rot_q[2],-rot_q[3],
daqn 0:2a7221ee9861 259 rot_q[1], 1.f , rot_q[3],-rot_q[2],
daqn 0:2a7221ee9861 260 rot_q[2],-rot_q[3], 1.f , rot_q[1],
daqn 0:2a7221ee9861 261 rot_q[3], rot_q[2],-rot_q[1], 1.f ;
daqn 0:2a7221ee9861 262 q = rot_q_otimes * q;
daqn 0:2a7221ee9861 263 // q00 = q[0]*q[0]; // Note that these values are based on
daqn 0:2a7221ee9861 264 // q11 = q[1]*q[1]; // rotated quatenion (L253), so they are diferent from
daqn 0:2a7221ee9861 265 // q22 = q[2]*q[2]; // the values calculated above (L228-237).
daqn 0:2a7221ee9861 266 // q33 = q[3]*q[3];
daqn 0:2a7221ee9861 267 // q /= sqrtf(q00 + q11 + q22 + q33);
daqn 0:2a7221ee9861 268
daqn 0:2a7221ee9861 269 q /= sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
daqn 0:2a7221ee9861 270 send_buff[0] = q[0];
daqn 0:2a7221ee9861 271 send_buff[1] = q[1];
daqn 0:2a7221ee9861 272 send_buff[2] = q[2];
daqn 0:2a7221ee9861 273 send_buff[3] = q[3];
daqn 0:2a7221ee9861 274 // send_buff[0] = 0.1f;
daqn 0:2a7221ee9861 275 // send_buff[1] = 0.2f;
daqn 0:2a7221ee9861 276 // send_buff[2] = 0.3f;
daqn 0:2a7221ee9861 277 // send_buff[3] = 0.4f;
daqn 0:2a7221ee9861 278 // pc.write(send_buf,4);
daqn 0:2a7221ee9861 279 pc.putc('\0');
daqn 0:2a7221ee9861 280 for (int i = 0; i < 16; i++)
daqn 0:2a7221ee9861 281 pc.putc(send_bufc[i]);
daqn 0:2a7221ee9861 282 // q00_22 = q00 - q22;
daqn 0:2a7221ee9861 283 // q11_33 = q11 - q33;
daqn 0:2a7221ee9861 284 // roll = atan2( 2*(q[2]*q[3]+q[0]*q[1]), q00_22 - q11_33);
daqn 0:2a7221ee9861 285 // pitch = asin (-2*(q[1]*q[3]-q[0]*q[2]));
daqn 0:2a7221ee9861 286 // yaw = atan2( 2*(q[1]*q[2]+q[0]*q[3]), q00_22 + q11_33);
daqn 0:2a7221ee9861 287
daqn 0:2a7221ee9861 288 // pc.printf("%11.7f ",timer.read());
daqn 0:2a7221ee9861 289 // pc.printf("%6.1f %6.1f %6.1f\r\n",
daqn 0:2a7221ee9861 290 // roll*180/PI, pitch*180/PI, yaw*180/PI);
daqn 0:2a7221ee9861 291 // pc.printf("%6.1f %6.1f\r\n",
daqn 0:2a7221ee9861 292 // roll*180/PI, pitch*180/PI);
daqn 0:2a7221ee9861 293 }