STM32F407飛控源碼超聲波定高與循跡功能

ID:307186 · 發(fā)表于 2018-4-13 10:58

STM32F407單片機的超聲波定高循跡飛控

APB1 42m
APB2 84m

TIMx clk = APBx *2

中斷優(yōu)先級：

第0組：所有4位用于指定響應優(yōu)先級；
第1組：最高1位用于指定搶占式優(yōu)先級，最低3位用于指定響應優(yōu)先級；
第2組：最高2位用于指定搶占式優(yōu)先級，最低2位用于指定響應優(yōu)先級；
第3組：最高3位用于指定搶占式優(yōu)先級，最低1位用于指定響應優(yōu)先級；
第4組：所有4位用于指定搶占式優(yōu)先級。
|先搶占，再響應|
1）如果指定的搶占式優(yōu)先級別或響應優(yōu)先級別超出了選定的優(yōu)先級分組所限定的范圍，將可能得到意想不到的結(jié)果；
2）搶占式優(yōu)先級別相同的中斷源之間沒有嵌套關系；
3）如果某個中斷源被指定為某個搶占式優(yōu)先級別，又沒有其它中斷源處于同一個搶占式優(yōu)先級別，則可以為這個中斷源指定任意有效的響應優(yōu)先級別。

事件記錄，beta0.4開始。

2015.5.18
beta 0.4  修復各種bug。

2015.5.19
beta 0.5  完善搖桿識別。

beta 0.6  pwm_in優(yōu)化

beta 0.7  修復bug，優(yōu)化rc，添加mapping

beta 0.9  起飛成功
beta 1.0  修復控制周期bug

beta 1.1  添加部分注釋

beta 1.2  優(yōu)化整定參數(shù)，修改bug

beta 1.3  完成flash存儲，上位機HID通信

beta 1.35 增加磁力計糾正航向

beta 1.36 增加磁力計校準時燈管提示

beta 1.37 增加mpu6050，ak8975，ms5611自檢，未通過的LED狀態(tài)分別是：間隔連續(xù)閃爍1次；間隔連續(xù)閃爍2次；間隔連續(xù)閃爍3次

beta 1.37.1 增加滑動窗口平均濾波函數(shù)，增加ks103超聲波讀取，該版?zhèn)浞荨?br />
beta 1.37.2 添加user數(shù)據(jù)，上位機顯示波形,串口超聲波改發(fā)送中斷。

beta 1.37.3 添加部分注釋，修改部分變量名，增加ano_private.lib。

beta 1.38 添加超聲波控制代碼（注意！暫未測試），修改磁力計糾正算法（重要更新），消除磁力計糾正YAW時對姿態(tài)的干擾。刪除ano_pribate.lib

beta 1.39 修復上一版數(shù)據(jù)傳輸一個小bug，添加環(huán)形緩存。

beta 1.39.1 超聲波直接控制高度，還有問題。

beta 1.40 超聲波定高ok，暫沒優(yōu)化參數(shù)，沒加上位機調(diào)參�？筛膽T導豎直方向速率控制。

beta 1.40.1
#define CTRL_HEIGHT 1    //0失能，1使能
#define HEIGHT_MODE 1    //0,無，1慣性控制高度，2氣壓計控制(空)，3超聲波控制高度

beta 1.41 修復磁力計糾正的一個bug。

beta 1.42 us100超聲波定高穩(wěn)定

beta 1.43
1.模式0（呼吸白，解鎖綠長），普通，油門直接輸出。模式1（呼吸淺綠，解鎖淺綠長），垂直速率控制（下一步改為氣壓定高）。模式2（呼吸紫，解鎖紫長），超聲波定高。
2.飛行時，只能向模式0切換，其他切換無效。
3.只有插上超聲波，模式3才有效。（這個未優(yōu)化，比如臨時拔下超聲波，也能進入該模式。）

beta 1.44
修復定高控制中積分的bug，優(yōu)化超聲波定高，優(yōu)化高度設定值調(diào)節(jié)速度。增加超聲波最大高度，可達到到1.5米比較穩(wěn)定（高度越低越穩(wěn)定）。

beta 1.46
氣壓計定高，測試版（無高度靜差修正）。

