px4Flow--替换超声波模块

几个必看的官方资料:

px4开发者指南:http://pixhawk.org/dev/px4flow

px4flow介绍:http://pixhawk.org/start?id=modules/px4flow


淘宝上的px4flow模块加上官方的超声波一般都要600左右,单买光流300多一点就可以搞定了,还有些300不到的不知道怎么样。然后就是将官方光流换成HC-SR04,这需要一个转换版,我买的光流模块,店家还不错:

px4Flow--替换超声波模块_第1张图片

直接下官方固件的话,距离为零,光流正常,接下来就是修改官方固件。

首先要清楚硬件接口的不同。这是我整理的一个官方超声波的中文版:https://download.csdn.net/download/hxudhdjuf/10305251。HC-SR04的Trig引脚与官方超声波接口的4号引脚相连,用于触发超声波模块发送超声波信号;echo引脚与2号引脚相连,接收反射回来的超声波信号,这里使用外部中断,并在产生中断时开始计时。官方4号引脚是PE8,2号引脚是PD13.

源代码在Flow/src/modules/flow/sonar.c里面,头文件在Flow/src/include/sonar.h中。

1、对C文件的修改。首先加入头文件:

#include "stm32f4xx_exti.h" 

#include "stm32f4xx_syscfg.h"

替换函数:

void sonar_trigger(){

GPIO_SetBits(GPIOE, GPIO_Pin_8);

for(int i=0;i<10;i++)
            for(int j=0;j<60;j++);
GPIO_ResetBits(GPIOE, GPIO_Pin_8);

}

#define Echo_Read() GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_13)

void EXTI15_10_IRQHandler (void)
{
    if(EXTI_GetITStatus(EXTI_Line13) != RESET) //asure generate EXTI Line
    {
        
        volatile uint32_t ultra_time=0;
EXTI_ClearITPendingBit(EXTI_Line13);
        
        if(Echo_Read() == 1)
            TIM_Cmd(TIM4, ENABLE);
        else if(Echo_Read() == 0)
        {
            TIM_Cmd(TIM4, DISABLE);
            ultra_time = TIM_GetCounter(TIM4);//us
            TIM_SetCounter(TIM4,0);

            //0.001 m,the speed is 340m/s when temperature is 15 degree.function:speed=331+0.6*t
            int temp =ultra_time* 17/100; 

    if ((temp > SONAR_MIN*SONAR_SCALE) && (temp < SONAR_MAX*SONAR_SCALE))
    {
       
        last_measure_time = measure_time;
measure_time = get_boot_time_us();
sonar_measure_time_interrupt = measure_time;
dt = ((float)(measure_time - last_measure_time)) / 1000000.0f;
valid_data = temp;
// the mode filter turned out to be more problematic
// than using the raw value of the sonar
//insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
sonar_mode = valid_data / SONAR_SCALE;
new_value = 1;
sonar_valid = true;
   
    else  sonar_valid = false;  
        }
    
    }

}

void sonar_config(void)
{
valid_data = 0;
GPIO_InitTypeDef GPIO_InitStructure;
// Enable GPIO clocks
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
//trig
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOE, &GPIO_InitStructure);
//echo
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;       
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;

GPIO_Init(GPIOD, &GPIO_InitStructure);


EXTI_InitTypeDef EXTI_InitStructure;
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOD, GPIO_PinSource13); 
EXTI_InitStructure.EXTI_Line = EXTI_Line13;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling; //
  EXTI_InitStructure.EXTI_LineCmd = ENABLE;

EXTI_Init(&EXTI_InitStructure); 


NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);

TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); 
TIM_TimeBaseStructure.TIM_Period = 41000-1; //not used
TIM_TimeBaseStructure.TIM_Prescaler =83; //1us pass,time count+1
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  
        TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_SetCounter(TIM4,0);
        TIM_ITConfig(TIM4,TIM_IT_Update,DISABLE);
TIM_Cmd(TIM4, DISABLE);

}

其他函数不用动,可以把多余的变量删掉,最小距离改小一点。

2、h文件中只要把  void UART4_IRQHandler(void);  换成   void EXTI15_10_IRQHandler (void);  就可以了。

3、改modules.mk文件和和CMakeLists.txt文件.mudules.mk和sonar.c在同一个目录中,CMakeLists.txt在Flow目录下。

在modules.mk文件中添加两个文件,就是外部中断需要需要用到的文件

px4Flow--替换超声波模块_第2张图片

CMakeLists.txt中也要加上。

px4Flow--替换超声波模块_第3张图片

这样基本就完成了。然后保存文件并编译下载就可以了。

px4Flow--替换超声波模块_第4张图片

这里的PD13引脚连的是TIM4ch2通道,所以也可以直接用定时器的输入捕获功能。

你可能感兴趣的:(optical,flow)