领航者飞控源码下载（注释非常详细）

ID:150692 · 发表于 2017-7-28 14:45

当大家看到这里就会有疑问，积分分离PID的这一段程序怎么来的，其实我也推导不出这段代码是怎么推导出来的，希望有大侠们给个指点，回复一下。

/* 角速度误差积分 */
      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;

事件记录，beta0.4开始。
2015.5.18
beta 0.4  修复各种bug。

2015.5.19
beta 0.5  完善摇杆识别。
beta 0.6  pwm_in优化
beta 0.7  修复bug，优化rc，添加mapping
beta 0.9  起飞成功
beta 1.0  修复控制周期bug
beta 1.1  添加部分注释
beta 1.2  优化整定参数，修改bug
beta 1.3  完成flash存储，上位机HID通信
beta 1.35 增加磁力计纠正航向
beta 1.36 增加磁力计校准时灯管提示
beta 1.37 增加mpu6050，ak8975，ms5611自检，未通过的LED状态分别是：间隔连续闪烁1次；间隔连续闪烁2次；间隔连续闪烁3次
beta 1.37.1 增加滑动窗口平均滤波函数，增加ks103超声波读取，该版备份。
beta 1.37.2 添加user数据，上位机显示波形,串口超声波改发送中断。
beta 1.37.3 添加部分注释，修改部分变量名，增加ano_private.lib。
beta 1.38 添加超声波控制代码（注意！暂未测试），修改磁力计纠正算法（重要更新），消除磁力计纠正YAW时对姿态的干扰。删除ano_pribate.lib
beta 1.39 修复上一版数据传输一个小bug，添加环形缓存。
beta 1.39.1 超声波直接控制高度，还有问题。
beta 1.40 超声波定高ok，暂没优化参数，没加上位机调参。可改惯导竖直方向速率控制。
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超声波定高稳定
beta 1.43
1.模式0（呼吸白，解锁绿长），普通，油门直接输出。模式1（呼吸浅绿，解锁浅绿长），垂直速率控制（下一步改为气压定高）。模式2（呼吸紫，解锁紫长），超声波定高。
2.飞行时，只能向模式0切换，其他切换无效。
3.只有插上超声波，模式3才有效。（这个未优化，比如临时拔下超声波，也能进入该模式。）
beta 1.44
修复定高控制中积分的bug，优化超声波定高，优化高度设定值调节速度。增加超声波最大高度，可达到到1.5米比较稳定（高度越低越稳定）。
beta 1.46
气压计定高，测试版（无高度静差修正）。
beta 1.47
气压计定高，测试版（无高度静差修正）。改进磁力计纠正算法，解决磁力计纠正时对姿态的干扰，修正磁力计纠正中180度的一个错误。
beta 1.48
修复气压计速率的一个bug，并调整可控制的速率。
beta 1.48.1
注释掉while(1);
beta 1.49 电赛版本，完善通信协议中串口接收通道数据部分。
beta 1.49.2 修改气压计定高速率环积分部分，优化定高效果。
beta 1.49.3 滤波算法测试。
beta 1.50 修改磁力计纠正方法
beta 1.50.2 增加罗盘倾斜纠正，修复数据传输一个bug。
beta 1.51 修改气压计融合，定高控制等几处小bug
beta 1.52 修改超声波定高一个小bug
beta 1.6 细节优化&新参数
beta 1.6.2 修正罗盘原始数据的一个错误。
beta 1.6.2_fix 上版漏掉一句代码，导致解锁后yaw混乱，I'm sorry。。。
beta 1.6.3 新融合策略，解决加速度零点漂移（水平姿态）。
beta 1.6.4 修复连接usb后，复位单片机偶尔卡死的问题；修复上版气压速度融合的小错误；修复LED的GPID初始化结构体未给初值造成可能出错的问题。
beta 1.6.5 小更新，增加加速度计判断，解决6050不同批次量程问题。

APB1 42m
APB2 84m

TIMx clk = APBx *2

中断优先级：

第0组：所有4位用于指定响应优先级；
第1组：最高1位用于指定抢占式优先级，最低3位用于指定响应优先级；
第2组：最高2位用于指定抢占式优先级，最低2位用于指定响应优先级；
第3组：最高3位用于指定抢占式优先级，最低1位用于指定响应优先级；
第4组：所有4位用于指定抢占式优先级。
|先抢占，再响应|
1）如果指定的抢占式优先级别或响应优先级别超出了选定的优先级分组所限定的范围，将可能得到意想不到的结果；
2）抢占式优先级别相同的中断源之间没有嵌套关系；
3）如果某个中断源被指定为某个抢占式优先级别，又没有其它中断源处于同一个抢占式优先级别，则可以为这个中断源指定任意有效的响应优先级别。

wz_acc_mms2 = (wz_acc/4096.0f) *10000 + hc_acc_i;

hc_acc_i += 0.4f *T *( (wz_speed - wz_speed_old)/T - wz_acc_mms2 );

wz_acc_mms2 = (wz_acc/4096.0f) *10000 + (hc_acc_lpf - wz_acc_mms2);

hc_acc_lpf += 0.4f *T *( (wz_speed - wz_speed_old)/T - hc_acc_lpf );

把hc_acc_i代成(hc_acc_lpf - wz_acc_mms2)，然后后边等效，就有了。
注意wz_speed里边包含hc_acc_i的积分，两次wz_speed做差后，又得到hc_acc_i单次的值。

全部资料下载地址：

领航者飞控源码.rar (2.06 MB, 下载次数: 182)