2020NSE_SSLB4 / Mbed 2 deprecated SDFileSystemMPUBMP_20200805

saving BMP and MPU,sending for CAN

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?

UserRevisionLine numberNew 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 }