一直想实现无人的避障功能，但是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_SUPPORTEDclass 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(
### DescriptionPWM 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 driverif (!strcmp(argv[myoptind], "stop")) {return sr04::stop();}// Print driver usage information.return sr04::usage();
}CMakeLists.txt文件
px4_add_module(MODULE drivers__sr04MAIN sr04COMPILE_FLAGS-Wno-cast-align # TODO: fix and enableSRCSsr04_main.cppSR04.cppDEPENDSdrivers_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 appsthe 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了，哈哈哈哈