TF模块在pixhawk上定高方案

benekwake007

使用TF01模块在四轴飞行器下垂直向下安装，用来检测四轴飞行器的对地高度。测试的飞控平台是pixhawk，我们设计了一个数据转接板，将TF01的数据接收解析后再转发给pixhawk飞控。

1     Pixhawk定高原理简述

pixhawk为APM、PX4的升级版，其中APM使用的是普通的8位单片机，运算能力有限，PX4是一个开始使用STM32F4的过渡性产品，而pixhawk是PX4基础上发展的更加完善和可靠的飞控。对于此系列开源飞控的介绍，可详细参考：http://ardupilot.org/，这个网站介绍的非常详细和可靠的介绍了pixhawk，完全可以按照这个网站来学习了解飞控。

Pixhawk定高的原理：

Lidar’s are used in flight modes  which have height control, such as Altitude Hold, Loiter and PosHold Mode.  The data from the sensor will be used until you exceed RNGFND_MAX_CM, after  that it switches to the barometer. Currently Lidar is not supported in Auto  Mode.    如上面所述，雷达在定高模式，悬停模式和定点模式中起作用。只有当雷达数据超过RNGFND_MAX_CM后才会转换为使用气压计定高。

Pixhawk上常用的距离传感器有数据的输入方式有模拟输入、I2C输入、串口方式输入和PWM输入方式。具体可详细参考http://ardupilot.org/copter/docs/common-rangefinder-landingpage.html。

2     串口输入模式

建议使用串口输入方式给pixhawk提供高度信息。同时需要注意的是，使用串口输入方式的情况下，TF01模块输出协议，固件版本最低为V3.3.3。

2.1.1接线图

图 1 (a) 串口输入方式连接示意图

图1 (b) 串口输入方式连接示意图

2.1  Mission Planner配置说明

    将飞控连至MP，在下面的CONFIG/TUNING栏里面选择左侧的Full Parameter List，找到并修改下面几个参数：

·        SERIAL4_PROTOCOL = 9 (Lidar)

·        SERIAL4_BAUD = 115

·        RNGFND_TYPE = 8 (LightWareSerial)

·        RNGFND_SCALING = 1

·        RNGFND_MIN_CM = 5

·        RNGFND_MAX_CM = 1200

·        RNGFND_GNDCLEAR = 5 单位是cm,或者使用更精确的值,取决于模块安装高度。

设置好这几个参数后，点击软件右侧的Write Params即可。

具体配置要点请参考图 2和图3：

图 2 串口参数配置

图 3 串口输入模式RNG参数配置

如果出现Bad Lidar Health错误，请首先检查TF01雷达发射窗口是否有红色LED光线发出，如果没有红色LED光线，请检查供电是否正常(SERIAL 4/5口供电pixhawk有时会供电不足)。如果发出了LED光线错误任然存在，请检查串口接线是否正确。如以上排查后仍无法显示正常距离值，请使用串口2连接（TELEM2）。接线方式无异如图1（b）所示，参数表中的串口配置项更改为

·        SERIAL2_PROTOCOL = 9 (Lidar)

·        SERIAL2_BAUD = 115

3     AD输入模式

       使用AD输入方式依赖一块额外的转发板。我们将TF01模块的数据先发送给一个STM32板，然后使用STM32板将数据转换为pixhawk能检测的AD数据。同样需要注意的是TF01模块、转接板和pixhawk需要共地处理。如图4：

图 4 模拟输入方式连接示意图

3.1  连线说明

       将距离数据使用模拟信号输入至pixhawk的连接方式如图5：

接线说明：

红色：3.3V供电

橙色：AD模拟信号

黑色：GND

图 5 模拟信号输入的接线说明

3.2  Mission Planner配置说明

    将飞控连至MP，在下面的CONFIG/TUNING栏里面选择左侧的Full Parameter List，找到并下面几个参数：

·        RNGFND_PIN = “14” for Pixhawk’s ADC 3.3v pin #2 OR “0”for APM2.x

·        RNGFND_MAX_CM = “1200” (i.e. 12m max range)

·        RNGFND_SCALING = “4” (i.e. 4m / 1v)

·        RNGFND_TYPE = “1” (Analog)

其中，STM32的DA模块只能输出0-3.3V的电压。实验中，我们设置为当测量距离为12m时，输入给pixhawk的电压为3V。RNGFND_SCALING设置为4。(用户也可以自行设置，但是转发板里的也需要相应的改动。)

设置好这几个参数后，点击软件右侧的Write Params即可。

具体配置要点请参考图6：

图 6 模拟输入模式RNG配置要点

3.3  STM32转发板参考程序

接收并解析TF01模块的距离信息：