beta 1.47
氣壓計定高，測試版（無高度靜差修正）。改進磁力計糾正算法，解決磁力計糾正時對姿態(tài)的干擾，修正磁力計糾正中180度的一個錯誤。

beta 1.48
修復氣壓計速率的一個bug，并調(diào)整可控制的速率。

beta 1.48.1
注釋掉while(1);

beta 1.49 電賽版本，完善通信協(xié)議中串口接收通道數(shù)據(jù)部分。

beta 1.49.2 修改氣壓計定高速率環(huán)積分部分，優(yōu)化定高效果。

beta 1.49.3 濾波算法測試。

beta 1.50 修改磁力計糾正方法

beta 1.50.2 增加羅盤傾斜糾正，修復數(shù)據(jù)傳輸一個bug。

beta 1.51 修改氣壓計融合，定高控制等幾處小bug

beta 1.52 修改超聲波定高一個小bug

beta 1.6 細節(jié)優(yōu)化&新參數(shù)

beta 1.6.2 修正羅盤原始數(shù)據(jù)的一個錯誤。

beta 1.6.2_fix 上版漏掉一句代碼，導致解鎖后yaw混亂，I'm sorry。。。

beta 1.6.3 新融合策略，解決加速度零點漂移（水平姿態(tài)）。

STM32F407單片機源程序如下:

