基于px4的hc-sr04-pwm超声波模块的驱动开发

    一直想实现无人的避障功能，但是px4源生代码又不支持避障，所以只能自己动手写。避障的基础条件还是获取距离数据，超声波模块就是最熟悉也是最简单的模块了。px4源生代码也支持了几种超声波模块，但是好像支持的几种又比较贵，而且都是用来当做高度使用的，不同的超声波型号都只支持一个模块的连接。总结一下，我写超声波驱动的原因就是1、想实现避障功能；2、现有的支持的超声波模块太贵；3、当前支持的超声波模块不能同时连接多个相同的模块，所以不能实现多个方向的距离探测功能。

在淘宝上买了hc-sr04超声波模块，3块钱一个，贼便宜了。同时还支持pwm，iic和uart三种通信方式，可以顺手练练几种不同通信方式的驱动编写了。

这次先写了基于pwm的驱动程序，相对来说比较简单些，正好可以学习一下怎么单独控制pixhawk飞控的aux引脚。

这里超声波模块的pwm通信方式的原理我就不说了，网上一大堆资料，而且模块的时候卖家也会给详细的资料。

如果写过STM32裸机程序，那么原理就非常简单了。只是在初始化的时候配置引脚为输出模式，然后通过控制引脚电平的高低来实现脉冲的输出。但是由于这是基于操作系统的开发，所以没有裸机开发来得那么直接，需要调用指定的函数实现这两个功能。这两个函数如下所示：

	px4_arch_configgpio(gpio);//配置函数
	px4_arch_gpiowrite(gpio, false);//写入函数

其中配置函数的的参数gpio是通过gpio = io_timer_channel_get_gpio_output(5);函数获取的。该函数封装了AUX引脚号到对应的stm32引脚的对应关系。

附录一下pixhawk2.4.8的aux引脚到stm32的引脚的对应关系，我觉得这个很有用：

aux_pin           stm32_pin          timer_ch

fmu_aux1           E14                      TIM1_CH4

fmu_aux2           E13                      TIM1_CH3

fmu_aux3           E11                      TIM1_CH2

fmu_aux4           E9                        TIM1_CH1

fmu_aux5           D13                      TIM4_CH2

fmu_aux6           D14                      TIM4_CH3

 

io_timer_channel_get_gpio_output(5);函数中的参数是输入的aux_pin的引脚号，但是需要用引脚号减一。这就很鸡肋既然已经封装了，为什么不更直接点，搞了好几天才发现这个问题，改了这个地方才成功。

现在能发送脉冲了，下面就得获取返回的pwm波的宽度了。以前写裸机程序的时候都是用中断来做，然后用计时器来计数。还是因为操作系统的限制，不能直接这么操作。还好，src/driver/pwm_input已经实现了pwm脉冲的计数，所以 只要运行它，就可以直接获取脉冲宽度，不好的是它是固定获取fmu_aux5引脚，通过定时器4来获取脉冲宽度，所以有一定的限制，但是也留下了可操作的空间。

现在发送和接受脉冲都有了，只要通电运行就行了。这里我还遇到了问题，直接将超声波模块的vcc和gnd接到了aux引脚的+和-引脚，没反应，一直以为是程序错误，后来用iic引脚的供电vcc和gnd进行供电，终于成功了。遇到相似情况的注意一下供电问题，应该是电压不够。

下面附上我写的代码：主要是借用了src/drivers/distance_sensor/ll40ls_pwm的框架

//这是sr04头文件sr04.h
#pragma once
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>

#include <uORB/topics/pwm_input.h>
#include <uORB/Subscription.hpp>
#include <board_config.h>
#include <drivers/device/device.h>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <perf/perf_counter.h>

#include <px4_platform_common/px4_config.h>//添加引脚

using namespace time_literals;

// Device limits
static constexpr float SR04_MIN_DISTANCE{0.05f};
static constexpr float SR04_MAX_DISTANCE{25.00f};
static constexpr float SR04_MAX_DISTANCE_V2{35.00f};

// Normal conversion wait time.
static constexpr uint32_t SR04_CONVERSION_INTERVAL{200_ms};//由于添加了脉冲，所以一点间隔时间

// Maximum time to wait for a conversion to complete.
static constexpr uint32_t SR04_CONVERSION_TIMEOUT{300_ms};//同时调大最大间隔时间

#define DIRECT_PWM_OUTPUT_CHANNELS 6

#if DIRECT_PWM_OUTPUT_CHANNELS >= 6
#define GPIO_VDD_RANGEFINDER_EN_CHAN 5 // use pin 6
#define LIDAR_LITE_PWM_SUPPORTED

