前言

软件：PX4 1.14.0稳定版
硬件：纳雷NRA12，pixhawk4
仿照原生固件tfmini的驱动进行编写
源码地址：
https://gitee.com/Mbot_admin/px4-1.14.0-csdn

修改
src/drivers/distance_sensor/CMakeLists.txt
添加

add_subdirectory(nra12)

修改
src/drivers/distance_sensor/Kconfig
添加

select DRIVERS_DISTANCE_SENSOR_NRA12

在src/drivers/distance_sensor/目录下添加一个nra12文件夹
在nra12文件夹下添加Kconfig，NRA12.cpp，NRA12.hpp，nra12_main.cpp，module.yaml，nra12_parser.cpp，nra12_parser.h。

Kconfig如下：

menuconfig DRIVERS_DISTANCE_SENSOR_NRA12bool "nra12"default n---help---Enable support for nra12

NRA12.cpp如下：

#include "NRA12.hpp"#include <lib/drivers/device/Device.hpp>
#include <fcntl.h>NRA12::NRA12(const char *port, uint8_t rotation) :ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),_px4_rangefinder(0, rotation)
{// store port namestrncpy(_port, port, sizeof(_port) - 1);// enforce null termination_port[sizeof(_port) - 1] = '\0';device::Device::DeviceId device_id;device_id.devid_s.devtype = DRV_DIST_DEVTYPE_NRA12;device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'if (bus_num < 10) {device_id.devid_s.bus = bus_num;}_px4_rangefinder.set_device_id(device_id.devid);_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
}NRA12::~NRA12()
{// make sure we are truly inactivestop();perf_free(_sample_perf);perf_free(_comms_errors);
}int
NRA12::init()
{int32_t hw_model = 1; // only one model so far...switch (hw_model) {case 1: // NRA12 (12m, 100 Hz)// Note:// Sensor specification shows 0.3m as minimum, but in practice// 0.3 is too close to minimum so chattering of invalid sensor decision// is happening sometimes. this cause EKF to believe inconsistent range readings.// So we set 0.4 as valid minimum._px4_rangefinder.set_min_distance(0.1f);_px4_rangefinder.set_max_distance(30.0f);_px4_rangefinder.set_fov(math::radians(1.15f));break;default:PX4_ERR("invalid HW model %" PRId32 ".", hw_model);return -1;}// statusint ret = 0;do { // create a scope to handle exit conditions using break// open fd_fd = ::open(_port, O_RDWR | O_NOCTTY);if (_fd < 0) {PX4_ERR("Error opening fd");return -1;}// baudrate 115200, 8 bits, no parity, 1 stop bitunsigned speed = B115200;termios uart_config{};int termios_state{};tcgetattr(_fd, &uart_config);// clear ONLCR flag (which appends a CR for every LF)uart_config.c_oflag &= ~ONLCR;// set baud rateif ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {PX4_ERR("CFG: %d ISPD", termios_state);ret = -1;break;}if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {PX4_ERR("CFG: %d OSPD\n", termios_state);ret = -1;break;}if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {PX4_ERR("baud %d ATTR", termios_state);ret = -1;break;}uart_config.c_cflag |= (CLOCAL | CREAD);	// ignore modem controlsuart_config.c_cflag &= ~CSIZE;uart_config.c_cflag |= CS8;			// 8-bit charactersuart_config.c_cflag &= ~PARENB;			// no parity bituart_config.c_cflag &= ~CSTOPB;			// only need 1 stop bituart_config.c_cflag &= ~CRTSCTS;		// no hardware flowcontrol// setup for non-canonical modeuart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);uart_config.c_oflag &= ~OPOST;// fetch bytes as they become availableuart_config.c_cc[VMIN] = 1;uart_config.c_cc[VTIME] = 1;if (_fd < 0) {PX4_ERR("FAIL: laser fd");ret = -1;break;}} while (0);// close the fd::close(_fd);_fd = -1;if (ret == PX4_OK) {start();}return ret;
}int
NRA12::collect()
{perf_begin(_sample_perf);// clear buffer if last read was too long agoint64_t read_elapsed = hrt_elapsed_time(&_last_read);// the buffer for read chars is buflen minus null terminationchar readbuf[sizeof(_linebuf)] {};unsigned readlen = sizeof(readbuf) - 1;int ret = 0;float distance_m = -1.0f;// Check the number of bytes available in the bufferint bytes_available = 0;::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);if (!bytes_available) {perf_end(_sample_perf);return 0;}// parse entire bufferconst hrt_abstime timestamp_sample = hrt_absolute_time();do {// read from the sensor (uart buffer)ret = ::read(_fd, &readbuf[0], readlen);
// ret = ::write(_fd,&readbuf,10);
// if (ret < 0) {
//     PX4_ERR("write err: %d", ret);
// }if (ret < 0) {PX4_ERR("read err: %d", ret);perf_count(_comms_errors);perf_end(_sample_perf);// only throw an error if we time outif (read_elapsed > (kCONVERSIONINTERVAL * 2)) {/* flush anything in RX buffer */tcflush(_fd, TCIFLUSH);return ret;} else {return -EAGAIN;}}_last_read = hrt_absolute_time();// parse bufferfor (int i = 0; i < ret; i++) {// PX4_INFO("readbuf=%x\n",readbuf[i]);// PX4_INFO("ret=%d\n",ret);// PX4_INFO("bytes_available=%d\n",bytes_available);nra12_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);}// bytes left to parsebytes_available -= ret;} while (bytes_available > 0);// no valid measurement after parsing bufferif (distance_m < 0.0f){perf_end(_sample_perf);return -EAGAIN;}// publish most recent valid measurement from buffer_px4_rangefinder.update(timestamp_sample, distance_m);perf_end(_sample_perf);return PX4_OK;
}void
NRA12::start()
{// schedule a cycle to start things (the sensor sends at 100Hz, but we run a bit faster to avoid missing data)ScheduleOnInterval(7_ms);
}void
NRA12::stop()
{ScheduleClear();
}void
NRA12::Run()
{// fds initialized?if (_fd < 0) {// open fd_fd = ::open(_port, O_RDWR | O_NOCTTY);}// perform collectionif (collect() == -EAGAIN) {// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bpsScheduleClear();ScheduleOnInterval(7_ms, 87 * 9);return;}
}void
NRA12::print_info()
{printf("Using port '%s'\n", _port);perf_print_counter(_sample_perf);perf_print_counter(_comms_errors);
}

