benewake 发表于 2017-11-23 17:24:17

TF模块在pixhawk上定高方案

使用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 modeswhich 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, afterthat it switches to the barometer. Currently Lidar is not supported in AutoMode.如上面所述,雷达在定高模式,悬停模式和定点模式中起作用。只有当雷达数据超过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.1Mission 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.2Mission 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.3STM32转发板参考程序接收并解析TF01模块的距离信息:
// 全局变量u16 distance = 0;// 串口使用的变量static u8Usart1buf;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);} // 串口中断函数voidUSART1_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 = USART_ReceiveData(USART1);                                      // 在此写代码接收程序                   if((pointer%USART1_BUF_SIZE >= 9))                   {                            // 包头包尾确认                            // 最好加上校验和的验证,此处为了简单,省略了校验和                            if(    (Usart1buf==0x59)                                     &&(Usart1buf==0x59))                            {                                     //距离                                     distance=((u16)((Usart1buf)<<8)                                                   |(u16)(Usart1buf));                            }                   }          }}
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模块测试的高度,单位mmfloat test_height;// 高度偏置,单位mmfloat 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。 因长度限制,图片及相关内容详细请关注或者联系北醒访问,期待您的光临!


页: [1]
查看完整版本: TF模块在pixhawk上定高方案