// 全局变量    u16 distance = 0;    // 串口使用的变量    static u8  Usart1buf[USART1_BUF_SIZE];    static u8 pointer = 0;       // 串口1初始化函数    void USART1_Init(void)    {                     USART_InitTypeDef USART1_InitStructure;             GPIO_InitTypeDef GPIO_InitStructure;             NVIC_InitTypeDef NVIC_InitStructure;                          // A口时钟             RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);             // A9 -> TX   ,  A10 -> RX             GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10;             GPIO_InitStructure.GPIO_Speed = GPIO_High_Speed;             GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;             GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;             GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;             GPIO_Init(GPIOA,&GPIO_InitStructure);                          // 复用功能配置             GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1);             GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1);             // USART1时钟             RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);             // USART1中断优先级             NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;             NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;             NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;             NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;             NVIC_Init(&NVIC_InitStructure);             // USART1初始化             USART_DeInit(USART1);             USART1_InitStructure.USART_BaudRate = 115200;             USART1_InitStructure.USART_WordLength =  USART_WordLength_8b;             USART1_InitStructure.USART_StopBits = USART_StopBits_1;             USART1_InitStructure.USART_Parity = USART_Parity_No;             USART1_InitStructure.USART_Mode =  USART_Mode_Rx|USART_Mode_Tx;             USART1_InitStructure.USART_HardwareFlowControl =  USART_HardwareFlowControl_None;             USART_Init(USART1,&USART1_InitStructure);             USART_Cmd(USART1,ENABLE);             USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);    }       // 串口中断函数    void  USART1_IRQHandler(void)    {                          if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET                ||  (USART_GetITStatus(USART1, USART_IT_ORE_RX) != RESET))             {                       USART_ClearITPendingBit(USART1, USART_IT_RXNE);                       Usart1buf[pointer++%USART1_BUF_SIZE] = USART_ReceiveData(USART1);                                             // 在此写代码接收程序                       if((pointer%USART1_BUF_SIZE >= 9))                       {                                // 包头包尾确认                                // 最好加上校验和的验证，此处为了简单，省略了校验和                                if(    (Usart1buf[pointer%USART1_BUF_SIZE-3]==0x59)                                         &&(Usart1buf[pointer%USART1_BUF_SIZE-4]==0x59))                                {                                         //距离                                         distance=((u16)((Usart1buf[pointer%USART1_BUF_SIZE-1])<<8)                                                         |(u16)(Usart1buf[pointer%USART1_BUF_SIZE-2]));                                }                       }                }    }

DAC的配置：

void DAC_Config(void)    {             GPIO_InitTypeDef GPIO_InitStructure;                RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);                                     RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);                GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;             GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;             GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;             GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;             GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;             GPIO_Init(GPIOA, &GPIO_InitStructure);                                DAC_InitTypeDef   DAC_InitStructure;             DAC_DeInit();             DAC_InitStructure.DAC_Trigger = DAC_Trigger_Software;             DAC_InitStructure.DAC_WaveGeneration =  DAC_WaveGeneration_None;             DAC_InitStructure.DAC_OutputBuffer =  DAC_OutputBuffer_Enable;             DAC_Init(DAC_Channel_1, &DAC_InitStructure);             DAC_Cmd(DAC_Channel_1,ENABLE);                          DAC_SetChannel1Data(DAC_Align_12b_R,0x1fff);                          DAC_SoftwareTriggerCmd(DAC_Channel_1,ENABLE);    }

主函数：

// TF01模块测试的高度，单位mm    float test_height;    // 高度偏置，单位mm    float bias = 180;    // 应该输出的模拟值    s16 analog=0;       // 主函数    void main(void)    {             // 中断分组的配置             NVIC_Config();             // 串口初始化             USART1_Init();             // DAC配置             DAC_Config();                          // 主循环             while(1)             {                       // 去除偏置                       test_height = distance - bias;                       // 数据转换，3V对应高度为12m，则：                       // test_height * 3.3 * 4096/（3 *  1200）= test_height * 4096 / 1320                       analog = (s16)(test_height*4096/1320);                       // 范围限制                       analog = analog < 4095 ? analog : 4095;                       analog = analog > 0 ? analog : 0;                       // 电压输出                       DAC_SetChannel1Data(DAC_Align_12b_R,analog);                       DAC_SoftwareTriggerCmd(DAC_Channel_1,ENABLE);             }    }

4     数据测试说明

    在Mission Planner的飞行数据栏里，点击左下方的Status栏，找到里面的sonarrange和sonarvoltage。其中sonarrange表示实际距离，sonarvoltage表示模拟输入电压。

参考图 7：

图 7 传感器距离测试参考图（测试传感器能否正确读取距离）

5     PID参数调试

pixhawk的PID飞行参数调试全部可以在Mission Planner上完成，请参考此网站调试pixhawk的参数http://ardupilot.org/copter/docs/common-tuning.html。