2020NSE_SSLB4 / Mbed 2 deprecated SDFileSystemMPUBMP_20200805

saving BMP and MPU,sending for CAN

Dependencies:   mbed SDFileSystem BME280 MPU9250

main.cpp@2:d83d5822dc25, 2020-08-07 (annotated)

Committer:
momoa
Date:
Fri Aug 07 10:08:33 2020 +0000
Revision:
2:d83d5822dc25
Parent:
1:d77a49b0e9ff 
initialize

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 2:d83d5822dc25 26 CAN can(PA_11, PA_12,100000); //pin21,22 rd,td
momoa 0:17ad5be2e01a 27 #define PI 3.14159265358979323846f
momoa 0:17ad5be2e01a 28 #define N 5 // 5回移動平均
momoa 0:17ad5be2e01a 29 #define sampleFreq 100.0f
momoa 0:17ad5be2e01a 30 #define beta 0.33f // gain（大きいと加速度による補正が早い）
momoa 2:d83d5822dc25 31 Timer timea;
momoa 0:17ad5be2e01a 32
momoa 0:17ad5be2e01a 33 union Float2Byte{
momoa 0:17ad5be2e01a 34 float _float;
momoa 0:17ad5be2e01a 35 char _byte[4];
momoa 2:d83d5822dc25 36 };
momoa 2:d83d5822dc25 37 typedef union Float2Byte Float2Byte;
momoa 0:17ad5be2e01a 38
momoa 0:17ad5be2e01a 39 float pressure_ave = 0.0f; // 気圧[hPa]
momoa 0:17ad5be2e01a 40 float pressure_new = 0.0f;
momoa 0:17ad5be2e01a 41 float buff_p[N];
momoa 0:17ad5be2e01a 42 float sum = 0.0f;
momoa 0:17ad5be2e01a 43 int cnt = 0;
momoa 0:17ad5be2e01a 44 float tem = 0.0f; // 温度[C]
momoa 0:17ad5be2e01a 45
momoa 1:d77a49b0e9ff 46 FILE *fp;
momoa 2:d83d5822dc25 47 CANMessage msg;
momoa 1:d77a49b0e9ff 48
momoa 0:17ad5be2e01a 49 void mpu_init(){
momoa 0:17ad5be2e01a 50 uint8_t whoami_MPU9250, whoami_AK8963;
momoa 0:17ad5be2e01a 51 float mag_init[3];
momoa 0:17ad5be2e01a 52
momoa 0:17ad5be2e01a 53 whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
momoa 0:17ad5be2e01a 54 whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
momoa 0:17ad5be2e01a 55 pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
momoa 0:17ad5be2e01a 56 pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
momoa 0:17ad5be2e01a 57
momoa 0:17ad5be2e01a 58 if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){
momoa 0:17ad5be2e01a 59 pc.printf("MPU9250 is detected.\n\r");
momoa 0:17ad5be2e01a 60 wait(0.1);
momoa 0:17ad5be2e01a 61 mpu.resetMPU9250();
momoa 0:17ad5be2e01a 62 mpu.initMPU9250();
momoa 0:17ad5be2e01a 63 wait(0.1);
momoa 0:17ad5be2e01a 64 mpu.initAK8963(mag_init);
momoa 0:17ad5be2e01a 65 mpu.getGres();
momoa 0:17ad5be2e01a 66 mpu.getAres();
momoa 0:17ad5be2e01a 67 mpu.getMres();
momoa 0:17ad5be2e01a 68 wait(0.1);
momoa 0:17ad5be2e01a 69 }
momoa 0:17ad5be2e01a 70 else{
momoa 0:17ad5be2e01a 71 pc.printf("Could not detect MPU9250.\n\r");
momoa 0:17ad5be2e01a 72 pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
momoa 0:17ad5be2e01a 73 whoami_MPU9250, whoami_AK8963);
momoa 0:17ad5be2e01a 74 while(1);
momoa 0:17ad5be2e01a 75 }
momoa 0:17ad5be2e01a 76 }
momoa 0:17ad5be2e01a 77
momoa 2:d83d5822dc25 78 void send(float senddata,int id){
momoa 2:d83d5822dc25 79 pc.printf("Master send()\n\r");
momoa 2:d83d5822dc25 80
momoa 2:d83d5822dc25 81 /*ID: 0x01*/
momoa 2:d83d5822dc25 82 Float2Byte sendFloat;
momoa 2:d83d5822dc25 83 sendFloat._float = senddata;
momoa 2:d83d5822dc25 84 //ここに送りたい値を入れる．
momoa 2:d83d5822dc25 85
momoa 2:d83d5822dc25 86 char serialData[4];
momoa 2:d83d5822dc25 87 for(int i=0;i<4;++i){
momoa 2:d83d5822dc25 88 serialData[i] = sendFloat._byte[i];
momoa 2:d83d5822dc25 89 pc.printf("send_char: %d\n\r", serialData[i]);
momoa 2:d83d5822dc25 90 }
momoa 2:d83d5822dc25 91 pc.printf("sendFloat: %f\n\r", sendFloat._float);
momoa 2:d83d5822dc25 92 if(can.write(CANMessage(id, serialData, 4))){
momoa 2:d83d5822dc25 93 pc.printf("Send.\n\r");
momoa 2:d83d5822dc25 94 }
momoa 2:d83d5822dc25 95 //myled = !myled;
momoa 2:d83d5822dc25 96 }
momoa 2:d83d5822dc25 97
momoa 0:17ad5be2e01a 98 int main(){
momoa 1:d77a49b0e9ff 99
momoa 1:d77a49b0e9ff 100 fp = fopen("/sd/log.txt","a");
momoa 0:17ad5be2e01a 101 if(fp != NULL){
momoa 2:d83d5822dc25 102 fprintf(fp,"Start.\n\r");
momoa 2:d83d5822dc25 103 fprintf(fp,"time,ax_ave,ay_ave,az_ave,gx_ave,gy_ave,gz_ave,Pre[hPa],Tem[C]\n\r");
momoa 0:17ad5be2e01a 104 fclose(fp);
momoa 0:17ad5be2e01a 105 }
momoa 0:17ad5be2e01a 106 else{
momoa 0:17ad5be2e01a 107 pc.printf("Failed.\n\r");
momoa 0:17ad5be2e01a 108 }
momoa 0:17ad5be2e01a 109
momoa 0:17ad5be2e01a 110 int16_t acc[3], gyr[3], mag[3];
momoa 1:d77a49b0e9ff 111 int cntsd = 0;
momoa 0:17ad5be2e01a 112
momoa 0:17ad5be2e01a 113 float buff_ax[N], buff_ay[N], buff_az[N];
momoa 0:17ad5be2e01a 114 float buff_gx[N], buff_gy[N], buff_gz[N];
momoa 0:17ad5be2e01a 115 float buff_mx[N], buff_my[N], buff_mz[N];
momoa 0:17ad5be2e01a 116 float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
momoa 0:17ad5be2e01a 117 float sum_gx = 0.0f, sum_gy = 0.0f, sum_gz = 0.0f;
momoa 0:17ad5be2e01a 118 float sum_mx = 0.0f, sum_my = 0.0f, sum_mz = 0.0f;
momoa 0:17ad5be2e01a 119 float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
momoa 0:17ad5be2e01a 120 float gx_new = 0.0f, gy_new = 0.0f, gz_new = 0.0f;
momoa 0:17ad5be2e01a 121 float mx_new = 0.0f, my_new = 0.0f, mz_new = 0.0f;
momoa 0:17ad5be2e01a 122 float gx_i = 0.0f, gy_i = 0.0f, gz_i = 0.0f;
momoa 0:17ad5be2e01a 123 float ax = 0.0f, ay = 0.0f, az = 0.0f;
momoa 0:17ad5be2e01a 124 float gx = 0.0f, gy = 0.0f, gz = 0.0f;
momoa 0:17ad5be2e01a 125 float mx = 0.0f, my = 0.0f, mz = 0.0f;
momoa 0:17ad5be2e01a 126 int cnt = 0;
momoa 0:17ad5be2e01a 127 float a_norm = 0.0f;
momoa 0:17ad5be2e01a 128
momoa 0:17ad5be2e01a 129 float psi = 0.0f, cta = 0.0f, eta =0.0f;
momoa 0:17ad5be2e01a 130 float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
momoa 0:17ad5be2e01a 131 float _gx, _gy, _gz;
momoa 0:17ad5be2e01a 132 float norm;
momoa 0:17ad5be2e01a 133 float s0, s1, s2, s3;
momoa 0:17ad5be2e01a 134 float qdot0, qdot1, qdot2, qdot3;
momoa 0:17ad5be2e01a 135 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, _q0q0, _q1q1, _q2q2, _q3q3;
momoa 0:17ad5be2e01a 136
momoa 2:d83d5822dc25 137 float f = 0.0;
momoa 2:d83d5822dc25 138
momoa 0:17ad5be2e01a 139 wait(0.1);
momoa 0:17ad5be2e01a 140 mpu_init();
momoa 0:17ad5be2e01a 141 bmp.initialize();
momoa 0:17ad5be2e01a 142 wait(2.0); // 静止させるまで待つ
momoa 0:17ad5be2e01a 143
momoa 0:17ad5be2e01a 144 // 初期値
momoa 0:17ad5be2e01a 145 for(int i=0;i<N;i++){
momoa 0:17ad5be2e01a 146 mpu.readGyroData(gyr);
momoa 0:17ad5be2e01a 147 mpu.readAccelData(acc);
momoa 0:17ad5be2e01a 148 mpu.readMagData(mag);
momoa 0:17ad5be2e01a 149 buff_ax[i] = acc[0] / 2049.81;
momoa 0:17ad5be2e01a 150 buff_ay[i] = acc[1] / 2049.81;
momoa 0:17ad5be2e01a 151 buff_az[i] = acc[2] / 2049.81;
momoa 0:17ad5be2e01a 152 buff_gx[i] = gyr[0] * 0.03048;
momoa 0:17ad5be2e01a 153 buff_gy[i] = gyr[1] * 0.03048;
momoa 0:17ad5be2e01a 154 buff_gz[i] = gyr[2] * 0.03048;
momoa 0:17ad5be2e01a 155 buff_mx[i] = mag[0] * 0.15;
momoa 0:17ad5be2e01a 156 buff_my[i] = mag[1] * 0.15;
momoa 0:17ad5be2e01a 157 buff_mz[i] = mag[2] * 0.15;
momoa 0:17ad5be2e01a 158 sum_ax += buff_ax[i];
momoa 0:17ad5be2e01a 159 sum_ay += buff_ay[i];
momoa 0:17ad5be2e01a 160 sum_az += buff_az[i];
momoa 0:17ad5be2e01a 161 sum_gx += buff_gx[i];
momoa 0:17ad5be2e01a 162 sum_gy += buff_gy[i];
momoa 0:17ad5be2e01a 163 sum_gz += buff_gz[i];
momoa 0:17ad5be2e01a 164 sum_mx += buff_mx[i];
momoa 0:17ad5be2e01a 165 sum_my += buff_my[i];
momoa 0:17ad5be2e01a 166 sum_mz += buff_mz[i];
momoa 0:17ad5be2e01a 167
momoa 0:17ad5be2e01a 168 buff_p[i] = bmp.getPressure();
momoa 0:17ad5be2e01a 169 sum += buff_p[i];
momoa 0:17ad5be2e01a 170 }
momoa 0:17ad5be2e01a 171
momoa 0:17ad5be2e01a 172 // 静止時角速度
momoa 0:17ad5be2e01a 173 gx_i = sum_gx / N;
momoa 0:17ad5be2e01a 174 gy_i = sum_gy / N;
momoa 0:17ad5be2e01a 175 gz_i = sum_gz / N;
momoa 0:17ad5be2e01a 176 pc.printf("gyr_ini:%f,%f,%f\n\r", gx_i, gy_i, gz_i);
momoa 0:17ad5be2e01a 177
momoa 0:17ad5be2e01a 178 wait(0.1);
momoa 2:d83d5822dc25 179 timea.start();
momoa 0:17ad5be2e01a 180 pc.printf("Start.\n\r");
momoa 1:d77a49b0e9ff 181
momoa 0:17ad5be2e01a 182 while(1){
momoa 2:d83d5822dc25 183
momoa 0:17ad5be2e01a 184 mpu.readGyroData(gyr);
momoa 0:17ad5be2e01a 185 mpu.readAccelData(acc);
momoa 0:17ad5be2e01a 186 mpu.readMagData(mag);
momoa 0:17ad5be2e01a 187
momoa 0:17ad5be2e01a 188 /* 5回移動平均 */
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 ax_new = acc[0] / 2049.81;
momoa 0:17ad5be2e01a 199 ay_new = acc[1] / 2049.81;
momoa 0:17ad5be2e01a 200 az_new = acc[2] / 2049.81;
momoa 0:17ad5be2e01a 201 gx_new = gyr[0] * 0.03048;
momoa 0:17ad5be2e01a 202 gy_new = gyr[1] * 0.03048;
momoa 0:17ad5be2e01a 203 gz_new = gyr[2] * 0.03048;
momoa 0:17ad5be2e01a 204 mx_new = mag[0] * 0.15;
momoa 0:17ad5be2e01a 205 my_new = mag[1] * 0.15;
momoa 0:17ad5be2e01a 206 mz_new = mag[2] * 0.15;
momoa 0:17ad5be2e01a 207 buff_ax[cnt] = ax_new;
momoa 0:17ad5be2e01a 208 buff_ay[cnt] = ay_new;
momoa 0:17ad5be2e01a 209 buff_az[cnt] = az_new;
momoa 0:17ad5be2e01a 210 buff_gx[cnt] = gx_new;
momoa 0:17ad5be2e01a 211 buff_gy[cnt] = gy_new;
momoa 0:17ad5be2e01a 212 buff_gz[cnt] = gz_new;
momoa 0:17ad5be2e01a 213 buff_mx[cnt] = mx_new;
momoa 0:17ad5be2e01a 214 buff_my[cnt] = my_new;
momoa 0:17ad5be2e01a 215 buff_mz[cnt] = mz_new;
momoa 0:17ad5be2e01a 216 sum_ax = sum_ax + buff_ax[cnt];
momoa 0:17ad5be2e01a 217 sum_ay = sum_ay + buff_ay[cnt];
momoa 0:17ad5be2e01a 218 sum_az = sum_az + buff_az[cnt];
momoa 0:17ad5be2e01a 219 sum_gx = sum_gx + buff_gx[cnt];
momoa 0:17ad5be2e01a 220 sum_gy = sum_gy + buff_gy[cnt];
momoa 0:17ad5be2e01a 221 sum_gz = sum_gz + buff_gz[cnt];
momoa 0:17ad5be2e01a 222 sum_mx = sum_mx + buff_mx[cnt];
momoa 0:17ad5be2e01a 223 sum_my = sum_my + buff_my[cnt];
momoa 0:17ad5be2e01a 224 sum_mz = sum_mz + buff_mz[cnt];
momoa 0:17ad5be2e01a 225 cnt++;
momoa 0:17ad5be2e01a 226 if(cnt == N) cnt = 0;
momoa 0:17ad5be2e01a 227
momoa 0:17ad5be2e01a 228 ax = sum_ax / N;
momoa 0:17ad5be2e01a 229 ay = sum_ay / N;
momoa 0:17ad5be2e01a 230 az = sum_az / N;
momoa 0:17ad5be2e01a 231 gx = sum_gx / N - gx_i;
momoa 0:17ad5be2e01a 232 gy = sum_gy / N - gy_i;
momoa 0:17ad5be2e01a 233 gz = sum_gz / N - gz_i;
momoa 0:17ad5be2e01a 234 mx = sum_mx / N;
momoa 0:17ad5be2e01a 235 my = sum_my / N;
momoa 0:17ad5be2e01a 236 mz = sum_mz / N;
momoa 0:17ad5be2e01a 237
momoa 0:17ad5be2e01a 238 /* 加速度ノルム */
momoa 0:17ad5be2e01a 239 a_norm = sqrt(ax*ax + ay*ay + az*az);
momoa 0:17ad5be2e01a 240
momoa 0:17ad5be2e01a 241 /* 姿勢角(Quaternion->Eulerangle) */
momoa 0:17ad5be2e01a 242 _gx = gx * PI / 180.f;
momoa 0:17ad5be2e01a 243 _gy = gy * PI / 180.f;
momoa 0:17ad5be2e01a 244 _gz = gz * PI / 180.f;
momoa 0:17ad5be2e01a 245 // qdot=0.5*q*w
momoa 0:17ad5be2e01a 246 qdot0 = 0.5f * ( - q1*_gx - q2*_gy - q3*_gz);
momoa 0:17ad5be2e01a 247 qdot1 = 0.5f * (q0*_gx + q2*_gz - q3*_gy);
momoa 0:17ad5be2e01a 248 qdot2 = 0.5f * (q0*_gy - q1*_gz + q3*_gx);
momoa 0:17ad5be2e01a 249 qdot3 = 0.5f * (q0*_gz + q1*_gy - q2*_gx );
momoa 0:17ad5be2e01a 250 // 加速度で補正する（動加速度下ではどうなん？動だけに）
momoa 0:17ad5be2e01a 251 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))){
momoa 0:17ad5be2e01a 252 norm = sqrt(ax*ax + ay*ay + az*az);
momoa 0:17ad5be2e01a 253 ax /= norm;
momoa 0:17ad5be2e01a 254 ay /= norm;
momoa 0:17ad5be2e01a 255 az /= norm;
momoa 0:17ad5be2e01a 256
momoa 0:17ad5be2e01a 257 _2q0 = 2.0f * q0;
momoa 0:17ad5be2e01a 258 _2q1 = 2.0f * q1;
momoa 0:17ad5be2e01a 259 _2q2 = 2.0f * q2;
momoa 0:17ad5be2e01a 260 _2q3 = 2.0f * q3;
momoa 0:17ad5be2e01a 261 _4q0 = 4.0f * q0;
momoa 0:17ad5be2e01a 262 _4q1 = 4.0f * q1;
momoa 0:17ad5be2e01a 263 _4q2 = 4.0f * q2;
momoa 0:17ad5be2e01a 264 _8q1 = 8.0f * q1;
momoa 0:17ad5be2e01a 265 _8q2 = 8.0f * q2;
momoa 0:17ad5be2e01a 266 _q0q0 = q0 * q0;
momoa 0:17ad5be2e01a 267 _q1q1 = q1 * q1;
momoa 0:17ad5be2e01a 268 _q2q2 = q2 * q2;
momoa 0:17ad5be2e01a 269 _q3q3 = q3 * q3;
momoa 0:17ad5be2e01a 270
momoa 0:17ad5be2e01a 271 s0 = _4q0*_q2q2 + _2q2*ax + _4q0*_q1q1 - _2q1*ay;
momoa 0:17ad5be2e01a 272 s1 = _4q1*_q3q3 - _2q3*ax + 4.0f*_q0q0*q1 - _2q0*ay - _4q1 + _8q1*_q1q1 + _8q1*_q2q2 + _4q1*az;
momoa 0:17ad5be2e01a 273 s2 = 4.0f*_q0q0*q2 + _2q0*ax + _4q2*_q3q3 - _2q3*ay - _4q2 + _8q2*_q1q1 + _8q2*_q2q2 + _4q2*az;
momoa 0:17ad5be2e01a 274 s3 = 4.0f*_q1q1*q3 - _2q1*ax + 4.0f*_q2q2*q3 - _2q2*ay;
momoa 0:17ad5be2e01a 275
momoa 0:17ad5be2e01a 276 norm = sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
momoa 0:17ad5be2e01a 277 s0 /= norm;
momoa 0:17ad5be2e01a 278 s1 /= norm;
momoa 0:17ad5be2e01a 279 s2 /= norm;
momoa 0:17ad5be2e01a 280 s3 /= norm;
momoa 0:17ad5be2e01a 281
momoa 0:17ad5be2e01a 282 qdot0 -= beta * s0;
momoa 0:17ad5be2e01a 283 qdot1 -= beta * s1;
momoa 0:17ad5be2e01a 284 qdot2 -= beta * s2;
momoa 0:17ad5be2e01a 285 qdot3 -= beta * s3;
momoa 0:17ad5be2e01a 286 }
momoa 0:17ad5be2e01a 287 // q+=qdot*dt
momoa 0:17ad5be2e01a 288 q0 += qdot0 * (1.0f / sampleFreq);
momoa 0:17ad5be2e01a 289 q1 += qdot1 * (1.0f / sampleFreq);
momoa 0:17ad5be2e01a 290 q2 += qdot2 * (1.0f / sampleFreq);
momoa 0:17ad5be2e01a 291 q3 += qdot3 * (1.0f / sampleFreq);
momoa 0:17ad5be2e01a 292
momoa 0:17ad5be2e01a 293 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
momoa 0:17ad5be2e01a 294 q0 /= norm;
momoa 0:17ad5be2e01a 295 q1 /= norm;
momoa 0:17ad5be2e01a 296 q2 /= norm;
momoa 0:17ad5be2e01a 297 q3 /= norm;
momoa 0:17ad5be2e01a 298
momoa 0:17ad5be2e01a 299 //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 300 //cta = asin(2.0f*q0*q2 - 2.0f*q1*q3) * 180.0f / PI;
momoa 0:17ad5be2e01a 301 //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 302
momoa 0:17ad5be2e01a 303 //pc.printf("a_norm:%f\n\r", a_norm);
momoa 0:17ad5be2e01a 304 pc.printf("acc_ave:%f,%f,%f\n\r", ax, ay, az);
momoa 0:17ad5be2e01a 305 pc.printf("gyr_ave:%f,%f,%f\n\r", gx, gy, gz);
momoa 0:17ad5be2e01a 306 //pc.printf("mag_ave:%f,%f,%f\n\r", mx, my, mz);
momoa 0:17ad5be2e01a 307 //pc.printf("%f,%f,%f,%f\n\r", q0, q1, q2, q3);
momoa 0:17ad5be2e01a 308 //pc.printf("angle: %f, %f, %f\n\r", psi, cta, eta);
momoa 0:17ad5be2e01a 309 //pc.printf("%f\n\r", timea.read());
momoa 0:17ad5be2e01a 310
momoa 0:17ad5be2e01a 311 sum = sum - buff_p[cnt];
momoa 0:17ad5be2e01a 312 pressure_new = bmp.getPressure();
momoa 0:17ad5be2e01a 313 buff_p[cnt] = pressure_new;
momoa 0:17ad5be2e01a 314 sum = sum + pressure_new;
momoa 0:17ad5be2e01a 315 cnt++;
momoa 0:17ad5be2e01a 316 if(cnt == N) cnt = 0;
momoa 0:17ad5be2e01a 317 pressure_ave = sum/N;
momoa 0:17ad5be2e01a 318 tem = bmp.getTemperature();
momoa 0:17ad5be2e01a 319
momoa 0:17ad5be2e01a 320 pc.printf("Pre[hPa]: %f\n\r", pressure_ave);
momoa 0:17ad5be2e01a 321 pc.printf("Tem[C] : %f\n\r", tem);
momoa 0:17ad5be2e01a 322
momoa 1:d77a49b0e9ff 323 if (cntsd == 0){
momoa 1:d77a49b0e9ff 324 fp = fopen("/sd/log.txt","a");
momoa 1:d77a49b0e9ff 325 }
momoa 0:17ad5be2e01a 326
momoa 0:17ad5be2e01a 327 if(fp != NULL){
momoa 0:17ad5be2e01a 328
momoa 0:17ad5be2e01a 329 //fprintf(fp,"a_norm:%f\n\r", a_norm);
momoa 2:d83d5822dc25 330 //fprintf(fp,"acc_ave:%f,%f,%f\n\r", ax, ay, az);
momoa 2:d83d5822dc25 331 //fprintf(fp,"gyr_ave:%f,%f,%f\n\r", gx, gy, gz);
momoa 0:17ad5be2e01a 332 //fprintf(fp,"mag_ave:%f,%f,%f\n\r", mx, my, mz);
momoa 0:17ad5be2e01a 333 //fprintf(fp,"%f,%f,%f,%f\n\r", q0, q1, q2, q3);
momoa 0:17ad5be2e01a 334 //fprintf(fp,"angle: %f, %f, %f\n\r", psi, cta, eta);
momoa 0:17ad5be2e01a 335 //fprintf(fp,"%f\n\r", timea.read());
momoa 1:d77a49b0e9ff 336
momoa 2:d83d5822dc25 337 //fprintf(fp,"Pre[hPa]: %f\n\r", pressure_ave);
momoa 2:d83d5822dc25 338 //fprintf(fp,"Tem[C] : %f\n\r", tem);
momoa 2:d83d5822dc25 339 //fprintf(fp,"ax_ave,ay_ave,az_ave,gx_ave,gy_ave,gz_ave,Pre[hPa],Tem[C]\n\r");
momoa 2:d83d5822dc25 340 fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r",timea.read(),ax, ay, az, gx, gy, gz, pressure_ave, tem);
momoa 2:d83d5822dc25 341
momoa 1:d77a49b0e9ff 342 cntsd++;
momoa 1:d77a49b0e9ff 343
momoa 1:d77a49b0e9ff 344 if(cntsd == 10){
momoa 0:17ad5be2e01a 345 fclose(fp);
momoa 1:d77a49b0e9ff 346 pc.printf("Save.\n\r");
momoa 2:d83d5822dc25 347 cntsd = 0;
momoa 0:17ad5be2e01a 348 }
momoa 1:d77a49b0e9ff 349 }else{
momoa 0:17ad5be2e01a 350 pc.printf("Failed.\n\r");
momoa 0:17ad5be2e01a 351 }
momoa 2:d83d5822dc25 352 timea.read();
momoa 2:d83d5822dc25 353 send(f,0x03);
momoa 2:d83d5822dc25 354 f = f + 0.86;
momoa 2:d83d5822dc25 355 pc.printf("%d", cntsd);
momoa 0:17ad5be2e01a 356 wait(0.001);
momoa 0:17ad5be2e01a 357 }
momoa 0:17ad5be2e01a 358 }