添加纳雷-nanoradar-MR72毫米波雷达驱动

src/drivers/distance_sensor/nanoradar_mr72/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2016-2020 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__distance_sensor__nanoradar_mr72
+	MAIN nanoradar_mr72
+	SRCS
+		nanoradar_mr72.cpp
+		nanoradar_mr72.hpp
+		nanoradar_mr72_main.cpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		drivers_rangefinder
+		px4_work_queue
+	)

src/drivers/distance_sensor/nanoradar_mr72/module.yaml

@@ -0,0 +1,6 @@
+module_name: nanoradar mr72
+serial_config:
+    - command: nanoradar_mr72 start ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_NR72_CFG
+        group: Sensors

src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72.cpp

@@ -0,0 +1,353 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016-2020 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.
+ *
+ ****************************************************************************/
+
+#include "nanoradar_mr72.hpp"
+
+#include <lib/drivers/device/Device.hpp>
+
+nanoradar_mr72::nanoradar_mr72(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.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_device_type(DRV_DIST_DEVTYPE_MR72);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
+	_px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_FORWARD_FACING);
+	_px4_rangefinder.set_hfov(math::radians(112.f));
+	_px4_rangefinder.set_vfov(math::radians(14.f));
+
+
+	_px4_rangefinder.set_min_distance(MR72_MIN_DISTANCE);
+	_px4_rangefinder.set_max_distance(MR72_MAX_DISTANCE);
+}
+
+nanoradar_mr72::~nanoradar_mr72()
+{
+	stop();
+
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int nanoradar_mr72::init()
+{
+	start();
+
+	return PX4_OK;
+}
+
+int nanoradar_mr72::collect()
+{
+	perf_begin(_sample_perf);
+
+	int bytes_processed = 0;
+	// int distance_cm = -1;
+	int index = 0;
+
+	float distance_m = -1.0f;
+
+	bool checksum_passed = false;
+
+	// Read from the sensor UART buffer.
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	int bytes_read = ::read(_file_descriptor, &_buffer[0], sizeof(_buffer));
+
+	float distances_m[5] = {0.0f}; 	//测量得到的5个距离
+	float snr[5] = {0.0f}; 		//测量得到的5个距离的信噪比
+	uint8_t number_of_detection = 0; 	// 目标输出状态1，探测到目标的个数
+	uint8_t roller_count = 0;		// 目标输出状态2，循环技术0-1-2-3，每个周期改变一次
+	struct radar_data{
+		uint8_t index;
+		uint16_t azimuth;
+		uint16_t range;
+		uint16_t speed;
+		uint8_t Rsvdl;
+		uint8_t RollCount;
+		uint8_t rcs;
+	}mr72_data;
+
+	struct radar_data_cal{
+		bool  valid; 		// 当前值是否有效
+		uint8_t index; 		// 目标ID
+		float azimuth_deg; 	// 目标方位角，单位为deg
+		float range_m; 		// 目标距离，单位为m
+		float speed_ms; 	// 目标速度，单位m/s
+		uint8_t Rsvdl;		// --
+		uint8_t RollCount;	// 计数位
+		uint8_t rcs;		// 目前固定为0
+	}mr72_data_cal; //目标输出信息真实值
+	struct radar_data_2{
+		uint8_t index;
+		uint8_t AzimuthH;
+		uint8_t RangeH;
+		uint8_t RangeL;
+		uint8_t AzimuthL;
+		uint8_t VrelH:3;
+		uint8_t Rsvdl:3;
+		uint8_t RollCount:2;
+		uint8_t VreL;
+		uint8_t Rcs;
+	}mr72_data_2;
+	union radar_union{
+		uint8_t[8] data;
+		struct radar_data_2;
+
+	}
+	struct mr72_data_cal mr72_data_cal[8];
+	uint8_t counter =0;
+	enum Receive_State{
+		Receive_idle = 		0;
+		Receive_sys_state = 	1;
+		Receiving_targets = 	2;
+		Receive_end = 		3;
+	}
+	uint8_t receive_state = Receive_State::Receive_idle;
+
+	if (bytes_read > 0) {
+		index = bytes_read - 14; // 14bytes
+
+		while (index >= 0 && !checksum_passed) {
+			if (_buffer[index] == MR72_PACKET_HDR && _buffer[index+1] == MR72_PACKET_HDR) { // 0XAA 0XAA 开头
+				bytes_processed = index;
+
+				while (bytes_processed < bytes_read && !checksum_passed) {
+					{
+						if( _buffer[index+12] ==  _buffer[index+13] && _buffer[index+12] == MR72_PACKET_END ){ //与第18个数相等 85
+
+							checksum_passed = true;
+
+							uint16_t message_id = (uint16_t)_buffer[index+2] + (uint16_t)_buffer[index+3] << 8;
+							switch(message_id){
+								case 0x200 :// MR72配置
+										break;
+								case 0x201 :// MR72返回值
+										break; //以上两个与CAN协议的相同
+								case 0x60A :// MR72系统状态
+										break;
+								case 0x70B :// 目标输出状态
+									number_of_detection = _buffer[index+4];
+									roller_count = _buffer[index+5] >> 6; // 8、9位
+									for(int i=0;i<7;i++){	// reset valid flag
+										mr72_data_cal[i].valid = false;
+									}
+									counter=0;
+									receive_state = Receive_State::Receive_sys_state;
+										break;
+								case 0x70C :// 目标输出信息
+									mr72_data.index = _buffer[index+4];
+									mr72_data.azimuth = (uint16_t)_buffer[index+5] << 8 + _buffer[index+8];
+									mr72_data.range = (uint16_t)_buffer[index+6] << 8 + _buffer[index+7];
+									mr72_data.speed = ((_buffer[index+9]&&0b11100000)>>5)<<8 + _buffer[index+10];
+									mr72_data.Rsvdl = ((_buffer[index+9]&&0b00011100)>>2);
+									mr72_data.RollCount = (_buffer[index+9]&&0b00000011);
+									mr72_data.rcs = _buffer[index+11];
+
+									mr72_data_cal[counter].index = mr72_data.index;
+									mr72_data_cal[counter].RollCount = mr72_data.RollCount;
+									mr72_data_cal[counter].range_m = mr72_data.range * 0.01f;
+									mr72_data_cal[counter].speed_ms = mr72_data.speed * 0.05f - 35.0f;
+									mr72_data_cal[counter].azimuth_deg = mr72_data.azimuth * 0.01f - 90.0f;
+									mr72_data_cal[counter].rcs = 0;
+									mr72_data_cal[counter].valid = true;
+									if(counter<number_of_detection-1) {
+										receive_state = Receive_State::Receive_targets;
+									}else if(counter == number_of_detection-1) {
+										receive_state = Receive_State::Receive_end;
+									}
+									counter++;
+										break;
+							}
+
+						}
+
+
+					}
+
+					bytes_processed++;
+				}
+			}
+
+			index--;
+
+		}
+	}
+
+	if (!checksum_passed) {
+		return -EAGAIN;
+	}
+
+	float min = distances_m[0];
+	int8_t min_index = 1;
+	int8_t empty_index = -1;
+	for(int i = 0; i < 5; i++){
+		if(distances_m[i] > 0.01f){
+			if(min > distances_m[i]){
+				min = distances_m[i];
+				min_index = i;
+			}
+			empty_index = 1; //排除5个数都是0
+		}
+	}
+
+	distance_m = min;
+
+	if (empty_index == -1){
+		distance_m = MR72_MAX_DISTANCE + 2.0f;// 以上5个数值都是0，即没有障碍物，则直接给最大距离即可。
+	}
+
+	// TODO
+	// 给个滤波？ 在位置较低的时候不使用此数据？
+
+
+	// 这里给的信噪比原始值，与其规定的有所区别
+	_px4_rangefinder.update(timestamp_sample, distance_m,  min_index==-1 ? 98 : ((int8_t)snr[min_index]) );
+
+	perf_end(_sample_perf);
+
+	return PX4_OK;
+}
+
+int nanoradar_mr72::open_serial_port(const speed_t speed)
+{
+	// File descriptor initialized?
+	if (_file_descriptor > 0) {
+		PX4_DEBUG("serial port already open");
+		return PX4_OK;
+	}
+
+	// Configure port flags for read/write, non-controlling, non-blocking.
+	int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	// Open the serial port.
+	_file_descriptor = ::open(_port, flags);
+
+	if (_file_descriptor < 0) {
+		PX4_ERR("open failed (%i)", errno);
+		return PX4_ERROR;
+	}
+
+	if (!isatty(_file_descriptor)) {
+		PX4_WARN("not a serial device");
+		return PX4_ERROR;
+	}
+
+	termios uart_config{};
+
+	// Store the current port configuration. attributes.
+	tcgetattr(_file_descriptor, &uart_config);
+
+	uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+
+	// Clear ONLCR flag (which appends a CR for every LF).
+	uart_config.c_oflag &= ~ONLCR;
+
+	// No parity, one stop bit.
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	// No line processing - echo off, echo newline off, canonical mode off, extended input processing off, signal chars off
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+
+	// Set the input baud rate in the uart_config struct.
+	int termios_state = cfsetispeed(&uart_config, speed);
+
+	if (termios_state < 0) {
+		PX4_ERR("CFG: %d ISPD", termios_state);
+		::close(_file_descriptor);
+		return PX4_ERROR;
+	}
+
+	// Set the output baud rate in the uart_config struct.
+	termios_state = cfsetospeed(&uart_config, speed);
+
+	if (termios_state < 0) {
+		PX4_ERR("CFG: %d OSPD", termios_state);
+		::close(_file_descriptor);
+		return PX4_ERROR;
+	}
+
+	// Apply the modified port attributes.
+	termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
+
+	if (termios_state < 0) {
+		PX4_ERR("baud %d ATTR", termios_state);
+		::close(_file_descriptor);
+		return PX4_ERROR;
+	}
+
+	PX4_INFO("successfully opened UART port %s", _port);
+	return PX4_OK;
+}
+
+void nanoradar_mr72::Run()
+{
+	// Ensure the serial port is open.
+	open_serial_port();
+
+	collect();
+}
+
+void nanoradar_mr72::start()
+{
+	// Schedule the driver at regular intervals.
+	ScheduleOnInterval(MR72_MEASURE_INTERVAL, 0);
+}
+
+void nanoradar_mr72::stop()
+{
+	// Ensure the serial port is closed.
+	::close(_file_descriptor);
+
+	// Clear the work queue schedule.
+	ScheduleClear();
+}
+
+void nanoradar_mr72::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+}