class SR04 : public px4::ScheduledWorkItem
{
public:
	SR04(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
	virtual ~SR04();

	int init();
	void start();
	void stop();

	void print_info();
protected:

	int collect();
	int measure();

	void Run() override;

private:
	uint32_t get_measure_interval() const { return SR04_CONVERSION_INTERVAL; };


	uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)};

	pwm_input_s _pwm{};

	PX4Rangefinder	_px4_rangefinder;

	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "sr04: comms errors")};
	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "sr04: read")};
	perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "sr04: resets")};
	perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "sr04: zero resets")};

		struct hrt_call		_engagecall;
	struct hrt_call		_disengagecall;
        uint32_t gpio{};
};

#endif

sr04.cpp文件

#include "SR04.h"



#include <px4_arch/io_timer.h>

SR04::SR04(const uint8_t rotation) :
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
	_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation)
{
	_px4_rangefinder.set_min_distance(SR04_MIN_DISTANCE);
	_px4_rangefinder.set_max_distance(SR04_MAX_DISTANCE);
	_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian

	//px4_arch_configgpio(GPIO_TIM4_CH3OUT_2);//aux6输出



}

SR04::~SR04()
{
	stop();
	perf_free(_sample_perf);
	perf_free(_comms_errors);
	perf_free(_sensor_resets);
	perf_free(_sensor_zero_resets);
}


int
SR04::init()
{
//在初始化函数中进行引脚的配置，同时将该引脚拉低
	gpio = io_timer_channel_get_gpio_output(5);//获取fmu_aux6引脚为输出
	px4_arch_configgpio(gpio);
	px4_arch_gpiowrite(gpio, false);//将该引脚拉低
		// schedule trigger on and off calls
	//hrt_call_every(&_engagecall, 0, 500000, (hrt_callout)&engage, this);

		// schedule trigger on and off calls
	//hrt_call_every(&_disengagecall, 0 + (30 * 1000), 50000,(hrt_callout)&disengage, this);
	start();

	return PX4_OK;
}

void
SR04::start()
{

	ScheduleOnInterval(get_measure_interval());
}

void
SR04::stop()
{
	ScheduleClear();
}

void
SR04::Run()
{

	measure();
}

int
SR04::measure()
{
	perf_begin(_sample_perf);

	const hrt_abstime timestamp_sample = hrt_absolute_time();
	px4_arch_gpiowrite(gpio, true);//将引脚拉高，产生脉冲的上升沿
	int i=5000;
	while(i-->0);//保持一会
	px4_arch_gpiowrite(gpio, false);//将引脚拉低，产生脉冲的下降沿，也就形成了一个脉冲
	if (PX4_OK != collect()) {
		PX4_DEBUG("collection error");
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return PX4_ERROR;
	}

	const float current_distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */

	PX4_DEBUG("measured");
	/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
	if (current_distance <= 0.0f) {
		perf_count(_sensor_zero_resets);
		perf_end(_sample_perf);
	}

	_px4_rangefinder.update(timestamp_sample, current_distance);

	perf_end(_sample_perf);
	return PX4_OK;
}

int
SR04::collect()
{
	pwm_input_s pwm_input;

	if (_sub_pwm_input.update(&pwm_input)) {

		_pwm = pwm_input;

		return PX4_OK;
	}

	return EAGAIN;
}

void
SR04::print_info()
{
	perf_print_counter(_sample_perf);
	perf_print_counter(_comms_errors);
	perf_print_counter(_sensor_resets);
	perf_print_counter(_sensor_zero_resets);
	printf("poll interval:  %u \n", get_measure_interval());
}

sr04_main.cpp 这个文件没什么要改的，只要改下相应的名字就行

/****************************************************************************
 *
 *   Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
 *
 * 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.
 *
 ****************************************************************************/

/**
 * @file SR04.cpp
 * @author Allyson Kreft
 * @author Johan Jansen <jnsn.johan@gmail.com>
 * @author Ban Siesta <bansiesta@gmail.com>
 * @author James Goppert <james.goppert@gmail.com>
 *
 * Interface for the PulsedLight Lidar-Lite range finders.
 */

#include <cstdlib>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <systemlib/err.h>

#include <board_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>

#include "SR04.h"



/**
 * @brief Local functions in support of the shell command.
 */
