px4flow 添加/替换HC-SR04 US-100 US-015超声波

前段时间在x宝上购买了近600的px4flow光流，其自带的MB1043超声波在使用过程中发现测量数据老是跳变（很有可能是受电器噪声干扰，也可能是其他干扰，一直没弄明白），而国产3块多的超声波就没有问题，测量数据很稳定。没想到m帝这么贵的东西竟然如此不靠谱。所以打算换掉自带的超声波，使用市面上常见的US100超声波。

搜索全网，找到博主 hxudhdjuf 的px4Flow–替换超声波模块和PX4-flow代码编译两篇文章，按照文章的步骤编写调试，一直没有成功，花了一个通宵，修改了文章中sonar.c中配置（其他步骤请参照上述文章），编译下载后，用QGroundControl可以读出超声波数据，现将sonar.c的代码全部贴上，希望看到的能有所帮助。

引脚配置：
官方的4号引脚对应的PE8连接Trig，2号引脚对应的PD13连接Echo。测量数据如下图：
编译环境是ubuntu1604，px4代码编译对windows用户不友好，需要有Ubuntu系统和相应的编译环境。我这里是VMware虚拟机装的Ubuntu，一样也可以

关于编译环境安装见博主pix_csdn的文章：
Linux系统下搭建PX4/Pixhawk原生固件编译环境

/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *   Author: Laurens Mackay <mackayl@student.ethz.ch>
 *   		 Dominik Honegger <dominik.honegger@inf.ethz.ch>
 *   		 Petri Tanskanen <tpetri@inf.ethz.ch>
 *   		 Samuel Zihlmann <samuezih@ee.ethz.ch>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include "stm32f4xx.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_adc.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_tim.h"
#include "stm32f4xx_dma.h"
#include "misc.h"
#include "utils.h"
#include "usart.h"
#include "settings.h"
#include "sonar.h"
#include "sonar_mode_filter.h"
#include "stm32f4xx_exti.h" 
#include "stm32f4xx_syscfg.h"

#define SONAR_SCALE	1000.0f
#define SONAR_MIN	0.01f		/** 0.01m sonar minimum distance */
#define SONAR_MAX	4.0f		/** 4.0m sonar maximum distance */

#define atoi(nptr)  strtol((nptr), NULL, 10)
extern uint32_t get_boot_time_us(void);

//static char data_buffer[5]; // array for collecting decoded data

static volatile uint32_t last_measure_time = 0;
static volatile uint32_t measure_time = 0;
static volatile float dt = 0.0f;
static volatile int valid_data;
static volatile int data_counter = 0;
static volatile int data_valid = 0;
static volatile int new_value = 0;

static volatile uint32_t sonar_measure_time_interrupt = 0;
static volatile uint32_t sonar_measure_time = 0;

/* kalman filter states */
float x_pred = 0.0f; // m
float v_pred = 0.0f;
float x_post = 0.0f; // m
float v_post = 0.0f; // m/s

float sonar_raw = 0.0f;  // m

float sonar_mode = 0.0f;
bool sonar_valid = false;				/**< the mode of all sonar measurements */

/**
  * @brief  Triggers the sonar to measure the next value
  *
  * see datasheet for more info
  */
void sonar_trigger()
{
	volatile int i,j;
	GPIO_SetBits(GPIOE, GPIO_Pin_8);
	for(i=0;i<10;i++)
            for(j=0;j<15;j++);//12us
	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.0f/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;  
		}
	}

}

/**
  * @brief  Basic Kalman filter
  */
static void sonar_filter(void)
{
	/* no data for long time */
	if (dt > 0.25f) // more than 2 values lost
	{
		v_pred = 0;
	}

	x_pred = x_post + dt * v_pred;
	v_pred = v_post;

	float x_new = sonar_mode;
	sonar_raw = x_new;
	x_post = x_pred + global_data.param[PARAM_SONAR_KALMAN_L1] * (x_new - x_pred);
	v_post = v_pred + global_data.param[PARAM_SONAR_KALMAN_L2] * (x_new - x_pred);

}


/**
  * @brief  Read out newest sonar data
  *
  * @param  sonar_value_filtered Filtered return value
  * @param  sonar_value_raw Raw return value
  */
bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw)
{
	/* getting new data with only around 10Hz */
	if (new_value) {
		sonar_filter();
		new_value = 0;
		sonar_measure_time = get_boot_time_us();
	}

	/* catch post-filter out of band values */
	if (x_post < SONAR_MIN || x_post > SONAR_MAX) {
		sonar_valid = false;
	}

	*sonar_value_filtered = x_post;
	*sonar_value_raw = sonar_raw;

	return sonar_valid;
}

/**
 * @brief  Configures the sonar sensor Peripheral.
 */
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_PP;
	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;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG,ENABLE);
	EXTI_ClearITPendingBit(EXTI_Line13);
	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOD, EXTI_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 = 61000-1; //not used
	TIM_TimeBaseStructure.TIM_Prescaler =84-1; //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);
}

uint32_t get_sonar_measure_time()
{
    return sonar_measure_time;
}

uint32_t get_sonar_measure_time_interrupt()
{
    return sonar_measure_time_interrupt;
}