src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72.hpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016-2020 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 nanoradar_mr72.hpp
+ * @author wp <wp@uav.com>
+ *
+ * Driver for the MR72 radar from Aerotenna
+ */
+
+#pragma once
+
+#include <fcntl.h>
+#include <poll.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/perf/perf_counter.h>
+
+using namespace time_literals;
+
+#define MR72_MEASURE_INTERVAL       	10_ms
+#define MR72_MAX_DISTANCE	        30.0f
+#define MR72_MIN_DISTANCE	        1.0f
+#define MR72_VERSION	        	1
+
+#define MR72_CAN	        	0
+#define MR72_UART_SECTION	        0
+#define MR72_UART_POINTS	        1
+
+// #if MR72_VERSION == 1
+#define MR72_PACKET_HDR     170
+#define MR72_PACKET_END     85
+#define MR72_BUFFER_LENGTH  18
+// #else
+// #define MR72_PACKET_HDR     72
+// #define MR72_BUFFER_LENGTH  9
+// #endif
+
+/**
+ * Assume standard deviation to be equal to sensor resolution.
+ * Static bench tests have shown that the sensor ouput does
+ * not vary if the unit is not moved.
+ */
+#define SENS_VARIANCE           0.045f * 0.045f
+
+class nanoradar_mr72 : public px4::ScheduledWorkItem
+{
+public:
+	/**
+	 * Default Constructor
+	 * @param port The serial port to open for communicating with the sensor.
+	 * @param rotation The sensor rotation relative to the vehicle body.
+	 */
+	nanoradar_mr72(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	~nanoradar_mr72() override;
+
+	int init();
+	void print_info();
+
+private:
+
+	/**
+	 * Reads data from serial UART and places it into a buffer.
+	 */
+	int collect();
+
+	/**
+	 * Opens and configures the UART serial communications port.
+	 * @param speed The baudrate (speed) to configure the serial UART port.
+	 */
+	int open_serial_port(const speed_t speed = B115200);
+
+	void Run() override;
+	void start();
+	void stop();
+
+	PX4Rangefinder _px4_rangefinder;
+
+	char _port[20] {};
+
+	int _file_descriptor{-1};
+
+	uint8_t _buffer[MR72_BUFFER_LENGTH] {};
+
+	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")};
+
+};

src/drivers/distance_sensor/nanoradar_mr72/nanoradar_mr72_main.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2016-2020 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.
+ *
+ ****************************************************************************/
+
+#include "nanoradar_mr72.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+namespace nanoradar_mr72
+{
+
+nanoradar_mr72 *g_dev{nullptr};
+
+static int start(const char *port, uint8_t rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+		return -1;
+	}
+
+	if (port == nullptr) {
+		PX4_ERR("serial port required");
+		return -1;
+	}
+
+	// Instantiate the driver.
+	g_dev = new nanoradar_mr72(port, rotation);
+
+	if (g_dev == nullptr) {
+		return -1;
+	}
+
+	if (g_dev->init() != PX4_OK) {
+		delete g_dev;
+		g_dev = nullptr;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+static int status()
+{
+	if (g_dev == nullptr) {
+		PX4_ERR("driver not running");
+		return -1;
+	}
+
+	g_dev->print_info();
+
+	return 0;
+}
+
+static int usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+Serial bus driver for the Aerotenna upatch radar.
+
+### Examples
+
+Attempt to start driver on a specified serial device.
+$ nanoradar_mr72 start -d /dev/ttyS1
+Stop driver
+$ nanoradar_mr72 stop
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("nanoradar_mr72", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
+	PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "Serial device", false);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
+	return PX4_OK;
+}
+
+} // namespace nanoradar_mr72
+
+extern "C" __EXPORT int nanoradar_mr72_main(int argc, char *argv[])
+{
+	uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
+	const char *device_path = nullptr;
+	int ch;
+	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:
+			return nanoradar_mr72::usage();
+		}
+	}
+
+	if (myoptind >= argc) {
+		return nanoradar_mr72::usage();
+	}
+
+	if (!strcmp(argv[myoptind], "start")) {
+		return nanoradar_mr72::start(device_path, rotation);
+
+	} else if (!strcmp(argv[myoptind], "stop")) {
+		return nanoradar_mr72::stop();
+
+	} else if (!strcmp(argv[myoptind], "status")) {
+		return nanoradar_mr72::status();
+	}
+
+	return nanoradar_mr72::usage();
+}

src/drivers/distance_sensor/upatch_radar/AerotennaUPatch.cpp

@@ -99,7 +99,6 @@ int AerotennaUPatch::collect()
 
 	float distances_m[5] = {0.0f}; 	//测量得到的5个距离
 	float snr[5] = {0.0f}; 		//测量得到的5个距离的信噪比
-	_px4_rangefinder.set_vfov((float)(bytes_read));
 
 	if (bytes_read > 0) {
 		index = bytes_read - 18; // 18bytes

src/drivers/drv_sensor.h

@@ -196,6 +196,8 @@
 #define DRV_GPS_DEVTYPE_SIM 0xAF
 
 #define DRV_DIST_DEVTYPE_UPATCH      0xB0
+#define DRV_DIST_DEVTYPE_MR72        0xB2
+#define DRV_DIST_DEVTYPE_ENZHAO        0xB4
 
 
 #define DRV_DEVTYPE_UNUSED		0xff