Atsumi Toda
/
UAV_Logger1
UAVの姿勢推定に使用するプログラム。
Diff: main.cpp
- Revision:
- 4:21a356ae0747
- Parent:
- 3:3fa7882a5fd0
diff -r 3fa7882a5fd0 -r 21a356ae0747 main.cpp --- a/main.cpp Wed Jul 24 12:00:01 2019 +0000 +++ b/main.cpp Mon Aug 19 02:50:13 2019 +0000 @@ -1,104 +1,84 @@ +/* +This system is vertion 2.0. Thsi program enable us to read signals from receiver ,measure balance of circit, and actuate servos. +*/ + #include "mbed.h" +#include "string.h" +#include "MPU6050.h" + #include <stdio.h> #include <errno.h> -#include "MPU6050.h" -#include "HMC5883L.h" -#include "Vector.h" -#include "Matrix.h" -#include "Vector_Matrix_operator.h" - -// Block devices -//#include "SPIFBlockDevice.h" -//#include "DataFlashBlockDevice.h" #include "SDBlockDevice.h" -//#include "HeapBlockDevice.h" - -// File systems - -//#include "LittleFileSystem.h" #include "FATFileSystem.h" +#include "myConstants.h" + +#define READBUFFERSIZE 70 +#define DELIMITER "," #define M_PI 3.141592 #define PI2 (2*M_PI) -/*地磁気センサの補正値(足し合わせる)*/ -#define MAG_FIX_X 338 -#define MAG_FIX_Y 20 -#define MAG_FIX_Z -50 - /*ジャイロセンサの補正値(引く)*/ #define GYRO_FIX_X 290.5498 #define GYRO_FIX_Y 99.29363 #define GYRO_FIX_Z 61.41011 -// File system declaration -FATFileSystem fileSystem("fs"); +/*---------ピン指定-------------------*/ +DigitalOut myled(LED1); + +DigitalIn switch_off(p30);//sdカードへの書き込みを終了させる + +InterruptIn button(p5);//GPSモジュールからの1pps信号を読み取る(立ち下がり割り込み) -/*-----割り込み--------*/ -Ticker flipper; -/*--------------------*/ -/*-----タイマー---------*/ -Timer passed_time; -/*---------------------*/ -/*-------ピン指定------------*/ +InterruptIn input_from_Aileron_1(p11); +InterruptIn input_from_elevator(p12);// +InterruptIn input_from_throttle(p13);// +InterruptIn input_from_rudder(p14);// +InterruptIn input_from_gear(p15);// +InterruptIn input_from_Aileron_2(p16);// +InterruptIn input_from_pitch(p17);// +InterruptIn input_from_AUX5(p18);// + + +MPU6050 mpu(p28, p27);//sda scl + +Serial pc(USBTX, USBRX); // tx, rx +Serial gps(p9,p10);// gpsとの接続 tx, rx +SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs + //declare PWM pins here PwmOut ESC (p21); PwmOut Elevator_servo(p22); PwmOut Rudder_servo(p23); +PwmOut Aileron_R_servo(p25); +PwmOut Aileron_L_servo(p26); + +/*-----------------------------------*/ + +/*-----ファイルポインタ-----*/ +// File system declaration +FATFileSystem fileSystem("fs"); +/*-----------------------*/ + +/*------------------------GPSの変数------------------------*/ +int GPS_flag_update=0; +int GPS_interruptIn=0; + +float longtude_gps,latitude_gps,azimuth_gps,speed_gps; + +//char NMEA_sentense[READBUFFERSIZE]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A"; +char NMEA_sentense[READBUFFERSIZE]; + +/*--------------------------------------------------------*/ -SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs -MPU6050 mpu(p28, p27);//sda scl -HMC5883L compass(p28, p27);//sda scl - -RawSerial gps(p9,p10); //serial port for gps_module -RawSerial pc(USBTX, USBRX); - -/*--------------------------*/ - -DigitalIn switch_off(p30); -DigitalOut led2(LED2); - -/*-----------グローバル変数-----------*/ -double Euler_angle[3]; -double Euler_angle_Initiated[3]; - -float g_hokui,g_tokei; -int fp_count=0; -int gps_flag=0; -int gps_flag_conversion=0; - -char gps_data[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10箱を用意している -char gps_data_2[10]="000000000";//末尾にnullがあるので、要素が9でも配列では10の箱を用意している - -unsigned int tokei_int_receive; -unsigned int hokui_int_receive; - -float tokei_float_receive; -float hokui_float_receive; - -float a[3];//加速度の値 -float g[3];//ジャイロの値[rad/s] -float dpsX,dpsY,dpsZ; -float AccX,AccY,AccZ; - -int sensor_array[6]; -int16_t mag[3]; - -double Roll_Acc,Pitch_Acc; -double Yaw; -double Azi_mag;//地磁気からの方位 -double Quaternion_from_acc[4];//加速度と地磁気からのQuaternion - -/*----------------------------------*/ -/*--------------------------行列、ベクトル-----------------------------*/ - -Vector Quaternion_atitude_from_omega(4); -Matrix Omega_matrix(4,4),Half_matrix(4,4); -Matrix Complement_matrix_1(4,4),Complement_matrix_2(4,4); - -/*-------------------------------------------------------------------*/ +/*--------------------------角度とセンサ--------------------------*/ +float sensor_array[6]; +float Acc_Pitch_Roll[2]; +float Gyro_Pitch_Roll[2];//pitch and roll +float omega_z; +/*--------------------------------------------------------------*/ /*----------PID コントロール-----------------*/ #define Gain_Kp 0.8377 @@ -116,292 +96,81 @@ #define UPPER_DUTY 0.0020 #define LOWER_DUTY 0.0010 +#define THRESHOLD 0.0018 + double pitch_duty,roll_duty,yaw_duty; +double first_period,second_period,third_period,fourth_period; +double fifth_period,sixth_period,seventh_period,eighth_period; + +double first_period_old=0.0,second_period_old=0.0,third_period_old=0.0,fourth_period_old=0.0; +double fifth_period_old=0.0,sixth_period_old=0.0,seventh_period_old=0.0,eighth_period_old=0.0; + +double throttle_duty,elevator_duty,rudder_duty,switch_duty,aileron_duty; + /*------------------------------------------*/ - -/*プロトタイプ宣言*/ -void toString_V(Vector& v); // ベクトルデバッグ用 -void gps_rx(); -void gps_convertion(); -/*--------------*/ - -void toString_V(Vector& v) -{ - - for(int i=0; i<v.GetDim(); i++) { - pc.printf("%.6f\t", v.GetComp(i+1)); - } - pc.printf("\r\n"); - -} - -void scan_update(int box_sensor[6],int16_t m[3]){ - - int a[3];//加速度の値 - int g[3];//角速度の値 +/*-----タイマー---------*/ +Timer passed_time;//経過時間の計算 +Timer ch_time;// - int16_t raw_compass[3];//地磁気センサの値 - - mpu.setAcceleroRange(0);//acceleration range is +-4G - mpu.setGyroRange(0);//gyro rate is +-degree per second(dps) - - mpu.getAcceleroRaw(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; - mpu.getGyroRaw(g); // 角速度を格納する配列[rad/s] - compass.getXYZ(raw_compass);//地磁気の値を格納する配列 - - box_sensor[0]=-g[0]; - box_sensor[1]=g[1]; - box_sensor[2]=-g[2]; - - box_sensor[3]=a[0]; - box_sensor[4]=-a[1]; - box_sensor[5]=a[2]; - - m[0]=(raw_compass[0]);//x - m[1]=(raw_compass[2]);//y - m[2]=(raw_compass[1]);//z - -} +float Time_new,Time_old=0.0; +float Servo_command=0.002; +/*---------------------*/ +/*---------PWM割り込みフラグ-------*/ +int PWM_flag_update=0; +/*-------------------------------*/ -void gps_rx(){ - - if((gps.getc()=='$')&&(gps_flag==0)){ - for(int i=0;i<9;i++){ - gps_data[i]=gps.getc(); - //pc.putc(gps_data[i]); - } - - gps_data[9]='\0'; - - for(int i=0;i<9;i++){ - gps_data_2[i]=gps.getc(); - //pc.putc(gps_data[i]); - } - - gps_data_2[9]='\0'; - - }//if(twe.getc()==':') - - //pc.printf("%s,%s\r\n",gps_data,gps_data_2); - - gps_flag=1; - - }//gps_rx ends +/*----------プロトタイプ宣言----------*/ +void gps_rx(); +void gps_read(); -void gps_convertion(){ - while(1){ - if(gps_flag==1){ - tokei_int_receive=atoi(gps_data); - hokui_int_receive=atoi(gps_data_2); - - //pc.printf("%d,%d\r\n",tokei_int_receive,hokui_int_receive); - - tokei_float_receive=float(tokei_int_receive)/pow(10.0,6.0)-100.0; - hokui_float_receive=float(hokui_int_receive)/pow(10.0,6.0)-100.0; - - gps_flag_conversion=1; - - }else{gps_flag_conversion=0;} - }//while ends - - }//ends +double Degree_to_PWM_duty(double degree); +double PID_duty_pitch(double target,double vol,float dt); +double PID_duty_rudder(double target,double vol,float dt,float yaw_rate); +double PID_duty_roll(double target,double vol,float dt); -void Acc_to_Quaternion(int x_acc,int y_acc,int z_acc,double Yaw,double qua[4]){ - - double Roll_before = double(y_acc)/double(z_acc); - double Roll = atan(Roll_before); - - double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) ); - double Pitch = -atan(Pitch_before); - - //Yaw=0.0; - - /*Quaternionの要素へオイラー角を変換する*/ - 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); - 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); - 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); - 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); - - - } - -void Acc_to_Pitch_Roll(int x_acc,int y_acc,int z_acc,double qua[2]){ - - double Roll_before = double(y_acc)/double(z_acc); - double Roll = atan(Roll_before); - - double Pitch_before = double(x_acc/sqrt(double(y_acc*y_acc+z_acc*z_acc)) ); - double Pitch = -atan(Pitch_before); +void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]); +void scan_update(float box_sensor[6]); +void sensor_update(); - - qua[0]=Pitch; - qua[1]=Roll; - - } +//gpsの1pps信号の割り込み +void flip(); -void Correct_n(int x_acc,int y_acc,int z_acc,double n[4]){ - - //y_acc*=-1.0; - - n[0]=-double(y_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc)); - n[1]=double(x_acc)/sqrt(double(y_acc*y_acc+x_acc*x_acc)); - n[2]=0.0; - - n[3]=sqrt(double(x_acc*x_acc+y_acc*y_acc))/double(z_acc); - - } -double Geomagnetism_correction(int x_acc,int y_acc,int z_acc,int16_t magnet[3]){ - - Vector Mag_from_sensor(3),Mag_fixed_sensor(3); //座標系 - Matrix Rotation(3, 3); //検算の行列 +void Initialize_ESC(); +void Activate_ESC(); - double Correct_vector_n[4];//回転行列の成分 - Correct_n(x_acc,y_acc,z_acc, Correct_vector_n); - - double n_x=Correct_vector_n[0]; - double n_y=Correct_vector_n[1]; - double n_z=Correct_vector_n[2]; - double theta=-Correct_vector_n[3]; - - float Rotate_element[9] = { - n_x*n_x*(1-cos(theta))+cos(theta) , n_x*n_y*(1-cos(theta)) , n_y*sin(theta), - n_x*n_y*(1-cos(theta)) ,n_y*n_y*(1-cos(theta))+cos(theta) , -n_x*sin(theta), - n_y*sin(theta) , n_x*sin(theta) , cos(theta) - }; - - Rotation.SetComps(Rotate_element); - //x y z - float Geomagnetism[3]={ float(magnet[1]+MAG_FIX_X) - , float(magnet[0]+MAG_FIX_Y) - , float( (-1.0)*(magnet[2]+MAG_FIX_Z ))}; - - Mag_from_sensor.SetComps(Geomagnetism); - - Mag_fixed_sensor=Rotation*Mag_from_sensor;//地磁気センサの値を加速度センサによる傾きで補正。 - - float Mag_fixed_x= Mag_fixed_sensor.GetComp(1); - float Mag_fixed_y= Mag_fixed_sensor.GetComp(2); - - double Azi=atan2(double(Mag_fixed_y),double(Mag_fixed_x)); - - /* - if(Azi < 0.0) // fix sign - Azi += PI2; - - if(Azi > PI2) // fix overflow - Azi -= PI2; - */ - - return Azi; - - } +void Input_Aileron_1(); +void Input_elevator(); +void Input_throttle(); +void Input_rudder(); +void Input_gear(); +void Input_Aileron_2(); +void Input_pitch(); +void Input_AUX5(); -//void led2_thread(void const *argument) { -void imu_thread() { - while (true) { - }//while ends -} - -void Quaternion_to_Euler(float q0,float q1,float q2,float q3,double Euler[3]){ - - //float pitch,roll,yaw; - - Euler[0]=RAD_TO_DEG*atan2(double( 2*(q2*q3+q0*q1)), double (q0*q0-q1*q1-q2*q2+q3*q3) ); - Euler[1]=-RAD_TO_DEG*asin(double(2*(q1*q3-q0*q2))); - Euler[2]=RAD_TO_DEG*atan2(double(2*(q1*q2+q0*q3)), double(q0*q0+q1*q1-q2*q2-q3*q3)); +void Input_Aileron_1_fall(); +void Input_elevator_fall(); +void Input_throttle_fall(); +void Input_rudder_fall(); +void Input_gear_fall(); +void Input_Aileron_2_fall(); +void Input_pitch_fall(); +void Input_AUX5_fall(); +/*---------------------------------*/ - } - -double Degree_to_PWM_duty(double degree){ - - double duty=0.0; - - duty=(0.2/1000)/20.0*degree; - - if((duty+NUTRAL)>=UPPER_DUTY){ - duty=UPPER_DUTY-NUTRAL; - }else if((duty+NUTRAL)<=LOWER_DUTY){ - duty=NUTRAL-LOWER_DUTY; - }else{} - - return duty; +int main() { - } - -double PID_duty(double target,double vol,float dt){ - //ここで角度の単位で偏差は入力される - //出力はPWMで - double duty=0.0; - double add_duty=0.0; - - Prop_p=target-vol; - - pc.printf("%f/r/n",Prop_p); - - Int_p+=Prop_p*double(dt); - Dif_p=(Prop_p - Pre_Prop_p) / double(dt); - - Pre_Prop_p=Prop_p; - - //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; - - add_duty=1.0*Prop_p; - - duty+=add_duty; - duty=Degree_to_PWM_duty(duty); - - return duty; - -} - -void Initialize_ESC(){ - - ESC.pulsewidth(0.002); wait(3); - ESC.pulsewidth(0.001); - wait(5); - } + /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/ -void Activate_ESC(){ - ESC.pulsewidth(0.001); - wait(5); - - } - -// Entry point for the example -int main() -{ - gps.baud(115200);//GT-720Fボーレート - pc.baud(115200); - //imu_thread - - compass.init();//hmc5883の起動 - - Thread thread1 (gps_convertion); - gps.attach(gps_rx, Serial::RxIrq); - - /*define period of serve and ESC*/ - ESC.period(SERVO_PERIOD); - Elevator_servo.period(SERVO_PERIOD); - Rudder_servo.period(SERVO_PERIOD); - /*------------------------------*/ - Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms - Rudder_servo.pulsewidth(NUTRAL); - - Activate_ESC(); - - float Time_old=0.0;//時間初期化 - passed_time.start();//タイマー開始 - - pc.printf("--- Mbed OS filesystem example ---\n"); + pc.printf("--- Mbed OS filesystem example ---\n"); // Try to mount the filesystem pc.printf("Mounting the filesystem... "); @@ -422,215 +191,539 @@ } // Open the numbers file - pc.printf("Opening \"/fs/numbers.txt\"... "); + pc.printf("Opening \"/fs/log.csv\"... "); fflush(stdout); - FILE* f = fopen("/fs/numbers.txt", "r+"); + FILE* f = fopen("/fs/log.csv", "r+"); + FILE* fp = fopen("/fs/log.txt", "r+"); + pc.printf("%s\n", (!f ? "Fail :(" : "OK")); if (!f) { // Create the numbers file if it doesn't exist pc.printf("No file found, creating a new file... "); fflush(stdout); - f = fopen("/fs/numbers.txt", "w+"); + f = fopen("/fs/log.csv", "w+"); + fp = fopen("/fs/log.txt", "w+"); + pc.printf("%s\n", (!f ? "Fail :(" : "OK")); } - - /*初期姿勢のQuaternionの設定*/ - scan_update(sensor_array,mag);//9軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z - Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag); - - Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc); + - float Initialize_atitude[4]; - Initialize_atitude[0]=Quaternion_from_acc[0]; - Initialize_atitude[1]=Quaternion_from_acc[1]; - Initialize_atitude[2]=Quaternion_from_acc[2]; - Initialize_atitude[3]=Quaternion_from_acc[3]; - - Quaternion_atitude_from_omega.SetComps(Initialize_atitude);//ジャイロで計算するQuaternionの初期化 - while(1){ - - /*gpsから来た文字列の処理 - if((gps_flag==1)&&(gps_flag_conversion==1)){ - - err = fprintf(f,"%f,%f\r\n",tokei_float_receive,hokui_float_receive); - - gps_flag=0; - gps_flag_conversion=0; - - }else{} - */ - - scan_update(sensor_array,mag);//9軸の値の取得 mag[0]=y mag[1]=x mag[2]=-Z - float Time_new=float(passed_time.read());//時間の取得 - - //地磁気の補正用に使う。 - //pc.printf("%d,%d,%d\r\n",mag[1],mag[0],mag[2]); - - //ジャイロの補正用に使う - //pc.printf("%d,%d,%d\r\n",sensor_array[0],sensor_array[1],sensor_array[2]); + /*define period of serve and ESC*/ + ESC.period(SERVO_PERIOD); + Elevator_servo.period(SERVO_PERIOD); + Rudder_servo.period(SERVO_PERIOD); + Aileron_R_servo.period(SERVO_PERIOD); + Aileron_L_servo.period(SERVO_PERIOD); + /*------------------------------*/ + Elevator_servo.pulsewidth(NUTRAL); // servo position determined by a pulsewidth between 1-2ms + Rudder_servo.pulsewidth(NUTRAL); + Aileron_R_servo.pulsewidth(NUTRAL); + Aileron_L_servo.pulsewidth(NUTRAL); + + Activate_ESC();//Activate ESC + + /*--------------ピン変化割り込みに対応した関数の宣言--------------*/ + button.fall(&flip);//GPSモジュールからの1pps信号を立ち下がり割り込みで読み取る。 + + input_from_throttle.rise(&Input_throttle);// + + input_from_Aileron_1.fall(&Input_Aileron_1_fall); + input_from_elevator.fall(&Input_elevator_fall);// + input_from_throttle.fall(&Input_throttle_fall);// + input_from_rudder.fall(&Input_rudder_fall);// + input_from_gear.fall(&Input_gear_fall); + input_from_Aileron_2.fall(&Input_Aileron_2_fall);// + input_from_pitch.fall(&Input_pitch_fall);// + input_from_AUX5.fall(&Input_AUX5_fall);// + + ch_time.start();//時間計測スタート + /*----------------------------------------------------------*/ + + pc.baud(115200); + gps.baud(115200); + + //gps.attach(gps_rx, Serial::RxIrq);//シリアル割り込み + //pc.attach(gps_rx, Serial::RxIrq);//シリアル割り込み + + passed_time.start();//タイマー開始 + Time_old=float(passed_time.read()); + + /*初期姿勢角の算出*/ + scan_update(sensor_array);//角速度、加速度を求める + Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll); + Gyro_Pitch_Roll[0]=Acc_Pitch_Roll[0];//初期姿勢角の算出 Pitch + Gyro_Pitch_Roll[1]=Acc_Pitch_Roll[1];//初期姿勢角の算出 Roll + + pc.printf("Start!!\r\n"); + + while(1) { + + switch (GPS_interruptIn) { + case 1: + + gps_rx(); + gps_read(); + GPS_interruptIn--; + break; - /*----------------ジャイロセンサからQuaternionを求める----------------*/ - float omega_x=float(( float(sensor_array[0])-GYRO_FIX_X )/ 7505.7); - float omega_y=float(( float(sensor_array[1])-GYRO_FIX_Y )/ 7505.7); - float omega_z=float(( float(sensor_array[2])-GYRO_FIX_Z )/ 7505.7); - - float omega[16] = { + default: - 0.0 , -omega_x , -omega_y, -omega_z, - omega_x ,0.0 ,omega_z ,-omega_y , - omega_y , -omega_z , 0.0 , omega_x , - omega_z , omega_y ,-omega_x , 0.0 - - }; - - Omega_matrix.SetComps(omega); + sensor_update(); + break; + }//switch end + + /* + ここでプロポの信号のパルス幅を読み取って、手動or 自動の切り替えを行う必要がある。 + 以下のpitch control等は自動操縦とみなしてよい。 + */ + + + if(switch_duty>THRESHOLD){//オートパイロット - float half[16] ={ - (Time_new-Time_old)*0.5,0.0,0.0,0.0, - 0.0,(Time_new-Time_old)*0.5,0.0,0.0, - 0.0,0.0,(Time_new-Time_old)*0.5,0.0, - 0.0,0.0,0.0,(Time_new-Time_old)*0.5 - }; - + //Pitch control + //pitch_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old)); + elevator_duty=NUTRAL+PID_duty_pitch(0.0,Gyro_Pitch_Roll[0],(Time_new-Time_old)); + Elevator_servo.pulsewidth(elevator_duty); - Half_matrix.SetComps(half); - //Quaternion_atitude_from_omega+=(Time_new-Time_old)* - Quaternion_atitude_from_omega +=Half_matrix*Omega_matrix*Quaternion_atitude_from_omega; + //roll control + //roll_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old)) + aileron_duty=NUTRAL+PID_duty_roll(0.0,Gyro_Pitch_Roll[1],(Time_new-Time_old)); - Quaternion_atitude_from_omega=Quaternion_atitude_from_omega.Normalize(); + //角速度だけフィードバック + rudder_duty=NUTRAL+PID_duty_rudder(0.0,0.0,(Time_new-Time_old),omega_z); + ///pc.printf("%f\r\n",rudder_duty); - //Time_old=Time_new;//時間の更新 - - /* - pc.printf("%f,%f,%f,%f\r\n" - ,Quaternion_atitude_from_omega.GetComp(1),Quaternion_atitude_from_omega.GetComp(2) - ,Quaternion_atitude_from_omega.GetComp(3),Quaternion_atitude_from_omega.GetComp(4)); - */ - - /*----------------------------------------------------------------*/ - - /* - err = fprintf(f,"A,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n" - ,val,sensor_array[0],sensor_array[1],sensor_array[2],sensor_array[3],sensor_array[4],sensor_array[5] - ,sensor_array[6],sensor_array[7],sensor_array[8]); - */ - - /*--------------------加速センサで地磁気の値を補正する --------------------*/ - - if(9.8065*1.1 > - sqrt(double(sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5])) - / 16384.0 * 9.81) - { + //スロットルはマニュアル + //Servo_command + if( (float(passed_time.read())-Servo_command)>0.003 ){ - Azi_mag=Geomagnetism_correction(sensor_array[3],sensor_array[4],sensor_array[5],mag); - - //pc.printf("Azimath=%f\r\n",Azi_mag*RAD_TO_DEG); - - double pitch_and_roll[2]; - Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],pitch_and_roll); - - //pc.printf("Pitch %f,Roll %f\r\n",pitch_and_roll[0]*RAD_TO_DEG,pitch_and_roll[1]*RAD_TO_DEG); + ESC.pulsewidth(throttle_duty); + Elevator_servo.pulsewidth(elevator_duty); + Rudder_servo.pulsewidth(rudder_duty); + Aileron_R_servo.pulsewidth(aileron_duty); + Aileron_L_servo.pulsewidth(aileron_duty); - Acc_to_Quaternion(sensor_array[3],sensor_array[4],sensor_array[5],Azi_mag,Quaternion_from_acc); - - /*Quaternion_from_acc_fixedの要素をfloatへ変換する*/ - float Quaternion_from_acc_fixed[4]; - Quaternion_from_acc_fixed[0]=float(Quaternion_from_acc[0]); - Quaternion_from_acc_fixed[1]=float(Quaternion_from_acc[1]); - Quaternion_from_acc_fixed[2]=float(Quaternion_from_acc[2]); - Quaternion_from_acc_fixed[3]=float(Quaternion_from_acc[3]); - - Vector Qua_acc(4); - - Qua_acc.SetComps(Quaternion_from_acc_fixed); - /*---------------------------------------------*/ - - /*---------------------相補フィルタのゲインに用いる---------------------*/ - float comp_1[16]={0.95,0.0,0.0,0.0, - 0.0,0.95,0.0,0.0, - 0.0,0.0,0.95,0.0, - 0.0,0.0,0.0,0.95 - }; - /*--------------------------------------------------------------------*/ - /*---------------------quaternionの時間微分に用いる---------------------*/ - float comp_2[16]={0.05,0.0,0.0,0.0, - 0.0,0.05,0.0,0.0, - 0.0,0.0,0.05,0.0, - 0.0,0.0,0.0,0.05 - }; - - Complement_matrix_1.SetComps(comp_1); - Complement_matrix_2.SetComps(comp_2); - - /*--------------------------------------------------------------------*/ + Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新 + }else{} + + }else{//マニュアルモード + + if( (float(passed_time.read())-Servo_command)>0.003 ){ + + ESC.pulsewidth(throttle_duty); + Elevator_servo.pulsewidth(elevator_duty); + Rudder_servo.pulsewidth(rudder_duty); + Aileron_R_servo.pulsewidth(aileron_duty); + Aileron_L_servo.pulsewidth(aileron_duty); + + Servo_command=float(passed_time.read());//サーボを動かしたタイミングの更新 + + }else{} - - /*-------------------加速度と地磁気でジャイロデータを補正する------------------------*/ - - Quaternion_atitude_from_omega=Complement_matrix_1*Quaternion_atitude_from_omega - +Complement_matrix_2*Qua_acc; - - /*----------------------------------------------------------------------------*/ - - - /* - pc.printf("%f,%f,%f,%f,%f,%f,%f,%f\r\n" - ,Quaternion_from_acc[0],Quaternion_atitude_from_omega.GetComp(1) - ,Quaternion_from_acc[1],Quaternion_atitude_from_omega.GetComp(2) - ,Quaternion_from_acc[2],Quaternion_atitude_from_omega.GetComp(3) - ,Quaternion_from_acc[3],Quaternion_atitude_from_omega.GetComp(4)); - */ - - }else{} + } + + pc.printf("%f,%f,%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps,latitude_gps,longtude_gps); + err = fprintf(f,"%f,%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1],azimuth_gps); + err = fprintf(fp,"%f,%f\r\n",latitude_gps,longtude_gps); + + Time_old=Time_new;//時間更新 + + if(switch_off==1){ + //fclose(fp); + //flipper.detach(); + + err = fclose(f); + err = fclose(fp); + + pc.printf("Writing finish!"); + myled=0; + break; + + } + + //wait(0.002);//この値は10倍されると考える + }//while ends + +}//main ends + +void gps_rx(){ + + // __disable_irq(); // 割り込み禁止 + + int i=0; + + while(gps.getc()!='$'){ + } + while( (NMEA_sentense[i]=gps.getc()) != '\r'){ + //while( (NMEA_sentense[i]=pc.getc()) != '\r'){ + //pc.putc(NMEA_sentense[i]); + i++; + if(i==READBUFFERSIZE){ + i=(READBUFFERSIZE-1); + break; + } + } + NMEA_sentense[i]='\0'; + //pc.printf("\r\n"); + + if(GPS_flag_update==0){ + GPS_flag_update++; + } + + //__enable_irq(); // 割り込み許可 + +}//void gps_rx + +void gps_read(){ + /*------gps のNMEAフォーマットの処理プログラム----------------*/ + float temp=0.0, deg=0.0, min=0.0; + char *pszTime;//UTC時刻 + char* pszLat;////緯度 + char* pszLong;//経度 + char* pszSp;//速度 + char* pszSpd;//速度方向 + char* pszDate;//日付 + + //strtokで処理した文字列は内容がNullになるので、テストでは、毎回NMEA_sentense[128]を初期化しないといけない + //char NMEA_sentense[128]="$GPRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A"; + + //Time_old=float(passed_time.read()); + + + + if((GPS_flag_update==1)&&(0==strncmp( "GPRMC", NMEA_sentense, 5 ))){//GPS_flag_updateも判定に加えた方がいい + //if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){//GPS_flag_updateも判定に加えた方がいい + + strtok( NMEA_sentense, DELIMITER ); // $GPRMC + pszTime = strtok( NULL, DELIMITER ); // UTC時刻 + strtok( NULL, DELIMITER ); // ステータス + pszLat = strtok( NULL, DELIMITER ); // 緯度(dddmm.mmmm) + strtok( NULL, DELIMITER ); // 北緯か南緯か + pszLong = strtok( NULL, DELIMITER ); // 経度(dddmm.mmmm) + strtok( NULL, DELIMITER ); // 東経か西経か + + pszSp = strtok( NULL, DELIMITER ); // 速度(vvv.v) + pszSpd = strtok( NULL, DELIMITER ); // 速度方向(ddd.d) + pszDate = strtok( NULL, DELIMITER ); // 日付 + + if( NULL != pszLong ){//pszLongがnullでないのならば + temp = atof(pszLat); + deg = (int)(temp/100); + min = temp - deg * 100; + latitude_gps = deg + min / 60;//latitudeの算出 + + temp = atof(pszLong); + deg = (int)(temp/100); + min = temp - deg * 100; + longtude_gps = deg + min / 60;//longtudeの算出 + + speed_gps=atof(pszSp)*KNOT_TO_METER_PER_SEC; + + azimuth_gps=atof(pszSpd); + + GPS_flag_update--; + + //pc.printf("%f,%f\r\n",longtude_gps,latitude_gps); + + }else{}//end of ( NULL != pszLong ) + + }else{}//end of if(0==strncmp( "$GPRMC", NMEA_sentense, 6 )){ + } + + +void sensor_update(){ + scan_update(sensor_array);//角速度、加速度を求める + + Time_new=float(passed_time.read());//時間の更新 + + Gyro_Pitch_Roll[0]+=sensor_array[1]*(Time_new-Time_old);//初期姿勢角の算出 Pitch + Gyro_Pitch_Roll[1]+=sensor_array[0]*(Time_new-Time_old);//初期姿勢角の算出 Roll + + + /*--------------------加速センサでジャイロの値を補正する --------------------*/ + + + + if(9.8065*1.05 > sqrt((sensor_array[3]*sensor_array[3]+sensor_array[4]*sensor_array[4]+sensor_array[5]*sensor_array[5]))) + { + Acc_to_Pitch_Roll(sensor_array[3],sensor_array[4],sensor_array[5],Acc_Pitch_Roll); + Gyro_Pitch_Roll[0]*=0.95;// + Gyro_Pitch_Roll[1]*=0.95;// + Gyro_Pitch_Roll[0]+=(0.05*Acc_Pitch_Roll[0]);// + Gyro_Pitch_Roll[1]+=(0.05*Acc_Pitch_Roll[1]);// + + + }else{} + + //pc.printf("%f,%f,%f\r\n",Time_new,Acc_Pitch_Roll[0]*RAD_TO_DEG,Acc_Pitch_Roll[1]*RAD_TO_DEG); + //pc.printf("%f,%f,%f\r\n",Time_new,Gyro_Pitch_Roll[0],Gyro_Pitch_Roll[1]); + + //Time_old=Time_new; + + } +double Degree_to_PWM_duty(double degree){ + + double duty=0.0; + + duty=(0.2/1000)/20.0*degree; + + if((duty+NUTRAL)>=UPPER_DUTY){ + duty=UPPER_DUTY-NUTRAL; + }else if((duty+NUTRAL)<=LOWER_DUTY){ + duty=NUTRAL-LOWER_DUTY; + }else{} + + return duty; + + } + +double PID_duty_pitch(double target,double vol,float dt){ + //ここで角度の単位で偏差は入力される + //出力はPWMで + double duty=0.0; + double add_duty=0.0; + + Prop_p=target-vol; + + //pc.printf("%f/r/n",Prop_p); + + Int_p+=Prop_p*double(dt); + Dif_p=(Prop_p - Pre_Prop_p) / double(dt); + + Pre_Prop_p=Prop_p; + + //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; + + add_duty=1.0*Prop_p; + + duty+=add_duty; + duty=Degree_to_PWM_duty(duty); + + return duty; + +} + +double PID_duty_rudder(double target,double vol,float dt,float yaw_rate){ + //ここで角度の単位で偏差は入力される + //出力はPWMで + double duty=0.0; + double add_duty=0.0; + + Prop_p=target-vol; + + //pc.printf("%f/r/n",Prop_p); + + Int_p+=Prop_p*double(dt); + Dif_p=(Prop_p - Pre_Prop_p) / double(dt); + + Pre_Prop_p=Prop_p; + + //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; - /* - pc.printf("%f,%f,%f,%f,%f\r\n" - ,Time_new - ,Quaternion_atitude_from_omega.GetComp(1) - ,Quaternion_atitude_from_omega.GetComp(2) - ,Quaternion_atitude_from_omega.GetComp(3) - ,Quaternion_atitude_from_omega.GetComp(4)); - */ - /*-----------Quaternionからオイラー角への変換-----------*/ - Quaternion_to_Euler(Quaternion_atitude_from_omega.GetComp(1) - ,Quaternion_atitude_from_omega.GetComp(2) - ,Quaternion_atitude_from_omega.GetComp(3) - ,Quaternion_atitude_from_omega.GetComp(4) - ,Euler_angle); - - /*-----------オイラー角の表示----------------------*/ + //add_duty=1.0*Prop_p; + //今回は角速度に反応させてラダーを動かす + add_duty=double(yaw_rate)*0.1; + + duty+=add_duty; + duty=Degree_to_PWM_duty(duty); + + return duty; + +} + +double PID_duty_roll(double target,double vol,float dt){ + //ここで角度の単位で偏差は入力される + //出力はPWMで + double duty=0.0; + double add_duty=0.0; + + Prop_p=target-vol; + + //pc.printf("%f/r/n",Prop_p); + + Int_p+=Prop_p*double(dt); + Dif_p=(Prop_p - Pre_Prop_p) / double(dt); + + Pre_Prop_p=Prop_p; + + //add_duty=Gain_Kp*Prop_p+Gain_Ki*Int_p+Gain_Kd*Dif_p; + + add_duty=1.0*Prop_p; + + duty+=add_duty; + duty=Degree_to_PWM_duty(duty); + + return duty; - pc.printf("%f,%f,%f,%f,%f\r\n" - ,Time_new - ,Euler_angle[0] - ,Euler_angle[1] - ,Euler_angle[2] - ); - - /*--------------------------------------------------*/ - //Pitch control - pitch_duty=NUTRAL+PID_duty(0.0,Euler_angle[1],(Time_new-Time_old)); - Elevator_servo.pulsewidth(pitch_duty); - +} +void Initialize_ESC(){ + + ESC.pulsewidth(0.002); + wait(3); + + ESC.pulsewidth(0.001); + wait(5); + + } + + +void Activate_ESC(){ + + ESC.pulsewidth(0.001); + wait(5); + + } + +void Acc_to_Pitch_Roll(float x_acc,float y_acc,float z_acc,float qua[2]){ + + float Roll_before = (y_acc)/(z_acc); + float Roll = atan(Roll_before); - Time_old=Time_new;//時間の更新 - - wait(0.001); - - /*----------------------------------------------------------------*/ + float Pitch_before = (x_acc/sqrt((y_acc*y_acc+z_acc*z_acc)) ); + float Pitch = -atan(Pitch_before); + + qua[0]=Pitch*RAD_TO_DEG; + qua[1]=Roll*RAD_TO_DEG; + //float Acc_Pitch_Roll[2]; + + } + +void scan_update(float box_sensor[6]){ + + float a[3];//加速度の値 + float g[3];//ジャイロの値[rad/s] + + //int16_t raw_compass[3];//地磁気センサの値 + + mpu.setAcceleroRange(0);//acceleration range is +-4G + mpu.setGyroRange(0);//gyro rate is +-degree per second(dps) + + mpu.getAccelero(a);// 加速度を格納する配列[m/s2] a[0] is x axis,a[1] is y axis,a[2] is z axis; + mpu.getGyro(g);// 角速度を格納する配列[rad/s] + + //pc.printf("%f,%f,%f\r\n",g[0],g[1],g[2]); + + //compass.getXYZ(raw_compass);//地磁気の値を格納する配列 + + box_sensor[0]=((-g[0])-(GYRO_FIX_X/7505.7))*RAD_TO_DEG; + box_sensor[1]=((g[1])-(GYRO_FIX_Y/7505.7))*RAD_TO_DEG; + box_sensor[2]=((-g[2])-(GYRO_FIX_Z/7505.7))*RAD_TO_DEG; + + omega_z=box_sensor[2]; + + box_sensor[3]=a[0]; + box_sensor[4]=-a[1]; + box_sensor[5]=a[2]; + + //pc.printf("%f,%f,%f\r\n",box_sensor[0],box_sensor[1],box_sensor[2]); +} + +void flip(){ + + GPS_interruptIn++; + +}; + + +//ch1 +void Input_Aileron_1(){ + + first_period_old=ch_time.read(); + //pc.printf("ch1=%f",second_period); + }; + +void Input_Aileron_1_fall(){ + first_period=ch_time.read(); + aileron_duty=first_period-first_period_old; +}; + + +//ch2 +void Input_elevator(){ + second_period_old=ch_time.read(); +}; - if(switch_off==1){ - // Close the file which also flushes any cached writes - pc.printf("Closing \"/fs/numbers.txt\"... "); - err = fclose(f); - break; - } - - }//while(1) ends -}//main ends +void Input_elevator_fall(){ + second_period=ch_time.read(); + elevator_duty=second_period-second_period_old; + }; + + +//ch3 +void Input_throttle(){ + + first_period_old=ch_time.read(); + second_period_old=ch_time.read(); + third_period_old=ch_time.read(); + fourth_period_old=ch_time.read(); + fifth_period_old=ch_time.read(); + sixth_period_old=ch_time.read(); + + +} + +void Input_throttle_fall(){ + third_period=ch_time.read(); + + /*デコード*/ + throttle_duty=third_period-third_period_old; + //rudder_duty=fourth_period-fourth_period_old; + //elevator_duty=second_period-second_period_old; + //aileron_duty=first_period-first_period_old; + + //pc.printf("throttle_duty=%f:rudder_duty=%f:elevator_duty=%f\r\n",throttle_duty,rudder_duty,elevator_duty); +} + +//ch4 +void Input_rudder(){ + fourth_period_old=ch_time.read(); +} + +void Input_rudder_fall(){ + fourth_period=ch_time.read(); + rudder_duty=fourth_period-fourth_period_old; +} + +//ch5 +void Input_gear(){ + fifth_period_old=ch_time.read(); + +} + +void Input_gear_fall(){ + fifth_period=ch_time.read(); + switch_duty=fifth_period-fifth_period_old; +} + +//ch6 +void Input_Aileron_2(){ + sixth_period_old=ch_time.read(); + } + +void Input_Aileron_2_fall(){ + sixth_period=ch_time.read(); + }; + +//ch7 +void Input_pitch(){ + seventh_period_old=ch_time.read(); +} + +void Input_pitch_fall(){ + seventh_period=ch_time.read(); + } + +//ch8 +void Input_AUX5(){ + eighth_period_old=ch_time.read(); +} + +void Input_AUX5_fall(){ + eighth_period=ch_time.read(); +}