SD ni Hozon dekinai program
Dependencies: mbed SDFileSystem BME280 MPU9250
main.cpp@1:d77a49b0e9ff, 2020-07-31 (annotated)
- Committer:
- momoa
- Date:
- Fri Jul 31 04:44:04 2020 +0000
- Revision:
- 1:d77a49b0e9ff
- Parent:
- 0:17ad5be2e01a
- Child:
- 2:d83d5822dc25
saving MPU and BMP date;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
momoa | 0:17ad5be2e01a | 1 | /* |
momoa | 0:17ad5be2e01a | 2 | MPU9250から9軸データを取得してCAN送信・シリアル送信. |
momoa | 0:17ad5be2e01a | 3 | ノルムは5回移動平均から算出. |
momoa | 0:17ad5be2e01a | 4 | 生データも送る. |
momoa | 0:17ad5be2e01a | 5 | 姿勢角を出す. |
momoa | 0:17ad5be2e01a | 6 | <MPU9250ピン設定> |
momoa | 0:17ad5be2e01a | 7 | CS~ : HIGH |
momoa | 0:17ad5be2e01a | 8 | ADO : LOW |
momoa | 0:17ad5be2e01a | 9 | SDA/SCL 10kΩでpullup |
momoa | 0:17ad5be2e01a | 10 | |
momoa | 0:17ad5be2e01a | 11 | BMP280から気圧・温度を取得してCAN送信・シリアル送信. |
momoa | 0:17ad5be2e01a | 12 | 気圧->5回移動平均. |
momoa | 0:17ad5be2e01a | 13 | <BMP280ピン設定> |
momoa | 0:17ad5be2e01a | 14 | CSB : HIGH |
momoa | 0:17ad5be2e01a | 15 | SDO : LOW |
momoa | 0:17ad5be2e01a | 16 | */ |
momoa | 0:17ad5be2e01a | 17 | |
momoa | 0:17ad5be2e01a | 18 | #include "SDFileSystem.h" |
momoa | 0:17ad5be2e01a | 19 | SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd"); // the pinout on the mbed Cool Components workshop board |
momoa | 0:17ad5be2e01a | 20 | #include "mbed.h" |
momoa | 0:17ad5be2e01a | 21 | #include "BME280.h" |
momoa | 0:17ad5be2e01a | 22 | #include "MPU9250.h" |
momoa | 0:17ad5be2e01a | 23 | Serial pc(PA_9,PA_10); //pin19,20 TX,RX |
momoa | 0:17ad5be2e01a | 24 | BME280 bmp = BME280(PB_7, PB_6); // pin30,29 SDA,SCL |
momoa | 0:17ad5be2e01a | 25 | MPU9250 mpu = MPU9250(PB_7, PB_6); // pin30,29 SDA,SCL |
momoa | 0:17ad5be2e01a | 26 | #define PI 3.14159265358979323846f |
momoa | 0:17ad5be2e01a | 27 | #define N 5 // 5回移動平均 |
momoa | 0:17ad5be2e01a | 28 | #define sampleFreq 100.0f |
momoa | 0:17ad5be2e01a | 29 | #define beta 0.33f // gain(大きいと加速度による補正が早い) |
momoa | 0:17ad5be2e01a | 30 | |
momoa | 0:17ad5be2e01a | 31 | char senddate[5]; |
momoa | 0:17ad5be2e01a | 32 | union Float2Byte{ |
momoa | 0:17ad5be2e01a | 33 | float _float; |
momoa | 0:17ad5be2e01a | 34 | char _byte[4]; |
momoa | 0:17ad5be2e01a | 35 | }f2b; |
momoa | 0:17ad5be2e01a | 36 | |
momoa | 0:17ad5be2e01a | 37 | float pressure_ave = 0.0f; // 気圧[hPa] |
momoa | 0:17ad5be2e01a | 38 | float pressure_new = 0.0f; |
momoa | 0:17ad5be2e01a | 39 | float buff_p[N]; |
momoa | 0:17ad5be2e01a | 40 | float sum = 0.0f; |
momoa | 0:17ad5be2e01a | 41 | int cnt = 0; |
momoa | 0:17ad5be2e01a | 42 | float tem = 0.0f; // 温度[C] |
momoa | 0:17ad5be2e01a | 43 | |
momoa | 1:d77a49b0e9ff | 44 | FILE *fp; |
momoa | 1:d77a49b0e9ff | 45 | |
momoa | 0:17ad5be2e01a | 46 | void mpu_init(){ |
momoa | 0:17ad5be2e01a | 47 | uint8_t whoami_MPU9250, whoami_AK8963; |
momoa | 0:17ad5be2e01a | 48 | float mag_init[3]; |
momoa | 0:17ad5be2e01a | 49 | |
momoa | 0:17ad5be2e01a | 50 | whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); |
momoa | 0:17ad5be2e01a | 51 | whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); |
momoa | 0:17ad5be2e01a | 52 | pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常 |
momoa | 0:17ad5be2e01a | 53 | pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常 |
momoa | 0:17ad5be2e01a | 54 | |
momoa | 0:17ad5be2e01a | 55 | if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){ |
momoa | 0:17ad5be2e01a | 56 | pc.printf("MPU9250 is detected.\n\r"); |
momoa | 0:17ad5be2e01a | 57 | wait(0.1); |
momoa | 0:17ad5be2e01a | 58 | mpu.resetMPU9250(); |
momoa | 0:17ad5be2e01a | 59 | mpu.initMPU9250(); |
momoa | 0:17ad5be2e01a | 60 | wait(0.1); |
momoa | 0:17ad5be2e01a | 61 | mpu.initAK8963(mag_init); |
momoa | 0:17ad5be2e01a | 62 | mpu.getGres(); |
momoa | 0:17ad5be2e01a | 63 | mpu.getAres(); |
momoa | 0:17ad5be2e01a | 64 | mpu.getMres(); |
momoa | 0:17ad5be2e01a | 65 | wait(0.1); |
momoa | 0:17ad5be2e01a | 66 | } |
momoa | 0:17ad5be2e01a | 67 | else{ |
momoa | 0:17ad5be2e01a | 68 | pc.printf("Could not detect MPU9250.\n\r"); |
momoa | 0:17ad5be2e01a | 69 | pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n", |
momoa | 0:17ad5be2e01a | 70 | whoami_MPU9250, whoami_AK8963); |
momoa | 0:17ad5be2e01a | 71 | while(1); |
momoa | 0:17ad5be2e01a | 72 | } |
momoa | 0:17ad5be2e01a | 73 | } |
momoa | 0:17ad5be2e01a | 74 | |
momoa | 0:17ad5be2e01a | 75 | int main(){ |
momoa | 1:d77a49b0e9ff | 76 | |
momoa | 1:d77a49b0e9ff | 77 | fp = fopen("/sd/log.txt","a"); |
momoa | 0:17ad5be2e01a | 78 | if(fp != NULL){ |
momoa | 0:17ad5be2e01a | 79 | fprintf(fp,"Start.\n"); |
momoa | 0:17ad5be2e01a | 80 | fclose(fp); |
momoa | 0:17ad5be2e01a | 81 | } |
momoa | 0:17ad5be2e01a | 82 | else{ |
momoa | 0:17ad5be2e01a | 83 | pc.printf("Failed.\n\r"); |
momoa | 0:17ad5be2e01a | 84 | } |
momoa | 0:17ad5be2e01a | 85 | |
momoa | 0:17ad5be2e01a | 86 | int16_t acc[3], gyr[3], mag[3]; |
momoa | 1:d77a49b0e9ff | 87 | int cntsd = 0; |
momoa | 0:17ad5be2e01a | 88 | |
momoa | 0:17ad5be2e01a | 89 | float buff_ax[N], buff_ay[N], buff_az[N]; |
momoa | 0:17ad5be2e01a | 90 | float buff_gx[N], buff_gy[N], buff_gz[N]; |
momoa | 0:17ad5be2e01a | 91 | float buff_mx[N], buff_my[N], buff_mz[N]; |
momoa | 0:17ad5be2e01a | 92 | float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f; |
momoa | 0:17ad5be2e01a | 93 | float sum_gx = 0.0f, sum_gy = 0.0f, sum_gz = 0.0f; |
momoa | 0:17ad5be2e01a | 94 | float sum_mx = 0.0f, sum_my = 0.0f, sum_mz = 0.0f; |
momoa | 0:17ad5be2e01a | 95 | float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f; |
momoa | 0:17ad5be2e01a | 96 | float gx_new = 0.0f, gy_new = 0.0f, gz_new = 0.0f; |
momoa | 0:17ad5be2e01a | 97 | float mx_new = 0.0f, my_new = 0.0f, mz_new = 0.0f; |
momoa | 0:17ad5be2e01a | 98 | float gx_i = 0.0f, gy_i = 0.0f, gz_i = 0.0f; |
momoa | 0:17ad5be2e01a | 99 | float ax = 0.0f, ay = 0.0f, az = 0.0f; |
momoa | 0:17ad5be2e01a | 100 | float gx = 0.0f, gy = 0.0f, gz = 0.0f; |
momoa | 0:17ad5be2e01a | 101 | float mx = 0.0f, my = 0.0f, mz = 0.0f; |
momoa | 0:17ad5be2e01a | 102 | int cnt = 0; |
momoa | 0:17ad5be2e01a | 103 | float a_norm = 0.0f; |
momoa | 0:17ad5be2e01a | 104 | |
momoa | 0:17ad5be2e01a | 105 | float psi = 0.0f, cta = 0.0f, eta =0.0f; |
momoa | 0:17ad5be2e01a | 106 | float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; |
momoa | 0:17ad5be2e01a | 107 | float _gx, _gy, _gz; |
momoa | 0:17ad5be2e01a | 108 | float norm; |
momoa | 0:17ad5be2e01a | 109 | float s0, s1, s2, s3; |
momoa | 0:17ad5be2e01a | 110 | float qdot0, qdot1, qdot2, qdot3; |
momoa | 0:17ad5be2e01a | 111 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, _q0q0, _q1q1, _q2q2, _q3q3; |
momoa | 0:17ad5be2e01a | 112 | |
momoa | 0:17ad5be2e01a | 113 | wait(0.1); |
momoa | 0:17ad5be2e01a | 114 | mpu_init(); |
momoa | 0:17ad5be2e01a | 115 | bmp.initialize(); |
momoa | 0:17ad5be2e01a | 116 | wait(2.0); // 静止させるまで待つ |
momoa | 0:17ad5be2e01a | 117 | |
momoa | 0:17ad5be2e01a | 118 | // 初期値 |
momoa | 0:17ad5be2e01a | 119 | for(int i=0;i<N;i++){ |
momoa | 0:17ad5be2e01a | 120 | mpu.readGyroData(gyr); |
momoa | 0:17ad5be2e01a | 121 | mpu.readAccelData(acc); |
momoa | 0:17ad5be2e01a | 122 | mpu.readMagData(mag); |
momoa | 0:17ad5be2e01a | 123 | buff_ax[i] = acc[0] / 2049.81; |
momoa | 0:17ad5be2e01a | 124 | buff_ay[i] = acc[1] / 2049.81; |
momoa | 0:17ad5be2e01a | 125 | buff_az[i] = acc[2] / 2049.81; |
momoa | 0:17ad5be2e01a | 126 | buff_gx[i] = gyr[0] * 0.03048; |
momoa | 0:17ad5be2e01a | 127 | buff_gy[i] = gyr[1] * 0.03048; |
momoa | 0:17ad5be2e01a | 128 | buff_gz[i] = gyr[2] * 0.03048; |
momoa | 0:17ad5be2e01a | 129 | buff_mx[i] = mag[0] * 0.15; |
momoa | 0:17ad5be2e01a | 130 | buff_my[i] = mag[1] * 0.15; |
momoa | 0:17ad5be2e01a | 131 | buff_mz[i] = mag[2] * 0.15; |
momoa | 0:17ad5be2e01a | 132 | sum_ax += buff_ax[i]; |
momoa | 0:17ad5be2e01a | 133 | sum_ay += buff_ay[i]; |
momoa | 0:17ad5be2e01a | 134 | sum_az += buff_az[i]; |
momoa | 0:17ad5be2e01a | 135 | sum_gx += buff_gx[i]; |
momoa | 0:17ad5be2e01a | 136 | sum_gy += buff_gy[i]; |
momoa | 0:17ad5be2e01a | 137 | sum_gz += buff_gz[i]; |
momoa | 0:17ad5be2e01a | 138 | sum_mx += buff_mx[i]; |
momoa | 0:17ad5be2e01a | 139 | sum_my += buff_my[i]; |
momoa | 0:17ad5be2e01a | 140 | sum_mz += buff_mz[i]; |
momoa | 0:17ad5be2e01a | 141 | |
momoa | 0:17ad5be2e01a | 142 | buff_p[i] = bmp.getPressure(); |
momoa | 0:17ad5be2e01a | 143 | sum += buff_p[i]; |
momoa | 0:17ad5be2e01a | 144 | } |
momoa | 0:17ad5be2e01a | 145 | |
momoa | 0:17ad5be2e01a | 146 | // 静止時角速度 |
momoa | 0:17ad5be2e01a | 147 | gx_i = sum_gx / N; |
momoa | 0:17ad5be2e01a | 148 | gy_i = sum_gy / N; |
momoa | 0:17ad5be2e01a | 149 | gz_i = sum_gz / N; |
momoa | 0:17ad5be2e01a | 150 | pc.printf("gyr_ini:%f,%f,%f\n\r", gx_i, gy_i, gz_i); |
momoa | 0:17ad5be2e01a | 151 | |
momoa | 0:17ad5be2e01a | 152 | wait(0.1); |
momoa | 0:17ad5be2e01a | 153 | pc.printf("Start.\n\r"); |
momoa | 1:d77a49b0e9ff | 154 | |
momoa | 0:17ad5be2e01a | 155 | while(1){ |
momoa | 0:17ad5be2e01a | 156 | //timea.start(); |
momoa | 0:17ad5be2e01a | 157 | mpu.readGyroData(gyr); |
momoa | 0:17ad5be2e01a | 158 | mpu.readAccelData(acc); |
momoa | 0:17ad5be2e01a | 159 | mpu.readMagData(mag); |
momoa | 0:17ad5be2e01a | 160 | |
momoa | 0:17ad5be2e01a | 161 | /* 5回移動平均 */ |
momoa | 0:17ad5be2e01a | 162 | sum_ax = sum_ax - buff_ax[cnt]; |
momoa | 0:17ad5be2e01a | 163 | sum_ay = sum_ay - buff_ay[cnt]; |
momoa | 0:17ad5be2e01a | 164 | sum_az = sum_az - buff_az[cnt]; |
momoa | 0:17ad5be2e01a | 165 | sum_gx = sum_gx - buff_gx[cnt]; |
momoa | 0:17ad5be2e01a | 166 | sum_gy = sum_gy - buff_gy[cnt]; |
momoa | 0:17ad5be2e01a | 167 | sum_gz = sum_gz - buff_gz[cnt]; |
momoa | 0:17ad5be2e01a | 168 | sum_mx = sum_mx - buff_mx[cnt]; |
momoa | 0:17ad5be2e01a | 169 | sum_my = sum_my - buff_my[cnt]; |
momoa | 0:17ad5be2e01a | 170 | sum_mz = sum_mz - buff_mz[cnt]; |
momoa | 0:17ad5be2e01a | 171 | ax_new = acc[0] / 2049.81; |
momoa | 0:17ad5be2e01a | 172 | ay_new = acc[1] / 2049.81; |
momoa | 0:17ad5be2e01a | 173 | az_new = acc[2] / 2049.81; |
momoa | 0:17ad5be2e01a | 174 | gx_new = gyr[0] * 0.03048; |
momoa | 0:17ad5be2e01a | 175 | gy_new = gyr[1] * 0.03048; |
momoa | 0:17ad5be2e01a | 176 | gz_new = gyr[2] * 0.03048; |
momoa | 0:17ad5be2e01a | 177 | mx_new = mag[0] * 0.15; |
momoa | 0:17ad5be2e01a | 178 | my_new = mag[1] * 0.15; |
momoa | 0:17ad5be2e01a | 179 | mz_new = mag[2] * 0.15; |
momoa | 0:17ad5be2e01a | 180 | buff_ax[cnt] = ax_new; |
momoa | 0:17ad5be2e01a | 181 | buff_ay[cnt] = ay_new; |
momoa | 0:17ad5be2e01a | 182 | buff_az[cnt] = az_new; |
momoa | 0:17ad5be2e01a | 183 | buff_gx[cnt] = gx_new; |
momoa | 0:17ad5be2e01a | 184 | buff_gy[cnt] = gy_new; |
momoa | 0:17ad5be2e01a | 185 | buff_gz[cnt] = gz_new; |
momoa | 0:17ad5be2e01a | 186 | buff_mx[cnt] = mx_new; |
momoa | 0:17ad5be2e01a | 187 | buff_my[cnt] = my_new; |
momoa | 0:17ad5be2e01a | 188 | buff_mz[cnt] = mz_new; |
momoa | 0:17ad5be2e01a | 189 | sum_ax = sum_ax + buff_ax[cnt]; |
momoa | 0:17ad5be2e01a | 190 | sum_ay = sum_ay + buff_ay[cnt]; |
momoa | 0:17ad5be2e01a | 191 | sum_az = sum_az + buff_az[cnt]; |
momoa | 0:17ad5be2e01a | 192 | sum_gx = sum_gx + buff_gx[cnt]; |
momoa | 0:17ad5be2e01a | 193 | sum_gy = sum_gy + buff_gy[cnt]; |
momoa | 0:17ad5be2e01a | 194 | sum_gz = sum_gz + buff_gz[cnt]; |
momoa | 0:17ad5be2e01a | 195 | sum_mx = sum_mx + buff_mx[cnt]; |
momoa | 0:17ad5be2e01a | 196 | sum_my = sum_my + buff_my[cnt]; |
momoa | 0:17ad5be2e01a | 197 | sum_mz = sum_mz + buff_mz[cnt]; |
momoa | 0:17ad5be2e01a | 198 | cnt++; |
momoa | 0:17ad5be2e01a | 199 | if(cnt == N) cnt = 0; |
momoa | 0:17ad5be2e01a | 200 | |
momoa | 0:17ad5be2e01a | 201 | ax = sum_ax / N; |
momoa | 0:17ad5be2e01a | 202 | ay = sum_ay / N; |
momoa | 0:17ad5be2e01a | 203 | az = sum_az / N; |
momoa | 0:17ad5be2e01a | 204 | gx = sum_gx / N - gx_i; |
momoa | 0:17ad5be2e01a | 205 | gy = sum_gy / N - gy_i; |
momoa | 0:17ad5be2e01a | 206 | gz = sum_gz / N - gz_i; |
momoa | 0:17ad5be2e01a | 207 | mx = sum_mx / N; |
momoa | 0:17ad5be2e01a | 208 | my = sum_my / N; |
momoa | 0:17ad5be2e01a | 209 | mz = sum_mz / N; |
momoa | 0:17ad5be2e01a | 210 | |
momoa | 0:17ad5be2e01a | 211 | /* 加速度ノルム */ |
momoa | 0:17ad5be2e01a | 212 | a_norm = sqrt(ax*ax + ay*ay + az*az); |
momoa | 0:17ad5be2e01a | 213 | |
momoa | 0:17ad5be2e01a | 214 | /* 姿勢角(Quaternion->Eulerangle) */ |
momoa | 0:17ad5be2e01a | 215 | _gx = gx * PI / 180.f; |
momoa | 0:17ad5be2e01a | 216 | _gy = gy * PI / 180.f; |
momoa | 0:17ad5be2e01a | 217 | _gz = gz * PI / 180.f; |
momoa | 0:17ad5be2e01a | 218 | // qdot=0.5*q*w |
momoa | 0:17ad5be2e01a | 219 | qdot0 = 0.5f * ( - q1*_gx - q2*_gy - q3*_gz); |
momoa | 0:17ad5be2e01a | 220 | qdot1 = 0.5f * (q0*_gx + q2*_gz - q3*_gy); |
momoa | 0:17ad5be2e01a | 221 | qdot2 = 0.5f * (q0*_gy - q1*_gz + q3*_gx); |
momoa | 0:17ad5be2e01a | 222 | qdot3 = 0.5f * (q0*_gz + q1*_gy - q2*_gx ); |
momoa | 0:17ad5be2e01a | 223 | // 加速度で補正する(動加速度下ではどうなん?動だけに) |
momoa | 0:17ad5be2e01a | 224 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))){ |
momoa | 0:17ad5be2e01a | 225 | norm = sqrt(ax*ax + ay*ay + az*az); |
momoa | 0:17ad5be2e01a | 226 | ax /= norm; |
momoa | 0:17ad5be2e01a | 227 | ay /= norm; |
momoa | 0:17ad5be2e01a | 228 | az /= norm; |
momoa | 0:17ad5be2e01a | 229 | |
momoa | 0:17ad5be2e01a | 230 | _2q0 = 2.0f * q0; |
momoa | 0:17ad5be2e01a | 231 | _2q1 = 2.0f * q1; |
momoa | 0:17ad5be2e01a | 232 | _2q2 = 2.0f * q2; |
momoa | 0:17ad5be2e01a | 233 | _2q3 = 2.0f * q3; |
momoa | 0:17ad5be2e01a | 234 | _4q0 = 4.0f * q0; |
momoa | 0:17ad5be2e01a | 235 | _4q1 = 4.0f * q1; |
momoa | 0:17ad5be2e01a | 236 | _4q2 = 4.0f * q2; |
momoa | 0:17ad5be2e01a | 237 | _8q1 = 8.0f * q1; |
momoa | 0:17ad5be2e01a | 238 | _8q2 = 8.0f * q2; |
momoa | 0:17ad5be2e01a | 239 | _q0q0 = q0 * q0; |
momoa | 0:17ad5be2e01a | 240 | _q1q1 = q1 * q1; |
momoa | 0:17ad5be2e01a | 241 | _q2q2 = q2 * q2; |
momoa | 0:17ad5be2e01a | 242 | _q3q3 = q3 * q3; |
momoa | 0:17ad5be2e01a | 243 | |
momoa | 0:17ad5be2e01a | 244 | s0 = _4q0*_q2q2 + _2q2*ax + _4q0*_q1q1 - _2q1*ay; |
momoa | 0:17ad5be2e01a | 245 | s1 = _4q1*_q3q3 - _2q3*ax + 4.0f*_q0q0*q1 - _2q0*ay - _4q1 + _8q1*_q1q1 + _8q1*_q2q2 + _4q1*az; |
momoa | 0:17ad5be2e01a | 246 | s2 = 4.0f*_q0q0*q2 + _2q0*ax + _4q2*_q3q3 - _2q3*ay - _4q2 + _8q2*_q1q1 + _8q2*_q2q2 + _4q2*az; |
momoa | 0:17ad5be2e01a | 247 | s3 = 4.0f*_q1q1*q3 - _2q1*ax + 4.0f*_q2q2*q3 - _2q2*ay; |
momoa | 0:17ad5be2e01a | 248 | |
momoa | 0:17ad5be2e01a | 249 | norm = sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); |
momoa | 0:17ad5be2e01a | 250 | s0 /= norm; |
momoa | 0:17ad5be2e01a | 251 | s1 /= norm; |
momoa | 0:17ad5be2e01a | 252 | s2 /= norm; |
momoa | 0:17ad5be2e01a | 253 | s3 /= norm; |
momoa | 0:17ad5be2e01a | 254 | |
momoa | 0:17ad5be2e01a | 255 | qdot0 -= beta * s0; |
momoa | 0:17ad5be2e01a | 256 | qdot1 -= beta * s1; |
momoa | 0:17ad5be2e01a | 257 | qdot2 -= beta * s2; |
momoa | 0:17ad5be2e01a | 258 | qdot3 -= beta * s3; |
momoa | 0:17ad5be2e01a | 259 | } |
momoa | 0:17ad5be2e01a | 260 | // q+=qdot*dt |
momoa | 0:17ad5be2e01a | 261 | q0 += qdot0 * (1.0f / sampleFreq); |
momoa | 0:17ad5be2e01a | 262 | q1 += qdot1 * (1.0f / sampleFreq); |
momoa | 0:17ad5be2e01a | 263 | q2 += qdot2 * (1.0f / sampleFreq); |
momoa | 0:17ad5be2e01a | 264 | q3 += qdot3 * (1.0f / sampleFreq); |
momoa | 0:17ad5be2e01a | 265 | |
momoa | 0:17ad5be2e01a | 266 | norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
momoa | 0:17ad5be2e01a | 267 | q0 /= norm; |
momoa | 0:17ad5be2e01a | 268 | q1 /= norm; |
momoa | 0:17ad5be2e01a | 269 | q2 /= norm; |
momoa | 0:17ad5be2e01a | 270 | q3 /= norm; |
momoa | 0:17ad5be2e01a | 271 | |
momoa | 0:17ad5be2e01a | 272 | //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:17ad5be2e01a | 273 | //cta = asin(2.0f*q0*q2 - 2.0f*q1*q3) * 180.0f / PI; |
momoa | 0:17ad5be2e01a | 274 | //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:17ad5be2e01a | 275 | |
momoa | 0:17ad5be2e01a | 276 | //pc.printf("a_norm:%f\n\r", a_norm); |
momoa | 0:17ad5be2e01a | 277 | pc.printf("acc_ave:%f,%f,%f\n\r", ax, ay, az); |
momoa | 0:17ad5be2e01a | 278 | pc.printf("gyr_ave:%f,%f,%f\n\r", gx, gy, gz); |
momoa | 0:17ad5be2e01a | 279 | //pc.printf("mag_ave:%f,%f,%f\n\r", mx, my, mz); |
momoa | 0:17ad5be2e01a | 280 | //pc.printf("%f,%f,%f,%f\n\r", q0, q1, q2, q3); |
momoa | 0:17ad5be2e01a | 281 | //pc.printf("angle: %f, %f, %f\n\r", psi, cta, eta); |
momoa | 0:17ad5be2e01a | 282 | //pc.printf("%f\n\r", timea.read()); |
momoa | 0:17ad5be2e01a | 283 | |
momoa | 0:17ad5be2e01a | 284 | sum = sum - buff_p[cnt]; |
momoa | 0:17ad5be2e01a | 285 | pressure_new = bmp.getPressure(); |
momoa | 0:17ad5be2e01a | 286 | buff_p[cnt] = pressure_new; |
momoa | 0:17ad5be2e01a | 287 | sum = sum + pressure_new; |
momoa | 0:17ad5be2e01a | 288 | cnt++; |
momoa | 0:17ad5be2e01a | 289 | if(cnt == N) cnt = 0; |
momoa | 0:17ad5be2e01a | 290 | pressure_ave = sum/N; |
momoa | 0:17ad5be2e01a | 291 | tem = bmp.getTemperature(); |
momoa | 0:17ad5be2e01a | 292 | |
momoa | 0:17ad5be2e01a | 293 | pc.printf("Pre[hPa]: %f\n\r", pressure_ave); |
momoa | 0:17ad5be2e01a | 294 | pc.printf("Tem[C] : %f\n\r", tem); |
momoa | 0:17ad5be2e01a | 295 | |
momoa | 1:d77a49b0e9ff | 296 | if (cntsd == 0){ |
momoa | 1:d77a49b0e9ff | 297 | fp = fopen("/sd/log.txt","a"); |
momoa | 1:d77a49b0e9ff | 298 | } |
momoa | 0:17ad5be2e01a | 299 | |
momoa | 0:17ad5be2e01a | 300 | if(fp != NULL){ |
momoa | 0:17ad5be2e01a | 301 | |
momoa | 0:17ad5be2e01a | 302 | //fprintf(fp,"a_norm:%f\n\r", a_norm); |
momoa | 0:17ad5be2e01a | 303 | fprintf(fp,"acc_ave:%f,%f,%f\n\r", ax, ay, az); |
momoa | 0:17ad5be2e01a | 304 | fprintf(fp,"gyr_ave:%f,%f,%f\n\r", gx, gy, gz); |
momoa | 0:17ad5be2e01a | 305 | //fprintf(fp,"mag_ave:%f,%f,%f\n\r", mx, my, mz); |
momoa | 0:17ad5be2e01a | 306 | //fprintf(fp,"%f,%f,%f,%f\n\r", q0, q1, q2, q3); |
momoa | 0:17ad5be2e01a | 307 | //fprintf(fp,"angle: %f, %f, %f\n\r", psi, cta, eta); |
momoa | 0:17ad5be2e01a | 308 | //fprintf(fp,"%f\n\r", timea.read()); |
momoa | 1:d77a49b0e9ff | 309 | |
momoa | 0:17ad5be2e01a | 310 | fprintf(fp,"Pre[hPa]: %f\n\r", pressure_ave); |
momoa | 0:17ad5be2e01a | 311 | fprintf(fp,"Tem[C] : %f\n\r", tem); |
momoa | 1:d77a49b0e9ff | 312 | cntsd++; |
momoa | 1:d77a49b0e9ff | 313 | |
momoa | 1:d77a49b0e9ff | 314 | if(cntsd == 10){ |
momoa | 0:17ad5be2e01a | 315 | fclose(fp); |
momoa | 1:d77a49b0e9ff | 316 | pc.printf("Save.\n\r"); |
momoa | 1:d77a49b0e9ff | 317 | int cntsd = 0; |
momoa | 0:17ad5be2e01a | 318 | break; |
momoa | 0:17ad5be2e01a | 319 | } |
momoa | 1:d77a49b0e9ff | 320 | }else{ |
momoa | 0:17ad5be2e01a | 321 | pc.printf("Failed.\n\r"); |
momoa | 0:17ad5be2e01a | 322 | } |
momoa | 1:d77a49b0e9ff | 323 | //timea.read |
momoa | 0:17ad5be2e01a | 324 | wait(0.001); |
momoa | 0:17ad5be2e01a | 325 | } |
momoa | 0:17ad5be2e01a | 326 | } |