|
之前做電子設(shè)計(jì)競(jìng)賽做的定高算法,硬件是基于STM32F103的飛控,算法主要是串級(jí)PID,使用的是超聲波測(cè)距模塊。由于整個(gè)飛控工程比較大,現(xiàn)將核心算法貼出來(lái)供大家參考。
單片機(jī)源程序如下:
- #include "height_ctrl.h"
- #include "include.h"
- #include "mymath.h"
- extern float Thr_Weight;
- extern Gravity V;//重力分量
- /******************高度控制變量********************/
- float height_ctrl_out;
- float wz_acc;
- float keepheight_thr;
- /*-------------------------------------------------*/
- /******************超聲波變量********************/
- #define ULTRA_SPEED 300 // mm/s
- #define ULTRA_INT 300 // 積分幅度
- u8 lock=0;
- extern s8 ultra_start_f;
- extern float US100_Alt_delta;
- float exp_height_speed,exp_height;
- float ultra_speed,ultra_speed_acc;
- float ultra_dis_lpf;
- float ultra_ctrl_out;
- _st_height_pid_v ultra_ctrl;
- _st_height_pid ultra_pid;
- /*-------------------------------------------------*/
- /******************速度環(huán)變量********************/
- float wz_speed;
- float wz_acc_mms2;
- float tempacc_lpf;
- u8 lock_spd_crl=0;
- _st_height_pid_v wz_speed_pid_v;
- _st_height_pid wz_speed_pid;
- _st_height_pid ultra_wz_speed_pid;
- _st_height_pid baro_wz_speed_pid;
- /*-------------------------------------------------*/
- /******************氣壓計(jì)變量********************/
- float baro_height,baro_height_old;
- #define BARO_SPEED 300 // mm/s
- #define BARO_INT 300 // 積分幅度
- u8 lock_BARO=0;//定高鎖定當(dāng)前以及清零用
- #define BARO_SPEED_NUM 10
- float baro_speed_arr[BARO_SPEED_NUM + 1];
- #define ACC_SPEED_NUM 50
- float acc_speed_arr[ACC_SPEED_NUM + 1];
- float baro_dis_lpf,baro_dis_kalman;
- float baro_ctrl_out;
- _st_height_pid_v baro_ctrl;
- _st_height_pid baro_pid;
- u16 baro_cnt[2];
- u16 acc_cnt[2];
- float baro_speed,baro_speed_lpf;
- float baro_height;
- float Pressure_groud;
- /*-------------------------------------------------*/
- void Height_Ctrl(float T,float thr)
- {
- static u8 height_ctrl_start_f;
- static float thr_lpf;
- static float height_thr;
- static float wz_acc_temp,wz_acc1;
- static float hc_speed_i,wz_speed_0;
- static float h_speed;
-
- switch( height_ctrl_start_f )
- {
- case 0:
-
- if( sensor.acc.averag.z > 7000 )//注意這里,指水平靜態(tài)下的Z值要比這個(gè)大,表示水平垂直
- {
- height_ctrl_start_f = 1;
- }
- break;
-
- case 1:
-
- LockForKeepHigh(thr_lpf);//一旦切到控高模式,立即保存當(dāng)前的距離值,油門(mén)值、相關(guān)清零。
- if((flag.FlightMode==ULTRASONIC_High && lock==1 &&lock_spd_crl==0)|| (flag.FlightMode==ATMOSPHERE_High && lock_BARO==1 &&lock_spd_crl==0))
- {
- keepheight_thr=thr_lpf;
- wz_speed_0=0;
- hc_speed_i=0;
- hc_speed_i=0;
- wz_speed=0;
- wz_speed_pid_v.err_old=0;
- wz_speed_pid_v.err=0;
- wz_speed_pid_v.err_i=0;
- lock=2;
- lock_BARO=2;
- lock_spd_crl=1;
- }
- else if(flag.FlightMode==MANUAL_High)
- {
- lock=0;
- lock_BARO=0;
- lock_spd_crl=0;//切回手動(dòng)模式后置零,以使下次進(jìn)入定高模式是能順利置零速度內(nèi)環(huán)
- }
-
- height_thr = LIMIT( thr , 0, 700 );
- thr_lpf += ( 1 / ( 1 + 1 / ( 2.0f *3.14f *T ) ) ) *( height_thr - thr_lpf );//對(duì)油門(mén)值低通濾波修正
- userdata1[0]= thr_lpf;//調(diào)試用
- /*下面的低通濾波用于測(cè)試對(duì)比效果,最終沒(méi)有選用*/
- //wz_acc += ( 1 / ( 1 + 1 / ( 8 *3.14f *T ) ) ) *my_deathzoom( (V.z *(sensor.acc.averag.z- sensor.acc.quiet.z) + V.x *sensor.acc.averag.x + V.y *sensor.acc.averag.y - wz_acc),100 );//
- //wz_acc_mms2 = (float)(wz_acc/8192.0f) *9800 ;
- //加速度的靜態(tài)零點(diǎn)是讀取的EEPROM數(shù)據(jù),但是每次飛行都可能不一樣,能每次起飛前校準(zhǔn)一次最好,可以單獨(dú)校準(zhǔn)Z軸的,自行設(shè)計(jì)程序
- wz_acc_temp = V.z *(sensor.acc.averag.z- sensor.acc.quiet.z) + V.x *sensor.acc.averag.x + V.y *sensor.acc.averag.y;//
- Moving_Average( wz_acc_temp,acc_speed_arr,ACC_SPEED_NUM,acc_cnt ,&wz_acc1 );
- tempacc_lpf= (float)(wz_acc1/8192.0f) *9800;//9800 *T;由于是+-4G共8G,65535/8g=8192 g,加速度,mms2毫米每平方秒
- if(abs(tempacc_lpf)<50)tempacc_lpf=0;//簡(jiǎn)單消除下噪聲
- userdata1[2]=tempacc_lpf;
- wz_speed_0 += tempacc_lpf *T;//加速度計(jì)積分成速度
-
- if( ultra_start_f == 1 )////不管是啥模式,只要更新了超聲波數(shù)據(jù)就進(jìn)行運(yùn)算
- {
- Ultra_dataporcess(15.0f*TT);
- ultra_start_f=2;
- }
-
- if(baro_start_f==1)//不管是啥模式,只要更新了氣壓數(shù)據(jù)就進(jìn)行運(yùn)算
- {
- Baro_dataporcess(8.0f*TT); //8.0f*TT這么長(zhǎng)時(shí)間更新一次氣壓計(jì)數(shù)據(jù)
- userdata2[10]=baro_height;//Baro_Height_Source
- baro_dis_delta=baro_height-baro_dis_old;
- baro_dis_old=baro_height;//氣壓計(jì)速度可以繼續(xù)優(yōu)化,這里比較粗糙
- //Moving_Average( (float)( baro_dis_delta/(8.0f*TT)),baro_speed_arr,BARO_SPEED_NUM,baro_cnt ,&baro_speed ); //單位mm/s
- userdata1[11]=baro_speed_lpf=0.4* baro_dis_delta/(8.0f*TT);//baro_speed這里乘以系數(shù)以削減該值
- baro_start_f=2;
- }
-
- if(flag.FlightMode==ULTRASONIC_High)
- {
- h_speed=ultra_speed;
- wz_speed_0 = 0.99 *wz_speed_0 + 0.01*h_speed;//超聲波垂直速度互補(bǔ)濾波
- }
- else if(flag.FlightMode==ATMOSPHERE_High)
- {
- h_speed=baro_speed_lpf;
- wz_speed_0 = 0.99 *wz_speed_0 + 0.01*h_speed;//氣壓計(jì)垂直速度互補(bǔ)濾波,系數(shù)可調(diào)
- }
- userdata1[3] =h_speed;//h_speed是高度環(huán)傳到速度環(huán)的實(shí)測(cè)高度方向速度【但可能是錯(cuò)誤的,氣壓計(jì)速度不可靠】
- userdata1[4]=wz_speed_0;//調(diào)試用
- hc_speed_i += 0.4f *T *( h_speed - wz_speed );//速度偏差積分,乘以了0.4系數(shù)
- hc_speed_i = LIMIT( hc_speed_i, -500, 500 );//積分限幅
- userdata1[5] =hc_speed_i;//這個(gè)沒(méi)顯示
- wz_speed_0 += ( 1 / ( 1 + 1 / ( 0.1f *3.14f *T ) ) ) *( h_speed - wz_speed_0 ) ;//0.1實(shí)測(cè)速度修正加速度算的速度
- userdata1[6] = wz_speed=wz_speed_0 + hc_speed_i;//經(jīng)過(guò)修正的速度+經(jīng)過(guò)限幅的增量式速度積分
- if( flag.FlightMode == ATMOSPHERE_High)
- {
- if(baro_start_f==2)//說(shuō)明有新數(shù)據(jù)且被上面移動(dòng)均值濾波使用過(guò)了
- {
- baro_start_f = -1;
- Baro_Ctrl(8.0f*TT,thr_lpf);
- }
- height_speed_ctrl(T,keepheight_thr,baro_ctrl_out,baro_speed_lpf);
- }
- if( flag.FlightMode == ULTRASONIC_High)
- {
- height_speed_ctrl(T,keepheight_thr,ultra_ctrl_out,ultra_speed);//系數(shù)原來(lái)是0.4
-
- if( ultra_start_f == 2 )//超聲波數(shù)據(jù)更新了且被運(yùn)算了
- {
- Ultra_Ctrl(15.0f*TT,thr_lpf);//realtime是周期為 TT 秒的,#define TT 0.0025//控制周期2.5ms
- ultra_start_f = -1;
- }
- }
-
- /*******************************************控制高度輸出********************************************/
- if(flag.FlightMode==ULTRASONIC_High || flag.FlightMode==ATMOSPHERE_High || flag.FlightMode==ACC_High)//注意這里,模式
- {
- height_ctrl_out = wz_speed_pid_v.pid_out;
- }
- else
- {
- height_ctrl_out = thr;
- }
- userdata2[0]=exp_height;
-
- break;
- default: break;
- }
- }
- void WZ_Speed_PID_Init()
- {
- ultra_wz_speed_pid.kp = 0.25;// //超聲波定高的速度環(huán)PID,會(huì)被EEPROM里的重新賦值的
- ultra_wz_speed_pid.ki = 0.08; //0.12
- ultra_wz_speed_pid.kd = 8;
- baro_wz_speed_pid.kp = 0.1;// 氣壓定高的速度環(huán)PID,會(huì)被EEPROM里的重新賦值的
- baro_wz_speed_pid.ki = 0.06;
- baro_wz_speed_pid.kd = 8;//1.4*10,在計(jì)算式乘以了10,已刪除
- wz_speed_pid.kp = 0.1;//速度環(huán)默認(rèn)PID,真正用不到被重新賦值
- wz_speed_pid.ki = 0.08;
- wz_speed_pid.kd = 8;
- }
- void height_speed_ctrl(float T,float thr_keepheight,float exp_z_speed,float h_speed)
- {
- userdata1[1] = thr_keepheight;
- //wz_speed被用于參與超聲波高度D運(yùn)算
- userdata1[7]=wz_speed_pid_v.err = wz_speed_pid.kp *( exp_z_speed - wz_speed );//
- userdata1[8]=wz_speed_pid_v.err_d = wz_speed_pid.kd * (-tempacc_lpf) *T;//(wz_speed_pid_v.err - wz_speed_pid_v.err_old);
- //wz_speed_pid_v.err_i += wz_speed_pid.ki *wz_speed_pid_v.err *T;
- wz_speed_pid_v.err_i += wz_speed_pid.ki *( exp_z_speed - h_speed ) *T;//期望速度與實(shí)際速度誤差積分
- wz_speed_pid_v.err_i = LIMIT(wz_speed_pid_v.err_i,-Thr_Weight *200,Thr_Weight *200);
- userdata1[9]=wz_speed_pid_v.err_i;
- wz_speed_pid_v.pid_out = thr_keepheight + Thr_Weight *LIMIT((wz_speed_pid.kp *exp_z_speed + wz_speed_pid_v.err + wz_speed_pid_v.err_d + wz_speed_pid_v.err_i),-300,300);//積分原來(lái)沒(méi)有乘wz_speed_pid.kp
- userdata1[10]=wz_speed_pid_v.pid_out-thr_keepheight;
- wz_speed_pid_v.err_old = wz_speed_pid_v.err;
- }
- void Baro_PID_Init()
- {
- baro_pid.kp = 0.15;//氣壓定高的高度位置環(huán)PID,會(huì)被EEPROM里的重新賦值的
- baro_pid.ki = 0;
- baro_pid.kd = 1.5; //2.5
- }
- void Baro_dataporcess(float T)
- {
- float baro_dis_tmp;
-
- baro_dis_tmp = Moving_Median(2,5,baro_height);//對(duì)超聲波測(cè)量的距離進(jìn)行移動(dòng)中位值濾波
- if( baro_dis_tmp < Baro_MAX_Height )
- {
- if( ABS(baro_dis_tmp - baro_dis_lpf) < 100 )
- {
- baro_dis_lpf += ( 1 / ( 1 + 1 / ( 2.0f *3.14f *T ) ) ) *(baro_dis_tmp - baro_dis_lpf) ;
- }
- else if( ABS(baro_dis_tmp - baro_dis_lpf) < 500 )
- {
- baro_dis_lpf += ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) *(baro_dis_tmp- baro_dis_lpf) ;
- }
- else
- {
- baro_dis_lpf += ( 1 / ( 1 + 1 / ( 0.5f *3.14f *T ) ) ) *(baro_dis_tmp- baro_dis_lpf) ;
- }
- }
- userdata2[2]=baro_dis_lpf;//調(diào)試用
- userdata2[3]+= ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) *(Baro_Height_Source- userdata2[3]) ;//調(diào)試用
- userdata2[4]=baro_speed;//調(diào)試用
- }
- void Baro_Ctrl(float T,float thr)
- {
- exp_height_speed = BARO_SPEED *my_deathzoom_2(thr - 500,100)/400.0f; //20這里具體根據(jù)自己起飛油門(mén)定,油門(mén)控制高度+-ULTRA_SPEEDmm / s
- exp_height_speed = LIMIT(exp_height_speed ,-BARO_SPEED,BARO_SPEED);
-
- if( exp_height > Baro_MAX_Height )//限定高度,可以根據(jù)實(shí)際修改
- {
- if( exp_height_speed > 0 )
- {
- exp_height_speed = 0;
- }
- }
- else if( exp_height < Baro_MIN_Height )
- {
- if( exp_height_speed < 0 )
- {
- exp_height_speed = 0;
- }
- }
-
- exp_height += exp_height_speed *T;//累積期望高度,因?yàn)槠谕俣瓤赡芨淖兞?nbsp;
- baro_ctrl.err = baro_pid.kp*(exp_height - baro_dis_lpf);//baro_dis_lpfmm 對(duì)高度誤差乘以Kp
- userdata2[5]=baro_ctrl.err;
- baro_ctrl.err_i += baro_pid.ki *baro_ctrl.err *T;//對(duì)高度誤差積分
- userdata2[6]=baro_ctrl.err_i = LIMIT(baro_ctrl.err_i,-Thr_Weight *BARO_INT,Thr_Weight *BARO_INT);//積分限幅,融合了油門(mén)權(quán)重
- //對(duì)于D,融合了加速度計(jì)運(yùn)算得到的距離
- userdata2[7]=0.4f *(baro_ctrl.err - baro_ctrl.err_old);
- baro_ctrl.err_d = baro_pid.kd *( 0.6f *(-wz_speed*T) + 0.4f *(baro_ctrl.err - baro_ctrl.err_old) );
- userdata2[8]=baro_ctrl.err_d;
- baro_ctrl.pid_out = baro_ctrl.err + baro_ctrl.err_i + baro_ctrl.err_d;
- baro_ctrl.pid_out = LIMIT(baro_ctrl.pid_out,-300,300);
- userdata2[9]=baro_ctrl_out = baro_ctrl.pid_out;
- baro_ctrl.err_old = baro_ctrl.err;
- }
- void Ultra_PID_Init()
- {
- ultra_pid.kp = 0.5;//超聲波定高的高度位置環(huán)PID,會(huì)被EEPROM里的重新賦值的
- ultra_pid.ki = 0;
- ultra_pid.kd = 2.5; //2.5
- }
- void Ultra_dataporcess(float T)
- {
- float ultra_sp_tmp,ultra_dis_tmp;
- ultra_dis_tmp = Moving_Median(1,5,US100_Alt);//對(duì)超聲波測(cè)量的距離進(jìn)行移動(dòng)中位值濾波
- userdata2[1]=ultra_dis_tmp;
- if( ultra_dis_tmp < Ultrasonic_MAX_Height )
- {
- if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 100 )
- {
- ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 4.0f *3.14f *T ) ) ) *(ultra_dis_tmp - ultra_dis_lpf) ;
- }
- else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 200 )
- {
- ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 2.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
- }
- else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 400 )
- {
- ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 1.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
- }
- else
- {
- ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 0.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
- }
- }
-
- //注意超聲波測(cè)量的 距離的時(shí)效性問(wèn)題,避免反復(fù)被計(jì)算
- ultra_sp_tmp = Moving_Median(0,5,US100_Alt_delta/T); //對(duì)超聲波測(cè)距出來(lái)兩次之間距離差計(jì)算的速度中值濾波 ultra_delta/T;
- if( ultra_dis_tmp < Ultrasonic_MAX_Height )//小于3米,注意這里,萬(wàn)一超出了呢?
- {
- if( ABS(ultra_sp_tmp) < 100 )//運(yùn)動(dòng)速度小于100mm/s
- {
- ultra_speed += ( 1 / ( 1 + 1 / ( 4 *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
- }//ultra_speed會(huì)傳遞給速度環(huán),作為當(dāng)前速度的反饋,原來(lái)是4
- else
- {
- ultra_speed += ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
- }//系數(shù)越大作用越小,原來(lái)是1
- }
- }
- void Ultra_Ctrl(float T,float thr)
- {
- exp_height_speed = ULTRA_SPEED *my_deathzoom_2(thr - 500,100)/300.0f; //20這里具體根據(jù)自己起飛油門(mén)定,油門(mén)控制高度+-ULTRA_SPEEDmm / s
- exp_height_speed = LIMIT(exp_height_speed ,-ULTRA_SPEED,ULTRA_SPEED);
-
- if( exp_height > Ultrasonic_MAX_Height )//超出超聲波高度穩(wěn)定范圍則不執(zhí)行
- {
- if( exp_height_speed > 0 )
- {
- exp_height_speed = 0;
- }
- }
- else if( exp_height < Ultrasonic_MIN_Height )
- {
- if( exp_height_speed < 0 )
- {
- exp_height_speed = 0;
- }
- }
-
- exp_height += exp_height_speed *T;//累積期望高度,因?yàn)槠谕俣瓤赡芨淖兞?br />
- ultra_ctrl.err = ( ultra_pid.kp*(exp_height - ultra_dis_lpf) );//mm 對(duì)高度誤差乘以Kp
- userdata2[5]=ultra_ctrl.err;
- ultra_ctrl.err_i += ultra_pid.ki *ultra_ctrl.err *T;//對(duì)高度誤差積分
- userdata2[6]=ultra_ctrl.err_i = LIMIT(ultra_ctrl.err_i,-Thr_Weight *ULTRA_INT,Thr_Weight *ULTRA_INT);//積分限幅,融合了油門(mén)權(quán)重
- //對(duì)于D,融合了加速度計(jì)運(yùn)算得到的距離
- userdata2[7]=0.4f *(ultra_ctrl.err_old - ultra_ctrl.err);
- ……………………
- …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼
所有資料51hei提供下載:
height_ctrl.rar
(4.54 KB, 下載次數(shù): 97)
2019-10-21 20:52 上傳
點(diǎn)擊文件名下載附件
定高算法 下載積分: 黑幣 -5
|
評(píng)分
-
查看全部評(píng)分
|