NRA12.hpp如下：

#pragma once#include <termios.h>#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <uORB/topics/distance_sensor.h>#include "nra12_parser.h"#define NRA12_DEFAULT_PORT	"/dev/ttyS3"using namespace time_literals;class NRA12 : public px4::ScheduledWorkItem
{
public:NRA12(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);virtual ~NRA12();int init();void print_info();private:int collect();void Run() override;void start();void stop();PX4Rangefinder	_px4_rangefinder;NRA12_PARSE_STATE _parse_state {NRA12_PARSE_STATE::STATE0_UNSYNC};char _linebuf[24] {};char _port[20] {};static constexpr int kCONVERSIONINTERVAL{9_ms};int _fd{-1};unsigned int _linebuf_index{0};hrt_abstime _last_read{0};perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};};

module.yaml 如下：

module_name: nanoradar nra12 lidar
serial_config:- command: nra12 start -d ${SERIAL_DEV}port_config_param:name: SENS_NRA12_CFGgroup: Sensors

nra12_main.cpp如下：

#include "NRA12.hpp"#include <px4_platform_common/getopt.h>/*** Local functions in support of the shell command.*/
namespace nra12
{NRA12	*g_dev{nullptr};int start(const char *port, uint8_t rotation);
int status();
int stop();
int usage();int
start(const char *port, uint8_t rotation)
{if (g_dev != nullptr) {PX4_ERR("already started");return PX4_OK;}// Instantiate the driver.g_dev = new NRA12(port, rotation);if (g_dev == nullptr) {PX4_ERR("driver start failed");return PX4_ERROR;}if (OK != g_dev->init()) {PX4_ERR("driver start failed");delete g_dev;g_dev = nullptr;return PX4_ERROR;}return PX4_OK;
}int
status()
{if (g_dev == nullptr) {PX4_ERR("driver not running");return 1;}printf("state @ %p\n", g_dev);g_dev->print_info();return 0;
}int stop()
{if (g_dev != nullptr) {PX4_INFO("stopping driver");delete g_dev;g_dev = nullptr;PX4_INFO("driver stopped");} else {PX4_ERR("driver not running");return 1;}return PX4_OK;
}int
usage()
{PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
### DescriptionSerial bus driver for the Benewake NRA12 LiDAR.Most boards are configured to enable/start the driver on a specified UART using the SENS_NRA12_CFG parameter.Setup/usage information: https://docs.px4.io/master/en/sensor/sonar.html### ExamplesAttempt to start driver on a specified serial device.
$ sonar start -d /dev/ttyS1
Stop driver
$ sonar stop
)DESCR_STR");PRINT_MODULE_USAGE_NAME("nra12", "driver");PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status");PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status");return PX4_OK;
}} // namespaceextern "C" __EXPORT int nra12_main(int argc, char *argv[])
{int ch = 0;uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;const char *device_path = NRA12_DEFAULT_PORT;int myoptind = 1;const char *myoptarg = nullptr;while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {switch (ch) {case 'R':rotation = (uint8_t)atoi(myoptarg);break;case 'd':device_path = myoptarg;break;default:PX4_WARN("Unknown option!");return PX4_ERROR;}}if (myoptind >= argc) {PX4_ERR("unrecognized command");return nra12::usage();}if (!strcmp(argv[myoptind], "start")) {if (strcmp(device_path, "") != 0) {return nra12::start(device_path, rotation);} else {PX4_WARN("Please specify device path!");return nra12::usage();}} else if (!strcmp(argv[myoptind], "stop")) {return nra12::stop();} else if (!strcmp(argv[myoptind], "status")) {return nra12::status();}return nra12::usage();
}

