Atsumi Toda / Mbed OS UAV_Logger4

UAV_Logger1から改造。サーボ機構のプログラムを追加

Dependencies:   MPU6050_alter HMC5883L

main.cpp@2:e6496a794bde, 2019-05-24 (annotated)

Committer:
Joeatsumi
Date:
Fri May 24 05:57:12 2019 +0000
Revision:
2:e6496a794bde
Parent:
1:9a5f06e7969e
Child:
3:3fa7882a5fd0
mpu6050 and GHC5883L for atitude estimation for UAV.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hudakz 0:87642278ede6 1 #include "mbed.h"
hudakz 0:87642278ede6 2 #include <stdio.h>
hudakz 0:87642278ede6 3 #include <errno.h>
Joeatsumi 2:e6496a794bde 4 #include "MPU6050.h"
Joeatsumi 2:e6496a794bde 5 #include "HMC5883L.h"
Joeatsumi 2:e6496a794bde 6
Joeatsumi 2:e6496a794bde 7 #include "Vector.h"
Joeatsumi 2:e6496a794bde 8 #include "Matrix.h"
Joeatsumi 2:e6496a794bde 9 #include "Vector_Matrix_operator.h"
hudakz 0:87642278ede6 10
hudakz 0:87642278ede6 11 // Block devices
hudakz 0:87642278ede6 12 //#include "SPIFBlockDevice.h"
hudakz 0:87642278ede6 13 //#include "DataFlashBlockDevice.h"
hudakz 0:87642278ede6 14 #include "SDBlockDevice.h"
hudakz 0:87642278ede6 15 //#include "HeapBlockDevice.h"
hudakz 0:87642278ede6 16
hudakz 0:87642278ede6 17 // File systems
hudakz 0:87642278ede6 18
hudakz 0:87642278ede6 19 //#include "LittleFileSystem.h"
hudakz 0:87642278ede6 20 #include "FATFileSystem.h"
hudakz 0:87642278ede6 21
Joeatsumi 2:e6496a794bde 22 #define M_PI 3.141592
Joeatsumi 2:e6496a794bde 23 #define PI2 (2*M_PI)
Joeatsumi 2:e6496a794bde 24
Joeatsumi 2:e6496a794bde 25 /*地磁気センサの補正値（足し合わせる）*/
Joeatsumi 2:e6496a794bde 26 #define MAG_FIX_X 338
Joeatsumi 2:e6496a794bde 27 #define MAG_FIX_Y 20
Joeatsumi 2:e6496a794bde 28 #define MAG_FIX_Z -50
Joeatsumi 2:e6496a794bde 29
Joeatsumi 2:e6496a794bde 30 /*ジャイロセンサの補正値（引く）*/
Joeatsumi 2:e6496a794bde 31 #define GYRO_FIX_X 290.5498
Joeatsumi 2:e6496a794bde 32 #define GYRO_FIX_Y 99.29363
Joeatsumi 2:e6496a794bde 33 #define GYRO_FIX_Z 61.41011
hudakz 0:87642278ede6 34
hudakz 0:87642278ede6 35 // File system declaration
hudakz 0:87642278ede6 36 FATFileSystem fileSystem("fs");
hudakz 0:87642278ede6 37
Joeatsumi 2:e6496a794bde 38 /*-----割り込み--------*/
Joeatsumi 2:e6496a794bde 39 Ticker flipper;
Joeatsumi 2:e6496a794bde 40 /*--------------------*/
Joeatsumi 2:e6496a794bde 41 /*-----タイマー---------*/
Joeatsumi 2:e6496a794bde 42 Timer passed_time;
Joeatsumi 2:e6496a794bde 43 /*---------------------*/
Joeatsumi 2:e6496a794bde 44 /*-------ピン指定------------*/
Joeatsumi 2:e6496a794bde 45 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
Joeatsumi 2:e6496a794bde 46 MPU6050 mpu(p28, p27);//sda scl
Joeatsumi 2:e6496a794bde 47 HMC5883L compass(p28, p27);//sda scl
Joeatsumi 2:e6496a794bde 48
Joeatsumi 2:e6496a794bde 49 RawSerial gps(p9,p10); //serial port for gps_module
Joeatsumi 2:e6496a794bde 50 RawSerial pc(USBTX, USBRX);
Joeatsumi 2:e6496a794bde 51
Joeatsumi 2:e6496a794bde 52 /*--------------------------*/
Joeatsumi 2:e6496a794bde 53
Joeatsumi 2:e6496a794bde 54 DigitalIn switch_off(p30);
Joeatsumi 2:e6496a794bde 55 DigitalOut led2(LED2);
Joeatsumi 2:e6496a794bde 56
Joeatsumi 2:e6496a794bde 57 /*-----------グローバル変数-----------*/
Joeatsumi 2:e6496a794bde 58 float g_hokui,g_tokei;
Joeatsumi 2:e6496a794bde 59 int fp_count=0;
Joeatsumi 2:e6496a794bde 60 int gps_flag=0;
Joeatsumi 2:e6496a794bde 61 int gps_flag_conversion=0;
Joeatsumi 2:e6496a794bde 62
Joeatsumi 2:e6496a794bde 63 char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
Joeatsumi 2:e6496a794bde 64 char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している
Joeatsumi 2:e6496a794bde 65
Joeatsumi 2:e6496a794bde 66 unsigned int tokei_int_receive;
Joeatsumi 2:e6496a794bde 67 unsigned int hokui_int_receive;
Joeatsumi 2:e6496a794bde 68
Joeatsumi 2:e6496a794bde 69 float tokei_float_receive;
Joeatsumi 2:e6496a794bde 70 float hokui_float_receive;
Joeatsumi 2:e6496a794bde 71
Joeatsumi 2:e6496a794bde 72 float a[3];//加速度の値
Joeatsumi 2:e6496a794bde 73 float g[3];//ジャイロの値[rad/s]
Joeatsumi 2:e6496a794bde 74 float dpsX,dpsY,dpsZ;
Joeatsumi 2:e6496a794bde 75 float AccX,AccY,AccZ;
Joeatsumi 2:e6496a794bde 76
Joeatsumi 2:e6496a794bde 77 int sensor_array[6];
Joeatsumi 2:e6496a794bde 78 int16_t mag[3];
Joeatsumi 2:e6496a794bde 79
Joeatsumi 2:e6496a794bde 80 double Roll_Acc,Pitch_Acc;
Joeatsumi 2:e6496a794bde 81 double Yaw;
Joeatsumi 2:e6496a794bde 82 double Azi_mag;//地磁気からの方位
Joeatsumi 2:e6496a794bde 83 double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion
Joeatsumi 2:e6496a794bde 84
Joeatsumi 2:e6496a794bde 85 /*----------------------------------*/
Joeatsumi 2:e6496a794bde 86 /*--------------------------行列、ベクトル-----------------------------*/
Joeatsumi 2:e6496a794bde 87
Joeatsumi 2:e6496a794bde 88 Vector Quaternion_atitude_from_omega(4);
Joeatsumi 2:e6496a794bde 89 Matrix Omega_matrix(4,4),Half_matrix(4,4);
Joeatsumi 2:e6496a794bde 90 Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);
Joeatsumi 2:e6496a794bde 91
Joeatsumi 2:e6496a794bde 92 /*-------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 93
Joeatsumi 2:e6496a794bde 94 /*プロトタイプ宣言*/
Joeatsumi 2:e6496a794bde 95 void toString_V(Vector& v); // ベクトルデバッグ用
Joeatsumi 2:e6496a794bde 96 void gps_rx();
Joeatsumi 2:e6496a794bde 97 void gps_convertion();
Joeatsumi 2:e6496a794bde 98 /*--------------*/
Joeatsumi 2:e6496a794bde 99
Joeatsumi 2:e6496a794bde 100 void toString_V(Vector& v)
Joeatsumi 2:e6496a794bde 101 {
Joeatsumi 2:e6496a794bde 102
Joeatsumi 2:e6496a794bde 103 for(int i=0; i<v.GetDim(); i++) {
Joeatsumi 2:e6496a794bde 104 pc.printf("%.6f\t", v.GetComp(i+1));
Joeatsumi 2:e6496a794bde 105 }
Joeatsumi 2:e6496a794bde 106 pc.printf("\r\n");
Joeatsumi 2:e6496a794bde 107
Joeatsumi 2:e6496a794bde 108 }
Joeatsumi 2:e6496a794bde 109
Joeatsumi 2:e6496a794bde 110 void scan_update(int box_sensor[6],int16_t m[3]){
Joeatsumi 2:e6496a794bde 111
Joeatsumi 2:e6496a794bde 112 int a[3];//加速度の値
Joeatsumi 2:e6496a794bde 113 int g[3];//角速度の値
Joeatsumi 2:e6496a794bde 114
Joeatsumi 2:e6496a794bde 115 int16_t raw_compass[3];//地磁気センサの値
Joeatsumi 2:e6496a794bde 116
Joeatsumi 2:e6496a794bde 117 mpu.setAcceleroRange(0);//acceleration range is +-4G
Joeatsumi 2:e6496a794bde 118 mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
Joeatsumi 2:e6496a794bde 119
Joeatsumi 2:e6496a794bde 120 mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
Joeatsumi 2:e6496a794bde 121 mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s]
Joeatsumi 2:e6496a794bde 122 compass.getXYZ(raw_compass);//地磁気の値を格納する配列
Joeatsumi 2:e6496a794bde 123
Joeatsumi 2:e6496a794bde 124 box_sensor[0]=-g[0];
Joeatsumi 2:e6496a794bde 125 box_sensor[1]=g[1];
Joeatsumi 2:e6496a794bde 126 box_sensor[2]=-g[2];
Joeatsumi 2:e6496a794bde 127
Joeatsumi 2:e6496a794bde 128 box_sensor[3]=a[0];
Joeatsumi 2:e6496a794bde 129 box_sensor[4]=-a[1];
Joeatsumi 2:e6496a794bde 130 box_sensor[5]=a[2];
Joeatsumi 2:e6496a794bde 131
Joeatsumi 2:e6496a794bde 132 m[0]=(raw_compass[0]);//x
Joeatsumi 2:e6496a794bde 133 m[1]=(raw_compass[2]);//y
Joeatsumi 2:e6496a794bde 134 m[2]=(raw_compass[1]);//z
Joeatsumi 2:e6496a794bde 135
Joeatsumi 2:e6496a794bde 136 }
Joeatsumi 2:e6496a794bde 137
Joeatsumi 2:e6496a794bde 138 void gps_rx(){
Joeatsumi 2:e6496a794bde 139
Joeatsumi 2:e6496a794bde 140 if((gps.getc()=='$')&&(gps_flag==0)){
Joeatsumi 2:e6496a794bde 141 for(int i=0;i<9;i++){
Joeatsumi 2:e6496a794bde 142 gps_data[i]=gps.getc();
Joeatsumi 2:e6496a794bde 143 //pc.putc(gps_data[i]);
Joeatsumi 2:e6496a794bde 144 }
Joeatsumi 2:e6496a794bde 145
Joeatsumi 2:e6496a794bde 146 gps_data[9]='\0';
Joeatsumi 2:e6496a794bde 147
Joeatsumi 2:e6496a794bde 148 for(int i=0;i<9;i++){
Joeatsumi 2:e6496a794bde 149 gps_data_2[i]=gps.getc();
Joeatsumi 2:e6496a794bde 150 //pc.putc(gps_data[i]);
Joeatsumi 2:e6496a794bde 151 }
Joeatsumi 2:e6496a794bde 152
Joeatsumi 2:e6496a794bde 153 gps_data_2[9]='\0';
Joeatsumi 2:e6496a794bde 154
Joeatsumi 2:e6496a794bde 155 }//if(twe.getc()==':')
Joeatsumi 2:e6496a794bde 156
Joeatsumi 2:e6496a794bde 157 //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
Joeatsumi 2:e6496a794bde 158
Joeatsumi 2:e6496a794bde 159 gps_flag=1;
Joeatsumi 2:e6496a794bde 160
Joeatsumi 2:e6496a794bde 161 }//gps_rx ends
Joeatsumi 2:e6496a794bde 162
Joeatsumi 2:e6496a794bde 163 void gps_convertion(){
Joeatsumi 2:e6496a794bde 164 while(1){
Joeatsumi 2:e6496a794bde 165 if(gps_flag==1){
Joeatsumi 2:e6496a794bde 166 tokei_int_receive=atoi(gps_data);
Joeatsumi 2:e6496a794bde 167 hokui_int_receive=atoi(gps_data_2);
Joeatsumi 2:e6496a794bde 168
Joeatsumi 2:e6496a794bde 169 //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
Joeatsumi 2:e6496a794bde 170
Joeatsumi 2:e6496a794bde 171 tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
Joeatsumi 2:e6496a794bde 172 hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;
Joeatsumi 2:e6496a794bde 173
Joeatsumi 2:e6496a794bde 174 gps_flag_conversion=1;
Joeatsumi 2:e6496a794bde 175
Joeatsumi 2:e6496a794bde 176 }else{gps_flag_conversion=0;}
Joeatsumi 2:e6496a794bde 177 }//while ends
Joeatsumi 2:e6496a794bde 178
Joeatsumi 2:e6496a794bde 179 }//ends
Joeatsumi 2:e6496a794bde 180
Joeatsumi 2:e6496a794bde 181 void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
Joeatsumi 2:e6496a794bde 182
Joeatsumi 2:e6496a794bde 183 double Roll_before = double(y_acc)/double(z_acc);
Joeatsumi 2:e6496a794bde 184 double Roll = atan(Roll_before);
Joeatsumi 2:e6496a794bde 185
Joeatsumi 2:e6496a794bde 186 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
Joeatsumi 2:e6496a794bde 187 double Pitch = -atan(Pitch_before);
Joeatsumi 2:e6496a794bde 188
Joeatsumi 2:e6496a794bde 189 //Yaw=0.0;
Joeatsumi 2:e6496a794bde 190
Joeatsumi 2:e6496a794bde 191 /*Quaternionの要素へオイラー角を変換する*/
Joeatsumi 2:e6496a794bde 192 qua[0]=cos(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
Joeatsumi 2:e6496a794bde 193 qua[1]=sin(Roll/2.0)*cos(Pitch/2.0)*cos(Yaw/2.0)-cos(Roll/2.0)*sin(Pitch/2.0)*sin(Yaw/2.0);
Joeatsumi 2:e6496a794bde 194 qua[2]=cos(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0)+sin(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0);
Joeatsumi 2:e6496a794bde 195 qua[3]=cos(Roll/2.0)*cos(Pitch/2.0)*sin(Yaw/2.0)-sin(Roll/2.0)*sin(Pitch/2.0)*cos(Yaw/2.0);
Joeatsumi 2:e6496a794bde 196
Joeatsumi 2:e6496a794bde 197
Joeatsumi 2:e6496a794bde 198 }
Joeatsumi 2:e6496a794bde 199
Joeatsumi 2:e6496a794bde 200 void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
Joeatsumi 2:e6496a794bde 201
Joeatsumi 2:e6496a794bde 202 double Roll_before = double(y_acc)/double(z_acc);
Joeatsumi 2:e6496a794bde 203 double Roll = atan(Roll_before);
Joeatsumi 2:e6496a794bde 204
Joeatsumi 2:e6496a794bde 205 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
Joeatsumi 2:e6496a794bde 206 double Pitch = -atan(Pitch_before);
Joeatsumi 2:e6496a794bde 207
Joeatsumi 2:e6496a794bde 208
Joeatsumi 2:e6496a794bde 209 qua[0]=Pitch;
Joeatsumi 2:e6496a794bde 210 qua[1]=Roll;
Joeatsumi 2:e6496a794bde 211
Joeatsumi 2:e6496a794bde 212 }
Joeatsumi 2:e6496a794bde 213
Joeatsumi 2:e6496a794bde 214 void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
Joeatsumi 2:e6496a794bde 215
Joeatsumi 2:e6496a794bde 216 //y_acc*=-1.0;
Joeatsumi 2:e6496a794bde 217
Joeatsumi 2:e6496a794bde 218 n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
Joeatsumi 2:e6496a794bde 219 n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
Joeatsumi 2:e6496a794bde 220 n[2]=0.0;
Joeatsumi 2:e6496a794bde 221
Joeatsumi 2:e6496a794bde 222 n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
Joeatsumi 2:e6496a794bde 223
Joeatsumi 2:e6496a794bde 224 }
Joeatsumi 2:e6496a794bde 225
Joeatsumi 2:e6496a794bde 226 double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
Joeatsumi 2:e6496a794bde 227
Joeatsumi 2:e6496a794bde 228 Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系
Joeatsumi 2:e6496a794bde 229 Matrix Rotation(3, 3); //検算の行列
Joeatsumi 2:e6496a794bde 230
Joeatsumi 2:e6496a794bde 231 double Correct_vector_n[4];//回転行列の成分
Joeatsumi 2:e6496a794bde 232 Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
Joeatsumi 2:e6496a794bde 233
Joeatsumi 2:e6496a794bde 234 double n_x=Correct_vector_n[0];
Joeatsumi 2:e6496a794bde 235 double n_y=Correct_vector_n[1];
Joeatsumi 2:e6496a794bde 236 double n_z=Correct_vector_n[2];
Joeatsumi 2:e6496a794bde 237 double theta=-Correct_vector_n[3];
Joeatsumi 2:e6496a794bde 238
Joeatsumi 2:e6496a794bde 239 float Rotate_element[9] = {
Joeatsumi 2:e6496a794bde 240 n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta),
Joeatsumi 2:e6496a794bde 241 n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta),
Joeatsumi 2:e6496a794bde 242 n_y*sin(theta) , n_x*sin(theta) , cos(theta)
Joeatsumi 2:e6496a794bde 243 };
Joeatsumi 2:e6496a794bde 244
Joeatsumi 2:e6496a794bde 245 Rotation.SetComps(Rotate_element);
Joeatsumi 2:e6496a794bde 246 //x y z
Joeatsumi 2:e6496a794bde 247 float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X)
Joeatsumi 2:e6496a794bde 248 , float(magnet[0]+MAG_FIX_Y)
Joeatsumi 2:e6496a794bde 249 , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
Joeatsumi 2:e6496a794bde 250
Joeatsumi 2:e6496a794bde 251 Mag_from_sensor.SetComps(Geomagnetism);
Joeatsumi 2:e6496a794bde 252
Joeatsumi 2:e6496a794bde 253 Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
Joeatsumi 2:e6496a794bde 254
Joeatsumi 2:e6496a794bde 255 float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
Joeatsumi 2:e6496a794bde 256 float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
Joeatsumi 2:e6496a794bde 257
Joeatsumi 2:e6496a794bde 258 double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
Joeatsumi 2:e6496a794bde 259
Joeatsumi 2:e6496a794bde 260 /*
Joeatsumi 2:e6496a794bde 261 if(Azi < 0.0) // fix sign
Joeatsumi 2:e6496a794bde 262 Azi += PI2;
Joeatsumi 2:e6496a794bde 263
Joeatsumi 2:e6496a794bde 264 if(Azi > PI2) // fix overflow
Joeatsumi 2:e6496a794bde 265 Azi -= PI2;
Joeatsumi 2:e6496a794bde 266 */
Joeatsumi 2:e6496a794bde 267
Joeatsumi 2:e6496a794bde 268 return Azi;
Joeatsumi 2:e6496a794bde 269
Joeatsumi 2:e6496a794bde 270 }
Joeatsumi 2:e6496a794bde 271
Joeatsumi 2:e6496a794bde 272 //void led2_thread(void const *argument) {
Joeatsumi 2:e6496a794bde 273 void imu_thread() {
Joeatsumi 2:e6496a794bde 274 while (true) {
Joeatsumi 2:e6496a794bde 275 }//while ends
Joeatsumi 2:e6496a794bde 276 }
Joeatsumi 2:e6496a794bde 277
hudakz 0:87642278ede6 278 // Entry point for the example
hudakz 0:87642278ede6 279 int main()
hudakz 0:87642278ede6 280 {
Joeatsumi 2:e6496a794bde 281 gps.baud(115200);//GT-720Fボーレート
Joeatsumi 2:e6496a794bde 282 pc.baud(115200);
Joeatsumi 2:e6496a794bde 283 //imu_thread
Joeatsumi 2:e6496a794bde 284
Joeatsumi 2:e6496a794bde 285 compass.init();//hmc5883の起動
Joeatsumi 2:e6496a794bde 286
Joeatsumi 2:e6496a794bde 287 Thread thread1 (gps_convertion);
Joeatsumi 2:e6496a794bde 288 gps.attach(gps_rx, Serial::RxIrq);
Joeatsumi 2:e6496a794bde 289
Joeatsumi 2:e6496a794bde 290 float Time_old=0.0;//時間初期化
Joeatsumi 2:e6496a794bde 291 passed_time.start();//タイマー開始
Joeatsumi 2:e6496a794bde 292
Joeatsumi 2:e6496a794bde 293 pc.printf("--- Mbed OS filesystem example ---\n");
hudakz 0:87642278ede6 294
hudakz 0:87642278ede6 295 // Try to mount the filesystem
Joeatsumi 2:e6496a794bde 296 pc.printf("Mounting the filesystem... ");
hudakz 0:87642278ede6 297 fflush(stdout);
hudakz 0:87642278ede6 298
hudakz 0:87642278ede6 299 int err = fileSystem.mount(&blockDevice);
Joeatsumi 2:e6496a794bde 300 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 301 if (err) {
hudakz 0:87642278ede6 302 // Reformat if we can't mount the filesystem
hudakz 0:87642278ede6 303 // this should only happen on the first boot
Joeatsumi 2:e6496a794bde 304 pc.printf("No filesystem found, formatting... ");
hudakz 0:87642278ede6 305 fflush(stdout);
hudakz 0:87642278ede6 306 err = fileSystem.reformat(&blockDevice);
Joeatsumi 2:e6496a794bde 307 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 308 if (err) {
hudakz 0:87642278ede6 309 error("error: %s (%d)\n", strerror(-err), err);
hudakz 0:87642278ede6 310 }
hudakz 0:87642278ede6 311 }
hudakz 0:87642278ede6 312
hudakz 0:87642278ede6 313 // Open the numbers file
Joeatsumi 2:e6496a794bde 314 pc.printf("Opening \"/fs/numbers.txt\"... ");
hudakz 0:87642278ede6 315 fflush(stdout);
hudakz 0:87642278ede6 316
hudakz 0:87642278ede6 317 FILE* f = fopen("/fs/numbers.txt", "r+");
Joeatsumi 2:e6496a794bde 318 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 2:e6496a794bde 319
hudakz 0:87642278ede6 320 if (!f) {
hudakz 0:87642278ede6 321 // Create the numbers file if it doesn't exist
Joeatsumi 2:e6496a794bde 322 pc.printf("No file found, creating a new file... ");
hudakz 0:87642278ede6 323 fflush(stdout);
hudakz 0:87642278ede6 324 f = fopen("/fs/numbers.txt", "w+");
Joeatsumi 2:e6496a794bde 325 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 326 }
Joeatsumi 2:e6496a794bde 327
Joeatsumi 2:e6496a794bde 328 /*初期姿勢のQuaternionの設定*/
Joeatsumi 2:e6496a794bde 329 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
Joeatsumi 2:e6496a794bde 330 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
Joeatsumi 2:e6496a794bde 331
Joeatsumi 2:e6496a794bde 332 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
Joeatsumi 2:e6496a794bde 333
Joeatsumi 2:e6496a794bde 334 float Initialize_atitude[4];
Joeatsumi 2:e6496a794bde 335 Initialize_atitude[0]=Quaternion_from_acc[0];
Joeatsumi 2:e6496a794bde 336 Initialize_atitude[1]=Quaternion_from_acc[1];
Joeatsumi 2:e6496a794bde 337 Initialize_atitude[2]=Quaternion_from_acc[2];
Joeatsumi 2:e6496a794bde 338 Initialize_atitude[3]=Quaternion_from_acc[3];
Joeatsumi 2:e6496a794bde 339
Joeatsumi 2:e6496a794bde 340 Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
Joeatsumi 2:e6496a794bde 341
Joeatsumi 2:e6496a794bde 342
Joeatsumi 2:e6496a794bde 343 while(1){
Joeatsumi 2:e6496a794bde 344
Joeatsumi 2:e6496a794bde 345 /*gpsから来た文字列の処理
Joeatsumi 2:e6496a794bde 346 if((gps_flag==1)&&(gps_flag_conversion==1)){
Joeatsumi 2:e6496a794bde 347
Joeatsumi 2:e6496a794bde 348 err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
Joeatsumi 2:e6496a794bde 349
Joeatsumi 2:e6496a794bde 350 gps_flag=0;
Joeatsumi 2:e6496a794bde 351 gps_flag_conversion=0;
Joeatsumi 2:e6496a794bde 352
Joeatsumi 2:e6496a794bde 353 }else{}
Joeatsumi 2:e6496a794bde 354 */
Joeatsumi 2:e6496a794bde 355
Joeatsumi 2:e6496a794bde 356 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
Joeatsumi 2:e6496a794bde 357 float Time_new=float(passed_time.read());//時間の取得
Joeatsumi 2:e6496a794bde 358
Joeatsumi 2:e6496a794bde 359 //地磁気の補正用に使う。
Joeatsumi 2:e6496a794bde 360 //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
Joeatsumi 2:e6496a794bde 361
Joeatsumi 2:e6496a794bde 362 //ジャイロの補正用に使う
Joeatsumi 2:e6496a794bde 363 //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
Joeatsumi 2:e6496a794bde 364
Joeatsumi 2:e6496a794bde 365 /*----------------ジャイロセンサからQuaternionを求める----------------*/
Joeatsumi 2:e6496a794bde 366 float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7);
Joeatsumi 2:e6496a794bde 367 float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7);
Joeatsumi 2:e6496a794bde 368 float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7);
Joeatsumi 2:e6496a794bde 369
Joeatsumi 2:e6496a794bde 370 float omega[16] = {
Joeatsumi 2:e6496a794bde 371
Joeatsumi 2:e6496a794bde 372 0.0 , -omega_x , -omega_y, -omega_z,
Joeatsumi 2:e6496a794bde 373 omega_x ,0.0 ,omega_z ,-omega_y ,
Joeatsumi 2:e6496a794bde 374 omega_y , -omega_z , 0.0 , omega_x ,
Joeatsumi 2:e6496a794bde 375 omega_z , omega_y ,-omega_x , 0.0
Joeatsumi 2:e6496a794bde 376
Joeatsumi 2:e6496a794bde 377 };
hudakz 0:87642278ede6 378
Joeatsumi 2:e6496a794bde 379 Omega_matrix.SetComps(omega);
Joeatsumi 2:e6496a794bde 380
Joeatsumi 2:e6496a794bde 381 float half[16] ={
Joeatsumi 2:e6496a794bde 382 (Time_new-Time_old)*0.5,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 383 0.0,(Time_new-Time_old)*0.5,0.0,0.0,
Joeatsumi 2:e6496a794bde 384 0.0,0.0,(Time_new-Time_old)*0.5,0.0,
Joeatsumi 2:e6496a794bde 385 0.0,0.0,0.0,(Time_new-Time_old)*0.5
Joeatsumi 2:e6496a794bde 386 };
Joeatsumi 2:e6496a794bde 387
Joeatsumi 2:e6496a794bde 388
Joeatsumi 2:e6496a794bde 389 Half_matrix.SetComps(half);
Joeatsumi 2:e6496a794bde 390 //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
Joeatsumi 2:e6496a794bde 391 Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
Joeatsumi 2:e6496a794bde 392
Joeatsumi 2:e6496a794bde 393 Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
Joeatsumi 2:e6496a794bde 394
Joeatsumi 2:e6496a794bde 395
Joeatsumi 2:e6496a794bde 396 Time_old=Time_new;//時間の更新
Joeatsumi 2:e6496a794bde 397
Joeatsumi 2:e6496a794bde 398 /*
Joeatsumi 2:e6496a794bde 399 pc.printf("%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 400 ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 401 ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 402 */
Joeatsumi 2:e6496a794bde 403
Joeatsumi 2:e6496a794bde 404 /*----------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 405
Joeatsumi 2:e6496a794bde 406 /*
Joeatsumi 2:e6496a794bde 407 err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
Joeatsumi 2:e6496a794bde 408 ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
Joeatsumi 2:e6496a794bde 409 ,sensor_array[6],sensor_array[7],sensor_array[8]);
Joeatsumi 2:e6496a794bde 410 */
Joeatsumi 2:e6496a794bde 411
Joeatsumi 2:e6496a794bde 412 /*--------------------加速センサで地磁気の値を補正する --------------------*/
Joeatsumi 2:e6496a794bde 413
Joeatsumi 2:e6496a794bde 414 if(9.8065*1.1 >
Joeatsumi 2:e6496a794bde 415 sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
Joeatsumi 2:e6496a794bde 416 / 16384.0 * 9.81)
Joeatsumi 2:e6496a794bde 417 {
Joeatsumi 2:e6496a794bde 418
Joeatsumi 2:e6496a794bde 419 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
Joeatsumi 2:e6496a794bde 420
Joeatsumi 2:e6496a794bde 421 pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);
Joeatsumi 2:e6496a794bde 422
Joeatsumi 2:e6496a794bde 423 double pitch_and_roll[2];
Joeatsumi 2:e6496a794bde 424 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
Joeatsumi 2:e6496a794bde 425
Joeatsumi 2:e6496a794bde 426 //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG);
Joeatsumi 2:e6496a794bde 427
Joeatsumi 2:e6496a794bde 428 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
Joeatsumi 2:e6496a794bde 429
Joeatsumi 2:e6496a794bde 430 /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
Joeatsumi 2:e6496a794bde 431 float Quaternion_from_acc_fixed[4];
Joeatsumi 2:e6496a794bde 432 Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
Joeatsumi 2:e6496a794bde 433 Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
Joeatsumi 2:e6496a794bde 434 Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
Joeatsumi 2:e6496a794bde 435 Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
Joeatsumi 2:e6496a794bde 436
Joeatsumi 2:e6496a794bde 437 Vector Qua_acc(4);
Joeatsumi 2:e6496a794bde 438
Joeatsumi 2:e6496a794bde 439 Qua_acc.SetComps(Quaternion_from_acc_fixed);
Joeatsumi 2:e6496a794bde 440 /*---------------------------------------------*/
Joeatsumi 2:e6496a794bde 441
Joeatsumi 2:e6496a794bde 442 /*---------------------相補フィルタのゲインに用いる---------------------*/
Joeatsumi 2:e6496a794bde 443 float comp_1[16]={0.95,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 444 0.0,0.95,0.0,0.0,
Joeatsumi 2:e6496a794bde 445 0.0,0.0,0.95,0.0,
Joeatsumi 2:e6496a794bde 446 0.0,0.0,0.0,0.95
Joeatsumi 2:e6496a794bde 447 };
Joeatsumi 2:e6496a794bde 448 /*--------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 449 /*---------------------quaternionの時間微分に用いる---------------------*/
Joeatsumi 2:e6496a794bde 450 float comp_2[16]={0.05,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 451 0.0,0.05,0.0,0.0,
Joeatsumi 2:e6496a794bde 452 0.0,0.0,0.05,0.0,
Joeatsumi 2:e6496a794bde 453 0.0,0.0,0.0,0.05
Joeatsumi 2:e6496a794bde 454 };
Joeatsumi 2:e6496a794bde 455
Joeatsumi 2:e6496a794bde 456 Complement_matrix_1.SetComps(comp_1);
Joeatsumi 2:e6496a794bde 457 Complement_matrix_2.SetComps(comp_2);
Joeatsumi 2:e6496a794bde 458
Joeatsumi 2:e6496a794bde 459 /*--------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 460
Joeatsumi 2:e6496a794bde 461
Joeatsumi 2:e6496a794bde 462 /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
Joeatsumi 2:e6496a794bde 463 /*
Joeatsumi 2:e6496a794bde 464 Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
Joeatsumi 2:e6496a794bde 465 +Complement_matrix_2*Qua_acc;
Joeatsumi 2:e6496a794bde 466 */
Joeatsumi 2:e6496a794bde 467 /*----------------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 468
Joeatsumi 2:e6496a794bde 469
Joeatsumi 2:e6496a794bde 470 /*
Joeatsumi 2:e6496a794bde 471 pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 472 ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
Joeatsumi 2:e6496a794bde 473 ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 474 ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
Joeatsumi 2:e6496a794bde 475 ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 476 */
Joeatsumi 2:e6496a794bde 477
Joeatsumi 2:e6496a794bde 478 }else{}
Joeatsumi 2:e6496a794bde 479
Joeatsumi 2:e6496a794bde 480 /*
Joeatsumi 2:e6496a794bde 481 pc.printf("%f,%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 482 ,Time_new
Joeatsumi 2:e6496a794bde 483 ,Quaternion_atitude_from_omega.GetComp(1)
Joeatsumi 2:e6496a794bde 484 ,Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 485 ,Quaternion_atitude_from_omega.GetComp(3)
Joeatsumi 2:e6496a794bde 486 ,Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 487 */
Joeatsumi 2:e6496a794bde 488 wait(0.001);
Joeatsumi 2:e6496a794bde 489
Joeatsumi 2:e6496a794bde 490 /*----------------------------------------------------------------*/
hudakz 0:87642278ede6 491
Joeatsumi 2:e6496a794bde 492 if(switch_off==1){
Joeatsumi 2:e6496a794bde 493 // Close the file which also flushes any cached writes
Joeatsumi 2:e6496a794bde 494 pc.printf("Closing \"/fs/numbers.txt\"... ");
Joeatsumi 2:e6496a794bde 495 err = fclose(f);
Joeatsumi 2:e6496a794bde 496 break;
Joeatsumi 2:e6496a794bde 497 }
Joeatsumi 2:e6496a794bde 498
Joeatsumi 2:e6496a794bde 499 }//while(1) ends
Joeatsumi 2:e6496a794bde 500 }//main ends