写了一大堆，也不知道对不对，贴上来让大家看看
#include "Ano_AltCtrl.h" //高度控制
#include "Ano_Imu.h"
#include "Drv_icm20602.h"
#include "Ano_MagProcess.h"
#include "Drv_spl06.h"
#include "Ano_MotionCal.h" //运动计算
#include "Ano_FlightCtrl.h" //飞行控制
#include "Ano_MotorCtrl.h" //电机控制
#include "Ano_AttCtrl.h" //姿态控制
#include "Ano_LocCtrl.h" //位置控制
//自动起飞速度
static s16 auto_taking_off_speed;
#define AUTO_TAKE_OFF_KP 2.0f
extern _filter_1_st wz_spe_f1;
/**************************************************************************************************
***************************************************************************************************
**************************************************************************************************/
//硬生生没看懂这个一键起飞任务，只能盲目翻译了下
//自动启飞降落任务
void Auto_Take_Off_Land_Task(u8 dT_ms)
{
static u16 take_off_ok_cnt;
//执行一键起飞任务
one_key_take_off_task(dT_ms);
if(flag.fly_ready) //如果已经准备好了
{
if(flag.taking_off) //如果可以起飞
{
if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL) //如果尚未自动启飞
{
flag.auto_take_off_land = AUTO_TAKE_OFF; //起飞
}
}
}
else
{
auto_taking_off_speed = 0;
flag.auto_take_off_land = AUTO_TAKE_OFF_NULL;
}
if(flag.auto_take_off_land ==AUTO_TAKE_OFF) //接上，如果起飞，
{
take_off_ok_cnt += dT_ms; //延迟等待至2500
auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);
auto_taking_off_speed = LIMIT(auto_taking_off_speed,0,150);
//得到速度并进行限幅
if(take_off_ok_cnt>=2500 || (Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out <5))//(auto_ref_height>AUTO_TAKE_OFF_HEIGHT)
{ //如果cnt>2500或者是期望高度和实际高度误差在允许范围内
flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH; //自动启飞完成
}
if(take_off_ok_cnt >1000 && ABS(fs.speed_set_h_norm[Z])>0.1f)// 一定已经taking_off
{
flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH; //自动启飞完成
}
}
else
{
take_off_ok_cnt = 0;
if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH) //接上 如果自动启飞完成
{
auto_taking_off_speed = 0; //起飞速度为0
}
}
if(flag.auto_take_off_land == AUTO_LAND) //如果自动降落
{
auto_taking_off_speed = -60; //减速
}
}
/**************************************************************************************************
***************************************************************************************************
**************************************************************************************************/
//Altitude 高度
//Attitude 姿态
//区分清楚
// 2->外环高度环
// 1->内环高度速度环
/*此部分也是使用串级PID，外环是高度环，内环是高度速度环，和角度环和角速度环进行对比
很容易分析清楚，只不过那个是三个参数，这个只有一个参数。
很明显，这个串级PID对应的是油门 Thr，在上一篇中讲到过，四个通道应该分别对应一个PID
其中三个欧拉角每个对应一个通道，唯独缺了油门，于是乎，高度环与高度速度环补上了这个遗憾
*/
//同样是来自文件"Ano_Pid.h"中的结构体，一看到这里，基本上我们就可以确定后面必然有"Ano_Pid.c"
//中的PID函数，这个结构体定义为PID函数提供了两个入口参数，起到初始化作用，和上一篇介绍串级PID
//完全一个套路
_PID_arg_st alt_arg_2;
_PID_val_st alt_val_2;
/*高度环PID参数初始化*/
//为后面调用PID函数做铺垫
void Alt_2level_PID_Init()
{
alt_arg_2.kp = Ano_Parame.set.pid_alt_2level[KP];
alt_arg_2.ki = Ano_Parame.set.pid_alt_2level[KI];
alt_arg_2.kd_ex = 0.00f;
alt_arg_2.kd_fb = Ano_Parame.set.pid_alt_2level[KD];
alt_arg_2.k_ff = 0.0f;
}
//先写外环，为什么呢？
//一个理由，外环的输出即为内环的输入
//外环高度环控制
//引用位置速度环中的loc_ctrl_2结构体
void Alt_2level_Ctrl(float dT_s)
{
Auto_Take_Off_Land_Task(1000*dT_s);
//这个速度 = ？速度 + 自动启飞速度
fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;
//wcz的输出给了外环反馈 wcz一直没看懂什么意思
//这里使用了“Ano_LocCtrl.c”中提到的loc_ctrl_2 结构体，很重要啊，
//因为此文件里面的期望和反馈都是来自“Ano_LocCtrl.c”文件中的loc_ctrl_2/loc_ctrl_1 结构体
loc_ctrl_2.fb[Z] = wcz_hei_fus.out;
//如果这个速度不为0，则认为还未到达期望的高度，则标志置0
if(fs.alt_ctrl_speed_set != 0)
{
flag.ct_alt_hold = 0;
}
else
{
//如果期望与反馈的的数值之差限制在一定程度之内，那么我们就认为目的基本达到，
//即认为现实高度已到达期望高度，标志置1
if(ABS(loc_ctrl_1.exp[Z] - loc_ctrl_1.fb[Z])<20)
{
flag.ct_alt_hold = 1;
}
}
//如果已经成功起飞
if(flag.taking_off == 1)
{
//如果现实高度已到达期望高度，就执行PID函数
if(flag.ct_alt_hold == 1)
{
//这个函数的期待和反馈都是来自文件“Ano_LocCtrl.c”中的，如上
//参数结构体和数据结构体是来自上述介绍过的
//期望值不再是之前说的遥控器的打杆值了，反馈值很可能就是气压计和加速度计
//融合后测得的数据得来的
PID_calculate( dT_s, //周期（单位：秒）
0, //前馈值
loc_ctrl_2.exp[Z], //期望值（设定值）
loc_ctrl_2.fb[Z], //反馈值（）
&alt_arg_2, //PID参数结构体
&alt_val_2, //PID数据结构体
100, //积分误差限幅
0 //integration limit，积分限幅
);
}
else
{
//如果高度未达到，期望=反馈+误差，不过这个有什么意思呢？
//我们一般都是为了得出误差，这里求出期望？误差哪里来?
loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z] + alt_val_2.err;
}
}
else
{
//未成功起飞 期望=反馈 ? 误差为0？ 这两个else到底是要干嘛？
loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];
alt_val_2.out = 0;
}
//输出限幅，最终还不是为了得出外环输出，以便做内环输入
alt_val_2.out = LIMIT(alt_val_2.out,-150,150);
}
//结构体，如上
_PID_arg_st alt_arg_1;
_PID_val_st alt_val_1;
//内环参数初始化，为下面PID函数做准备
/*高度速度环PID参数初始化*/
void Alt_1level_PID_Init()
{
alt_arg_1.kp = Ano_Parame.set.pid_alt_1level[KP];
alt_arg_1.ki = Ano_Parame.set.pid_alt_1level[KI];
alt_arg_1.kd_ex = 0.00f;
alt_arg_1.kd_fb = Ano_Parame.set.pid_alt_1level[KD];
alt_arg_1.k_ff = 0.0f;
}
//static u8 thr_start_ok;
static float err_i_comp;
//重要
//内环高度速度环控制
//引用位置速度环中的loc_ctrl_1变结构体
void Alt_1level_Ctrl(float dT_s)
{
u8 out_en;
//标志位，成功起飞就输出使能
out_en = (flag.taking_off != 0) ? 1 : 0;
//油门模式为自动
flag.thr_mode = THR_AUTO;//THR_MANUAL;
//内环的期望 = 这个速度 + 外环的输出
loc_ctrl_1.exp[Z] = fs.alt_ctrl_speed_set + alt_val_2.out;
if(0) //(flag.thr_mode == THR_MANUAL)
{
loc_ctrl_1.fb[Z] = 0;
}
else
{
//这个wcz一直没看懂什么意思，但是这里获取到了反馈值用作内环反馈
loc_ctrl_1.fb[Z] = wcz_spe_fus.out;
}
//调用PID函数，内环期望值和反馈值都是来自“Ano_LocCtrl.c”中的，如上
//同样引用上述结构体
//内环期望值很明显，外环输出已经作用，内环反馈如上外环反馈，原理应该都差不多
PID_calculate( dT_s, //周期（单位：秒）
0, //前馈值
loc_ctrl_1.exp[Z], //期望值（设定值）
loc_ctrl_1.fb[Z] , //反馈值（）
&alt_arg_1, //PID参数结构体
&alt_val_1, //PID数据结构体
100, //积分误差限幅
(THR_INTE_LIM *10 - err_i_comp)*out_en //integration limit，积分限幅
);
//如果起飞就进行低通滤波
if(flag.taking_off == 1)
{
LPF_1_(1.0f,dT_s,THR_START *10,err_i_comp);//err_i_comp = THR_START *10;
}
else
{
err_i_comp = 0;
}
//高度内环输出进行限幅
alt_val_1.out = LIMIT(alt_val_1.out,-err_i_comp,MAX_THR *10);
//高度内环输出最终转化为位置速度环的输出
loc_ctrl_1.out[Z] = out_en *FINAL_P *(alt_val_1.out + err_i_comp - 0.2f *imu_data.w_acc[Z]);
//最后转化为油门控制量
mc.ct_val_thr = loc_ctrl_1.out[Z];
}
整个流程也还算是比较清晰，高度环和高度速度环，外环是高度，内环是高度速度，高度和高度速度可以通过融合气压计和加速度计的数据得到，因此不难画出其串级PID图，类似角速度和角加速度一样。只是代码实现的有点曲折，期望的得到也没那么直接，油门只能靠增量而无法像三个欧拉角一样，可以靠打杆的多少实现绝对的运动，总不能说我飞行的总高度是一定的吧，打杆打多少就飞到空中多少，之前一直不是很理解因而一直炸鸡。
总结下，高度环期望和反馈都是来自位置速度环，外环输出作为内环输入，同时内环反馈也是来自位置速度环，最后内环输出又给了位置速度环的输出，最后转化为油门控制量。
先写这么多吧，有点心累…
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
