前言

软件：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_NRA12
	bool "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 name
	strncpy(_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 inactive
	stop();

	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;
	}

	// status
	int 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 bit
		unsigned 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 rate
		if ((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 controls
		uart_config.c_cflag &= ~CSIZE;
		uart_config.c_cflag |= CS8;			// 8-bit characters
		uart_config.c_cflag &= ~PARENB;			// no parity bit
		uart_config.c_cflag &= ~CSTOPB;			// only need 1 stop bit
		uart_config.c_cflag &= ~CRTSCTS;		// no hardware flowcontrol

		// setup for non-canonical mode
		uart_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 available
		uart_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 ago
	int64_t read_elapsed = hrt_elapsed_time(&_last_read);

	// the buffer for read chars is buflen minus null termination
	char readbuf[sizeof(_linebuf)] {};
	unsigned readlen = sizeof(readbuf) - 1;

	int ret = 0;
	float distance_m = -1.0f;

	// Check the number of bytes available in the buffer
	int bytes_available = 0;
	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
	if (!bytes_available) {
		perf_end(_sample_perf);
		return 0;
	}

	// parse entire buffer
	const 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 out
			if (read_elapsed > (kCONVERSIONINTERVAL * 2)) {
				/* flush anything in RX buffer */
				tcflush(_fd, TCIFLUSH);
				return ret;

			} else {
				return -EAGAIN;
			}
		}

		_last_read = hrt_absolute_time();

		// parse buffer
		for (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 parse
		bytes_available -= ret;

	} while (bytes_available > 0);

	// no valid measurement after parsing buffer
	if (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 collection
	if (collect() == -EAGAIN) {
		// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
		ScheduleClear();
		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_CFG
        group: 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(
### Description

Serial 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

### Examples

Attempt 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;
}

} // namespace

extern "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"
};
#endif

int 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_DEBUG
	printf("state: NRA12_PARSE_STATE%s\n", parser_state[*state]);
#endif

	return 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，然后观察能观察到测距传感器的数据输出