ドローン用計測制御基板の作り方vol.2で使用したピッチ制御プログラムです。
Dependencies: mbed MPU6050_alter SDFileSystem
Diff: main.cpp
- Revision:
- 0:e647f6de3d26
diff -r 000000000000 -r e647f6de3d26 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 06 15:03:57 2020 +0000 @@ -0,0 +1,505 @@ +//================================================== +//Auto pilot(prototype2) +// +//MPU board: mbed LPC1768 +//Multiplexer TC74HC157AP +//Accelerometer +Gyro sensor : GY-521 +//2019/11/17 A.Toda +//================================================== +#include "mbed.h" +#include "MPU6050.h" +#include "SDFileSystem.h" + +#include "Vector.h" +#include "Matrix.h" +#include "Vector_Matrix_operator.h" + +#include "math.h" + +//================================================== +#define RAD_TO_DEG 57.2957795f // 180 / π +#define MAX_MEAN_COUNTER 500 + +#define ACC_X -1.205178682//offset of x-axi accelerometer +#define ACC_Y -0.141728488//offset of y-axi accelerometer +#define ACC_Z -0.339272785//offset of z-axi accelerometer + +#define ACC_GAIN_X 1.0 +#define ACC_GAIN_Y 0.995906146 +#define ACC_GAIN_Z 1.017766039 + +#define THRESHOLD_PWM 0.0015 +#define M_A_PWM 0.0017 +#define SERVO_PERIOD 0.020 + +#define PITCH_TARGET 0.0 + +/* +8/2/2020でのゲイン +#define P_P_GAIN 3.0 +#define P_I_GAIN 0.5 +#define P_D_GAIN 1.0 +*/ + +#define OFFSET_PWM_ELE 0.0015 +#define MAX_PWM_ELE 0.00175 +#define MIN_PWM_ELE 0.00125 + +#define LPF_ELE_K 0.110//この値が小さいとローパスフィルタが強くなる 0.050は小さすぎる + //0.200は動きが機敏過ぎる。 +//================================================== + +//Port Setting +SDFileSystem sd(p5, p6, p7, p8, "sd");//pins for sd slot +MPU6050 mpu(p9, p10); //Accelerometer + Gyro + //(SDA,SCLK) +DigitalIn logging_terminater(p16); +InterruptIn reading_port(p18); +DigitalOut mux_switch(p19); +PwmOut ELE(p21); + +Serial pc(USBTX, USBRX); //UART + +//Pointer of sd card +FILE *fp; + +//================================================== +//Accelerometer and gyro data +//================================================== +double acc[3]; //variables for accelerometer +double gyro[3]; //variables for gyro + +double offset_gyro_x=0.0; +double offset_gyro_y=0.0; + +double sum_gyro_x=0.0; +double sum_gyro_y=0.0; + +double threshold_acc,threshold_acc_ini; + +double dev =0.0; +double i_dev=0.0; +double d_dev=0.0; + +double old_i_dev=0.0; +//================================================== +//Atitude data +//================================================== +double roll_and_pitch_acc[2];//atitude from acceleromter +double roll_and_pitch[2];//atitude from gyro and acceleromter + +/*--------------------------行列、ベクトル-----------------------------*/ +Matrix rate_angle_matrix(4,4);//角速度行列 クォータニオンの更新に使う +Vector quaternion(4),pre_quaternion(4),dump_1(4);;//クォータニオン +/*-------------------------------------------------------------------*/ + +//================================================== +//Timer valiables +//================================================== +Timer ch_time;//timer for calculate pulse width +Timer passed_time;//timer for calculate atitude + +double measured_pre_pulse=0.0; +double measured_pulse=0.0; + +double time_new; +double time_old; + +double pulse_width_ele,deflection_ele,old_deflection_ele; + +//================================================= +//PID Gain +//================================================== +float P_P_GAIN,P_I_GAIN,P_D_GAIN; +int P_GAIN,I_GAIN,D_GAIN; + +//================================================= +//エレベータの舵角 +//================================================= +double pitch_command; + +//================================================= +//手動か自動かのインジケータ0なら手動、1なら自動 +//================================================= +int m_a_indicater; +//================================================= +//Functions for rising and falind edge interrution +//================================================= +//rise edge +void rising_edge(){ + ch_time.reset();//reset timer counter + measured_pre_pulse=ch_time.read(); + +} + +//falling edge +void falling_edge(){ + + measured_pre_pulse=(ch_time.read()-measured_pre_pulse); + //pc.printf("The pulse width=%f\r\n",measured_pre_pulse); + if(measured_pre_pulse>M_A_PWM){ + mux_switch=1; + m_a_indicater=1;//set indicater as auto + }else{ + mux_switch=0; + m_a_indicater=0;//set indicater as manual + } +} + +//terminate logging +void end_of_log(){ + //flipper.detach(); + fclose(fp);//close "Atitude_angles.csv" + pc.printf("Logging was terminated."); + + } +//================================================== +//Gyro and accelerometer functions +//================================================== +//get data +void aquisition_sensor_values(double *a,double *g){ + + float ac[3],gy[3]; + + mpu.getAccelero(ac);//get acceleration (Accelerometer) + //x_axis acc[0] + //y_axis acc[1] + //z_axis acc[2] + mpu.getGyro(gy); //get rate of angle(Gyro) + //x_axis gyro[0] + //y_axis gyro[1] + //z_axis gyro[2] + + //Invertion for direction of Accelerometer axis + ac[0]*=(-1.0); + ac[2]*=(-1.0); + + ac[0]=(ac[0]-ACC_X)/ACC_GAIN_X; + ac[1]=(ac[1]-ACC_Y)/ACC_GAIN_Y; + ac[2]=(ac[2]-ACC_Z)/ACC_GAIN_Z; + + //Unit convertion of rate of angle(radian to degree) + gy[0]*=RAD_TO_DEG; + gy[0]*=(-1.0); + + gy[1]*=RAD_TO_DEG; + gy[2]*=RAD_TO_DEG; + gy[2]*=(-1.0); + + for(int i=0;i<3;i++){ + a[i]=double(ac[i]); + g[i]=double(gy[i]); + } + g[0]-=offset_gyro_x;//offset rejection + g[1]-=offset_gyro_y;//offset rejection + + return; + +} + +//calculate offset of gyro +void offset_calculation_for_gyro(){ + + //Accelerometer and gyro setting + mpu.setAcceleroRange(0);//acceleration range is +-2G + mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps) + + //calculate offset of gyro + for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){ + aquisition_sensor_values(acc,gyro); + sum_gyro_x+=gyro[0]; + sum_gyro_y+=gyro[1]; + wait(0.01); + } + + offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER; + offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER; + + return; +} + +//atitude calculation from acceleromter +void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){ + + roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll + roll_and_pitch[1] = atan(a[0]/sqrt( (a[1]*a[1]+a[2]*a[2]) ) )*RAD_TO_DEG;//pitch + + return; +} + +//quaternion to euler +void quaternion_to_euler(Vector& qua,double *roll_and_pitch ){ + + double q0=double (qua.GetComp(1)); + double q1=double (qua.GetComp(2)); + double q2=double (qua.GetComp(3)); + double q3=double (qua.GetComp(4)); + + roll_and_pitch[0]=atan((q2*q3+q0*q1)/(q0*q0-q1*q1-q2*q2+q3*q3)) ;//roll + roll_and_pitch[1]=-asin(2*(q1*q3-q0*q2)); + + return; + } + +//quaternion to euler +void euler_to_quaternion(Vector& qua,double *roll_and_pitch ){ + + double roll_rad_2=(roll_and_pitch[0]/57.3)/2.0; + double pitch_rad_2=(roll_and_pitch[1]/57.3)/2.0; + double yaw_rad_2=0.0; + + float q0= cos(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2) + +sin(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2); + + float q1= sin(roll_rad_2)*cos(pitch_rad_2)*cos(yaw_rad_2) + -cos(roll_rad_2)*sin(pitch_rad_2)*sin(yaw_rad_2); + + float q2= cos(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2) + +sin(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2); + + float q3= cos(roll_rad_2)*cos(pitch_rad_2)*sin(yaw_rad_2) + -sin(roll_rad_2)*sin(pitch_rad_2)*cos(yaw_rad_2); + + //クォータニオン行列の作成(クォータニオンの演算に用いる) + float quaternion_elements[4]={q0,q1,q2,q3}; + qua.SetComps(quaternion_elements); + + + return; + } + +//atitude calculation +void atitude_update(){ + + //慣性センサの計測 + aquisition_sensor_values(acc,gyro); + //角速度行列の作成(クォータニオンの演算に用いる) + float rate_angles[16]={0.0,-float(gyro[0]),-float(gyro[1]),-float(gyro[2]) + ,float(gyro[0]),0.0,float(gyro[2]),-float(gyro[1]) + ,float(gyro[1]),-float(gyro[2]),0.0,float(gyro[0]) + ,float(gyro[0]),float(gyro[1]),-float(gyro[2]),0.0}; + + rate_angle_matrix.SetComps(rate_angles); + //クォータニオンの演算 + //pc.printf("クォータニオンの演算\r\n"); + float coefficents=0.5*(time_new-time_old); + float coefficents_elements[16]={coefficents,0.0,0.0,0.0 + ,0.0,coefficents,0.0,0.0 + ,0.0,0.0,coefficents,0.0 + ,0.0,0.0,0.0,coefficents}; + Vector coefficents_vector(4); + coefficents_vector.SetComps(coefficents_elements); + + dump_1=rate_angle_matrix*coefficents_vector; + quaternion=dump_1+pre_quaternion; + + //正規化 + //pc.printf("正規化\r\n"); + quaternion=quaternion.Normalize(); + //クォータニオンからオイラー角への変換 + quaternion_to_euler(quaternion,roll_and_pitch_acc ); + + threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); + + if((threshold_acc>=0.9*threshold_acc_ini) + &&(threshold_acc<=1.1*threshold_acc_ini)){ + + atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc); + roll_and_pitch[0] = 0.98*roll_and_pitch[0] + 0.02*roll_and_pitch_acc[0]; + roll_and_pitch[1] = 0.98*roll_and_pitch[1] + 0.02*roll_and_pitch_acc[1]; + + }else{} + //補正したオイラー角をクォータニオンへ変換 + euler_to_quaternion(pre_quaternion,roll_and_pitch_acc ); + + //microSDに記録する + //経過時間,ロール角,ピッチ角,操縦方式,慣性センサの値 + fprintf(fp, "%f,%f,%f,%d,%f,%f,%f,%f,%f,%f\r\n" + ,time_new,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater,gyro[0],gyro[1],acc[0],acc[1],acc[2],pitch_command); + + return; + +} + +//elevation commnad to PWM +double elevation_to_PWM(double elevation_command){ + + /* + PWM信号0.25msが舵角の16.45度に相当する. + */ + + double PWM_pitch = (elevation_command*1.519)/100000+ OFFSET_PWM_ELE; + //double PWM_pitch = ( ((elevation_command)*6.0/1000.0)/1000.0 )+ OFFSET_PWM_ELE; + + /*PWMコマンドの上限と下限の設定*/ + if(PWM_pitch>MAX_PWM_ELE){ + PWM_pitch=MAX_PWM_ELE; + + }else if(PWM_pitch<MIN_PWM_ELE){ + PWM_pitch=MIN_PWM_ELE; + } + + return PWM_pitch; + + } + +double deflection_of_ele(double pitch){ + + double add_deflection=((pitch-PITCH_TARGET)*6.0/1000.0)/1000.0; + + return add_deflection; + } + +//PID controller +double pitch_PID_controller(double pitch,double target,double gyro_pitch){ + + dev=target-pitch; + + //アンチワインドアップ + i_dev=old_i_dev+dev*(time_new-time_old); + if(i_dev>=25.0){ + i_dev=25.0; + }else if(i_dev<=-25.0){ + i_dev=-25.0; + } + //アンチワインドアップ終わり + + old_i_dev=i_dev; + + d_dev=-gyro_pitch; + + pitch_command = double(P_P_GAIN*dev+P_I_GAIN*i_dev+P_D_GAIN*d_dev); + + double pwm_command = elevation_to_PWM(pitch_command);//pwm信号に変換 + + return pwm_command; + + } + +//LPF + +double LPF_pitch(double c_com,double old_com){ + + double lpf_output=(1-LPF_ELE_K)*old_com+LPF_ELE_K*c_com; + + return lpf_output; + } + +//================================================== +//Main +//================================================== +int main() { + + wait(5.0); + + //UART initialization + pc.baud(115200); + + //define servo period + ELE.period(SERVO_PERIOD); // servo requires a 20ms period + pulse_width_ele=0.0015; + + //timer starts + ch_time.start(); + passed_time.start(); + + time_old=0.0; + + //declare interrupitons + reading_port.rise(rising_edge); + reading_port.fall(falling_edge); + + /* + mux_switch=0;//set circit as manual mode + m_a_indicater=0;//set indicater as manual + */ + + //gyro and accelerometer initialization + offset_calculation_for_gyro(); + + //determine initilal atitude + aquisition_sensor_values(acc,gyro); + atitude_estimation_from_accelerometer(acc,roll_and_pitch); + euler_to_quaternion(pre_quaternion,roll_and_pitch); + + threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); + + /* + PIDゲインをsdカードのテキストファイルから読み取る + int型しか読みとれないので、希望する値の10倍をテキストファイルに書き込んでいる。 + Pゲインを3.0,Iゲインを0.5,Dゲインを1.0とする場合 + + 30,5,10 + + のようにテキストデータをsdカードに用意する。 + */ + //open PID gain file in sd card + FILE*ga = fopen("/sd/Gain_data.txt", "r"); + if(ga == NULL) { + error("Could not open file for write\n"); + } + + while (!feof(ga)) { + int n = fscanf(ga, "%d,%d,%d",&P_GAIN, &I_GAIN, &D_GAIN); + + if(n!= 3){ + error("Could not read 3 elements"); + } + } + + P_P_GAIN=P_GAIN/10.0; + P_I_GAIN=I_GAIN/10.0; + P_D_GAIN=D_GAIN/10.0; + + fclose(ga); + + //create folder(sd) in sd card + mkdir("/sd", 0777); + //create "Atitude_angles.csv" in folder(sd) + fp = fopen("/sd/Atitude_angles.csv", "a");//ファイルがある場合は追加で書き込み + + if(fp == NULL) { + error("Could not open file for write\n"); + } + + //Logging starts + pc.printf("Logging starts."); + + //write PID gain on sd card + fprintf(fp,"%f,%f,%f\r\n",P_P_GAIN,P_I_GAIN,P_D_GAIN); + + //while + while(1){ + + if(logging_terminater==1){ + end_of_log(); + }else{} + + time_new=passed_time.read(); + + atitude_update(); + + time_old=time_new; + + /* + ここから先でサーボの操舵角を姿勢角に応じて変化させる。関数pitch_PID_controller + はpitch角とpitch角速度に応じてサーボのパルス幅を返す関数である。 + */ + + deflection_ele = pitch_PID_controller(roll_and_pitch[1],PITCH_TARGET,gyro[1]); + + //LPF + deflection_ele = LPF_pitch(deflection_ele,old_deflection_ele); + old_deflection_ele = deflection_ele; + + //servo output + ELE.pulsewidth(deflection_ele); + + //PCにつないでデバッグを行う際に表示する + pc.printf("%f,%f,%f,%f,%d\r\n",time_new,deflection_ele,roll_and_pitch[0],roll_and_pitch[1],m_a_indicater); + + wait(0.002); + + }//while ends + +}//main ends \ No newline at end of file