/****************************************************************
@本程序只供學習使用，未經(jīng)作者許可，不得用于其它任何用途
+編者：普哥
+描述：飛控核心控制程序（PID控制）
+編寫日期：2017/3/10
+版權(quán)所有，盜版必糾。
****************************************************************/
#include "ctrl.h"
#include "height_ctrl.h"
#include "search.h"
ctrl_t ctrl_1;//內(nèi)環(huán)，角速度控制結(jié)構(gòu)體
ctrl_t ctrl_2;//外環(huán)，角度(姿態(tài))控制結(jié)構(gòu)體
/*-----------------------------------------------
+實現(xiàn)功能：恢復默認控制幅度
-------------------------------------------------*/
void Ctrl_Para_Init() //設置默認參數(shù)
{
/* 微分控制幅度*/
ctrl_1.PID[PIDROLL].kdamp = 1;
ctrl_1.PID[PIDPITCH].kdamp = 1;
ctrl_1.PID[PIDYAW].kdamp = 1;
/* 角速度的偏差權(quán)重比例系數(shù)*/
ctrl_1.FB = 0.20; //外 0<fb<1
}
xyz_f_t except_A = {0,0,0};//遙控控制期望姿態(tài)角度
xyz_f_t ctrl_angle_offset = {0,0,0};//期望姿態(tài)角度偏移
xyz_f_t compensation;
/*----------------------------------------------------------
+ 實現(xiàn)功能：姿態(tài)PID控制角速度由任務調(diào)度調(diào)用周期5ms
+ 調(diào)用參數(shù)：兩次調(diào)用間隔
----------------------------------------------------------*/
void CTRL_2(float T) //外環(huán) 角度控制
{
/* 期望角度遙控器控制量中心死區(qū)30 單位微秒*/
except_A.x = MAX_CTRL_ANGLE *( my_deathzoom( ( CH_filter[ROL]) ,30 )/500.0f );
except_A.y = MAX_CTRL_ANGLE *( my_deathzoom( (-CH_filter[PIT]) ,30 )/500.0f );
if( Thr_Low == 0 )//遙控器控制量的油門拉起
{
/* 期望航向姿態(tài)由遙控器航向控制積分*/
except_A.z += (s16)( MAX_CTRL_YAW_SPEED *( my_deathzoom_2( (CH_filter[YAW]) ,40 )/500.0f ) ) *T ;
}
else //遙控器控制量的油門低
{
except_A.z += 1 *3.14 *T *( Yaw - except_A.z );//期望航向姿態(tài)為當前姿態(tài)
}
except_A.z = To_180_degrees(except_A.z);//角度范圍控制在+-180角度
/* 得到角度誤差為姿態(tài)自穩(wěn) 與遙控器控制量之和 */
ctrl_2.err.x = To_180_degrees( ctrl_angle_offset.x + except_A.x - Roll );
ctrl_2.err.y = To_180_degrees( ctrl_angle_offset.y + except_A.y - Pitch );
ctrl_2.err.z = To_180_degrees( ctrl_angle_offset.z + except_A.z - Yaw );
/* 計算角度誤差權(quán)重 */
ctrl_2.err_weight.x = ABS(ctrl_2.err.x)/ANGLE_TO_MAX_AS;
ctrl_2.err_weight.y = ABS(ctrl_2.err.y)/ANGLE_TO_MAX_AS;
ctrl_2.err_weight.z = ABS(ctrl_2.err.z)/ANGLE_TO_MAX_AS;
/* 角度誤差微分（跟隨誤差曲線變化）*/
ctrl_2.err_d.x = 10 *ctrl_2.PID[PIDROLL].kd *(ctrl_2.err.x - ctrl_2.err_old.x) *( 0.005f/T ) *( 0.65f + 0.35f *ctrl_2.err_weight.x );
ctrl_2.err_d.y = 10 *ctrl_2.PID[PIDPITCH].kd *(ctrl_2.err.y - ctrl_2.err_old.y) *( 0.005f/T ) *( 0.65f + 0.35f *ctrl_2.err_weight.y );
ctrl_2.err_d.z = 10 *ctrl_2.PID[PIDYAW].kd *(ctrl_2.err.z - ctrl_2.err_old.z) *( 0.005f/T ) *( 0.65f + 0.35f *ctrl_2.err_weight.z );
/* 角度誤差積分 */
ctrl_2.err_i.x += ctrl_2.PID[PIDROLL].ki *ctrl_2.err.x *T;
ctrl_2.err_i.y += ctrl_2.PID[PIDPITCH].ki *ctrl_2.err.y *T;
ctrl_2.err_i.z += ctrl_2.PID[PIDYAW].ki *ctrl_2.err.z *T;
/* 角度誤差積分分離 */
ctrl_2.eliminate_I.x = Thr_Weight *CTRL_2_INT_LIMIT;
ctrl_2.eliminate_I.y = Thr_Weight *CTRL_2_INT_LIMIT;
ctrl_2.eliminate_I.z = Thr_Weight *CTRL_2_INT_LIMIT;
/* 角度誤差積分限幅 */
ctrl_2.err_i.x = LIMIT( ctrl_2.err_i.x, -ctrl_2.eliminate_I.x,ctrl_2.eliminate_I.x );
ctrl_2.err_i.y = LIMIT( ctrl_2.err_i.y, -ctrl_2.eliminate_I.y,ctrl_2.eliminate_I.y );
ctrl_2.err_i.z = LIMIT( ctrl_2.err_i.z, -ctrl_2.eliminate_I.z,ctrl_2.eliminate_I.z );
/* 對用于計算比例項輸出的角度誤差限幅 */
ctrl_2.err.x = LIMIT( ctrl_2.err.x, -90, 90 );
ctrl_2.err.y = LIMIT( ctrl_2.err.y, -90, 90 );
ctrl_2.err.z = LIMIT( ctrl_2.err.z, -90, 90 );
/* 角度PID輸出 */
ctrl_2.out.x = ctrl_2.PID[PIDROLL].kp *( ctrl_2.err.x + ctrl_2.err_d.x + ctrl_2.err_i.x ); //rol
ctrl_2.out.y = ctrl_2.PID[PIDPITCH].kp *( ctrl_2.err.y + ctrl_2.err_d.y + ctrl_2.err_i.y ); //pit
ctrl_2.out.z = ctrl_2.PID[PIDYAW].kp *( ctrl_2.err.z + ctrl_2.err_d.z + ctrl_2.err_i.z );
/* 記錄歷史數(shù)據(jù) */
ctrl_2.err_old.x = ctrl_2.err.x;
ctrl_2.err_old.y = ctrl_2.err.y;
ctrl_2.err_old.z = ctrl_2.err.z;
}
xyz_f_t except_AS;//期望角速度
float g_old[ITEMS];//記錄的陀螺儀數(shù)據(jù)
/*----------------------------------------------------------
+ 實現(xiàn)功能：角速度電機輸出量由任務調(diào)度調(diào)用周期2ms
+ 調(diào)用參數(shù)：兩次調(diào)用間隔
----------------------------------------------------------*/
void CTRL_1(float T) //x roll,y pitch,z yaw內(nèi)環(huán) 角速度控制
{
xyz_f_t EXP_LPF_TMP;//期望角速度
/* 給期望（目標）角速度 */
EXP_LPF_TMP.x = MAX_CTRL_ASPEED *(ctrl_2.out.x/ANGLE_TO_MAX_AS);
EXP_LPF_TMP.y = MAX_CTRL_ASPEED *(ctrl_2.out.y/ANGLE_TO_MAX_AS);
EXP_LPF_TMP.z = MAX_CTRL_ASPEED *(ctrl_2.out.z/ANGLE_TO_MAX_AS);
/* 期望角速度*/
except_AS.x = EXP_LPF_TMP.x;
except_AS.y = EXP_LPF_TMP.y;
except_AS.z = EXP_LPF_TMP.z;
/* 期望角速度限幅 */
except_AS.x = LIMIT(except_AS.x, -MAX_CTRL_ASPEED,MAX_CTRL_ASPEED ); //-300到300之間
except_AS.y = LIMIT(except_AS.y, -MAX_CTRL_ASPEED,MAX_CTRL_ASPEED );
except_AS.z = LIMIT(except_AS.z, -MAX_CTRL_ASPEED,MAX_CTRL_ASPEED );
/* 角速度直接微分（角加速度），負反饋可形成角速度的阻尼（阻礙角速度的變化）*/
ctrl_1.damp.x = ( mpu6050.Gyro_deg.x - g_old[A_X]) *( 0.002f/T );
ctrl_1.damp.y = (-mpu6050.Gyro_deg.y - g_old[A_Y]) *( 0.002f/T );
ctrl_1.damp.z = (-mpu6050.Gyro_deg.z - g_old[A_Z]) *( 0.002f/T );
/* 角速度誤差 */
ctrl_1.err.x = ( except_AS.x - mpu6050.Gyro_deg.x ) *(300.0f/MAX_CTRL_ASPEED);
ctrl_1.err.y = ( except_AS.y + mpu6050.Gyro_deg.y ) *(300.0f/MAX_CTRL_ASPEED); //-y
ctrl_1.err.z = ( except_AS.z + mpu6050.Gyro_deg.z ) *(300.0f/MAX_CTRL_ASPEED); //-z
/* 角速度誤差權(quán)重 */
ctrl_1.err_weight.x = ABS(ctrl_1.err.x)/MAX_CTRL_ASPEED;
ctrl_1.err_weight.y = ABS(ctrl_1.err.y)/MAX_CTRL_ASPEED;
ctrl_1.err_weight.z = ABS(ctrl_1.err.z)/MAX_CTRL_YAW_SPEED;
/* 角速度微分 */
ctrl_1.err_d.x = ( ctrl_1.PID[PIDROLL].kd *( -10 *ctrl_1.damp.x) *( 0.002f/T ) );
ctrl_1.err_d.y = ( ctrl_1.PID[PIDPITCH].kd *( -10 *ctrl_1.damp.y) *( 0.002f/T ) );
ctrl_1.err_d.z = ( ctrl_1.PID[PIDYAW].kd *( -10 *ctrl_1.damp.z) *( 0.002f/T ) );
/* 角速度誤差積分 */
ctrl_1.err_i.x += ctrl_1.PID[PIDROLL].ki *(ctrl_1.err.x - ctrl_1.damp.x) *T;
ctrl_1.err_i.y += ctrl_1.PID[PIDPITCH].ki *(ctrl_1.err.y - ctrl_1.damp.y) *T;
ctrl_1.err_i.z += ctrl_1.PID[PIDYAW].ki *(ctrl_1.err.z - ctrl_1.damp.z) *T;
/* 角速度誤差積分分離 */
ctrl_1.eliminate_I.x = Thr_Weight *CTRL_1_INT_LIMIT ;
ctrl_1.eliminate_I.y = Thr_Weight *CTRL_1_INT_LIMIT ;
ctrl_1.eliminate_I.z = Thr_Weight *CTRL_1_INT_LIMIT ;
/* 角速度誤差積分限幅 */
ctrl_1.err_i.x = LIMIT( ctrl_1.err_i.x, -ctrl_1.eliminate_I.x,ctrl_1.eliminate_I.x );
ctrl_1.err_i.y = LIMIT( ctrl_1.err_i.y, -ctrl_1.eliminate_I.y,ctrl_1.eliminate_I.y );
ctrl_1.err_i.z = LIMIT( ctrl_1.err_i.z, -ctrl_1.eliminate_I.z,ctrl_1.eliminate_I.z );
/* 角速度PID輸出 */
ctrl_1.out.x = 3 *( ctrl_1.FB *LIMIT((0.45f + 0.55f*ctrl_2.err_weight.x),0,1)*except_AS.x + ( 1 - ctrl_1.FB ) *ctrl_1.PID[PIDROLL].kp *( ctrl_1.err.x + ctrl_1.err_d.x + ctrl_1.err_i.x ) );
ctrl_1.out.y = 3 *( ctrl_1.FB *LIMIT((0.45f + 0.55f*ctrl_2.err_weight.y),0,1)*except_AS.y + ( 1 - ctrl_1.FB ) *ctrl_1.PID[PIDPITCH].kp *( ctrl_1.err.y + ctrl_1.err_d.y + ctrl_1.err_i.y ) );
ctrl_1.out.z = 3 *( ctrl_1.FB *LIMIT((0.45f + 0.55f*ctrl_2.err_weight.z),0,1)*except_AS.z + ( 1 - ctrl_1.FB ) *ctrl_1.PID[PIDYAW].kp *( ctrl_1.err.z + ctrl_1.err_d.z + ctrl_1.err_i.z ) );
Thr_Ctrl(T);// 電機油門量控制
All_Out(ctrl_1.out.x,ctrl_1.out.y,ctrl_1.out.z);//角速度控制量轉(zhuǎn)換到電機轉(zhuǎn)速的輸出量
/* 記錄角速度誤差積分*/
ctrl_1.err_old.x = ctrl_1.err.x;
ctrl_1.err_old.y = ctrl_1.err.y;
ctrl_1.err_old.z = ctrl_1.err.z;
/* 記錄角速度數(shù)據(jù)*/
g_old[A_X] = mpu6050.Gyro_deg.x ;
g_old[A_Y] = -mpu6050.Gyro_deg.y ;
g_old[A_Z] = -mpu6050.Gyro_deg.z ;
}
float thr_value;//油門賦值
u8 Thr_Low;//低油門信號判斷
float Thr_Weight; //濾波后油門數(shù)據(jù)
/*----------------------------------------------------------
+ 實現(xiàn)功能：油門信號控制
+ 調(diào)用參數(shù)：兩次調(diào)用間隔
----------------------------------------------------------*/
void Thr_Ctrl(float T)
{
static float thr;//油門值
static float Thr_tmp;//濾波后的油門值
thr = 500 + CH_filter[THR]; //油門值范圍 0 ~ 1000
Thr_tmp += 10 *3.14f *T *(thr/400.0f - Thr_tmp); //低通濾玻，油門數(shù)據(jù)積分濾波
Thr_Weight = LIMIT(Thr_tmp,0,1);// 濾波后油門數(shù)據(jù)限幅,后邊多處分離數(shù)據(jù)會用到這個值
if( thr < 100 )//低油門信號判斷
{
Thr_Low = 1;
}
else
{
Thr_Low = 0;
}
/* 允許高度控制*/
#if(CTRL_HEIGHT)
Height_Ctrl(T,thr);//油門控制高度
thr_value = Thr_Weight *height_ctrl_out; //油門賦值
#else
thr_value = thr; //實際使用值
#endif
thr_value = LIMIT(thr_value,0,10 *MAX_THR *MAX_PWM/100);//油門限幅
}
float motor[MAXMOTORS];//電機數(shù)量
float posture_value[MAXMOTORS];//姿態(tài)作用于電機位置的控制量
float curve[MAXMOTORS];//姿態(tài)對電機的實際控制量
/*----------------------------------------------------------
+ 實現(xiàn)功能：角速度控制量轉(zhuǎn)換到電機轉(zhuǎn)速的輸出量
+ 調(diào)用參數(shù)：角速度控制量
----------------------------------------------------------*/
void All_Out(float out_roll,float out_pitch,float out_yaw)
{
s16 motor_out[MAXMOTORS];
u8 i;//循環(huán)計數(shù)變量
float posture_value[MAXMOTORS];//姿態(tài)作用于電機位置的控制量
float curve[MAXMOTORS];//姿態(tài)對電機的實際控制量
float Line_Out=0,Line_LR_Out=0;//尋線PID輸出
float Point_X=0,Point_Y=0;//定點PID輸出
/* 允許循跡控制*/
#if(CTRL_SEARCH)
Line_Out = Line_ctrl_out; //循跡偏移PID輸出
Line_LR_Out = Line_LR_ctrl_out; //循跡偏移PID輸出
Point_X = Point_X_ctrl_out; //左右定點PID輸出
Point_Y = Point_Y_ctrl_out; //前后定點PID輸出
#else
Line_Out = 0
Line_LR_Out = 0
Point_X = 0
Point_Y = 0
#endif
/* 航向的角速度控制量限幅防止動力不足*/
out_yaw = LIMIT( out_yaw , -5*MAX_THR ,5*MAX_THR ); //50%
/*默認是四旋翼X模式，CCW是從上方向下看逆時針選擇，CW與CCW剛好相反,
注意前方偏右是連接1號電機電調(diào)，逆時針順序依次是2開始
CW2 1CCW
\ /
/ \
CCW3 4CW */
posture_value[0] = - out_roll + out_pitch + out_yaw - Line_Out -Point_X -Point_Y -Line_LR_Out ;//右前方，CCW
posture_value[1] = + out_roll + out_pitch - out_yaw + Line_Out +Point_X -Point_Y -Line_LR_Out ;//左前方，CW
posture_value[2] = + out_roll - out_pitch + out_yaw + Line_Out +Point_X +Point_Y +Line_LR_Out;//左后方，CCW
posture_value[3] = - out_roll - out_pitch - out_yaw - Line_Out -Point_X +Point_Y +Line_LR_Out ;//右后方，CW
for(i=0;i<4;i++)//作用于電機位置的控制量
{
posture_value[i] = LIMIT(posture_value[i], -1000,1000 );//姿態(tài)作用于電機位置的控制量限幅
}
/* 姿態(tài)對電機的實際控制量*/
curve[0] = (0.55f + 0.45f *ABS(posture_value[0])/1000.0f) *posture_value[0] ;
curve[1] = (0.55f + 0.45f *ABS(posture_value[1])/1000.0f) *posture_value[1] ;
curve[2] = (0.55f + 0.45f *ABS(posture_value[2])/1000.0f) *posture_value[2] ;
curve[3] = (0.55f + 0.45f *ABS(posture_value[3])/1000.0f) *posture_value[3] ;
/* 單個電機的總控制量*/
motor[0] = thr_value + Thr_Weight *curve[0] ;
motor[1] = thr_value + Thr_Weight *curve[1] ;
motor[2] = thr_value + Thr_Weight *curve[2] ;
motor[3] = thr_value + Thr_Weight *curve[3] ;
/* 是否解鎖 */
if(fly_ready)//已經(jīng)解鎖
{
if( !Thr_Low ) //遙控器的控制量油門拉起
{
for(i=0;i<4;i++)
{
/* 保證大于在電機最小起轉(zhuǎn)轉(zhuǎn)速*/
motor[i] = LIMIT(motor[i], (10 *READY_SPEED),(10*MAX_PWM) );
}
}
else //遙控器控制量的油門低
{
for(i=0;i<4;i++)
{
motor[i] = LIMIT(motor[i], 0,(10*MAX_PWM) );
}
}
}
else//未解鎖
{
for(i=0;i<4;i++)//電機停轉(zhuǎn)
{
motor[i] = 0;
}
}
/* int16到float數(shù)據(jù)類型轉(zhuǎn)換*/
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………