namespace sr04
{

SR04 *instance = nullptr;

int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
int status();
int stop();
int usage();

/**
 * Start the pwm driver.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
int
start_pwm(const uint8_t rotation)
{
	if (instance != nullptr) {
		PX4_ERR("already started");
		return PX4_OK;
	}

	instance = new SR04(rotation);

	if (instance == nullptr) {
		PX4_ERR("Failed to instantiate the driver");
		return PX4_ERROR;
	}

	// Initialize the sensor.
	if (instance->init() != PX4_OK) {
		PX4_ERR("Failed to initialize sr04.");
		delete instance;
		instance = nullptr;
		return PX4_ERROR;
	}

	// Start the driver.
	instance->start();

	PX4_INFO("driver started");
	return PX4_OK;
}

/**
 * @brief Prints status info about the driver.
 */
int
status()
{
	if (instance == nullptr) {
		PX4_ERR("driver not running");
		return PX4_ERROR;
	}

	instance->print_info();
	return PX4_OK;
}

/**
 * @brief Stops the driver
 */
int
stop()
{
	if (instance != nullptr) {
		delete instance;
		instance = nullptr;
	}

	return PX4_OK;
}

/**
 * @brief Displays driver usage at the console.
 */
int
usage()
{
	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description

PWM driver for Sr04 rangefinders.

The sensor/driver must be enabled using the parameter SENS_EN_SR04.

Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("sr04", "driver");
	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
	PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
	PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
	return PX4_OK;
}

} // namespace sr04


/**
 * @brief Driver 'main' command.
 */
extern "C" __EXPORT int sr04_main(int argc, char *argv[])
{
	const char *myoptarg = nullptr;

	int ch = 0;
	int myoptind = 1;

	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;

	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
		switch (ch) {
		case 'R':
			rotation = (uint8_t)atoi(myoptarg);
			PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
			break;

		default:
			return sr04::usage();
		}
	}

	if (myoptind >= argc) {
		return sr04::usage();
	}

	// Start the driver.
	if (!strcmp(argv[myoptind], "start")) {
		return sr04::start_pwm(rotation);
	}

	// Print the driver status.
	if (!strcmp(argv[myoptind], "status")) {
		return sr04::status();
	}

	// Stop the driver
	if (!strcmp(argv[myoptind], "stop")) {
		return sr04::stop();
	}

	// Print driver usage information.
	return sr04::usage();
}

CMakeLists.txt文件

px4_add_module(
	MODULE drivers__sr04
	MAIN sr04
	COMPILE_FLAGS
		-Wno-cast-align # TODO: fix and enable
	SRCS
		sr04_main.cpp
		SR04.cpp
	DEPENDS
		drivers_rangefinder
		#arch_io_pins # LidarLitePWM
	)

 

差不多了，下面只要将超声波模块的echo引脚接到fmu_aux5引脚，将trig引脚接到fmu_aux6引脚就行。

进入nsh，启动pwm_input start，然后sr04 start就可以实现超声波模块了。这时候应该能听到超声波模块有类似脉冲的声音。如果要获取距离数据，需要到src/examples/px4_simple_app中订阅distance_sensor主题，并打印输出就可以，参考代码如下：

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
 *
 * 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.
 *
 ****************************************************************************/

/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/distance_sensor.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{

	PX4_INFO("Hello Sky!");

	/* subscribe to sensor_combined topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	int distance_sensor_sub_fd = orb_subscribe(ORB_ID(distance_sensor));//订阅该topic
	/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);
	orb_set_interval(distance_sensor_sub_fd, 200);
	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		{ .fd = distance_sensor_sub_fd,   .events = POLLIN },//加入到订阅中
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};

	int error_counter = 0;

	for (int i = 0; i < 50000; i++) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = px4_poll(fds, 2, 1000);

		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			PX4_ERR("Got no data within a second");

		} else if (poll_ret < 0) {
			/* this is seriously bad - should be an emergency */
			if (error_counter < 10 || error_counter % 50 == 0) {
				/* use a counter to prevent flooding (and slowing us down) */
				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
			}

			error_counter++;

		} else {

			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct sensor_combined_s raw;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
					 (double)raw.accelerometer_m_s2[0],
					 (double)raw.accelerometer_m_s2[1],
					 (double)raw.accelerometer_m_s2[2]);

				/* set att and publish this information for other apps
				 the following does not have any meaning, it's just an example
				*/
				att.q[0] = raw.accelerometer_m_s2[0];
				att.q[1] = raw.accelerometer_m_s2[1];
				att.q[2] = raw.accelerometer_m_s2[2];

				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
			}
			if (fds[1].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct distance_sensor_s raw;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub_fd, &raw);//如果有新的距离值就copy过来
				PX4_INFO("distance_sensor:\t%8.4f\t%ld\t\n",//输出距离
					 (double)raw.current_distance,
					 (long)raw.timestamp);
			}

			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}

	PX4_INFO("exiting");

	return 0;
}

pwm功能做完了，但是pwm脉冲宽度又是通过定时器中断来获取的，aux引脚只有timer1和timer4，所以想接8个超声波模块的话，又不够用了。接下来就是通过iic方式获取超声波距离值，这下应该没什么问题了，不写了，我得赶紧去一直iic了，哈哈哈哈