Atsumi Toda / Mbed OS UAV_Logger4

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

Dependencies:   MPU6050_alter HMC5883L

main.cpp@3:3fa7882a5fd0, 2019-07-24 (annotated)

Committer:
Joeatsumi
Date:
Wed Jul 24 12:00:01 2019 +0000
Revision:
3:3fa7882a5fd0
Parent:
2:e6496a794bde 
AlteredUAV_Logger1;

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 3:3fa7882a5fd0 45 //declare PWM pins here
Joeatsumi 3:3fa7882a5fd0 46 PwmOut ESC (p21);
Joeatsumi 3:3fa7882a5fd0 47 PwmOut Elevator_servo(p22);
Joeatsumi 3:3fa7882a5fd0 48 PwmOut Rudder_servo(p23);
Joeatsumi 3:3fa7882a5fd0 49
Joeatsumi 3:3fa7882a5fd0 50
Joeatsumi 2:e6496a794bde 51 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
Joeatsumi 2:e6496a794bde 52 MPU6050 mpu(p28, p27);//sda scl
Joeatsumi 2:e6496a794bde 53 HMC5883L compass(p28, p27);//sda scl
Joeatsumi 2:e6496a794bde 54
Joeatsumi 2:e6496a794bde 55 RawSerial gps(p9,p10); //serial port for gps_module
Joeatsumi 2:e6496a794bde 56 RawSerial pc(USBTX, USBRX);
Joeatsumi 2:e6496a794bde 57
Joeatsumi 2:e6496a794bde 58 /*--------------------------*/
Joeatsumi 2:e6496a794bde 59
Joeatsumi 2:e6496a794bde 60 DigitalIn switch_off(p30);
Joeatsumi 2:e6496a794bde 61 DigitalOut led2(LED2);
Joeatsumi 2:e6496a794bde 62
Joeatsumi 2:e6496a794bde 63 /*-----------グローバル変数-----------*/
Joeatsumi 3:3fa7882a5fd0 64 double Euler_angle[3];
Joeatsumi 3:3fa7882a5fd0 65 double Euler_angle_Initiated[3];
Joeatsumi 3:3fa7882a5fd0 66
Joeatsumi 2:e6496a794bde 67 float g_hokui,g_tokei;
Joeatsumi 2:e6496a794bde 68 int fp_count=0;
Joeatsumi 2:e6496a794bde 69 int gps_flag=0;
Joeatsumi 2:e6496a794bde 70 int gps_flag_conversion=0;
Joeatsumi 2:e6496a794bde 71
Joeatsumi 2:e6496a794bde 72 char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している
Joeatsumi 2:e6496a794bde 73 char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している
Joeatsumi 2:e6496a794bde 74
Joeatsumi 2:e6496a794bde 75 unsigned int tokei_int_receive;
Joeatsumi 2:e6496a794bde 76 unsigned int hokui_int_receive;
Joeatsumi 2:e6496a794bde 77
Joeatsumi 2:e6496a794bde 78 float tokei_float_receive;
Joeatsumi 2:e6496a794bde 79 float hokui_float_receive;
Joeatsumi 2:e6496a794bde 80
Joeatsumi 2:e6496a794bde 81 float a[3];//加速度の値
Joeatsumi 2:e6496a794bde 82 float g[3];//ジャイロの値[rad/s]
Joeatsumi 2:e6496a794bde 83 float dpsX,dpsY,dpsZ;
Joeatsumi 2:e6496a794bde 84 float AccX,AccY,AccZ;
Joeatsumi 2:e6496a794bde 85
Joeatsumi 2:e6496a794bde 86 int sensor_array[6];
Joeatsumi 2:e6496a794bde 87 int16_t mag[3];
Joeatsumi 2:e6496a794bde 88
Joeatsumi 2:e6496a794bde 89 double Roll_Acc,Pitch_Acc;
Joeatsumi 2:e6496a794bde 90 double Yaw;
Joeatsumi 2:e6496a794bde 91 double Azi_mag;//地磁気からの方位
Joeatsumi 2:e6496a794bde 92 double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion
Joeatsumi 2:e6496a794bde 93
Joeatsumi 2:e6496a794bde 94 /*----------------------------------*/
Joeatsumi 2:e6496a794bde 95 /*--------------------------行列、ベクトル-----------------------------*/
Joeatsumi 2:e6496a794bde 96
Joeatsumi 2:e6496a794bde 97 Vector Quaternion_atitude_from_omega(4);
Joeatsumi 2:e6496a794bde 98 Matrix Omega_matrix(4,4),Half_matrix(4,4);
Joeatsumi 2:e6496a794bde 99 Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4);
Joeatsumi 2:e6496a794bde 100
Joeatsumi 2:e6496a794bde 101 /*-------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 102
Joeatsumi 3:3fa7882a5fd0 103 /*----------PID コントロール-----------------*/
Joeatsumi 3:3fa7882a5fd0 104 #define Gain_Kp 0.8377
Joeatsumi 3:3fa7882a5fd0 105 #define Gain_Ki 0.4450
Joeatsumi 3:3fa7882a5fd0 106 #define Gain_Kd 0.3890
Joeatsumi 3:3fa7882a5fd0 107
Joeatsumi 3:3fa7882a5fd0 108 double Prop_p,Int_p,Dif_p;
Joeatsumi 3:3fa7882a5fd0 109 double Pre_Prop_p;
Joeatsumi 3:3fa7882a5fd0 110 /*-----------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 111
Joeatsumi 3:3fa7882a5fd0 112 /*-----------サーボコントロール----------------*/
Joeatsumi 3:3fa7882a5fd0 113 //define period of servo here
Joeatsumi 3:3fa7882a5fd0 114 #define SERVO_PERIOD 0.020 // servo requires a 20ms period
Joeatsumi 3:3fa7882a5fd0 115 #define NUTRAL 0.0015
Joeatsumi 3:3fa7882a5fd0 116 #define UPPER_DUTY 0.0020
Joeatsumi 3:3fa7882a5fd0 117 #define LOWER_DUTY 0.0010
Joeatsumi 3:3fa7882a5fd0 118
Joeatsumi 3:3fa7882a5fd0 119 double pitch_duty,roll_duty,yaw_duty;
Joeatsumi 3:3fa7882a5fd0 120
Joeatsumi 3:3fa7882a5fd0 121 /*------------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 122
Joeatsumi 3:3fa7882a5fd0 123
Joeatsumi 2:e6496a794bde 124 /*プロトタイプ宣言*/
Joeatsumi 2:e6496a794bde 125 void toString_V(Vector& v); // ベクトルデバッグ用
Joeatsumi 2:e6496a794bde 126 void gps_rx();
Joeatsumi 2:e6496a794bde 127 void gps_convertion();
Joeatsumi 2:e6496a794bde 128 /*--------------*/
Joeatsumi 2:e6496a794bde 129
Joeatsumi 2:e6496a794bde 130 void toString_V(Vector& v)
Joeatsumi 2:e6496a794bde 131 {
Joeatsumi 2:e6496a794bde 132
Joeatsumi 2:e6496a794bde 133 for(int i=0; i<v.GetDim(); i++) {
Joeatsumi 2:e6496a794bde 134 pc.printf("%.6f\t", v.GetComp(i+1));
Joeatsumi 2:e6496a794bde 135 }
Joeatsumi 2:e6496a794bde 136 pc.printf("\r\n");
Joeatsumi 2:e6496a794bde 137
Joeatsumi 2:e6496a794bde 138 }
Joeatsumi 2:e6496a794bde 139
Joeatsumi 2:e6496a794bde 140 void scan_update(int box_sensor[6],int16_t m[3]){
Joeatsumi 2:e6496a794bde 141
Joeatsumi 2:e6496a794bde 142 int a[3];//加速度の値
Joeatsumi 2:e6496a794bde 143 int g[3];//角速度の値
Joeatsumi 2:e6496a794bde 144
Joeatsumi 2:e6496a794bde 145 int16_t raw_compass[3];//地磁気センサの値
Joeatsumi 2:e6496a794bde 146
Joeatsumi 2:e6496a794bde 147 mpu.setAcceleroRange(0);//acceleration range is +-4G
Joeatsumi 2:e6496a794bde 148 mpu.setGyroRange(0);//gyro rate is +-degree per second(dps)
Joeatsumi 2:e6496a794bde 149
Joeatsumi 2:e6496a794bde 150 mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis;
Joeatsumi 2:e6496a794bde 151 mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s]
Joeatsumi 2:e6496a794bde 152 compass.getXYZ(raw_compass);//地磁気の値を格納する配列
Joeatsumi 2:e6496a794bde 153
Joeatsumi 2:e6496a794bde 154 box_sensor[0]=-g[0];
Joeatsumi 2:e6496a794bde 155 box_sensor[1]=g[1];
Joeatsumi 2:e6496a794bde 156 box_sensor[2]=-g[2];
Joeatsumi 2:e6496a794bde 157
Joeatsumi 2:e6496a794bde 158 box_sensor[3]=a[0];
Joeatsumi 2:e6496a794bde 159 box_sensor[4]=-a[1];
Joeatsumi 2:e6496a794bde 160 box_sensor[5]=a[2];
Joeatsumi 2:e6496a794bde 161
Joeatsumi 2:e6496a794bde 162 m[0]=(raw_compass[0]);//x
Joeatsumi 2:e6496a794bde 163 m[1]=(raw_compass[2]);//y
Joeatsumi 2:e6496a794bde 164 m[2]=(raw_compass[1]);//z
Joeatsumi 2:e6496a794bde 165
Joeatsumi 2:e6496a794bde 166 }
Joeatsumi 2:e6496a794bde 167
Joeatsumi 2:e6496a794bde 168 void gps_rx(){
Joeatsumi 2:e6496a794bde 169
Joeatsumi 2:e6496a794bde 170 if((gps.getc()=='$')&&(gps_flag==0)){
Joeatsumi 2:e6496a794bde 171 for(int i=0;i<9;i++){
Joeatsumi 2:e6496a794bde 172 gps_data[i]=gps.getc();
Joeatsumi 2:e6496a794bde 173 //pc.putc(gps_data[i]);
Joeatsumi 2:e6496a794bde 174 }
Joeatsumi 2:e6496a794bde 175
Joeatsumi 2:e6496a794bde 176 gps_data[9]='\0';
Joeatsumi 2:e6496a794bde 177
Joeatsumi 2:e6496a794bde 178 for(int i=0;i<9;i++){
Joeatsumi 2:e6496a794bde 179 gps_data_2[i]=gps.getc();
Joeatsumi 2:e6496a794bde 180 //pc.putc(gps_data[i]);
Joeatsumi 2:e6496a794bde 181 }
Joeatsumi 2:e6496a794bde 182
Joeatsumi 2:e6496a794bde 183 gps_data_2[9]='\0';
Joeatsumi 2:e6496a794bde 184
Joeatsumi 2:e6496a794bde 185 }//if(twe.getc()==':')
Joeatsumi 2:e6496a794bde 186
Joeatsumi 2:e6496a794bde 187 //pc.printf("%s,%s\r\n",gps_data,gps_data_2);
Joeatsumi 2:e6496a794bde 188
Joeatsumi 2:e6496a794bde 189 gps_flag=1;
Joeatsumi 2:e6496a794bde 190
Joeatsumi 2:e6496a794bde 191 }//gps_rx ends
Joeatsumi 2:e6496a794bde 192
Joeatsumi 2:e6496a794bde 193 void gps_convertion(){
Joeatsumi 2:e6496a794bde 194 while(1){
Joeatsumi 2:e6496a794bde 195 if(gps_flag==1){
Joeatsumi 2:e6496a794bde 196 tokei_int_receive=atoi(gps_data);
Joeatsumi 2:e6496a794bde 197 hokui_int_receive=atoi(gps_data_2);
Joeatsumi 2:e6496a794bde 198
Joeatsumi 2:e6496a794bde 199 //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive);
Joeatsumi 2:e6496a794bde 200
Joeatsumi 2:e6496a794bde 201 tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0;
Joeatsumi 2:e6496a794bde 202 hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0;
Joeatsumi 2:e6496a794bde 203
Joeatsumi 2:e6496a794bde 204 gps_flag_conversion=1;
Joeatsumi 2:e6496a794bde 205
Joeatsumi 2:e6496a794bde 206 }else{gps_flag_conversion=0;}
Joeatsumi 2:e6496a794bde 207 }//while ends
Joeatsumi 2:e6496a794bde 208
Joeatsumi 2:e6496a794bde 209 }//ends
Joeatsumi 2:e6496a794bde 210
Joeatsumi 2:e6496a794bde 211 void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){
Joeatsumi 2:e6496a794bde 212
Joeatsumi 2:e6496a794bde 213 double Roll_before = double(y_acc)/double(z_acc);
Joeatsumi 2:e6496a794bde 214 double Roll = atan(Roll_before);
Joeatsumi 2:e6496a794bde 215
Joeatsumi 2:e6496a794bde 216 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
Joeatsumi 2:e6496a794bde 217 double Pitch = -atan(Pitch_before);
Joeatsumi 2:e6496a794bde 218
Joeatsumi 2:e6496a794bde 219 //Yaw=0.0;
Joeatsumi 2:e6496a794bde 220
Joeatsumi 2:e6496a794bde 221 /*Quaternionの要素へオイラー角を変換する*/
Joeatsumi 2:e6496a794bde 222 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 223 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 224 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 225 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 226
Joeatsumi 2:e6496a794bde 227
Joeatsumi 2:e6496a794bde 228 }
Joeatsumi 2:e6496a794bde 229
Joeatsumi 2:e6496a794bde 230 void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){
Joeatsumi 2:e6496a794bde 231
Joeatsumi 2:e6496a794bde 232 double Roll_before = double(y_acc)/double(z_acc);
Joeatsumi 2:e6496a794bde 233 double Roll = atan(Roll_before);
Joeatsumi 2:e6496a794bde 234
Joeatsumi 2:e6496a794bde 235 double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) );
Joeatsumi 2:e6496a794bde 236 double Pitch = -atan(Pitch_before);
Joeatsumi 2:e6496a794bde 237
Joeatsumi 2:e6496a794bde 238
Joeatsumi 2:e6496a794bde 239 qua[0]=Pitch;
Joeatsumi 2:e6496a794bde 240 qua[1]=Roll;
Joeatsumi 2:e6496a794bde 241
Joeatsumi 2:e6496a794bde 242 }
Joeatsumi 2:e6496a794bde 243
Joeatsumi 2:e6496a794bde 244 void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){
Joeatsumi 2:e6496a794bde 245
Joeatsumi 2:e6496a794bde 246 //y_acc*=-1.0;
Joeatsumi 2:e6496a794bde 247
Joeatsumi 2:e6496a794bde 248 n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
Joeatsumi 2:e6496a794bde 249 n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc));
Joeatsumi 2:e6496a794bde 250 n[2]=0.0;
Joeatsumi 2:e6496a794bde 251
Joeatsumi 2:e6496a794bde 252 n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc);
Joeatsumi 2:e6496a794bde 253
Joeatsumi 2:e6496a794bde 254 }
Joeatsumi 2:e6496a794bde 255
Joeatsumi 2:e6496a794bde 256 double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){
Joeatsumi 2:e6496a794bde 257
Joeatsumi 2:e6496a794bde 258 Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系
Joeatsumi 2:e6496a794bde 259 Matrix Rotation(3, 3); //検算の行列
Joeatsumi 2:e6496a794bde 260
Joeatsumi 2:e6496a794bde 261 double Correct_vector_n[4];//回転行列の成分
Joeatsumi 2:e6496a794bde 262 Correct_n(x_acc,y_acc,z_acc, Correct_vector_n);
Joeatsumi 2:e6496a794bde 263
Joeatsumi 2:e6496a794bde 264 double n_x=Correct_vector_n[0];
Joeatsumi 2:e6496a794bde 265 double n_y=Correct_vector_n[1];
Joeatsumi 2:e6496a794bde 266 double n_z=Correct_vector_n[2];
Joeatsumi 2:e6496a794bde 267 double theta=-Correct_vector_n[3];
Joeatsumi 2:e6496a794bde 268
Joeatsumi 2:e6496a794bde 269 float Rotate_element[9] = {
Joeatsumi 2:e6496a794bde 270 n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta),
Joeatsumi 2:e6496a794bde 271 n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta),
Joeatsumi 2:e6496a794bde 272 n_y*sin(theta) , n_x*sin(theta) , cos(theta)
Joeatsumi 2:e6496a794bde 273 };
Joeatsumi 2:e6496a794bde 274
Joeatsumi 2:e6496a794bde 275 Rotation.SetComps(Rotate_element);
Joeatsumi 2:e6496a794bde 276 //x y z
Joeatsumi 2:e6496a794bde 277 float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X)
Joeatsumi 2:e6496a794bde 278 , float(magnet[0]+MAG_FIX_Y)
Joeatsumi 2:e6496a794bde 279 , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))};
Joeatsumi 2:e6496a794bde 280
Joeatsumi 2:e6496a794bde 281 Mag_from_sensor.SetComps(Geomagnetism);
Joeatsumi 2:e6496a794bde 282
Joeatsumi 2:e6496a794bde 283 Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。
Joeatsumi 2:e6496a794bde 284
Joeatsumi 2:e6496a794bde 285 float Mag_fixed_x= Mag_fixed_sensor.GetComp(1);
Joeatsumi 2:e6496a794bde 286 float Mag_fixed_y= Mag_fixed_sensor.GetComp(2);
Joeatsumi 2:e6496a794bde 287
Joeatsumi 2:e6496a794bde 288 double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x));
Joeatsumi 2:e6496a794bde 289
Joeatsumi 2:e6496a794bde 290 /*
Joeatsumi 2:e6496a794bde 291 if(Azi < 0.0) // fix sign
Joeatsumi 2:e6496a794bde 292 Azi += PI2;
Joeatsumi 2:e6496a794bde 293
Joeatsumi 2:e6496a794bde 294 if(Azi > PI2) // fix overflow
Joeatsumi 2:e6496a794bde 295 Azi -= PI2;
Joeatsumi 2:e6496a794bde 296 */
Joeatsumi 2:e6496a794bde 297
Joeatsumi 2:e6496a794bde 298 return Azi;
Joeatsumi 2:e6496a794bde 299
Joeatsumi 2:e6496a794bde 300 }
Joeatsumi 2:e6496a794bde 301
Joeatsumi 2:e6496a794bde 302 //void led2_thread(void const *argument) {
Joeatsumi 2:e6496a794bde 303 void imu_thread() {
Joeatsumi 2:e6496a794bde 304 while (true) {
Joeatsumi 2:e6496a794bde 305 }//while ends
Joeatsumi 2:e6496a794bde 306 }
Joeatsumi 2:e6496a794bde 307
Joeatsumi 3:3fa7882a5fd0 308 void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){
Joeatsumi 3:3fa7882a5fd0 309
Joeatsumi 3:3fa7882a5fd0 310 //float pitch,roll,yaw;
Joeatsumi 3:3fa7882a5fd0 311
Joeatsumi 3:3fa7882a5fd0 312 Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) );
Joeatsumi 3:3fa7882a5fd0 313 Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2)));
Joeatsumi 3:3fa7882a5fd0 314 Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3));
Joeatsumi 3:3fa7882a5fd0 315
Joeatsumi 3:3fa7882a5fd0 316 }
Joeatsumi 3:3fa7882a5fd0 317
Joeatsumi 3:3fa7882a5fd0 318
Joeatsumi 3:3fa7882a5fd0 319 double Degree_to_PWM_duty(double degree){
Joeatsumi 3:3fa7882a5fd0 320
Joeatsumi 3:3fa7882a5fd0 321 double duty=0.0;
Joeatsumi 3:3fa7882a5fd0 322
Joeatsumi 3:3fa7882a5fd0 323 duty=(0.2/1000)/20.0*degree;
Joeatsumi 3:3fa7882a5fd0 324
Joeatsumi 3:3fa7882a5fd0 325 if((duty+NUTRAL)>=UPPER_DUTY){
Joeatsumi 3:3fa7882a5fd0 326 duty=UPPER_DUTY-NUTRAL;
Joeatsumi 3:3fa7882a5fd0 327 }else if((duty+NUTRAL)<=LOWER_DUTY){
Joeatsumi 3:3fa7882a5fd0 328 duty=NUTRAL-LOWER_DUTY;
Joeatsumi 3:3fa7882a5fd0 329 }else{}
Joeatsumi 3:3fa7882a5fd0 330
Joeatsumi 3:3fa7882a5fd0 331 return duty;
Joeatsumi 3:3fa7882a5fd0 332
Joeatsumi 3:3fa7882a5fd0 333 }
Joeatsumi 3:3fa7882a5fd0 334
Joeatsumi 3:3fa7882a5fd0 335 double PID_duty(double target,double vol,float dt){
Joeatsumi 3:3fa7882a5fd0 336 //ここで角度の単位で偏差は入力される
Joeatsumi 3:3fa7882a5fd0 337 //出力はPWMで
Joeatsumi 3:3fa7882a5fd0 338 double duty=0.0;
Joeatsumi 3:3fa7882a5fd0 339 double add_duty=0.0;
Joeatsumi 3:3fa7882a5fd0 340
Joeatsumi 3:3fa7882a5fd0 341 Prop_p=target-vol;
Joeatsumi 3:3fa7882a5fd0 342
Joeatsumi 3:3fa7882a5fd0 343 pc.printf("%f/r/n",Prop_p);
Joeatsumi 3:3fa7882a5fd0 344
Joeatsumi 3:3fa7882a5fd0 345 Int_p+=Prop_p*double(dt);
Joeatsumi 3:3fa7882a5fd0 346 Dif_p=(Prop_p - Pre_Prop_p) / double(dt);
Joeatsumi 3:3fa7882a5fd0 347
Joeatsumi 3:3fa7882a5fd0 348 Pre_Prop_p=Prop_p;
Joeatsumi 3:3fa7882a5fd0 349
Joeatsumi 3:3fa7882a5fd0 350 //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p;
Joeatsumi 3:3fa7882a5fd0 351
Joeatsumi 3:3fa7882a5fd0 352 add_duty=1.0*Prop_p;
Joeatsumi 3:3fa7882a5fd0 353
Joeatsumi 3:3fa7882a5fd0 354 duty+=add_duty;
Joeatsumi 3:3fa7882a5fd0 355 duty=Degree_to_PWM_duty(duty);
Joeatsumi 3:3fa7882a5fd0 356
Joeatsumi 3:3fa7882a5fd0 357 return duty;
Joeatsumi 3:3fa7882a5fd0 358
Joeatsumi 3:3fa7882a5fd0 359 }
Joeatsumi 3:3fa7882a5fd0 360
Joeatsumi 3:3fa7882a5fd0 361 void Initialize_ESC(){
Joeatsumi 3:3fa7882a5fd0 362
Joeatsumi 3:3fa7882a5fd0 363 ESC.pulsewidth(0.002);
Joeatsumi 3:3fa7882a5fd0 364 wait(3);
Joeatsumi 3:3fa7882a5fd0 365
Joeatsumi 3:3fa7882a5fd0 366 ESC.pulsewidth(0.001);
Joeatsumi 3:3fa7882a5fd0 367 wait(5);
Joeatsumi 3:3fa7882a5fd0 368
Joeatsumi 3:3fa7882a5fd0 369 }
Joeatsumi 3:3fa7882a5fd0 370
Joeatsumi 3:3fa7882a5fd0 371
Joeatsumi 3:3fa7882a5fd0 372 void Activate_ESC(){
Joeatsumi 3:3fa7882a5fd0 373
Joeatsumi 3:3fa7882a5fd0 374 ESC.pulsewidth(0.001);
Joeatsumi 3:3fa7882a5fd0 375 wait(5);
Joeatsumi 3:3fa7882a5fd0 376
Joeatsumi 3:3fa7882a5fd0 377 }
Joeatsumi 3:3fa7882a5fd0 378
hudakz 0:87642278ede6 379 // Entry point for the example
hudakz 0:87642278ede6 380 int main()
hudakz 0:87642278ede6 381 {
Joeatsumi 2:e6496a794bde 382 gps.baud(115200);//GT-720Fボーレート
Joeatsumi 2:e6496a794bde 383 pc.baud(115200);
Joeatsumi 2:e6496a794bde 384 //imu_thread
Joeatsumi 2:e6496a794bde 385
Joeatsumi 2:e6496a794bde 386 compass.init();//hmc5883の起動
Joeatsumi 2:e6496a794bde 387
Joeatsumi 2:e6496a794bde 388 Thread thread1 (gps_convertion);
Joeatsumi 2:e6496a794bde 389 gps.attach(gps_rx, Serial::RxIrq);
Joeatsumi 2:e6496a794bde 390
Joeatsumi 3:3fa7882a5fd0 391 /*define period of serve and ESC*/
Joeatsumi 3:3fa7882a5fd0 392 ESC.period(SERVO_PERIOD);
Joeatsumi 3:3fa7882a5fd0 393 Elevator_servo.period(SERVO_PERIOD);
Joeatsumi 3:3fa7882a5fd0 394 Rudder_servo.period(SERVO_PERIOD);
Joeatsumi 3:3fa7882a5fd0 395 /*------------------------------*/
Joeatsumi 3:3fa7882a5fd0 396 Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms
Joeatsumi 3:3fa7882a5fd0 397 Rudder_servo.pulsewidth(NUTRAL);
Joeatsumi 3:3fa7882a5fd0 398
Joeatsumi 3:3fa7882a5fd0 399 Activate_ESC();
Joeatsumi 3:3fa7882a5fd0 400
Joeatsumi 2:e6496a794bde 401 float Time_old=0.0;//時間初期化
Joeatsumi 2:e6496a794bde 402 passed_time.start();//タイマー開始
Joeatsumi 2:e6496a794bde 403
Joeatsumi 2:e6496a794bde 404 pc.printf("--- Mbed OS filesystem example ---\n");
hudakz 0:87642278ede6 405
hudakz 0:87642278ede6 406 // Try to mount the filesystem
Joeatsumi 2:e6496a794bde 407 pc.printf("Mounting the filesystem... ");
hudakz 0:87642278ede6 408 fflush(stdout);
hudakz 0:87642278ede6 409
hudakz 0:87642278ede6 410 int err = fileSystem.mount(&blockDevice);
Joeatsumi 2:e6496a794bde 411 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 412 if (err) {
hudakz 0:87642278ede6 413 // Reformat if we can't mount the filesystem
hudakz 0:87642278ede6 414 // this should only happen on the first boot
Joeatsumi 2:e6496a794bde 415 pc.printf("No filesystem found, formatting... ");
hudakz 0:87642278ede6 416 fflush(stdout);
hudakz 0:87642278ede6 417 err = fileSystem.reformat(&blockDevice);
Joeatsumi 2:e6496a794bde 418 pc.printf("%s\n", (err ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 419 if (err) {
hudakz 0:87642278ede6 420 error("error: %s (%d)\n", strerror(-err), err);
hudakz 0:87642278ede6 421 }
hudakz 0:87642278ede6 422 }
hudakz 0:87642278ede6 423
hudakz 0:87642278ede6 424 // Open the numbers file
Joeatsumi 2:e6496a794bde 425 pc.printf("Opening \"/fs/numbers.txt\"... ");
hudakz 0:87642278ede6 426 fflush(stdout);
hudakz 0:87642278ede6 427
hudakz 0:87642278ede6 428 FILE* f = fopen("/fs/numbers.txt", "r+");
Joeatsumi 2:e6496a794bde 429 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 2:e6496a794bde 430
hudakz 0:87642278ede6 431 if (!f) {
hudakz 0:87642278ede6 432 // Create the numbers file if it doesn't exist
Joeatsumi 2:e6496a794bde 433 pc.printf("No file found, creating a new file... ");
hudakz 0:87642278ede6 434 fflush(stdout);
hudakz 0:87642278ede6 435 f = fopen("/fs/numbers.txt", "w+");
Joeatsumi 2:e6496a794bde 436 pc.printf("%s\n", (!f ? "Fail :(" : "OK"));
hudakz 0:87642278ede6 437 }
Joeatsumi 2:e6496a794bde 438
Joeatsumi 2:e6496a794bde 439 /*初期姿勢のQuaternionの設定*/
Joeatsumi 2:e6496a794bde 440 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
Joeatsumi 2:e6496a794bde 441 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
Joeatsumi 2:e6496a794bde 442
Joeatsumi 2:e6496a794bde 443 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
Joeatsumi 2:e6496a794bde 444
Joeatsumi 2:e6496a794bde 445 float Initialize_atitude[4];
Joeatsumi 2:e6496a794bde 446 Initialize_atitude[0]=Quaternion_from_acc[0];
Joeatsumi 2:e6496a794bde 447 Initialize_atitude[1]=Quaternion_from_acc[1];
Joeatsumi 2:e6496a794bde 448 Initialize_atitude[2]=Quaternion_from_acc[2];
Joeatsumi 2:e6496a794bde 449 Initialize_atitude[3]=Quaternion_from_acc[3];
Joeatsumi 2:e6496a794bde 450
Joeatsumi 2:e6496a794bde 451 Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化
Joeatsumi 2:e6496a794bde 452
Joeatsumi 2:e6496a794bde 453
Joeatsumi 2:e6496a794bde 454 while(1){
Joeatsumi 2:e6496a794bde 455
Joeatsumi 2:e6496a794bde 456 /*gpsから来た文字列の処理
Joeatsumi 2:e6496a794bde 457 if((gps_flag==1)&&(gps_flag_conversion==1)){
Joeatsumi 2:e6496a794bde 458
Joeatsumi 2:e6496a794bde 459 err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive);
Joeatsumi 2:e6496a794bde 460
Joeatsumi 2:e6496a794bde 461 gps_flag=0;
Joeatsumi 2:e6496a794bde 462 gps_flag_conversion=0;
Joeatsumi 2:e6496a794bde 463
Joeatsumi 2:e6496a794bde 464 }else{}
Joeatsumi 2:e6496a794bde 465 */
Joeatsumi 2:e6496a794bde 466
Joeatsumi 2:e6496a794bde 467 scan_update(sensor_array,mag);//９軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z
Joeatsumi 2:e6496a794bde 468 float Time_new=float(passed_time.read());//時間の取得
Joeatsumi 2:e6496a794bde 469
Joeatsumi 2:e6496a794bde 470 //地磁気の補正用に使う。
Joeatsumi 2:e6496a794bde 471 //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]);
Joeatsumi 2:e6496a794bde 472
Joeatsumi 2:e6496a794bde 473 //ジャイロの補正用に使う
Joeatsumi 2:e6496a794bde 474 //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]);
Joeatsumi 2:e6496a794bde 475
Joeatsumi 2:e6496a794bde 476 /*----------------ジャイロセンサからQuaternionを求める----------------*/
Joeatsumi 2:e6496a794bde 477 float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7);
Joeatsumi 2:e6496a794bde 478 float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7);
Joeatsumi 2:e6496a794bde 479 float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7);
Joeatsumi 2:e6496a794bde 480
Joeatsumi 2:e6496a794bde 481 float omega[16] = {
Joeatsumi 2:e6496a794bde 482
Joeatsumi 2:e6496a794bde 483 0.0 , -omega_x , -omega_y, -omega_z,
Joeatsumi 2:e6496a794bde 484 omega_x ,0.0 ,omega_z ,-omega_y ,
Joeatsumi 2:e6496a794bde 485 omega_y , -omega_z , 0.0 , omega_x ,
Joeatsumi 2:e6496a794bde 486 omega_z , omega_y ,-omega_x , 0.0
Joeatsumi 2:e6496a794bde 487
Joeatsumi 2:e6496a794bde 488 };
hudakz 0:87642278ede6 489
Joeatsumi 2:e6496a794bde 490 Omega_matrix.SetComps(omega);
Joeatsumi 2:e6496a794bde 491
Joeatsumi 2:e6496a794bde 492 float half[16] ={
Joeatsumi 2:e6496a794bde 493 (Time_new-Time_old)*0.5,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 494 0.0,(Time_new-Time_old)*0.5,0.0,0.0,
Joeatsumi 2:e6496a794bde 495 0.0,0.0,(Time_new-Time_old)*0.5,0.0,
Joeatsumi 2:e6496a794bde 496 0.0,0.0,0.0,(Time_new-Time_old)*0.5
Joeatsumi 2:e6496a794bde 497 };
Joeatsumi 2:e6496a794bde 498
Joeatsumi 2:e6496a794bde 499
Joeatsumi 2:e6496a794bde 500 Half_matrix.SetComps(half);
Joeatsumi 2:e6496a794bde 501 //Quaternion_atitude_from_omega+=(Time_new-Time_old)*
Joeatsumi 2:e6496a794bde 502 Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega;
Joeatsumi 2:e6496a794bde 503
Joeatsumi 2:e6496a794bde 504 Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize();
Joeatsumi 2:e6496a794bde 505
Joeatsumi 2:e6496a794bde 506
Joeatsumi 3:3fa7882a5fd0 507 //Time_old=Time_new;//時間の更新
Joeatsumi 2:e6496a794bde 508
Joeatsumi 2:e6496a794bde 509 /*
Joeatsumi 2:e6496a794bde 510 pc.printf("%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 511 ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 512 ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 513 */
Joeatsumi 2:e6496a794bde 514
Joeatsumi 2:e6496a794bde 515 /*----------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 516
Joeatsumi 2:e6496a794bde 517 /*
Joeatsumi 2:e6496a794bde 518 err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"
Joeatsumi 2:e6496a794bde 519 ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5]
Joeatsumi 2:e6496a794bde 520 ,sensor_array[6],sensor_array[7],sensor_array[8]);
Joeatsumi 2:e6496a794bde 521 */
Joeatsumi 2:e6496a794bde 522
Joeatsumi 2:e6496a794bde 523 /*--------------------加速センサで地磁気の値を補正する --------------------*/
Joeatsumi 2:e6496a794bde 524
Joeatsumi 2:e6496a794bde 525 if(9.8065*1.1 >
Joeatsumi 2:e6496a794bde 526 sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))
Joeatsumi 2:e6496a794bde 527 / 16384.0 * 9.81)
Joeatsumi 2:e6496a794bde 528 {
Joeatsumi 2:e6496a794bde 529
Joeatsumi 2:e6496a794bde 530 Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag);
Joeatsumi 2:e6496a794bde 531
Joeatsumi 3:3fa7882a5fd0 532 //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG);
Joeatsumi 2:e6496a794bde 533
Joeatsumi 2:e6496a794bde 534 double pitch_and_roll[2];
Joeatsumi 2:e6496a794bde 535 Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll);
Joeatsumi 2:e6496a794bde 536
Joeatsumi 2:e6496a794bde 537 //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 538
Joeatsumi 2:e6496a794bde 539 Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc);
Joeatsumi 2:e6496a794bde 540
Joeatsumi 2:e6496a794bde 541 /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/
Joeatsumi 2:e6496a794bde 542 float Quaternion_from_acc_fixed[4];
Joeatsumi 2:e6496a794bde 543 Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]);
Joeatsumi 2:e6496a794bde 544 Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]);
Joeatsumi 2:e6496a794bde 545 Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]);
Joeatsumi 2:e6496a794bde 546 Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]);
Joeatsumi 2:e6496a794bde 547
Joeatsumi 2:e6496a794bde 548 Vector Qua_acc(4);
Joeatsumi 2:e6496a794bde 549
Joeatsumi 2:e6496a794bde 550 Qua_acc.SetComps(Quaternion_from_acc_fixed);
Joeatsumi 2:e6496a794bde 551 /*---------------------------------------------*/
Joeatsumi 2:e6496a794bde 552
Joeatsumi 2:e6496a794bde 553 /*---------------------相補フィルタのゲインに用いる---------------------*/
Joeatsumi 2:e6496a794bde 554 float comp_1[16]={0.95,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 555 0.0,0.95,0.0,0.0,
Joeatsumi 2:e6496a794bde 556 0.0,0.0,0.95,0.0,
Joeatsumi 2:e6496a794bde 557 0.0,0.0,0.0,0.95
Joeatsumi 2:e6496a794bde 558 };
Joeatsumi 2:e6496a794bde 559 /*--------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 560 /*---------------------quaternionの時間微分に用いる---------------------*/
Joeatsumi 2:e6496a794bde 561 float comp_2[16]={0.05,0.0,0.0,0.0,
Joeatsumi 2:e6496a794bde 562 0.0,0.05,0.0,0.0,
Joeatsumi 2:e6496a794bde 563 0.0,0.0,0.05,0.0,
Joeatsumi 2:e6496a794bde 564 0.0,0.0,0.0,0.05
Joeatsumi 2:e6496a794bde 565 };
Joeatsumi 2:e6496a794bde 566
Joeatsumi 2:e6496a794bde 567 Complement_matrix_1.SetComps(comp_1);
Joeatsumi 2:e6496a794bde 568 Complement_matrix_2.SetComps(comp_2);
Joeatsumi 2:e6496a794bde 569
Joeatsumi 2:e6496a794bde 570 /*--------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 571
Joeatsumi 2:e6496a794bde 572
Joeatsumi 2:e6496a794bde 573 /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/
Joeatsumi 3:3fa7882a5fd0 574
Joeatsumi 2:e6496a794bde 575 Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega
Joeatsumi 2:e6496a794bde 576 +Complement_matrix_2*Qua_acc;
Joeatsumi 3:3fa7882a5fd0 577
Joeatsumi 2:e6496a794bde 578 /*----------------------------------------------------------------------------*/
Joeatsumi 2:e6496a794bde 579
Joeatsumi 2:e6496a794bde 580
Joeatsumi 2:e6496a794bde 581 /*
Joeatsumi 2:e6496a794bde 582 pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 583 ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1)
Joeatsumi 2:e6496a794bde 584 ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 585 ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3)
Joeatsumi 2:e6496a794bde 586 ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 587 */
Joeatsumi 2:e6496a794bde 588
Joeatsumi 2:e6496a794bde 589 }else{}
Joeatsumi 2:e6496a794bde 590
Joeatsumi 3:3fa7882a5fd0 591
Joeatsumi 2:e6496a794bde 592 /*
Joeatsumi 2:e6496a794bde 593 pc.printf("%f,%f,%f,%f,%f\r\n"
Joeatsumi 2:e6496a794bde 594 ,Time_new
Joeatsumi 2:e6496a794bde 595 ,Quaternion_atitude_from_omega.GetComp(1)
Joeatsumi 2:e6496a794bde 596 ,Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 2:e6496a794bde 597 ,Quaternion_atitude_from_omega.GetComp(3)
Joeatsumi 2:e6496a794bde 598 ,Quaternion_atitude_from_omega.GetComp(4));
Joeatsumi 2:e6496a794bde 599 */
Joeatsumi 3:3fa7882a5fd0 600 /*-----------Quaternionからオイラー角への変換-----------*/
Joeatsumi 3:3fa7882a5fd0 601 Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1)
Joeatsumi 3:3fa7882a5fd0 602 ,Quaternion_atitude_from_omega.GetComp(2)
Joeatsumi 3:3fa7882a5fd0 603 ,Quaternion_atitude_from_omega.GetComp(3)
Joeatsumi 3:3fa7882a5fd0 604 ,Quaternion_atitude_from_omega.GetComp(4)
Joeatsumi 3:3fa7882a5fd0 605 ,Euler_angle);
Joeatsumi 3:3fa7882a5fd0 606
Joeatsumi 3:3fa7882a5fd0 607 /*-----------オイラー角の表示----------------------*/
Joeatsumi 3:3fa7882a5fd0 608
Joeatsumi 3:3fa7882a5fd0 609 pc.printf("%f,%f,%f,%f,%f\r\n"
Joeatsumi 3:3fa7882a5fd0 610 ,Time_new
Joeatsumi 3:3fa7882a5fd0 611 ,Euler_angle[0]
Joeatsumi 3:3fa7882a5fd0 612 ,Euler_angle[1]
Joeatsumi 3:3fa7882a5fd0 613 ,Euler_angle[2]
Joeatsumi 3:3fa7882a5fd0 614 );
Joeatsumi 3:3fa7882a5fd0 615
Joeatsumi 3:3fa7882a5fd0 616 /*--------------------------------------------------*/
Joeatsumi 3:3fa7882a5fd0 617 //Pitch control
Joeatsumi 3:3fa7882a5fd0 618 pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old));
Joeatsumi 3:3fa7882a5fd0 619 Elevator_servo.pulsewidth(pitch_duty);
Joeatsumi 3:3fa7882a5fd0 620
Joeatsumi 3:3fa7882a5fd0 621
Joeatsumi 3:3fa7882a5fd0 622 Time_old=Time_new;//時間の更新
Joeatsumi 3:3fa7882a5fd0 623
Joeatsumi 2:e6496a794bde 624 wait(0.001);
Joeatsumi 2:e6496a794bde 625
Joeatsumi 2:e6496a794bde 626 /*----------------------------------------------------------------*/
hudakz 0:87642278ede6 627
Joeatsumi 2:e6496a794bde 628 if(switch_off==1){
Joeatsumi 2:e6496a794bde 629 // Close the file which also flushes any cached writes
Joeatsumi 2:e6496a794bde 630 pc.printf("Closing \"/fs/numbers.txt\"... ");
Joeatsumi 2:e6496a794bde 631 err = fclose(f);
Joeatsumi 2:e6496a794bde 632 break;
Joeatsumi 2:e6496a794bde 633 }
Joeatsumi 2:e6496a794bde 634
Joeatsumi 2:e6496a794bde 635 }//while(1) ends
Joeatsumi 2:e6496a794bde 636 }//main ends