匿名飞控TI版解析（1）【学习总结】
准备电赛的飞控题，买来了匿名的飞控学习一下，这里整理了一下匿名飞控中比较关键的几部分，学习了一下原理，然后代码解读都写注释里了，篇幅较长。
目录
一、遥控器信号接收
1.代码解读
2.ppm原理
二、传感器数据读取
1.SPI代码和原理
2.UART代码及原理
三、加速度计、陀螺仪数据处理
四、姿态解算
五、高度数据融合
六、PID控制
一、遥控器信号接收
匿名的飞控提供了ppm和sbus两种遥控器信号的接收，在这里我只使用了ppm，从程序里看应该是最多可以接收7个通道。
1.代码解读
（1）ppm/sbus初始化
首先是在 Drv_BspInit() 中调用了 Remote_Control_Init() ，
void Remote_Control_Init()
{
//
RC_IN_MODE = Ano_Parame.set.pwmInMode;///在参数初始化设置中默认设置为ppm模式
//
if(RC_IN_MODE == SBUS)
{
Drv_SbusInit();
}
else
{
Drv_PpmInit();
// PWM_IN_Init(RC_IN_MODE);
}
}在 Drv_PpmInit() 中初始化输入捕获，开启中断，这里是配置成向上计时模式，即捕获到上升沿后，进入中断，并以80M的频率计数，计数区间为0~0xffffff。
void Drv_PpmInit(void)
{
ROM_SysCtlPeripheralEnable(PPM_SYSCTL);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_WTIMER1);
/*GPIOC配置为定时器捕获模式*/
ROM_GPIOPinTypeTimer(PPM_PORTS, PPM_PIN);
ROM_GPIOPinConfigure(PPM_FUNCTION);
/*配置定时器5B为捕获上升沿*/
ROM_TimerConfigure( WTIMER1_BASE ,TIMER_CFG_SPLIT_PAIR | TIMER_CFG_B_CAP_TIME_UP );
ROM_TimerControlEvent(WTIMER1_BASE,TIMER_B,TIMER_EVENT_POS_EDGE);
ROM_TimerLoadSet( WTIMER1_BASE , TIMER_B , 0xffff );
ROM_TimerPrescaleSet( WTIMER1_BASE , TIMER_B , 0xff );
///这里是配置成向上计时模式，即捕获到上升沿后，进入中断，并以80M的频率计数，计数区间为0~0xffffff
/*开启定时器中断*/
TimerIntRegister(WTIMER1_BASE, TIMER_B , PPM_Decode);
ROM_IntPrioritySet( INT_WTIMER1B , USER_INT6);
ROM_TimerIntEnable( WTIMER1_BASE , TIMER_CAPB_EVENT);
ROM_TimerEnable( WTIMER1_BASE, TIMER_B );
ROM_IntEnable( INT_WTIMER1B );
}（2）中断中进行数据解析
当PPM输入引脚捕获到高电平脉冲上升沿时触发中断， PPM_Decode(void) 为中断服务函数，调用PPM_Cal(PulseHigh) 计算出当前脉冲宽度。脉冲宽度在1000us-2000us之间，一帧数据结束标志位脉冲宽度大于5000us，即留出了5ms作为空闲判断的标志。
static void PPM_Cal(uint32_t PulseHigh)
{
static uint8_t Chan = 0;
/*脉宽高于一定值说明一帧数据已经结束*/
if(PulseHigh > 5000)
{
/*一帧数据解析完成*/
Chan = 0;
}
else
{
/*脉冲高度正常*/
if (PulseHigh > PULSE_MIN && PulseHigh < PULSE_MAX)
{
if(Chan < 16)
{
ch_watch_dog_feed(Chan);
RC_PPM.Captures[Chan++] = PulseHigh;
}
}
}
}
static void PPM_Decode(void)
{
static uint32_t PeriodVal1,PeriodVal2 = 0;
static uint32_t PulseHigh;
/*清除中断标志*/
ROM_TimerIntClear( WTIMER1_BASE , TIMER_CAPB_EVENT );
/*获取捕获值*/
PeriodVal1 = ROM_TimerValueGet( WTIMER1_BASE , TIMER_B );
if( PeriodVal1 > PeriodVal2 )
PulseHigh = (PeriodVal1 - PeriodVal2) /80;
else
PulseHigh = (PeriodVal1 - PeriodVal2 + 0xffffff)/80;
PeriodVal2 = PeriodVal1;
PPM_Cal(PulseHigh);
}（3）遥控器任务
读出脉冲宽度后，调用下面的函数，先把脉冲宽度信息处理成合适的杆量信息。然后检测遥感状态，是否触发解锁、传感器校准等特殊动作。
void RC_duty_task(u8 dT_ms) //建议2ms调用一次
{
if(flag.start_ok)
{
/获得通道数据
// if(RC_IN_MODE == PWM)
// {
// for(u8 i=0;i2.ppm原理
ppm的原理其实比较简单，下面图基本可以说清楚，
其实就是是把多路PWM波压缩成一路ppm信号，通常20ms为一个周期，用一系列高电平脉冲之间的间隔时间表示每一路PWM波的脉宽，各通道的高电平信号是一个挨着一个的，最大的时间是2ms，而不是每个通道分配2ms的时间，但实际可能出现不是紧挨着的，但是对接码没影响。
这部分相关内容也可以参考：
https://blog.csdn.net/wxc971231/article/details/95983705
https://blog.csdn.net/nicekwell/article/details/53866204
二、传感器数据读取
匿名使用的icm20602、ak8975、spl06都是SPI通信，通过cs脚进行片选，占用资源少且速度快。另外匿名飞控上还有一点spi flash，不过看了一下代码感觉是没有用上。对于用户自己加传感器，匿名预留了5个串口，其中1是gps，2是数传，3空的，4是光流，5是空的。
1.SPI代码和原理
（1）以icm20602读取为例
我们平时使用的spi在TM4C123GH6PM中即使ssi外设，该芯片中包括四个同步串行接口（SSI）模块，是ti公司定义的一种高速、全双工、同步串行接口协议，兼容spi。首先是spi初始化，初始化clk、mosi、miso三个脚，读写函数。
void Drv_Spi0Init(void)
{
ROM_SysCtlPeripheralEnable( SYSCTL_PERIPH_SSI0 );
ROM_SysCtlPeripheralEnable(SPI0_SYSCTL);
/*配置IO口*/
ROM_GPIOPinTypeSSI(SPI0_PROT,SPI0_CLK_PIN|SPI0_RX_PIN|SPI0_TX_PIN);
ROM_GPIOPinConfigure(SPI0_CLK);
ROM_GPIOPinConfigure(SPI0_RX);
ROM_GPIOPinConfigure(SPI0_TX);
/* SSI配置 模式3(Polarity = 1 Phase = 1) 主设备模式 速率1MHz 数据长度8位*/
ROM_SSIConfigSetExpClk(SPI0_BASE, ROM_SysCtlClockGet(), SSI_FRF_MOTO_MODE_3, SSI_MODE_MASTER, 10000000, 8);
/*开启SSI0*/
ROM_SSIEnable(SPI0_BASE);
}
/* SPI读写函数 */
uint8_t Drv_Spi0SingleWirteAndRead(uint8_t SendData)
{
uint32_t ui_TempData;
uint8_t uc_ReceiveData;
/* 向SSI FIFO写入数据 */
ROM_SSIDataPut(SPI0_BASE, SendData);
/* 等待SSI不忙 */
while(ROM_SSIBusy(SPI0_BASE));
/* 从FIFO读取数据 */
ROM_SSIDataGet(SPI0_BASE, &ui_TempData);
/* 截取数据的低八位 */
uc_ReceiveData = ui_TempData & 0xff;
return uc_ReceiveData;
}
void Drv_Spi0Transmit(uint8_t *ucp_Data, uint16_t us_Size)
{
uint16_t i = 0;
/* 连续写入数据 */
for(i = 0; i < us_Size; i++)
{
Drv_Spi0SingleWirteAndRead(ucp_Data[i]);
}
}
void Drv_Spi0Receive(uint8_t *ucp_Data, uint16_t us_Size)
{
uint16_t i = 0;
/* 连续读取数据 */
for(i = 0; i < us_Size; i++)
{
ucp_Data[i] = Drv_Spi0SingleWirteAndRead(0xFF);
}
}
icm20602的cs脚初始化，并通过拉高拉低进行片选，
void Drv_Icm20602CSPinInit(void)
{
ROM_SysCtlPeripheralEnable(ICM_CSPIN_SYSCTL);
ROM_GPIOPinTypeGPIOOutput(ICM20602_CS_PORT,ICM20602_CS_PIN);
ROM_GPIOPinWrite(ICM20602_CS_PORT, ICM20602_CS_PIN,ICM20602_CS_PIN);///先拉高
}
static void icm20602_enable(u8 ena)
{
if(ena)
ROM_GPIOPinWrite(ICM20602_CS_PORT, ICM20602_CS_PIN,0);//拉低
else
ROM_GPIOPinWrite(ICM20602_CS_PORT, ICM20602_CS_PIN,ICM20602_CS_PIN);
}
static void icm20602_readbuf(u8 reg, u8 length, u8 *data)
{
icm20602_enable(1);
Drv_Spi0SingleWirteAndRead(reg|0x80);
Drv_Spi0Receive(data,length);
icm20602_enable(0);
}
static u8 icm20602_writebyte(u8 reg, u8 data)
{
u8 status;
icm20602_enable(1);
status = Drv_Spi0SingleWirteAndRead(reg);
Drv_Spi0SingleWirteAndRead(data);
icm20602_enable(0);
return status;
}（2）spi原理
SPI, Serial Perripheral Interface, 串行外围设备接口, 是 Motorola 公司推出的一种同步串行接口技术. SPI 总线在物理上是通过接在外围设备微控制器(PICmicro) 上面的微处理控制单元 (MCU) 上叫作同步串行端口(Synchronous Serial Port) 的模块(Module)来实现的, 它允许 MCU 以全双工的同步串行方式, 与各种外围设备进行高速数据通信.
①四线
SCK, Serial Clock, 主要的作用是 Master 设备往 Slave 设备传输时钟信号, 控制数据交换的时机以及速率;
SS/CS, Slave Select/Chip Select, 用于 Master 设备片选 Slave 设备, 使被选中的 Slave 设备能够被 Master 设备所访问;
SDO/MOSI, Serial Data Output/Master Out Slave In, 在 Master 上面也被称为 Tx-Channel, 作为数据的出口, 主要用于 SPI 设备发送数据;
SDI/MISO, Serial Data Input/Master In Slave Out, 在 Master 上面也被称为 Rx-Channel, 作为数据的入口, 主要用于SPI 设备接收数据;
②时序
CPOL=0，CPHA=0：此时空闲态时，SCLK处于低电平，数据采样是在第1个边沿，也就是SCLK由低电平到高电平的跳变，所以数据采样是在上升沿，数据发送是在下降沿。
CPOL=0，CPHA=1：此时空闲态时，SCLK处于低电平，数据发送是在第2个边沿，也就是 SCLK由低电平到高电平的跳变，所以数据采样是在下降沿，数据发送是在上升沿。
CPOL=1，CPHA=0：此时空闲态时，SCLK处于高电平，数据采集是在第1个边沿，也就是 SCLK由高电平到低电平的跳变，所以数据采集是在下降沿，数据发送是在上升沿。
CPOL=1，CPHA=1：此时空闲态时，SCLK处于高电平，数据发送是在第2个边沿，也就是 SCLK由高电平到低电平的跳变，所以数据采集是在上升沿，数据发送是在下降沿。
③内部机制
主要参考：http://bbs.chinaunix.net/thread-1916003-1-1.html
2.UART代码及原理
（1）以自己加的omv读取为例
用的底板上空闲的串口5，读取openmv的数据，首先是在初始化程序中调用 Drv_Uart5Init(115200) ，设置好波特率。在这里我用omv发送到数据是一帧12字节，用fifo6/8刚好接收一帧进一次中断。使用fifo就可以减少进入中断的次数，防止因频繁进入中断影响mcu效率。
void Drv_Uart5Init(uint32_t baudrate)
{
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART2);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
/*PD7解锁操作*/
HWREG(UART2_PORT + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(UART2_PORT + GPIO_O_CR) = UART2_PIN_TX;
HWREG(UART2_PORT + GPIO_O_LOCK) = 0x00;
/*GPIO的UART模式配置*/
ROM_GPIOPinConfigure(UART2_RX);
ROM_GPIOPinConfigure(UART2_TX);
ROM_GPIOPinTypeUART(UART2_PORT, UART2_PIN_TX | UART2_PIN_RX);
/*配置串口号波特率和时钟源*/
ROM_UARTConfigSetExpClk(UART2_BASE, ROM_SysCtlClockGet(), baudrate,(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
///8个数据位、一个停止位、无校验位
/*FIFO设置*/
ROM_UARTFIFOLevelSet(UART2_BASE,UART_FIFO_TX6_8,UART_FIFO_RX6_8);///设置为6/8满时触发中断
ROM_UARTFIFOEnable(UART2_BASE);
/*使能串口*/
ROM_UARTEnable( UART2_BASE );
/*使能UART0接收中断*/
UARTIntRegister(UART2_BASE,UART5_IRQHandler);
ROM_IntPrioritySet(INT_UART2, USER_INT2);
ROM_UARTTxIntModeSet(UART2_BASE,UART_TXINT_MODE_EOT);
ROM_UARTIntEnable(UART2_BASE,UART_INT_RX | UART_INT_RT | UART_INT_TX);///使能收、发、接收超时中断
}在中断中循环读出fifo中的12个字节，
void UART5_IRQHandler(void)
{
uint8_t com_data;
/*获取中断标志 原始中断状态 不屏蔽中断标志*/
uint32_t flag = ROM_UARTIntStatus(UART2_BASE,1);
/*清除中断标志*/
ROM_UARTIntClear(UART2_BASE,flag);
/*判断FIFO是否还有数据*/
while(ROM_UARTCharsAvail(UART2_BASE))
{
com_data=ROM_UARTCharGet(UART2_BASE);///读一个字节
Drv_omvGetOneByte(com_data);
}
if(flag & UART_INT_TX)
{
Drv_Uart5TxCheck();
}
}然后就是一个字节一个字节读取，这里写了一个简单的协议，防止数据发送出现问题。
void Drv_omvGetOneByte(u8 data)
{
if (omv_get_cnt == 0)
{
if (data == 0x68) //帧头
{
omv_data_buff[omv_get_cnt] = data;
omv_get_cnt = 1;
}
}
else if (omv_get_cnt == 1)
{
if (data == 0x69) //id
{
omv_data_buff[omv_get_cnt] = data;
omv_get_cnt = 2;
}
else
{
omv_get_cnt = 0;
}
}
else
{
omv_data_buff[omv_get_cnt] = data;
omv_get_cnt++;
if (omv_get_cnt >= 12)
{
omv_get_cnt = 0;
omv_data_analysis(); ///数据解析
if (omv_data_check(omv_data_buff)) ///数据校验
{
omv_data_out();
}
}
}
}（2）uart通信原理
①字符帧相关
起始位：先发出一个逻辑”0”的信号，表示传输字符的开始。
数据位：紧接着起始位之后。资料位的个数可以是4、5、6、7、8等，构成一个字符。通常采用ASCII码。从最低位开始传送，靠时钟定位。
奇偶校验位：资料位加上这一位后，使得“1”的位数应为偶数(偶校验)或奇数(奇校验)，以此来校验资料传送的正确性。
停止位：它是一个字符数据的结束标志。可以是1位、1.5位、2位的高电平。 由于数据是在传输线上定时的，并且每一个设备有其自己的时钟，很可能在通信中两台设备间出现了小小的不同步。因此停止位不仅仅是表示传输的结束，并且提供 计算机校正时钟同步的机会。适用于停止位的位数越多，不同时钟同步的容忍程度越大，但是数据传输率同时也越慢。
空闲位：处于逻辑“1”状态，表示当前线路上没有资料传送。
波特率：是衡量资料传送速率的指标。表示每秒钟传送的二进制位数。
②通信过程
收发寄存器：
输出缓冲寄存器，它接收CPU从数据总线上送来的并行数据，并加以保存。 [2]
输出移位寄存器，它接收从输出缓冲器送来的并行数据，以发送时钟的速率把数据逐位移出，即将并行数据转换为串行。
输入移位寄存器，它以接收时钟的速率把出现在串行数据输入线上的数据逐位移入，当数据装满后，并行送往输入缓冲寄存器，即将串行数据转换成并行数据。
输入缓冲寄存器，它从输入移位寄存器中接收并行数据，然后由CPU取走。
发送时，数据被写入发送FIFO。如果UART 被使能，则会按照预先设置好的参数（波特率、数据位、停止位、校验位等）开始发送数据，一直到发送FIFO 中没有数据。一旦向发送FIFO 写数据（如果FIFO 未空），UART 的忙标志位BUSY 就有效，并且在发送数据期间一直保持有效。BUSY 位仅在发送FIFO 为空，且已从移位寄存器发送最后一个字符，包括停止位时才变无效。即 UART 不再使能，它也可以指示忙状态。
在UART 接收器空闲时，如果数据输入变成“低电平”，即接收到了起始位，则接收计数器开始运行，并且数据在Baud16 的第8 个周期被采样。如果Rx 在Baud16 的第8 周期仍然为低电平，则起始位有效，否则会被认为是错误的起始位并将其忽略。如果起始位有效，则根据数据字符被编程的长度，在 Baud16 的每第 16 个周期对连续的数据位（即一个位周期之后）进行采样。如果奇偶校验模式使能，则还会检测奇偶校验位。
最后，如果Rx 为高电平，则有效的停止位被确认，否则发生帧错误。当接收到一个完整的字符时，将数据存放在接收FIFO 中。
三、加速度计、陀螺仪数据处理
直接从传感器中读出的数据和可用的数据还是有区别的，主要体现在：不在中心位置、旋转加速度对加速度的影响、校准值等，所以要对原始数据进行一系列处理。下面这个函数是放在1ms中断任务里面进行的，主要就是对六轴原始数据的处理。
void Sensor_Data_Prepare(u8 dT_ms)
{
float hz = 0 ;
if(dT_ms != 0) hz = 1000/dT_ms;
// MPU6050_Read();
// sensor_rotate_func(dT);
/*静止检测*/
motionless_check(dT_ms);
MPU6050_Data_Offset(); //校准函数///开机首先对陀螺仪进行自动校准，但是对加速度计不校准，加速度可用上位机或者摇杆指令校准
/*得出校准后的数据*/
for(u8 i=0;i<3;i++)
{
sensor_val[A_X+i] = sensor.Acc_Original[i] ;
sensor_val[G_X+i] = sensor.Gyro_Original[i] - save.gyro_offset[i] ;
//sensor_val[G_X+i] = (sensor_val[G_X+i] >>2) <<2;
}
/*可将整个传感器坐标进行旋转*/
// for(u8 j=0;j<3;j++)
// {
// float t = 0;
//
// for(u8 i=0;i<3;i++)
// {
//
// t += sensor_val[A_X + i] *wh_matrix[j][i];
// }
//
// sensor_val_rot[A_X + j] = t;
// }
// for(u8 j=0;j<3;j++)
// {
// float t = 0;
//
// for(u8 i=0;i<3;i++)
// {
//
// t += sensor_val[G_X + i] *wh_matrix[j][i];
// }
//
// sensor_val_rot[G_X + j] = t;
// }
/*赋值*/
for(u8 i = 0;i<6;i++)
{
sensor_val_rot[i] = sensor_val[i];
}
/*数据坐标转90度*/ ///这里的旋转是因为icm20602的安装导致和机身坐标系不一样，绕机身z轴转90度即可
sensor_val_ref[G_X] = sensor_val_rot[G_Y] ;
sensor_val_ref[G_Y] = -sensor_val_rot[G_X] ;
sensor_val_ref[G_Z] = sensor_val_rot[G_Z];
sensor_val_ref[A_X] = (sensor_val_rot[A_Y] - save.acc_offset[Y] ) ;
sensor_val_ref[A_Y] = -(sensor_val_rot[A_X] - save.acc_offset[X] ) ;
sensor_val_ref[A_Z] = (sensor_val_rot[A_Z] - save.acc_offset[Z] ) ;
/*单独校准z轴模长*/
mpu_auto_az();
//======================================================================
/*软件低通滤波*/
for(u8 i=0;i<3;i++)
{
//
gyr_f[3][X +i] = (sensor_val_ref[G_X + i] );
acc_f[3][X +i] = (sensor_val_ref[A_X + i] );
//
for(u8 j=3;j>0;j--)
{
//
gyr_f[j-1][X +i] += GYR_ACC_FILTER *(gyr_f[j][X +i] - gyr_f[j-1][X +i]);
acc_f[j-1][X +i] += GYR_ACC_FILTER *(acc_f[j][X +i] - acc_f[j-1][X +i]);
}
// LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[G_X + i],sensor.Gyro[X +i]);
// LPF_1_(100,dT_ms*1e-3f,sensor_val_ref[A_X + i],sensor.Acc[X +i]);
}
/*旋转加速度补偿*/
//======================================================================
for(u8 i=0;i<3;i++)///这部分是，如果icm安装的偏心，则测量的加速度会受到旋转的角加速度的影响，需要补偿。如果安装不对正，则要先去设置center_pos
{
center_pos.gyro_rad_old[i] = center_pos.gyro_rad[i];
center_pos.gyro_rad[i] = gyr_f[0][X + i] *RANGE_PN2000_TO_RAD;//0.001065f;
center_pos.gyro_rad_acc[i] = hz *(center_pos.gyro_rad[i] - center_pos.gyro_rad_old[i]);
}
center_pos.linear_acc[X] = +center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[Y] - center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[Z];
center_pos.linear_acc[Y] = -center_pos.gyro_rad_acc[Z] *center_pos.center_pos_cm[X] + center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Z];
center_pos.linear_acc[Z] = +center_pos.gyro_rad_acc[Y] *center_pos.center_pos_cm[X] - center_pos.gyro_rad_acc[X] *center_pos.center_pos_cm[Y];
//======================================================================
/*赋值*/
for(u8 i=0;i<3;i++)
{
sensor.Gyro[X+i] = gyr_f[0][i];//sensor_val_ref[G_X + i];
sensor.Acc[X+i] = acc_f[0][i] - center_pos.linear_acc[i]/RANGE_PN16G_TO_CMSS;//sensor_val_ref[A_X+i];//
}
/*转换单位*/
for(u8 i =0 ;i<3;i++)
{
/*陀螺仪转换到度每秒，量程+-2000度*/
sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;// /65535 * 4000; +-2000度 0.061
/*陀螺仪转换到弧度度每秒，量程+-2000度*/
sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i] *RANGE_PN2000_TO_RAD ;// 0.001065264436f //微调值 0.0010652f
/*加速度计转换到厘米每平方秒，量程+-8G*/
sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );// /65535 * 16*981; +-8G
}
}
四、姿态解算
姿态解算可用说是飞控中比较核心的部分，即将测得的陀螺仪、加速度计、磁力计的数据进行融合解算，方法有很多，匿名这里其实采用的是比较基础的一种方法，是一种融合磁力计的互补滤波。在1ms任务中调用 IMU_Update_Task(1) 如下，
void IMU_Update_Task(u8 dT_ms)
{
/*如果准备飞行，复位重力复位标记和磁力计复位标记*/
if(flag.unlock_sta )
{
imu_state.G_reset = imu_state.M_reset = 0; ///即重力和磁力正常修正
reset_imu_f = 0;
}
else
{
if(flag.motionless == 0)
{
// imu_state.G_reset = 1;//自动复位
//sensor.gyr_CALIBRATE = 2;
}
if(reset_imu_f==0 )//&& flag.motionless == 1)
{
imu_state.G_reset = 1;//自动复位
sensor.gyr_CALIBRATE = 2;//校准陀螺仪，不保存
reset_imu_f = 1; //已经置位复位标记
}
}
if(0)
{
imu_state.gkp = 0.0f;
imu_state.gki = 0.0f;
}
else
{
if(0)
{
imu_state.gkp = 0.2f;
}
else
{
/*设置重力互补融合修正kp系数*/
imu_state.gkp = 0.2f;//0.4f;
}
/*设置重力互补融合修正ki系数*/
imu_state.gki = 0.01f;
/*设置罗盘互补融合修正ki系数*/ 这里应该是kp系数
imu_state.mkp = 0.1f;
}
imu_state.M_fix_en = sens_hd_check.mag_ok; //磁力计修正使能 ///这里始终是使能的
/*姿态计算，更新，融合*/ ///核心部分
IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
//
}在一些修正标志使能、并设置修正系数之后，调用imu更新的函数。这一部分内容比较多，只来看几个关键部分，其他理论内容可以参考我之前的文章：https://blog.csdn.net/GWH_98/article/details/86811091
至于互补滤波修正，我想再总结一下。之前也没写过有关用磁力计修正的内容，这里也补充一下。加速计和磁力计都是用来修正陀螺仪的，其实在姿态解算中，陀螺仪积分置信度通常是最高的。加速计可以修正xoy平面，而磁力计可以修正绕z轴的转动，这样就可以对三个轴都进行修正。
加速计修正时，是将世界系重力分量[0,0,1]成旋转矩阵到机体系[vx,vy,vz]然后与加计测得的[ax,ay,az]（注意，这里是没有横向加速度时测得，即只有重力加速度分量，这是惯导的要求。但一般是有其他加速度的，所以总还是有误差的。）计算叉积，得到偏差。这里要搞清楚：世界系标准量是[0,0,1]，旋转矩阵是需要修正的，存在误差量是[vx,vy,vz]，机体系标准量是[ax,ay,az]。
磁力计修正时，也是同样的思路。如果让机头x轴对准正北，则世界系标准量应该是[bx,0,bz]这样一个值。但是这个值我们无法获得，不想加计直接是[0,0,1]。所以要从机体系标准值[mx,my,mz]推，即[mx,my,mz]乘旋转矩阵得到[hx,hy,hz]，此时这个值和我们的世界系标准量[bx,0,bz]存在一个关系，即hx2+hy2=bx2，这是由我们已经通过加计校准过xoy平面决定的，即此时只存在绕z轴误差。这样我们得到世界系标准量[bx,0,bz]就和加速度计原理一样修正就好了。
匿名中加计的修正部分和mahony算法大致相同，但磁力计部分做了一些简化。
//水平面方向向量 ///这里是计算水平面内航向坐标系下的单位向量，实际上就是偏航角的cos和sin
float hx_vec_reci = my_sqrt_reciprocal(my_pow(att_matrix[0][0]) + my_pow(att_matrix[1][0]));
imu->hx_vec[X] = att_matrix[0][0] *hx_vec_reci;
imu->hx_vec[Y] = att_matrix[1][0] *hx_vec_reci;
// 计算载体坐标下的运动加速度。(与姿态解算无关)
for(u8 i = 0;i<3;i++)
{
imu->a_acc[i] = (s32)(acc[i] - 981 *imu->z_vec[i]);
}
//计算世界坐标下的运动加速度。坐标系为北西天
for(u8 i = 0;i<3;i++)
{
s32 temp = 0;
for(u8 j = 0;j<3;j++)
{
temp += imu->a_acc[j] *att_matrix[i][j];
}
imu->w_acc[i] = temp;
}
///这里通过上面计算出的单位向量进行绕z轴转动的坐标变化，即将世界系转换到航向系，因为按照我们的操作习惯都是有头模式的遥控。
w2h_2d_trans(imu->w_acc,imu_data.hx_vec,imu->h_acc);//误差积分 ///pi修正中I项
vec_err_i[i] += LIMIT(vec_err[i],-0.1f,0.1f) *dT *ki_use;
// 构造增量旋转（含融合纠正）。
// d_angle[X] = (gyr[X] + (vec_err[X] + vec_err_i[X]) * kp_use - mag_yaw_err *imu->z_vec[X] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Y] = (gyr[Y] + (vec_err[Y] + vec_err_i[Y]) * kp_use - mag_yaw_err *imu->z_vec[Y] *kmp_use *RAD_PER_DEG) * dT / 2 ;
// d_angle[Z] = (gyr[Z] + (vec_err[Z] + vec_err_i[Z]) * kp_use - mag_yaw_err *imu->z_vec[Z] *kmp_use *RAD_PER_DEG) * dT / 2 ;
#ifdef USE_MAG ///这里感觉写的有点问题，I的修正项应该不需要再乘以kp了
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use + mag_yaw_err *imu->z_vec[i] *mkp_use) * dT / 2 ;
#else
d_angle[i] = (gyr[i] + (vec_err[i] + vec_err_i[i]) * kp_use ) * dT / 2 ;
#endif
}
// 计算姿态。 ///龙格库塔更新四元数
imu->w = imu->w - imu->x*d_angle[X] - imu->y*d_angle[Y] - imu->z*d_angle[Z];
imu->x = imu->w*d_angle[X] + imu->x + imu->y*d_angle[Z] - imu->z*d_angle[Y];
imu->y = imu->w*d_angle[Y] - imu->x*d_angle[Z] + imu->y + imu->z*d_angle[X];
imu->z = imu->w*d_angle[Z] + imu->x*d_angle[Y] - imu->y*d_angle[X] + imu->z;
q_norm_l = my_sqrt_reciprocal(imu->w*imu->w + imu->x*imu->x + imu->y*imu->y + imu->z*imu->z);
imu->w *= q_norm_l;
imu->x *= q_norm_l;
imu->y *= q_norm_l;
imu->z *= q_norm_l;五、高度数据融合
匿名飞控中有气压计和tof都可以测量高度，匿名的操作是，存在tof的时候就选择tof与惯导加速度进行一个融合计算；不存在tof时则用气压计与惯导加速度进行融合运算。用了一段时间感觉匿名的定高好像并不是很准确，而且也有点飘，不知道是不是自己菜的问题，听说acfly用气压计就很稳，但好像是半开源，有时间也想改一改匿名的定高部分吧。
下面这个函数是对高度数据的处理，由 WCZ_Fus_Task(11) 经过选择tof或气压计之后调用。
void WCZ_Data_Calc(u8 dT_ms,u8 wcz_f_pause,s32 wcz_acc_get,s32 ref_height)
{
static u8 cyc_xn;
float hz,ntimes_hz;
hz = safe_div(1000,dT_ms,0);
ntimes_hz = hz/N_TIMES;
wcz_ref_height = ref_height; ///tof或者是气压计的值
wcz_acc = wcz_acc_get; ///加速度计的值，低通简单滤过波
/
wcz_acc_deadzone = LIMIT(5 *(0.996f - imu_data.z_vec[Z] *imu_data.z_vec[Z]),0,1) *10;
// roll_acc_fix = (ABS(sensor.Gyro_deg[X]) + ABS(sensor.Gyro_deg[Y]) - 20) *0.1f;
// roll_acc_fix = LIMIT(roll_acc_fix,0,5);
//
// wz_acc_comp = roll_acc_fix ;//- LIMIT(5 *(0.996f - imu_data.z_vec[Z] *imu_data.z_vec[Z]),0,1) *5 ;
cyc_xn ++;
cyc_xn %= N_TIMES;
if(cyc_xn == 0)
{
wcz_ref_speed = (wcz_ref_height - ref_height_old) *ntimes_hz; ///由tof或者气压计直接微分算速度和加速度
wcz_ref_acc = (wcz_ref_speed - ref_speed_old) *ntimes_hz;
ref_height_old = wcz_ref_height;
ref_speed_old = wcz_ref_speed;
}
///低通滤波+积分
wcz_acc_fus.fix_ki = 0.05f;
wcz_acc_fus.in_est = wcz_acc;
wcz_acc_fus.in_obs = wcz_ref_acc;
wcz_acc_fus.ei_limit = 200;
inte_fix_filter(dT_ms*1e-3f,&wcz_acc_fus);
wcz_spe_fus.fix_kp = 0.5f;
wcz_spe_fus.in_est_d = my_deadzone(wcz_acc_fus.out,0,wcz_acc_deadzone);
wcz_spe_fus.in_obs = wcz_ref_speed;
wcz_spe_fus.e_limit = 200;
fix_inte_filter(dT_ms*1e-3f,&wcz_spe_fus);
wcz_hei_fus.fix_kp = 0.3f;
wcz_hei_fus.in_est_d = wcz_spe_fus.out;
wcz_hei_fus.in_obs = ref_height;
//wcz_hei_fus.e_limit = 200;
fix_inte_filter(dT_ms*1e-3f,&wcz_hei_fus);
///
}
六、PID控制
匿名飞控用的是经典控制pid算法，高度是有两个环的，位置控制值做到位置速度环，然后后面是角度环和角速度环，具体不展开讲了，这里主要说一下匿名用的pid算法。传统的pid算法想必应该都已经很熟悉了，匿名在这里其实也没有做太多特殊处理。主要就是家里前馈和微分先行。
- 前馈控制：就是按扰动量进行补偿，但用的话就是开环的，一般要和反馈控制结合使用。
- 微分先行pid：发生突变，而被调量的变化，通常总是比较缓慢的。这种输出量先行微分控制适合于给定值频繁变化的场合，可以避免给定值变化时可能引起的系统振荡，明显地改善了系统的动态特性。其实就是d参数对于期望值和反馈值的比重不同。分为d_exp和d_fb，如果这两个值相等，和普通的pid就一样了。
float PID_calculate( float dT_s, //周期（单位：秒）
float in_ff, //前馈值
float expect, //期望值（设定值）
float feedback, //反馈值（）
PID_arg_st *pid_arg, //PID参数结构体
PID_val_st *pid_val, //PID数据结构体
float inte_d_lim,//积分误差限幅
float inte_lim //integration limit，积分限幅
)
{
float differential,hz;
hz = safe_div(1.0f,dT_s,0);
// pid_arg->k_inc_d_norm = LIMIT(pid_arg->k_inc_d_norm,0,1);
pid_val->exp_d = (expect - pid_val->exp_old) *hz;///期望的差值
if(pid_arg->fb_d_mode == 0)
{
pid_val->fb_d = (feedback - pid_val->feedback_old) *hz;///反馈的差值
}
else
{
pid_val->fb_d = pid_val->fb_d_ex;
}
differential = (pid_arg->kd_ex *pid_val->exp_d - pid_arg->kd_fb *pid_val->fb_d);
pid_val->err = (expect - feedback);
pid_val->err_i += pid_arg->ki *LIMIT((pid_val->err ),-inte_d_lim,inte_d_lim )*dT_s;//)*T;//+ differential/pid_arg->kp
//pid_val->err_i += pid_arg->ki *(pid_val->err )*T;//)*T;//+ pid_arg->k_pre_d *pid_val->feedback_d
pid_val->err_i = LIMIT(pid_val->err_i,-inte_lim,inte_lim);
pid_val->out = pid_arg->k_ff *in_ff ///前馈值*前馈系数直接加进来
+ pid_arg->kp *pid_val->err
+differential
// + pid_arg->k_inc_d_norm *pid_val->err_d_lpf + (1.0f-pid_arg->k_inc_d_norm) *differential
+ pid_val->err_i;
pid_val->feedback_old = feedback;
pid_val->exp_old = expect;
return (pid_val->out);
}