280浏览
查看: 280|回复: 0

[教程] 【开源飞控】匿名飞控TI版解析(1)

[复制链接]
准备电赛的飞控题,买来了匿名的飞控学习一下,这里整理了一下匿名飞控中比较关键的几部分,学习了一下原理,然后代码解读都写注释里了,篇幅较长。


一、遥控器信号接收

匿名的飞控提供了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;i<CH_NUM;i++)
//                        {
//                                if(chn_en_bit & (1<<i))//(Rc_Pwm_In[i]!=0)//该通道有值,==0说明该通道未插线(PWM)
//                                {
//                                        CH_N[i] = 1.25f *((s16)Rc_Pwm_In[i] - 1500); //1100 -- 1900us,处理成大约+-500摇杆量

//                                }
//                                else
//                                {
//                                        CH_N[i] = 0;
//                                }
//                                CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
//                        }
//                }
//                else if(RC_IN_MODE == PPM)
                if(RC_IN_MODE == PPM || RC_IN_MODE == PWM)
                {
                        for(u8 i=0;i<CH_NUM;i++)
                        {
                                if(chn_en_bit & (1<<i))//(Rc_Ppm_In[i]!=0)//该通道有值
                                {
                                        CH_N[i] = ((s16)RC_PPM.Captures[i] - 1500); //1000 -- 2000us,处理成大约+-500摇杆量
                                }
                                else
                                {
                                        CH_N[i] = 0;
                                }
                                CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
                        }                
                }
                else//sbus
                {
                        for(u8 i=0;i<CH_NUM;i++)
                        {
                                if(chn_en_bit & (1<<i))//该通道有值
                                {
                                        CH_N[i] = 0.65f *((s16)Rc_Sbus_In[i] - 1024); //248 --1024 --1800,处理成大约+-500摇杆量
                                }
                                else
                                {
                                        CH_N[i] = 0;
                                }
                                CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
                        }                                        
                }

                ///
                //解锁监测        
                unlock(dT_ms);
                //摇杆触发功能监测
                stick_function(dT_ms);        
                //通道看门狗
                ch_watch_dog(dT_ms);

                //失控保护检查
                fail_safe_check(dT_ms);//3ms

        }
}

2.ppm原理

ppm的原理其实比较简单,下面图基本可以说清楚,
ppm.jpg

其实就是是把多路PWM波压缩成一路ppm信号,通常20ms为一个周期,用一系列高电平脉冲之间的间隔时间表示每一路PWM波的脉宽,各通道的高电平信号是一个挨着一个的,最大的时间是2ms,而不是每个通道分配2ms的时间,但实际可能出现不是紧挨着的,但是对接码没影响。

这部分相关内容也可以参考:

二、传感器数据读取

匿名使用的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由高电平到低电平的跳变,所以数据采集是在上升沿,数据发送是在下降沿。

③内部机制
20190727193501292.png

主要参考: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”状态,表示当前线路上没有资料传送。
  • 波特率:是衡量资料传送速率的指标。表示每秒钟传送的二进制位数。

20190728120732466.png

②通信过程

收发寄存器:

  • 输出缓冲寄存器,它接收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算法想必应该都已经很熟悉了,匿名在这里其实也没有做太多特殊处理。主要就是家里前馈和微分先行。

  • 前馈控制:就是按扰动量进行补偿,但用的话就是开环的,一般要和反馈控制结合使用。

20190727235652548.png

  • 微分先行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);
}

————————————————
版权声明:本文为CSDN博主「AngeloG」的原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接及本声明。
原文链接:https://blog.csdn.net/GWH_98/article/details/97509669

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2021 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail