2020NSE_SSLB4 / Mbed 2 deprecated MPU9250

getting acceleration and angular acceleration used in NSE2020

Dependencies:   mbed MPU9250

main.cpp@1:8e1e8b1574e9, 2020-07-29 (annotated)

Committer:
momoa
Date:
Wed Jul 29 06:14:54 2020 +0000
Revision:
1:8e1e8b1574e9
Parent:
0:547b5eb2838a 
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
momoa 0:547b5eb2838a 1 /*
momoa 0:547b5eb2838a 2 MPU9250から9軸データを取得してCAN送信・シリアル送信．
momoa 0:547b5eb2838a 3 ノルムは5回移動平均から算出．
momoa 0:547b5eb2838a 4 生データも送る．
momoa 0:547b5eb2838a 5 姿勢角を出す．
momoa 0:547b5eb2838a 6 <MPU9250ピン設定>
momoa 0:547b5eb2838a 7 CS~ : HIGH
momoa 0:547b5eb2838a 8 ADO : LOW
momoa 0:547b5eb2838a 9 SDA/SCL 10kΩでpullup
momoa 0:547b5eb2838a 10 */
momoa 0:547b5eb2838a 11 #include "mbed.h"
momoa 0:547b5eb2838a 12 #include "MPU9250.h"
momoa 0:547b5eb2838a 13 Serial pc(PA_9,PA_10);
momoa 0:547b5eb2838a 14 MPU9250 mpu = MPU9250(PB_7, PB_6); // pin30,29 SDA,SCL​
momoa 0:547b5eb2838a 15 #define PI 3.14159265358979323846f
momoa 0:547b5eb2838a 16 #define N 5 // 5回移動平均
momoa 0:547b5eb2838a 17 #define sampleFreq 100.0f
momoa 0:547b5eb2838a 18 #define beta 0.33f // gain（大きいと加速度による補正が早い）
momoa 0:547b5eb2838a 19
momoa 0:547b5eb2838a 20 char senddate[5];
momoa 0:547b5eb2838a 21 union Float2Byte{
momoa 0:547b5eb2838a 22 float _float;
momoa 0:547b5eb2838a 23 char _byte[4];
momoa 0:547b5eb2838a 24 }f2b;
momoa 0:547b5eb2838a 25
momoa 0:547b5eb2838a 26 void mpu_init(){
momoa 0:547b5eb2838a 27 uint8_t whoami_MPU9250, whoami_AK8963;
momoa 0:547b5eb2838a 28 float mag_init[3];
momoa 0:547b5eb2838a 29
momoa 0:547b5eb2838a 30 whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
momoa 0:547b5eb2838a 31 whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
momoa 0:547b5eb2838a 32 pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
momoa 0:547b5eb2838a 33 pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
momoa 0:547b5eb2838a 34
momoa 0:547b5eb2838a 35 if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){
momoa 0:547b5eb2838a 36 pc.printf("MPU9250 is detected.\n\r");
momoa 0:547b5eb2838a 37 wait(0.1);
momoa 0:547b5eb2838a 38 mpu.resetMPU9250();
momoa 0:547b5eb2838a 39 mpu.initMPU9250();
momoa 0:547b5eb2838a 40 wait(0.1);
momoa 0:547b5eb2838a 41 mpu.initAK8963(mag_init);
momoa 0:547b5eb2838a 42 mpu.getGres();
momoa 0:547b5eb2838a 43 mpu.getAres();
momoa 0:547b5eb2838a 44 mpu.getMres();
momoa 0:547b5eb2838a 45 wait(0.1);
momoa 0:547b5eb2838a 46 }
momoa 0:547b5eb2838a 47 else{
momoa 0:547b5eb2838a 48 pc.printf("Could not detect MPU9250.\n\r");
momoa 0:547b5eb2838a 49 pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
momoa 0:547b5eb2838a 50 whoami_MPU9250, whoami_AK8963);
momoa 0:547b5eb2838a 51 while(1);
momoa 0:547b5eb2838a 52 }
momoa 0:547b5eb2838a 53 }
momoa 0:547b5eb2838a 54
momoa 0:547b5eb2838a 55 int main(){
momoa 0:547b5eb2838a 56 int16_t acc[3], gyr[3], mag[3];
momoa 0:547b5eb2838a 57
momoa 0:547b5eb2838a 58 float buff_ax[N], buff_ay[N], buff_az[N];
momoa 0:547b5eb2838a 59 float buff_gx[N], buff_gy[N], buff_gz[N];
momoa 0:547b5eb2838a 60 float buff_mx[N], buff_my[N], buff_mz[N];
momoa 0:547b5eb2838a 61 float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
momoa 0:547b5eb2838a 62 float sum_gx = 0.0f, sum_gy = 0.0f, sum_gz = 0.0f;
momoa 0:547b5eb2838a 63 float sum_mx = 0.0f, sum_my = 0.0f, sum_mz = 0.0f;
momoa 0:547b5eb2838a 64 float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
momoa 0:547b5eb2838a 65 float gx_new = 0.0f, gy_new = 0.0f, gz_new = 0.0f;
momoa 0:547b5eb2838a 66 float mx_new = 0.0f, my_new = 0.0f, mz_new = 0.0f;
momoa 0:547b5eb2838a 67 float gx_i = 0.0f, gy_i = 0.0f, gz_i = 0.0f;
momoa 0:547b5eb2838a 68 float ax = 0.0f, ay = 0.0f, az = 0.0f;
momoa 0:547b5eb2838a 69 float gx = 0.0f, gy = 0.0f, gz = 0.0f;
momoa 0:547b5eb2838a 70 float mx = 0.0f, my = 0.0f, mz = 0.0f;
momoa 0:547b5eb2838a 71 int cnt = 0;
momoa 0:547b5eb2838a 72 float a_norm = 0.0f;
momoa 0:547b5eb2838a 73
momoa 0:547b5eb2838a 74 float psi = 0.0f, cta = 0.0f, eta =0.0f;
momoa 0:547b5eb2838a 75 float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
momoa 0:547b5eb2838a 76 float _gx, _gy, _gz;
momoa 0:547b5eb2838a 77 float norm;
momoa 0:547b5eb2838a 78 float s0, s1, s2, s3;
momoa 0:547b5eb2838a 79 float qdot0, qdot1, qdot2, qdot3;
momoa 0:547b5eb2838a 80 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, _q0q0, _q1q1, _q2q2, _q3q3;
momoa 0:547b5eb2838a 81
momoa 0:547b5eb2838a 82 wait(0.1);
momoa 0:547b5eb2838a 83 mpu_init();
momoa 0:547b5eb2838a 84 wait(2.0); // 静止させるまで待つ
momoa 0:547b5eb2838a 85
momoa 0:547b5eb2838a 86 // 初期値
momoa 0:547b5eb2838a 87 for(int i=0;i<N;i++){
momoa 0:547b5eb2838a 88 mpu.readGyroData(gyr);
momoa 0:547b5eb2838a 89 mpu.readAccelData(acc);
momoa 0:547b5eb2838a 90 mpu.readMagData(mag);
momoa 0:547b5eb2838a 91 buff_ax[i] = acc[0] / 2049.81;
momoa 0:547b5eb2838a 92 buff_ay[i] = acc[1] / 2049.81;
momoa 0:547b5eb2838a 93 buff_az[i] = acc[2] / 2049.81;
momoa 0:547b5eb2838a 94 buff_gx[i] = gyr[0] * 0.03048;
momoa 0:547b5eb2838a 95 buff_gy[i] = gyr[1] * 0.03048;
momoa 0:547b5eb2838a 96 buff_gz[i] = gyr[2] * 0.03048;
momoa 0:547b5eb2838a 97 buff_mx[i] = mag[0] * 0.15;
momoa 0:547b5eb2838a 98 buff_my[i] = mag[1] * 0.15;
momoa 0:547b5eb2838a 99 buff_mz[i] = mag[2] * 0.15;
momoa 0:547b5eb2838a 100 sum_ax += buff_ax[i];
momoa 0:547b5eb2838a 101 sum_ay += buff_ay[i];
momoa 0:547b5eb2838a 102 sum_az += buff_az[i];
momoa 0:547b5eb2838a 103 sum_gx += buff_gx[i];
momoa 0:547b5eb2838a 104 sum_gy += buff_gy[i];
momoa 0:547b5eb2838a 105 sum_gz += buff_gz[i];
momoa 0:547b5eb2838a 106 sum_mx += buff_mx[i];
momoa 0:547b5eb2838a 107 sum_my += buff_my[i];
momoa 0:547b5eb2838a 108 sum_mz += buff_mz[i];
momoa 0:547b5eb2838a 109 }
momoa 0:547b5eb2838a 110
momoa 0:547b5eb2838a 111 // 静止時角速度
momoa 0:547b5eb2838a 112 gx_i = sum_gx / N;
momoa 0:547b5eb2838a 113 gy_i = sum_gy / N;
momoa 0:547b5eb2838a 114 gz_i = sum_gz / N;
momoa 0:547b5eb2838a 115 pc.printf("gyr_ini:%f,%f,%f\n\r", gx_i, gy_i, gz_i);
momoa 0:547b5eb2838a 116
momoa 0:547b5eb2838a 117 wait(0.1);
momoa 0:547b5eb2838a 118 pc.printf("Start.\n\r");
momoa 0:547b5eb2838a 119 while(1){
momoa 0:547b5eb2838a 120 //timea.start();
momoa 0:547b5eb2838a 121 mpu.readGyroData(gyr);
momoa 0:547b5eb2838a 122 mpu.readAccelData(acc);
momoa 0:547b5eb2838a 123 mpu.readMagData(mag);
momoa 0:547b5eb2838a 124
momoa 0:547b5eb2838a 125 /* 5回移動平均 */
momoa 0:547b5eb2838a 126 sum_ax = sum_ax - buff_ax[cnt];
momoa 0:547b5eb2838a 127 sum_ay = sum_ay - buff_ay[cnt];
momoa 0:547b5eb2838a 128 sum_az = sum_az - buff_az[cnt];
momoa 0:547b5eb2838a 129 sum_gx = sum_gx - buff_gx[cnt];
momoa 0:547b5eb2838a 130 sum_gy = sum_gy - buff_gy[cnt];
momoa 0:547b5eb2838a 131 sum_gz = sum_gz - buff_gz[cnt];
momoa 0:547b5eb2838a 132 sum_mx = sum_mx - buff_mx[cnt];
momoa 0:547b5eb2838a 133 sum_my = sum_my - buff_my[cnt];
momoa 0:547b5eb2838a 134 sum_mz = sum_mz - buff_mz[cnt];
momoa 0:547b5eb2838a 135 ax_new = acc[0] / 2049.81;
momoa 0:547b5eb2838a 136 ay_new = acc[1] / 2049.81;
momoa 0:547b5eb2838a 137 az_new = acc[2] / 2049.81;
momoa 0:547b5eb2838a 138 gx_new = gyr[0] * 0.03048;
momoa 0:547b5eb2838a 139 gy_new = gyr[1] * 0.03048;
momoa 0:547b5eb2838a 140 gz_new = gyr[2] * 0.03048;
momoa 0:547b5eb2838a 141 mx_new = mag[0] * 0.15;
momoa 0:547b5eb2838a 142 my_new = mag[1] * 0.15;
momoa 0:547b5eb2838a 143 mz_new = mag[2] * 0.15;
momoa 0:547b5eb2838a 144 buff_ax[cnt] = ax_new;
momoa 0:547b5eb2838a 145 buff_ay[cnt] = ay_new;
momoa 0:547b5eb2838a 146 buff_az[cnt] = az_new;
momoa 0:547b5eb2838a 147 buff_gx[cnt] = gx_new;
momoa 0:547b5eb2838a 148 buff_gy[cnt] = gy_new;
momoa 0:547b5eb2838a 149 buff_gz[cnt] = gz_new;
momoa 0:547b5eb2838a 150 buff_mx[cnt] = mx_new;
momoa 0:547b5eb2838a 151 buff_my[cnt] = my_new;
momoa 0:547b5eb2838a 152 buff_mz[cnt] = mz_new;
momoa 0:547b5eb2838a 153 sum_ax = sum_ax + buff_ax[cnt];
momoa 0:547b5eb2838a 154 sum_ay = sum_ay + buff_ay[cnt];
momoa 0:547b5eb2838a 155 sum_az = sum_az + buff_az[cnt];
momoa 0:547b5eb2838a 156 sum_gx = sum_gx + buff_gx[cnt];
momoa 0:547b5eb2838a 157 sum_gy = sum_gy + buff_gy[cnt];
momoa 0:547b5eb2838a 158 sum_gz = sum_gz + buff_gz[cnt];
momoa 0:547b5eb2838a 159 sum_mx = sum_mx + buff_mx[cnt];
momoa 0:547b5eb2838a 160 sum_my = sum_my + buff_my[cnt];
momoa 0:547b5eb2838a 161 sum_mz = sum_mz + buff_mz[cnt];
momoa 0:547b5eb2838a 162 cnt++;
momoa 0:547b5eb2838a 163 if(cnt == N) cnt = 0;
momoa 0:547b5eb2838a 164
momoa 0:547b5eb2838a 165 ax = sum_ax / N;
momoa 0:547b5eb2838a 166 ay = sum_ay / N;
momoa 0:547b5eb2838a 167 az = sum_az / N;
momoa 0:547b5eb2838a 168 gx = sum_gx / N - gx_i;
momoa 0:547b5eb2838a 169 gy = sum_gy / N - gy_i;
momoa 0:547b5eb2838a 170 gz = sum_gz / N - gz_i;
momoa 0:547b5eb2838a 171 mx = sum_mx / N;
momoa 0:547b5eb2838a 172 my = sum_my / N;
momoa 0:547b5eb2838a 173 mz = sum_mz / N;
momoa 0:547b5eb2838a 174
momoa 0:547b5eb2838a 175 /* 加速度ノルム */
momoa 0:547b5eb2838a 176 a_norm = sqrt(ax*ax + ay*ay + az*az);
momoa 0:547b5eb2838a 177
momoa 0:547b5eb2838a 178 /* 姿勢角(Quaternion->Eulerangle) */
momoa 0:547b5eb2838a 179 _gx = gx * PI / 180.f;
momoa 0:547b5eb2838a 180 _gy = gy * PI / 180.f;
momoa 0:547b5eb2838a 181 _gz = gz * PI / 180.f;
momoa 0:547b5eb2838a 182 // qdot=0.5*q*w
momoa 0:547b5eb2838a 183 qdot0 = 0.5f * ( - q1*_gx - q2*_gy - q3*_gz);
momoa 0:547b5eb2838a 184 qdot1 = 0.5f * (q0*_gx + q2*_gz - q3*_gy);
momoa 0:547b5eb2838a 185 qdot2 = 0.5f * (q0*_gy - q1*_gz + q3*_gx);
momoa 0:547b5eb2838a 186 qdot3 = 0.5f * (q0*_gz + q1*_gy - q2*_gx );
momoa 0:547b5eb2838a 187 // 加速度で補正する（動加速度下ではどうなん？動だけに）
momoa 0:547b5eb2838a 188 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))){
momoa 0:547b5eb2838a 189 norm = sqrt(ax*ax + ay*ay + az*az);
momoa 0:547b5eb2838a 190 ax /= norm;
momoa 0:547b5eb2838a 191 ay /= norm;
momoa 0:547b5eb2838a 192 az /= norm;
momoa 0:547b5eb2838a 193
momoa 0:547b5eb2838a 194 _2q0 = 2.0f * q0;
momoa 0:547b5eb2838a 195 _2q1 = 2.0f * q1;
momoa 0:547b5eb2838a 196 _2q2 = 2.0f * q2;
momoa 0:547b5eb2838a 197 _2q3 = 2.0f * q3;
momoa 0:547b5eb2838a 198 _4q0 = 4.0f * q0;
momoa 0:547b5eb2838a 199 _4q1 = 4.0f * q1;
momoa 0:547b5eb2838a 200 _4q2 = 4.0f * q2;
momoa 0:547b5eb2838a 201 _8q1 = 8.0f * q1;
momoa 0:547b5eb2838a 202 _8q2 = 8.0f * q2;
momoa 0:547b5eb2838a 203 _q0q0 = q0 * q0;
momoa 0:547b5eb2838a 204 _q1q1 = q1 * q1;
momoa 0:547b5eb2838a 205 _q2q2 = q2 * q2;
momoa 0:547b5eb2838a 206 _q3q3 = q3 * q3;
momoa 0:547b5eb2838a 207
momoa 0:547b5eb2838a 208 s0 = _4q0*_q2q2 + _2q2*ax + _4q0*_q1q1 - _2q1*ay;
momoa 0:547b5eb2838a 209 s1 = _4q1*_q3q3 - _2q3*ax + 4.0f*_q0q0*q1 - _2q0*ay - _4q1 + _8q1*_q1q1 + _8q1*_q2q2 + _4q1*az;
momoa 0:547b5eb2838a 210 s2 = 4.0f*_q0q0*q2 + _2q0*ax + _4q2*_q3q3 - _2q3*ay - _4q2 + _8q2*_q1q1 + _8q2*_q2q2 + _4q2*az;
momoa 0:547b5eb2838a 211 s3 = 4.0f*_q1q1*q3 - _2q1*ax + 4.0f*_q2q2*q3 - _2q2*ay;
momoa 0:547b5eb2838a 212
momoa 0:547b5eb2838a 213 norm = sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
momoa 0:547b5eb2838a 214 s0 /= norm;
momoa 0:547b5eb2838a 215 s1 /= norm;
momoa 0:547b5eb2838a 216 s2 /= norm;
momoa 0:547b5eb2838a 217 s3 /= norm;
momoa 0:547b5eb2838a 218
momoa 0:547b5eb2838a 219 qdot0 -= beta * s0;
momoa 0:547b5eb2838a 220 qdot1 -= beta * s1;
momoa 0:547b5eb2838a 221 qdot2 -= beta * s2;
momoa 0:547b5eb2838a 222 qdot3 -= beta * s3;
momoa 0:547b5eb2838a 223 }
momoa 0:547b5eb2838a 224 // q+=qdot*dt
momoa 0:547b5eb2838a 225 q0 += qdot0 * (1.0f / sampleFreq);
momoa 0:547b5eb2838a 226 q1 += qdot1 * (1.0f / sampleFreq);
momoa 0:547b5eb2838a 227 q2 += qdot2 * (1.0f / sampleFreq);
momoa 0:547b5eb2838a 228 q3 += qdot3 * (1.0f / sampleFreq);
momoa 0:547b5eb2838a 229
momoa 0:547b5eb2838a 230 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
momoa 0:547b5eb2838a 231 q0 /= norm;
momoa 0:547b5eb2838a 232 q1 /= norm;
momoa 0:547b5eb2838a 233 q2 /= norm;
momoa 0:547b5eb2838a 234 q3 /= norm;
momoa 0:547b5eb2838a 235
momoa 0:547b5eb2838a 236 //psi = atan2(2.0f*q0*q1 + 2.0f*q2*q3, 2.0f*q0*q0 + 2.0f*q3*q3 - 1.0f) * 180.0f / PI;
momoa 0:547b5eb2838a 237 //cta = asin(2.0f*q0*q2 - 2.0f*q1*q3) * 180.0f / PI;
momoa 0:547b5eb2838a 238 //eta = atan2(2.0f*q0*q3 - 2.0f*q1*q2, 2.0f*q0*q0 + 2.0f*q1*q1 - 1.0f) * 180.0f / PI;
momoa 0:547b5eb2838a 239
momoa 0:547b5eb2838a 240 //pc.printf("a_norm:%f\n\r", a_norm);
momoa 0:547b5eb2838a 241 //pc.printf("acc_ave:%f,%f,%f\n\r", ax, ay, az);
momoa 1:8e1e8b1574e9 242 //pc.printf("gyr_ave:%f,%f,%f\n\r", gx, gy, gz);
momoa 0:547b5eb2838a 243 //pc.printf("mag_ave:%f,%f,%f\n\r", mx, my, mz);
momoa 0:547b5eb2838a 244 //pc.printf("%f,%f,%f,%f\n\r", q0, q1, q2, q3);
momoa 1:8e1e8b1574e9 245 pc.printf("angle: %f, %f, %f\n\r", psi, cta, eta);
momoa 0:547b5eb2838a 246 //pc.printf("%f\n\r", timea.read());
momoa 0:547b5eb2838a 247
momoa 0:547b5eb2838a 248 wait_us(50);
momoa 0:547b5eb2838a 249
momoa 0:547b5eb2838a 250 wait(0.001);
momoa 0:547b5eb2838a 251 }
momoa 0:547b5eb2838a 252 }