nra12_parser.cpp如下：

#include "nra12_parser.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <termios.h>#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>//#define NRA12_DEBUG#ifdef NRA12_DEBUG
#include <stdio.h>const char *parser_state[] = {"0_UNSYNC","3_GOT_DIST_L","4_GOT_DIST_H","8_GOT_QUALITY""9_GOT_CHECKSUM"
};
#endifint nra12_parse(char c, char *parserbuf, unsigned *parserbuf_index, NRA12_PARSE_STATE *state, float *dist)
{int ret = -1;//char *end;
// PX4_INFO("parse");switch (*state) {case NRA12_PARSE_STATE::STATE12_GOT_END2:// PX4_INFO("STATE12_GOT_END2");// PX4_INFO("c=%x",c);if (c == 0xaa) {*state = NRA12_PARSE_STATE::STATE1_GOT_START1;} else {*state = NRA12_PARSE_STATE::STATE0_UNSYNC;}break;case NRA12_PARSE_STATE::STATE0_UNSYNC:// PX4_INFO("STATE0_UNSYNC");// PX4_INFO("c=%x",c);if (c == 0xaa) {*state = NRA12_PARSE_STATE::STATE1_GOT_START1;}break;case NRA12_PARSE_STATE::STATE1_GOT_START1:// PX4_INFO("STATE1_GOT_START1");// PX4_INFO("c=%x",c);if (c == 0xaa) {*state = NRA12_PARSE_STATE::STATE2_GOT_START2;}else{*state = NRA12_PARSE_STATE::STATE0_UNSYNC;}break;case NRA12_PARSE_STATE::STATE2_GOT_START2:// PX4_INFO("STATE2_GOT_START2");// PX4_INFO("c=%x",c);if(c == 0x0c){*state = NRA12_PARSE_STATE::STATE2_GOT_0C;}else{*state = NRA12_PARSE_STATE::STATE0_UNSYNC;}break;case NRA12_PARSE_STATE::STATE2_GOT_0C:// PX4_INFO("STATE2_GOT_0C");// PX4_INFO("c=%x",c);if(c == 0x07){*state = NRA12_PARSE_STATE::STATE2_GOT_07;}else{*state = NRA12_PARSE_STATE::STATE0_UNSYNC;}break;case NRA12_PARSE_STATE::STATE2_GOT_07:// PX4_INFO("STATE2_GOT_07");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE3_GOT_Index;break;case NRA12_PARSE_STATE::STATE3_GOT_Index:// PX4_INFO("STATE3_GOT_Index");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE4_GOT_Res;break;case NRA12_PARSE_STATE::STATE4_GOT_Res:// PX4_INFO("STATE4_GOT_Res");// PX4_INFO("c=%x",c);parserbuf[1]=c;*state = NRA12_PARSE_STATE::STATE5_GOT_DIST_H;break;case NRA12_PARSE_STATE::STATE5_GOT_DIST_H:// PX4_INFO("STATE5_GOT_DIST_H");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE6_GOT_DIST_L;parserbuf[2]=c;break;case NRA12_PARSE_STATE::STATE6_GOT_DIST_L:// PX4_INFO("STATE6_GOT_DIST_L");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE7_GOT_UNUSE1;break;case NRA12_PARSE_STATE::STATE7_GOT_UNUSE1:// PX4_INFO("STATE7_GOT_UNUSE1");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE8_GOT_UNUSE2;break;case NRA12_PARSE_STATE::STATE8_GOT_UNUSE2://  PX4_INFO("STATE8_GOT_UNUSE2");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE9_GOT_UNUSE3;break;case NRA12_PARSE_STATE::STATE9_GOT_UNUSE3:// PX4_INFO("STATE9_GOT_UNUSE3");// PX4_INFO("c=%x",c);*state = NRA12_PARSE_STATE::STATE10_GOT_UNUSE4;break;case NRA12_PARSE_STATE::STATE10_GOT_UNUSE4://  PX4_INFO("STATE10_GOT_UNUSE4");// PX4_INFO("c=%x",c);if(c==0x55){*state = NRA12_PARSE_STATE::STATE11_GOT_END1;}break;case NRA12_PARSE_STATE::STATE11_GOT_END1://  PX4_INFO("STATE11_GOT_END1");// PX4_INFO("c=%x",c);if(c==0x55){*state = NRA12_PARSE_STATE::STATE12_GOT_END2;unsigned int t1 = parserbuf[2];unsigned int t2 = parserbuf[1];t2 <<= 8;t2 += t1;if (t2 < 0xFFFFu) {*dist = ((float)t2) / 100;}//	 PX4_INFO("dist=%lf\n",(double)*dist);}else {*state = NRA12_PARSE_STATE::STATE0_UNSYNC;}break;}#ifdef NRA12_DEBUGprintf("state: NRA12_PARSE_STATE%s\n", parser_state[*state]);
#endifreturn ret;
}

nra12_parser.h如下：

enum class NRA12_PARSE_STATE {STATE0_UNSYNC = 0,STATE1_GOT_START1,STATE2_GOT_START2,STATE2_GOT_0C,STATE2_GOT_07,STATE3_GOT_Index,STATE4_GOT_Res,STATE5_GOT_DIST_H,STATE6_GOT_DIST_L,STATE7_GOT_UNUSE1,STATE8_GOT_UNUSE2,STATE9_GOT_UNUSE3,STATE10_GOT_UNUSE4,STATE11_GOT_END1,STATE12_GOT_END2
};int nra12_parse(char c, char *parserbuf, unsigned *parserbuf_index, NRA12_PARSE_STATE *state, float *dist);

修改src/drivers/drv_sensor.h
添加

#define DRV_DIST_DEVTYPE_NRA12      0xC2

修改boards/px4/fmu-v5/default.px4board
添加

CONFIG_COMMON_DISTANCE_SENSOR_NRA12=y

最后编译并将固件下载到飞控，将NRA12通过串口连接到UART&I2C B口，将参数SENS_NRA12_CFG设置成TELEM/SERIAL4，然后观察能观察到测距传感器的数据输出