エラー判定用 '\0' + クォータニオンのfloat型4成分,合計17byteのデータをひたすらバイナリ形式で送り続ける.最初のバイトが '\0' でなかったらズレているので,適当なバイト数見送って先頭が '\0' になるよう合わせること.
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?
User | Revision | Line number | New 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 | } |