增加一些仿真测试打印数据

- cubepilot_cubeorange_test has the console enabled (used for test rack and bench debugging)

.ci/Jenkinsfile-hardware

@@ -673,153 +673,6 @@ pipeline {
           }
         }
 
-        stage("modalai_fc-v1_test") {
-          stages {
-            stage("build modalai_fc-v1_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
-                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkout scm
-                sh 'export'
-                sh 'make distclean'
-                sh 'ccache -s'
-                sh 'git fetch --tags'
-                sh 'make modalai_fc-v1_test'
-                sh 'make sizes'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_test'
-              }
-              post {
-                always {
-                    sh 'make distclean'
-                }
-              }
-            } // stage build
-            stage("test") {
-              agent {
-                label 'modalai_fc-v1'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'modalai_fc-v1_test'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                  }
-                }
-                stage("configure") {
-                  steps {
-                    // configure
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"'    // disable buzzer
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"'    // generic quadcopter
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"'       // update bootloader
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'                  // reboot to apply
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
-                  }
-                }
-                stage("status") {
-                  steps {
-                    statusFTDI()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    // run tests
-                    sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
-                  }
-                }
-                stage("reset") {
-                  steps {
-                    cleanupFTDI()
-                  }
-                }
-              }
-            } // stage test
-          }
-        }
-
-        stage("holybro_durandal-v1_test") {
-          stages {
-            stage("build holybro_durandal-v1_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-04-29'
-                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkout scm
-                sh 'export'
-                sh 'make distclean'
-                sh 'ccache -s'
-                sh 'git fetch --tags'
-                sh 'make holybro_durandal-v1_test'
-                sh 'make sizes'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'holybro_durandal-v1_test'
-              }
-              post {
-                always {
-                    sh 'make distclean'
-                }
-              }
-            } // stage build
-            stage("test") {
-              agent {
-                label 'holybro_durandal-v1'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'holybro_durandal-v1_test'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/holybro_durandal-v1_test/holybro_durandal-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                  }
-                }
-                stage("configure") {
-                  steps {
-                    // configure
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"'    // disable buzzer
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"'    // generic quadcopter
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"'       // update bootloader
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'                  // reboot to apply
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
-                  }
-                }
-                stage("status") {
-                  steps {
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dshot status"'
-                    statusFTDI()
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    // run tests
-                    sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
-                  }
-                }
-                stage("reset") {
-                  steps {
-                    cleanupFTDI()
-                  }
-                }
-              }
-            } // stage test
-          }
-        }
-
         stage("nxp_fmuk66-v3_test") {
           stages {
             stage("build nxp_fmuk66-v3_test") {

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -71,6 +71,11 @@ then
 	pga460 start
 fi
 
+if param greater -s SENS_EN_TFI2C 0
+then
+	tfmini_i2c start -X
+fi
+
 # Lightware i2c lidar sensor
 if param greater -s SENS_EN_SF1XX 0
 then

ROMFS/px4fmu_common/init.d/rcS

@@ -128,7 +128,7 @@ else
 	then
 		param reset_all
 	fi
-	if ver hwtypecmp V5X00 V5X90 V5Xa0
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
 	then
 		netman update -i eth0
 	fi

boards/cuav/nora/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -127,6 +129,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cuav/x7pro/src/board_config.h

@@ -117,8 +117,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)
 
-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */
 
 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)

boards/cuav/x7pro/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -127,6 +129,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cubepilot/cubeorange/default.cmake

@@ -15,7 +15,7 @@ px4_add_board(
 		TEL2:/dev/ttyS1
 		GPS1:/dev/ttyS2
 		# PX4IO:/dev/ttyS3
-		TEL3:/dev/ttyS4
+		# TEL3:/dev/ttyS4  # connected to ADS-B receiver
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc/ads1115

boards/cubepilot/cubeorange/init/rc.board_mavlink

@@ -5,3 +5,9 @@
 
 # Start MAVLink on the USB port
 mavlink start -d /dev/ttyACM0
+
+# Start ADS-B receiver mavlink connection if console not present
+if [ ! -e /dev/console ]
+then
+	mavlink start -d /dev/ttyS4 -b 57600 -m minimal
+fi

boards/cubepilot/cubeorange/src/board_config.h

@@ -62,7 +62,7 @@
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
 
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
-#define BOARD_ARMED_STATE_LED  LED_AMBER
+#define BOARD_ARMED_LED  LED_AMBER
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \

boards/cubepilot/cubeorange/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -118,6 +120,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/cubepilot/cubeyellow/src/board_config.h

@@ -62,7 +62,7 @@
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
 
 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
-#define BOARD_ARMED_STATE_LED  LED_AMBER
+#define BOARD_ARMED_LED  LED_AMBER
 
 /* ADC channels */
 #define PX4_ADC_GPIO  \

boards/cubepilot/io-v2/nuttx-config/nsh/defconfig

@@ -27,7 +27,7 @@ CONFIG_FDCLONE_DISABLE=y
 CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=280
+CONFIG_IDLETHREAD_STACKSIZE=316
 CONFIG_MAX_TASKS=2
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y

boards/holybro/durandal-v1/src/init.c

@@ -75,6 +75,8 @@
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 /****************************************************************************
  * Pre-Processor Definitions
  ****************************************************************************/
@@ -160,6 +162,16 @@ __EXPORT void board_on_reset(int status)
 __EXPORT void
 stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	board_on_reset(-1); /* Reset PWM first thing */
 
 	/* configure LEDs */

boards/mro/ctrl-zero-h7-oem/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -121,6 +123,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/mro/ctrl-zero-h7/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -121,6 +123,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/mro/pixracerpro/src/init.c

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>
 
+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -121,6 +123,16 @@ __EXPORT void board_on_reset(int status)
  ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);
 

boards/nxp/fmuk66-e/src/init.c

@@ -235,6 +235,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}
 
+	/* set up the serial DMA polling */
+#ifdef SERIAL_HAVE_DMA
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)kinetis_serial_dma_poll,
+		       NULL);
+#endif
+
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);

boards/nxp/fmuk66-v3/src/init.c

@@ -235,6 +235,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}
 
+	/* set up the serial DMA polling */
+#ifdef SERIAL_HAVE_DMA
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)kinetis_serial_dma_poll,
+		       NULL);
+#endif
+
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);

boards/px4/fmu-v2/default.cmake

@@ -76,7 +76,7 @@ px4_add_board(
 		#gyro_fft
 		land_detector
 		#landing_target_estimator
-		load_mon
+		#load_mon
 		#local_position_estimator
 		logger
 		mavlink
@@ -118,7 +118,7 @@ px4_add_board(
 		#sd_bench
 		#serial_test
 		#system_time
-		top
+		#top
 		#topic_listener
 		tune_control
 		#uorb

boards/px4/fmu-v2/multicopter.cmake

@@ -48,7 +48,7 @@ px4_add_board(
 		#events
 		land_detector
 		landing_target_estimator
-		load_mon
+		#load_mon
 		#local_position_estimator
 		logger
 		mavlink

boards/px4/fmu-v5/optimized.cmake

@@ -23,7 +23,8 @@ px4_add_board(
 	DRIVERS
 		adc/ads1115
 		adc/board_adc
-		barometer # all available barometer drivers
+		#barometer # all available barometer drivers
+		barometer/ms5611
 		batt_smbus
 		camera_capture
 		camera_trigger
@@ -116,7 +117,7 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
 		#serial_test
 		system_time

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -55,17 +55,20 @@ else
 
 fi
 
+# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
+ist8310 -X -b 1 -R 10 start
+
 # Possible internal Baro
 
 # Disable startup of internal baros if param is set to false
 if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I -a 0x77 start
-	if ver hwtypecmp V5X91 V5Xa1
+	if ver hwtypecmp V5X00 V5X90 V5Xa0
 	then
-		bmp388 -X -b 2 start
-	else
 		bmp388 -I start
+	else
+		bmp388 -X -b 2 start
 	fi
 
 fi

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -21,6 +21,9 @@ icm20649 -R 14 -s start
 # Internal magnetometer on I2c
 bmm150 -I start
 
+# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
+ist8310 -X -b 1 -R 10 start
+
 # Possible internal Baro
 bmp388 -I -a 0x77 start
 

boards/px4/io-v2/nuttx-config/nsh/defconfig

@@ -27,7 +27,7 @@ CONFIG_FDCLONE_DISABLE=y
 CONFIG_FDCLONE_STDIO=y
 CONFIG_HAVE_CXX=y
 CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=280
+CONFIG_IDLETHREAD_STACKSIZE=316
 CONFIG_MAX_TASKS=2
 CONFIG_MM_FILL_ALLOCATIONS=y
 CONFIG_MM_SMALL=y

fmuv3_build.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v3_default
+endtime=`date +'%Y-%m-%d %H:%M:%S'`
+date -R
+start_seconds=$(date --date="$starttime" +%s);
+end_seconds=$(date --date="$endtime" +%s);
+echo "本次运行时间： "$((end_seconds-start_seconds))"s"

gazebo_prx.sh

@@ -0,0 +1 @@
+make px4_sitl_default gazebo_iris_obs_avoid__ksql_airport

platforms/common/i2c_spi_buses.cpp

@@ -642,7 +642,7 @@ void I2CSPIDriverBase::print_status()
 	bool is_i2c_bus = _bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal;
 
 	if (is_i2c_bus) {
-		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, _i2c_address);
+		PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address());
 
 	} else {
 		PX4_INFO("Running on SPI Bus %i", _bus);

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -62,11 +62,13 @@ class I2CSPIInstance : public ListNode<I2CSPIInstance *>
 {
 public:
 	virtual ~I2CSPIInstance() = default;
+	virtual int8_t  get_i2c_address() {return _i2c_address;}
 
 private:
 	I2CSPIInstance(const char *module_name, I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type)
 		: _module_name(module_name), _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {}
 
+
 	friend class BusInstanceIterator;
 	friend class I2CSPIDriverBase;
 

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -96,6 +96,7 @@ private:
 
 	IntrusiveQueue<WorkItem *>	_q;
 	px4_sem_t			_process_lock;
+	px4_sem_t			_exit_lock;
 	const wq_config_t		&_config;
 	BlockingList<WorkItem *>	_work_items;
 	px4::atomic_bool		_should_exit{false};

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -59,11 +59,20 @@ WorkQueue::WorkQueue(const wq_config_t &config) :
 
 	px4_sem_init(&_process_lock, 0, 0);
 	px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE);
+
+	px4_sem_init(&_exit_lock, 0, 1);
+	px4_sem_setprotocol(&_exit_lock, SEM_PRIO_NONE);
 }
 
 WorkQueue::~WorkQueue()
 {
+
 	work_lock();
+
+	// Synchronize with ::Detach
+	px4_sem_wait(&_exit_lock);
+	px4_sem_destroy(&_exit_lock);
+
 	px4_sem_destroy(&_process_lock);
 	work_unlock();
 
@@ -83,11 +92,14 @@ bool WorkQueue::Attach(WorkItem *item)
 	}
 
 	work_unlock();
+
 	return false;
 }
 
 void WorkQueue::Detach(WorkItem *item)
 {
+	bool exiting = false;
+
 	work_lock();
 
 	_work_items.remove(item);
@@ -96,11 +108,21 @@ void WorkQueue::Detach(WorkItem *item)
 		// shutdown, no active WorkItems
 		PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name);
 
+		// Deletion of this work queue might happen right after request_stop or
+		// SignalWorkerThread. Use a separate lock to prevent premature deletion
+		px4_sem_wait(&_exit_lock);
+		exiting = true;
 		request_stop();
 		SignalWorkerThread();
 	}
 
 	work_unlock();
+
+	// In case someone is deleting this wq already, signal
+	// that it is now allowed
+	if (exiting) {
+		px4_sem_post(&_exit_lock);
+	}
 }
 
 void WorkQueue::Add(WorkItem *item)

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 76bb42f3ebd902102e844084b564274bf215ec9f
+Subproject commit bf660cba2af81f055002b3817c87b1f63a78fd09

platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp

@@ -328,6 +328,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
+				// stream transfer error, ensure TX DMA is also stopped before exiting early
+				stm32_dmastop(_tx_dma);
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;

platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp

@@ -373,6 +373,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
+				// stream transfer error, ensure TX DMA is also stopped before exiting early
+				stm32_dmastop(_tx_dma);
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;

src/drivers/distance_sensor/CMakeLists.txt

@@ -48,3 +48,4 @@ add_subdirectory(ulanding_radar)
 add_subdirectory(vl53l0x)
 add_subdirectory(vl53l1x)
 add_subdirectory(gy_us42)
+add_subdirectory(tfmini_i2c)

src/drivers/distance_sensor/tfmini_i2c/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2017-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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__tfmini_i2c
+	MAIN tfmini_i2c
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		tfmini_i2c.cpp
+		tfmini_i2c.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		drivers_rangefinder
+	)

src/drivers/distance_sensor/tfmini_i2c/module.yaml

@@ -0,0 +1,73 @@
+__max_tf_driver: &max_tf_driver 6
+
+module_name: Benewake i2c Driver
+
+parameters:
+    - group: Sensors
+      definitions:
+        SENS_EN_TFI2C:
+            description:
+                short: Enable Benewake i2c Driver
+                long: |
+                    Benewake TF Rangefinder i2c Driver
+            type: int32
+            min: 0
+            max: 6
+            default: 0
+            reboot_required: true
+
+        SENS_TF_${i}_ADDR:
+            description:
+                short: Benewake TF Sensor ${i} Address
+                long: |
+                    This parameter defines the address of the TFi2c sensor.
+            type: int32
+            min: 1
+            max: 254
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 16
+
+        SENS_TF_${i}_ROT:
+            description:
+                short: Benewake TF Sensor ${i} Rotation
+                long: |
+                    This parameter defines the rotation of the sensor relative to the platform.
+            type: enum
+            values:
+                25: ROTATION_DOWNWARD_FACING
+                24: ROTATION_UPWARD_FACING
+                12: ROTATION_BACKWARD_FACING
+                0: ROTATION_FORWARD_FACING
+                6: ROTATION_LEFT_FACING
+                2: ROTATION_RIGHT_FACING
+            reboot_required: true
+            min: 0
+            max: 25
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 0
+
+        SENS_TF_${i}_MAXD:
+            description:
+                short: Benewake TF Sensor ${i} Max Distance
+                long: |
+                    This parameter defines maximum measuring distance.
+            type: int32
+            min: 5
+            max: 20000
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 1200
+
+        SENS_TF_${i}_MIND:
+            description:
+                short: Benewake TF Sensor ${i} Min Distance
+                long: |
+                    This parameter defines min measuring distance.
+            type: int32
+            min: 5
+            max: 20000
+            num_instances: *max_tf_driver
+            instance_start: 0
+            default: 10

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -0,0 +1,429 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 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 tfmini_i2c.cpp
+ *
+ * Driver for the Benewake range finders Sensor connected via I2C.
+ */
+#include "tfmini_i2c.hpp"
+
+using namespace time_literals;
+
+#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
+
+/* Configuration Constants */
+#define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
+#define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
+
+
+#define TFMINI_I2C_MEASURE_INTERVAL                 30_ms // 60ms minimum for one sonar.
+#define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 20_ms // 30ms minimum between each sonar measurement (watch out for interference!).
+
+
+
+TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) :
+	I2C(DRV_DIST_DEVTYPE_TFMINI_I2C, MODULE_NAME, bus, address, bus_frequency),
+	ModuleParams(nullptr),
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
+{
+	set_device_type(DRV_DIST_DEVTYPE_TFMINI_I2C);
+	_measure_interval = TFMINI_I2C_MEASURE_INTERVAL;
+}
+
+TFMINI_I2C::~TFMINI_I2C()
+{
+	// Unadvertise the distance sensor topic.
+	if (_distance_sensor_topic != nullptr) {
+		orb_unadvertise(_distance_sensor_topic);
+	}
+
+	// Free perf counters.
+	perf_free(_comms_error);
+	perf_free(_sample_perf);
+}
+
+int32_t
+TFMINI_I2C::getTFi2cParam(uint8_t index,uint8_t type)
+{
+	const char *pname_format_tfi2c[4] = {"SENS_TF_%d_ADDR","SENS_TF_%d_ROT","SENS_TF_%d_MAXD","SENS_TF_%d_MIND"};
+	char p_name[16];
+	int32_t value;
+	sprintf(p_name, pname_format_tfi2c[type], index);
+	param_t param_h = param_find(p_name);
+
+	if (param_h != PARAM_INVALID) {
+		param_get(param_h, &value);
+	} else {
+		PX4_DEBUG("PARAM_INVALID: %s", p_name);
+	}
+	return value;
+}
+
+int
+TFMINI_I2C::init()
+{
+	if (_p_sensor_enabled.get() == 0) {
+		PX4_WARN("disabled");
+		return PX4_ERROR;
+	}
+
+	// Initialize the I2C device
+	if (I2C::init() != OK) {
+		return PX4_ERROR;
+	}
+	// Check for connected rangefinders on each i2c port by decrementing from the base address,
+	uint8_t address;
+	for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) {
+		// address = param_total_addr[index];
+		address = (uint8_t)getTFi2cParam(index,TF_PARAM_ADDR);
+		set_device_address(address);
+		px4_usleep(_measure_interval);
+		if (measure() == PX4_OK) {
+			px4_usleep(_measure_interval);
+			// Store I2C address、rotation、range
+			sensor_param_value[_sensor_count].addresses = address;
+			sensor_param_value[_sensor_count].rotations = (uint8_t)getTFi2cParam(index,TF_PARAM_ROT);
+			sensor_param_value[_sensor_count].max_range = getTFi2cParam(index,TF_PARAM_MAXD);
+			sensor_param_value[_sensor_count].min_range = getTFi2cParam(index,TF_PARAM_MIND);
+            		int set_ret = set_param();
+			printf("tf[%i]: address 0x%02x ,rototion %d, max:%d, min:%d, set param:%d\n", _sensor_count, \
+												get_device_address(), \
+												sensor_param_value[_sensor_count].rotations, \
+												sensor_param_value[_sensor_count].max_range, \
+												sensor_param_value[_sensor_count].min_range, \
+												set_ret);
+
+			_sensor_count++;
+			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
+				break;
+			}
+		}
+		px4_usleep(_measure_interval);
+	}
+	get_a_data = 0;
+	// Return an error if no sensors were detected.
+	if (_sensor_count == 0) {
+		PX4_ERR("no sensors discovered");
+		return PX4_ERROR;
+	}
+
+	// If more than one sonar is detected, adjust the meaure interval to avoid sensor interference.
+	if (_sensor_count > 1) {
+		_measure_interval = TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES;
+	}
+
+	PX4_INFO("Total sensors connected: %i", _sensor_count);
+	return PX4_OK;
+}
+
+int
+TFMINI_I2C::set_param()
+{
+	int ret;
+	const uint8_t CMD_SYSTEM_RESET[] =       { 0x5A, 0x04, 0x04, 0x62 };
+	const uint8_t CMD_OUTPUT_FORMAT_CM[] =   { 0x5A, 0x05, 0x05, 0x01, 0x65 };
+	const uint8_t CMD_ENABLE_DATA_OUTPUT[] = { 0x5A, 0x05, 0x07, 0x01, 0x67 };
+	const uint8_t CMD_FRAME_RATE_100HZ[] =   { 0x5A, 0x06, 0x03, 0x64, 0x00, 0xC7 };
+	const uint8_t CMD_SAVE_SETTINGS[] =      { 0x5A, 0x04, 0x11, 0x6F };
+	const uint8_t *cmds[] = {
+		CMD_OUTPUT_FORMAT_CM,
+		CMD_FRAME_RATE_100HZ,
+		CMD_ENABLE_DATA_OUTPUT,
+		CMD_SAVE_SETTINGS,
+	};
+	for (uint8_t i = 0; i < ARRAY_SIZE(cmds); i++) {
+		ret = tfi2c_transfer(cmds[i], cmds[i][1], nullptr, 0);
+		if (!ret) {
+		PX4_INFO(": Unable to set configuration register %u\n",cmds[i][2]);
+		return ret;
+		}
+		px4_usleep(100_ms);
+	}
+	tfi2c_transfer(CMD_SYSTEM_RESET, sizeof(CMD_SYSTEM_RESET), nullptr, 0);
+
+	return PX4_OK;
+}
+int
+TFMINI_I2C::measure()
+{
+	// Send the command to take a measurement.
+	const uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60 };
+	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
+	return ret_val;
+}
+
+int
+TFMINI_I2C::collect()
+{
+	uint16_t distance_cm = 0;
+	static uint32_t err_cnt;
+	const uint8_t Data_Len = 9;
+	static  uint8_t raw_data[Data_Len];
+	perf_begin(_sample_perf);
+
+	// Increment i2c adress to next sensor.
+	_sensor_index++;
+	_sensor_index %= _sensor_count;
+
+	// Set the sensor i2c adress for the active cycle.
+	set_device_address(sensor_param_value[_sensor_index].addresses );
+
+	// Transfer data from the bus.
+	int ret_val;
+
+	uint8_t CMD_READ_MEASUREMENT[] = { 0x5A,0x05,0x00,0x01,0x60 };
+	ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, Data_Len);
+
+	if (ret_val < 0) {
+		perf_count(_comms_error);
+		perf_end(_sample_perf);
+		return ret_val;
+	}
+	/**  header1, header2,distanceL,distanceH,timestampL,timestampH,checksum; **/
+	if(raw_data[0] == 0x59 && raw_data[1] == 0x59)
+	{
+		if(check_checksum(raw_data,Data_Len)){
+			distance_cm = ((uint16_t)raw_data[3] << 8) | raw_data[2];
+		}
+
+	}else{
+		err_cnt += 1;
+		return -EAGAIN;
+	}
+	if (distance_cm > sensor_param_value[_sensor_index].max_range || distance_cm < sensor_param_value[_sensor_index].min_range )	// 超出距离范围
+	{
+		distance_cm = sensor_param_value[_sensor_index].max_range ;
+	}
+
+	float distance_m = static_cast<float>(distance_cm) * 1e-2f;
+
+	distance_sensor_s report;
+	report.current_distance = distance_m;
+	report.device_id        = get_device_id();
+	report.max_distance     = sensor_param_value[_sensor_index].max_range / 100.0f;
+	report.min_distance     = sensor_param_value[_sensor_index].min_range / 100.0f;
+	report.orientation      = sensor_param_value[_sensor_index].rotations;
+	report.signal_quality   = -1;
+	report.timestamp        = hrt_absolute_time();
+	report.type             = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	report.variance         = 0.0f;
+	report.h_fov = math::radians(1.15f);
+	report.v_fov = math::radians(1.15f);
+
+	int instance_id;
+	orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id);
+	if (!get_a_data)
+	{
+		get_a_data = 1;
+	}
+	/** for test **/
+	// static uint64_t last_us ;
+	// static double dist_arr[6]={0.01,0.01,0.01,0.01,0.01,0.01};
+	// dist_arr[_sensor_index] = (double)distance_m;
+        // if(hrt_absolute_time() - last_us > 100 * 1_ms){
+        //     printf("dist:%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f\n",dist_arr[0],dist_arr[1],dist_arr[2],dist_arr[3],dist_arr[4],dist_arr[5]);
+        //     last_us = hrt_absolute_time();
+        // }
+	perf_end(_sample_perf);
+	return PX4_OK;
+}
+
+
+int
+TFMINI_I2C::check_checksum(uint8_t *arr, int pkt_len)
+{
+	uint8_t checksum = 0;
+	int i;
+
+	/* sum them all except the last (the checksum) */
+	for (i = 0; i < pkt_len - 1; i++) {
+	checksum += arr[i];
+	}
+	crc_clc = checksum;
+	return checksum == arr[pkt_len - 1];
+}
+
+int
+TFMINI_I2C::tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
+{
+	bool send_and_receive = false;
+	if (send != nullptr && send_len > 0) {
+
+		int ret = transfer(send, send_len, nullptr, 0);
+		send_and_receive = true;
+		if (ret != PX4_OK) {
+			return ret;
+		}
+	}
+
+	if (recv != nullptr && recv_len > 0) {
+		if(send_and_receive){
+			px4_usleep(500_us);
+		}
+		return transfer(nullptr, 0, recv, recv_len);
+	}
+
+	return PX4_ERROR;
+}
+
+
+void
+TFMINI_I2C::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_error);
+	PX4_INFO("poll interval:  %ums", _measure_interval / 1000);
+
+	for (int i = 0; i < _sensor_count; i++) {
+		PX4_INFO("sensor: %i, address %u", i, sensor_param_value[i].addresses );
+	}
+}
+
+void
+TFMINI_I2C::RunImpl()
+{
+	// Collect the sensor data.
+	if (collect() != PX4_OK) {
+		PX4_INFO("collection error");
+		px4_usleep(1_s);
+
+	}
+}
+
+int
+TFMINI_I2C::set_address(const uint8_t address)
+{
+	if (_sensor_count > 1) {
+		PX4_INFO("multiple sensors are connected");
+		return PX4_ERROR;
+	}
+
+	PX4_INFO("requested address: %u", address);
+
+	uint8_t shifted_address = address ;
+	uint8_t cmd[5] = {0x5A, 0x05, 0x0B, shifted_address,0};
+    	cmd[4] = 0x5A + 0x05 + 0x0B + shifted_address;
+	if (transfer(cmd, sizeof(cmd), nullptr, 0) != PX4_OK) {
+		PX4_INFO("could not set the address");
+	}
+
+	set_device_address(address);
+	PX4_INFO("device address: %u", get_device_address());
+	return PX4_OK;
+}
+
+void
+TFMINI_I2C::start()
+{
+	// Fetch parameter values.
+	ModuleParams::updateParams();
+	// Schedule the driver cycle at regular intervals.
+	ScheduleOnInterval(_measure_interval);
+}
+
+void
+TFMINI_I2C::custom_method(const BusCLIArguments &cli)
+{
+	set_address(cli.i2c_address);
+}
+
+I2CSPIDriverBase *TFMINI_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				      int runtime_instance)
+{
+	TFMINI_I2C *instance = new TFMINI_I2C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	instance->start();
+	return instance;
+}
+
+
+void
+TFMINI_I2C::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("tfmini_i2c", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16);
+	PRINT_MODULE_USAGE_COMMAND("set_address");
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x16);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int tfmini_i2c_main(int argc, char *argv[])
+{
+	using ThisDriver = TFMINI_I2C;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = TFMINI_I2C_BASE_ADDR;
+	cli.default_i2c_frequency = TFMINI_I2C_BUS_SPEED;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TFMINI_I2C);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	if (!strcmp(verb, "set_address")) {
+		return ThisDriver::module_custom_method(cli, iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.hpp

@@ -0,0 +1,170 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2019, 2021 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 tfmini_i2c.cpp
+ *
+ * Driver for the Benewake TFmini laser rangefinder series
+ */
+
+#pragma once
+
+#include <fcntl.h>
+#include <math.h>
+#include <poll.h>
+#include <semaphore.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <board_config.h>
+#include <containers/Array.hpp>
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+
+#include <systemlib/mavlink_log.h>
+
+
+
+class TFMINI_I2C : public device::I2C, public ModuleParams, public I2CSPIDriver<TFMINI_I2C>
+{
+public:
+	TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address);
+	virtual ~TFMINI_I2C();
+
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+	virtual int init() override;
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void print_status() override;
+
+	/**
+	 * Sets a new device address.
+	 * @param address The new sensor address to be set: 1-254 even addresses only.
+	 * @return Returns PX4_OK iff successful, PX4_ERROR otherwise.
+	 */
+	int set_address(const uint8_t address);
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void start();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 */
+	void RunImpl();
+
+protected:
+	void custom_method(const BusCLIArguments &cli) override;
+	int32_t getTFi2cParam(uint8_t index,uint8_t type);
+
+private:
+
+	/**
+	 * Collects the most recent sensor measurement data from the i2c bus.
+	 */
+	int collect();
+
+	/**
+	 * Sends an i2c measure command to start the next sonar ping.
+	 */
+	int measure();
+	int tfi2c_transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
+	int check_checksum(uint8_t *arr, int pkt_len);
+
+	int set_param();
+
+	enum tf_param_type {
+		TF_PARAM_ADDR = 0,
+		TF_PARAM_ROT,
+		TF_PARAM_MAXD,
+		TF_PARAM_MIND
+	};
+
+	static constexpr uint8_t RANGE_FINDER_MAX_SENSORS = 6;
+	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_addresses {};
+	// px4::Array<uint8_t, RANGE_FINDER_MAX_SENSORS> _sensor_rotations {};
+
+	int _measure_interval{0};	// Initialize the measure interval for a single sensor.
+
+	int _sensor_index{0};	// Initialize counter for cycling i2c adresses to zero.
+
+	int _sensor_count{0};
+
+	orb_advert_t _distance_sensor_topic{nullptr};
+
+	perf_counter_t _comms_error{perf_alloc(PC_ELAPSED, "tfmini_i2c_comms_error")};
+	perf_counter_t _sample_perf{perf_alloc(PC_COUNT, "tfmini_i2c_sample_perf")};
+
+    	uint8_t crc_clc;
+	struct sensor_param_s
+	{
+		int32_t max_range;
+		int32_t min_range;
+		uint8_t addresses;
+		uint8_t rotations;
+	};
+	sensor_param_s sensor_param_value[RANGE_FINDER_MAX_SENSORS];
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled
+
+	);
+    orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+    float avoid_distance;
+    int entra_avoid_area{0};
+    uint8_t get_a_data;
+};

src/drivers/drv_sensor.h

@@ -195,6 +195,7 @@
 
 #define DRV_GPS_DEVTYPE_SIM 0xAF
 
+#define DRV_DIST_DEVTYPE_TFMINI_I2C       0xB0
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp

@@ -34,7 +34,8 @@
 /**
  * @file rgbled_ncp5623c.cpp
  *
- * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C.
+ * Driver for the onboard RGB LED controller (NCP5623B or NCP5623C)
+ * connected via I2C.
  *
  * @author CUAVcaijie <caijie@cuav.net>
  */
@@ -52,7 +53,8 @@
 
 using namespace time_literals;
 
-#define ADDR			0x39	/**< I2C adress of NCP5623C */
+#define NCP5623B_ADDR       0x38  /**< I2C address of NCP5623B */
+#define NCP5623C_ADDR       0x39  /**< I2C address of NCP5623C */
 
 #define NCP5623_LED_CURRENT 0x20  /**< Current register */
 #define NCP5623_LED_PWM0    0x40  /**< pwm0 register */
@@ -66,7 +68,7 @@ using namespace time_literals;
 class RGBLED_NCP5623C : public device::I2C, public I2CSPIDriver<RGBLED_NCP5623C>
 {
 public:
-	RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address);
+	RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address, const int order);
 	virtual ~RGBLED_NCP5623C() = default;
 
 	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
@@ -77,8 +79,13 @@ public:
 	int		probe() override;
 
 	void			RunImpl();
+	virtual int8_t  get_i2c_address() {return get_device_address();}
 
 private:
+	int			send_led_rgb();
+	void			update_params();
+
+	int			write(uint8_t reg, uint8_t data);
 
 	float			_brightness{1.0f};
 	float			_max_brightness{1.0f};
@@ -94,16 +101,40 @@ private:
 
 	LedController		_led_controller;
 
-	int			send_led_rgb();
-	void			update_params();
-
-	int			write(uint8_t reg, uint8_t data);
+	uint8_t		_red{NCP5623_LED_PWM0};
+	uint8_t		_green{NCP5623_LED_PWM1};
+	uint8_t		_blue{NCP5623_LED_PWM2};
 };
 
-RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) :
+RGBLED_NCP5623C::RGBLED_NCP5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address,
+				 const int order) :
 	I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
 {
+	int ordering = order;
+	// ordering is RGB: Hundreds is Red, Tens is green and ones is Blue
+	// 123 would drive the
+	//      R LED from = NCP5623_LED_PWM0
+	//      G LED from = NCP5623_LED_PWM1
+	//      B LED from = NCP5623_LED_PWM2
+	// 321 would drive the
+	//      R LED from = NCP5623_LED_PWM2
+	//      G LED from = NCP5623_LED_PWM1
+	//      B LED from = NCP5623_LED_PWM0
+	const uint8_t sig[] = {NCP5623_LED_PWM0, NCP5623_LED_PWM1, NCP5623_LED_PWM2};
+	// Process ordering in lsd to msd order.(BGR)
+	uint8_t *color[] = {&_blue, &_green, &_red };
+	unsigned int s = 0;
+
+	for (unsigned int i = 0; i < arraySize(color); i++) {
+		s = (ordering % 10) - 1;
+
+		if (s < arraySize(sig)) {
+			*color[i] = sig[s];
+		}
+
+		ordering /= 10;
+	}
 }
 
 int
@@ -138,9 +169,15 @@ RGBLED_NCP5623C::init()
 int
 RGBLED_NCP5623C::probe()
 {
-	_retries = 4;
+	_retries = 2;
+	int status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
+
+	if (status == PX4_ERROR) {
+		set_device_address(NCP5623B_ADDR);
+		status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
+	}
 
-	return write(NCP5623_LED_CURRENT, 0x00);
+	return status;
 }
 
 void
@@ -212,14 +249,13 @@ RGBLED_NCP5623C::RunImpl()
 int
 RGBLED_NCP5623C::send_led_rgb()
 {
-
 	uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
 	uint8_t brightness = 0x1f * _max_brightness;
 
 	msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
-	msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
-	msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
-	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
+	msg[2] = _red | (uint8_t(_r * _brightness) & 0x1f);
+	msg[4] = _green | (uint8_t(_g * _brightness) & 0x1f);
+	msg[6] = _blue | (uint8_t(_b * _brightness) & 0x1f);
 
 	return transfer(&msg[0], 7, nullptr, 0);
 }
@@ -246,6 +282,7 @@ RGBLED_NCP5623C::print_usage()
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
 	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39);
+	PRINT_MODULE_USAGE_PARAM_INT('o', 123, 123, 321, "RGB PWM Assignment", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
 
@@ -253,7 +290,7 @@ I2CSPIDriverBase *RGBLED_NCP5623C::instantiate(const BusCLIArguments &cli, const
 		int runtime_instance)
 {
 	RGBLED_NCP5623C *instance = new RGBLED_NCP5623C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency,
-			cli.i2c_address);
+			cli.i2c_address, cli.custom1);
 
 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -273,9 +310,19 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[])
 	using ThisDriver = RGBLED_NCP5623C;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = 100000;
-	cli.i2c_address = ADDR;
+	cli.i2c_address = NCP5623C_ADDR;
+	cli.custom1 = 123;
+	int ch;
+
+	while ((ch = cli.getOpt(argc, argv, "o:")) != EOF) {
+		switch (ch) {
+		case 'o':
+			cli.custom1 = atoi(cli.optArg());
+			break;
+		}
+	}
 
-	const char *verb = cli.parseDefaultArguments(argc, argv);
+	const char *verb = cli.optArg();
 
 	if (!verb) {
 		ThisDriver::print_usage();

src/drivers/magnetometer/vtrantech/vcm1193l/vcm1193l_main.cpp

@@ -70,6 +70,8 @@ extern "C" int vcm1193l_main(int argc, char *argv[])
 	using ThisDriver = VCM1193L;
 	BusCLIArguments cli{true, false};
 	cli.default_i2c_frequency = I2C_SPEED;
+	cli.i2c_address = I2C_ADDRESS_DEFAULT;
+
 
 	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
 		switch (ch) {

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -384,6 +384,15 @@ void
 CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
 		const Vector2f &curr_vel)
 {
+		bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+	//////////////////////////////
 	_updateObstacleMap();
 
 	// read parameters
@@ -401,6 +410,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 	const hrt_abstime constrain_time = getTime();
 	int num_fov_bins = 0;
 
+	if (need_prt)
+	{
+		printf("[clc]setpoint_length:%.2f,contime:%d,obtime:%d,delt:%d\n",(double)setpoint_length,constrain_time , _obstacle_map_body_frame.timestamp,constrain_time - _obstacle_map_body_frame.timestamp);
+	}
 	if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
 		if (setpoint_length > 0.001f) {
 
@@ -416,6 +429,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 			// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
 			_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
 
+			if (need_prt)
+			{
+				printf("[clc]setpoint_dir = %.2f,%.2f\n",(double)setpoint_dir(0),(double)setpoint_dir(1));
+			}
+
 			// limit speed for safe flight
 			for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
 
@@ -483,6 +501,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec
 			}
 
 			setpoint = setpoint_dir * vel_max;
+			if (need_prt)
+			{
+				printf("[clc]setpoint = %.2f,%.2f\n",(double)setpoint(0),(double)setpoint(1));
+			}
 		}
 
 	} else {
@@ -509,10 +531,21 @@ void
 CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
 				    const Vector2f &curr_vel)
 {
+
+	bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+	/////////////////////////////////////
 	//calculate movement constraints based on range data
 	Vector2f new_setpoint = original_setpoint;
 	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
 
+
 	//warn user if collision prevention starts to interfere
 	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
 				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
@@ -521,6 +554,10 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 
 	_interfering = currently_interfering;
 
+	if (need_prt)
+	{
+		printf("setpoint:%.2f,%.2f,%d\n",(double)original_setpoint(0),(double)new_setpoint(0),_interfering);
+	}
 	// publish constraints
 	collision_constraints_s	constraints{};
 	constraints.timestamp = getTime();
@@ -531,6 +568,130 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max
 	original_setpoint = new_setpoint;
 }
 
+void
+CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_speed, const float &curr_pos,
+				    const float &curr_vel)
+{
+	//calculate movement constraints based on range data
+	float setpoint = original_setpoint;
+	// _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
+		// read parameters
+	static float min_distance = 0.2f;
+	static float max_distance = 12.0f;
+	static float up_distance;
+	static uint64_t _data_h_timestamps;
+	static uint64_t _dist_timestamps;
+
+	bool need_prt = false;
+	static uint8_t cnt;
+	cnt+=1;
+	if (cnt > 100)
+	{
+		// need_prt = true;
+		cnt = 0;
+	}
+
+	// 获取测距距离值相关
+	for (auto &dist_sens_sub : _distance_sensor_subs) {
+        distance_sensor_s distance_sensor;
+		if (dist_sens_sub.update(&distance_sensor)) {
+			// consider only instances with valid data and orientations useful for collision prevention
+			// 距离值有效，并且是朝上的距离
+			if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
+			(distance_sensor.orientation == distance_sensor_s::ROTATION_FORWARD_FACING)) {
+			// (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING)) {
+				// update message description
+				_dist_timestamps = distance_sensor.timestamp;
+				max_distance = math::min(max_distance,(distance_sensor.max_distance ));
+				min_distance = math::max(min_distance,(distance_sensor.min_distance ));
+				up_distance = distance_sensor.current_distance;
+			}
+		}
+	}
+    // 获取一些参数设置
+	const float col_prev_d = _param_cp_dist.get();
+	const float col_prev_dly = _param_cp_delay.get();
+	const bool move_no_data = _param_cp_go_nodata.get();
+	const float xy_p = _param_mpc_xy_p.get();
+	const float max_jerk = _param_mpc_jerk_max.get();
+	const float max_accel = _param_mpc_acc_hor.get();
+	const float setpoint_length = -setpoint;
+	const hrt_abstime constrain_time = getTime();
+
+
+    if (need_prt)
+    {
+        printf("dist:%.2f,dt:%d,%.2f,%.2f\n",(double)up_distance,constrain_time-_dist_timestamps,(double)min_distance,(double)max_distance);
+    }
+
+	if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US && up_distance > min_distance && up_distance < max_distance) {
+		if (setpoint_length > 0.001f) {
+			float vel_max = setpoint_length;
+            // 最小停留距离，测距最小值或者参数设置停留值
+			const float min_dist_to_keep = math::max(min_distance / 100.0f, col_prev_d);
+            // 删除过时的值
+			const hrt_abstime data_age = constrain_time - _data_h_timestamps;//TODO: _data_timestamps需要赋值
+			if (data_age > RANGE_STREAM_TIMEOUT_US) {
+				up_distance = UINT16_MAX * 0.01f;
+			}
+			const float distance = up_distance;
+			const float max_range = max_distance + 0.005f;
+
+
+            if (distance > min_distance && distance < UINT16_MAX * 0.01f) {
+                // calculate max allowed velocity with a P-controller (same gain as in the position controller)
+                const float curr_vel_parallel = math::max(0.f, curr_vel);
+                float delay_distance = curr_vel_parallel * col_prev_dly;
+                if (distance < max_range) {
+                    delay_distance += curr_vel_parallel * (data_age * 1e-6f);
+                }
+                const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
+                const float vel_max_posctrl = xy_p * stop_distance;
+
+                const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
+                float vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) ;
+                // constrain the velocity
+                if (vel_max_bin >= 0) {
+                    vel_max = math::min(vel_max, vel_max_bin);
+                }
+
+            }else if (distance >= UINT16_MAX * 0.01f ) {
+                if (!move_no_data) {
+                        vel_max = 0.f;
+                }
+            }
+            setpoint = -vel_max;
+		}
+	} else {
+		//allow no movement
+		// float vel_max = 0.f;
+		// setpoint = setpoint * vel_max;
+		// // if distance data is stale, switch to Loiter
+		// if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
+		// 	if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US
+		// 	    && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
+		// 		_publishVehicleCmdDoLoiter();
+		// 	}
+		// 	_last_timeout_warning = getTime();
+		// }
+	}
+    _data_h_timestamps = getTime();
+	/******************************************************************/
+	//warn user if collision prevention starts to interfere
+	bool currently_interfering = (setpoint < original_setpoint - 0.05f * max_speed
+				      || setpoint > original_setpoint + 0.05f * max_speed);
+	_interfering = currently_interfering;
+	original_setpoint = setpoint;
+	// if(_interfering){
+	// 	static uint8_t not_cnt;
+	// 	not_cnt += 1;
+	// 	if(not_cnt > 10){
+	// 		not_cnt = 0;
+	// 		printf("notic:%.2f\n",(double)original_setpoint);
+	// 	}
+	// }
+}
+
 void CollisionPrevention::_publishVehicleCmdDoLoiter()
 {
 	vehicle_command_s command{};

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -82,6 +82,10 @@ public:
 	void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
 			    const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
 
+
+	void modifySetpointH(float &original_setpoint, const float max_speed,
+			    const float &curr_alt, const float &curr_vel_z);
+
 protected:
 
 	obstacle_distance_s _obstacle_map_body_frame {};

src/lib/ecl

@@ -1 +1 @@
-Subproject commit 71fc1b81612fa9b5184d5abb93b69d109e9d0e4b
+Subproject commit b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c

src/lib/systemlib/system_params.c

@@ -191,6 +191,20 @@ PARAM_DEFINE_INT32(SYS_CAL_TMIN, 5);
  */
 PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10);
 
+/**
+ * Control if the vehicle has a GPS
+ *
+ * Disable this if the system has no GPS.
+ * If disabled, the sensors hub will not process sensor_gps,
+ * and GPS will not be available for the rest of the system.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
+
 /**
  * Control if the vehicle has a magnetometer
  *

src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -128,7 +128,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
 		}
 
-		prearm_ok = false;
+		// prearm_ok = false;
 
 	}
 

src/modules/ekf2/EKF2.cpp

@@ -353,7 +353,7 @@ void EKF2::Run()
 		}
 
 	} else {
-		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
+		const unsigned last_generation = _sensor_combined_sub.get_last_generation();
 		sensor_combined_s sensor_combined;
 		imu_updated = _sensor_combined_sub.update(&sensor_combined);
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -74,6 +74,18 @@ bool FlightTaskManualAcceleration::update()
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
 
 	_constraints.want_takeoff = _checkTakeoff();
+
+	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
+	if (_weathervane_yaw_handler && _weathervane_yaw_handler->is_active()) {
+		_yaw_setpoint = NAN;
+
+		// only enable the weathervane to change the yawrate when position lock is active (and thus the pos. sp. are NAN)
+		if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) {
+			// vehicle is steady
+			_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+		}
+	}
+
 	return ret;
 }
 

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -52,10 +52,17 @@ public:
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
+
 private:
 	StickAccelerationXY _stick_acceleration_xy;
 	StickYaw _stick_yaw;
 
 	void _ekfResetHandlerPositionXY() override;
 	void _ekfResetHandlerVelocityXY() override;
+
+	WeatherVane *_weathervane_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -43,7 +43,8 @@
 using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
-	_sticks(this)
+	_sticks(this),
+	_collision_prevention(this)
 {}
 
 bool FlightTaskManualAltitude::updateInitialize()
@@ -104,6 +105,24 @@ void FlightTaskManualAltitude::_scaleSticks()
 	// Use sticks input with deadzone and exponential curve for vertical velocity
 	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
+	/////////////////////////////////////////////////////////////////////
+	// collision prevention
+	float vel_z ;
+	if (_collision_prevention.is_active()) {
+		vel_z = vel_max_z * _sticks.getPositionExpo()(2);
+		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+		_velocity_setpoint(2) = vel_z;
+		// static uint8_t cnt;
+		// cnt+=1;
+		// if (cnt > 100)
+		// {
+		// 	printf(" vx:%.3f,vy:%.3f,vz:%.3f,vz2:%.3f\n",(double)_velocity_setpoint(0),(double)_velocity_setpoint(1),(double)_velocity_setpoint(2),(double)vel_z);
+		// 	printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
+		// 	cnt = 0;
+		// }
+	}
+
+
 }
 
 float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -358,6 +377,20 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 	_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
 
+	Vector2f setpoint = _acceleration_setpoint.xy();
+	if (_collision_prevention.is_active()) {
+		_collision_prevention.modifySetpoint(setpoint, 1.0f, _position.xy(), _velocity.xy());
+
+		// static uint8_t cnt;
+		// cnt+=1;
+		// if (cnt > 100)
+		// {
+		// 	printf(" ax:%.3f,ay:%.3f,ax2:%.3f,ay2:%.3f\n",(double)_acceleration_setpoint(0),(double)_acceleration_setpoint(1),(double)setpoint(0),(double)setpoint(1));
+		// 	// printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2));
+		// 	cnt = 0;
+		// }
+	}
+
 	_updateAltitudeLock();
 	_respectGroundSlowdown();
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -43,6 +43,7 @@
 #include "Sticks.hpp"
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
 #include <uORB/Subscription.hpp>
+#include <lib/collision_prevention/CollisionPrevention.hpp>
 
 class FlightTaskManualAltitude : public FlightTask
 {
@@ -151,4 +152,6 @@ private:
 	float _dist_to_ground_lock = NAN;
 
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+
 };

src/modules/logger/logged_topics.cpp

@@ -159,7 +159,7 @@ void LoggedTopics::add_default_topics()
 	// log all raw sensors at minimal rate (at least 1 Hz)
 	add_topic_multi("battery_status", 200, 2);
 	add_topic_multi("differential_pressure", 1000, 2);
-	add_topic_multi("distance_sensor", 1000);
+	add_topic_multi("distance_sensor", 100);
 	add_topic_multi("optical_flow", 1000, 1);
 	add_topic_multi("sensor_accel", 1000, 4);
 	add_topic_multi("sensor_baro", 1000, 4);

src/modules/mavlink/mavlink_main.cpp

@@ -1521,7 +1521,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
-		configure_stream_local("DISTANCE_SENSOR", 0.5f);
+		configure_stream_local("DISTANCE_SENSOR", 10.0f);
 		configure_stream_local("ESC_INFO", 1.0f);
 		configure_stream_local("ESC_STATUS", 1.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 0.5f);

src/modules/mavlink/mavlink_receiver.cpp

@@ -911,10 +911,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					(type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz
 				};
 
-				const matrix::Vector3f velocity_setpoint{R * velocity_body_sp};
-				setpoint.vx = velocity_setpoint(0);
-				setpoint.vy = velocity_setpoint(1);
-				setpoint.vz = velocity_setpoint(2);
+
+				const float yaw = matrix::Eulerf{R}(2);
+
+				setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
+				setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
+				setpoint.vz = velocity_body_sp(2);
 
 			} else {
 				setpoint.vx = NAN;
@@ -2521,28 +2523,25 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-	const uint64_t timestamp = hrt_absolute_time();
+	const uint64_t timestamp_sample = hrt_absolute_time();
 
 	/* airspeed */
 	{
 		airspeed_s airspeed{};
-
-		airspeed.timestamp = timestamp;
 		airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
 		airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
-
+		airspeed.air_temperature_celsius = 15.f;
+		airspeed.timestamp = hrt_absolute_time();
 		_airspeed_pub.publish(airspeed);
 	}
 
 	/* attitude */
 	{
 		vehicle_attitude_s hil_attitude{};
-
-		hil_attitude.timestamp = timestamp;
-
+		hil_attitude.timestamp_sample = timestamp_sample;
 		matrix::Quatf q(hil_state.attitude_quaternion);
 		q.copyTo(hil_attitude.q);
-
+		hil_attitude.timestamp = hrt_absolute_time();
 		_attitude_pub.publish(hil_attitude);
 	}
 
@@ -2550,13 +2549,13 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	{
 		vehicle_global_position_s hil_global_pos{};
 
-		hil_global_pos.timestamp = timestamp;
+		hil_global_pos.timestamp_sample = timestamp_sample;
 		hil_global_pos.lat = hil_state.lat / ((double)1e7);
 		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
-		hil_global_pos.eph = 2.0f;
-		hil_global_pos.epv = 4.0f;
-
+		hil_global_pos.eph = 2.f;
+		hil_global_pos.epv = 4.f;
+		hil_global_pos.timestamp = hrt_absolute_time();
 		_global_pos_pub.publish(hil_global_pos);
 	}
 
@@ -2565,32 +2564,31 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		const double lat = hil_state.lat * 1e-7;
 		const double lon = hil_state.lon * 1e-7;
 
-		map_projection_reference_s global_local_proj_ref;
-		map_projection_init(&global_local_proj_ref, lat, lon);
-
-		float global_local_alt0 = hil_state.alt / 1000.f;
+		if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) {
+			map_projection_init(&_global_local_proj_ref, lat, lon);
+			_global_local_alt0 = hil_state.alt / 1000.f;
+		}
 
-		float x = 0.0f;
-		float y = 0.0f;
-		map_projection_project(&global_local_proj_ref, lat, lon, &x, &y);
+		float x = 0.f;
+		float y = 0.f;
+		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
 
 		vehicle_local_position_s hil_local_pos{};
-		hil_local_pos.timestamp = timestamp;
-
-		hil_local_pos.ref_timestamp = global_local_proj_ref.timestamp;
-		hil_local_pos.ref_lat = math::degrees(global_local_proj_ref.lat_rad);
-		hil_local_pos.ref_lon = math::degrees(global_local_proj_ref.lon_rad);
-		hil_local_pos.ref_alt = global_local_alt0;
+		hil_local_pos.timestamp_sample = timestamp_sample;
+		hil_local_pos.ref_timestamp = _global_local_proj_ref.timestamp;
+		hil_local_pos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		hil_local_pos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		hil_local_pos.ref_alt = _global_local_alt0;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
 		hil_local_pos.v_xy_valid = true;
 		hil_local_pos.v_z_valid = true;
 		hil_local_pos.x = x;
 		hil_local_pos.y = y;
-		hil_local_pos.z = global_local_alt0 - hil_state.alt / 1000.0f;
-		hil_local_pos.vx = hil_state.vx / 100.0f;
-		hil_local_pos.vy = hil_state.vy / 100.0f;
-		hil_local_pos.vz = hil_state.vz / 100.0f;
+		hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f;
+		hil_local_pos.vx = hil_state.vx / 100.f;
+		hil_local_pos.vy = hil_state.vy / 100.f;
+		hil_local_pos.vz = hil_state.vz / 100.f;
 
 		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
 		hil_local_pos.heading = euler.psi();
@@ -2600,7 +2598,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vz_max = INFINITY;
 		hil_local_pos.hagl_min = INFINITY;
 		hil_local_pos.hagl_max = INFINITY;
-
+		hil_local_pos.timestamp = hrt_absolute_time();
 		_local_pos_pub.publish(hil_local_pos);
 	}
 
@@ -2618,7 +2616,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		if (_px4_accel != nullptr) {
 			// accel in mG
 			_px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f);
-			_px4_accel->update(timestamp, hil_state.xacc, hil_state.yacc, hil_state.zacc);
+			_px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc);
 		}
 	}
 
@@ -2634,20 +2632,18 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 
 		if (_px4_gyro != nullptr) {
-			_px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
+			_px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
 		}
 	}
 
 	/* battery status */
 	{
 		battery_status_s hil_battery_status{};
-
-		hil_battery_status.timestamp = timestamp;
 		hil_battery_status.voltage_v = 11.1f;
 		hil_battery_status.voltage_filtered_v = 11.1f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
-
+		hil_battery_status.timestamp = hrt_absolute_time();
 		_battery_pub.publish(hil_battery_status);
 	}
 }

src/modules/mavlink/mavlink_receiver.h

@@ -342,6 +342,9 @@ private:
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
 	uint16_t			_mom_switch_state{0};
 
+	map_projection_reference_s _global_local_proj_ref{};
+	float _global_local_alt0{NAN};
+
 	hrt_abstime			_last_utm_global_pos_com{0};
 
 	// Allocated if needed.

src/modules/px4iofirmware/mixer.cpp

@@ -74,7 +74,7 @@ static volatile bool mixer_servos_armed = false;
 static volatile bool should_arm = false;
 static volatile bool should_arm_nothrottle = false;
 static volatile bool should_always_enable_pwm = false;
-static volatile bool in_mixer = false;
+static volatile bool mix_failsafe = false;
 
 static bool new_fmu_data = false;
 static uint64_t last_fmu_update = 0;
@@ -94,31 +94,22 @@ enum mixer_source {
 static volatile mixer_source source;
 
 static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
-static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
+static int mixer_handle_text_create_mixer();
+static void mixer_mix_failsafe();
 
 static MixerGroup mixer_group;
 
-int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
-{
-	/* poor mans mutex */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) {
-		return 0;
-	}
-
-	in_mixer = true;
-	int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
-	*limits = mixer_group.get_saturation_status();
-	in_mixer = false;
-
-	return mixcount;
-}
-
 void
 mixer_tick()
 {
 	/* check if the mixer got modified */
 	mixer_handle_text_create_mixer();
 
+	if (mix_failsafe) {
+		mixer_mix_failsafe();
+		mix_failsafe = false;
+	}
+
 	/* check that we are receiving fresh data from the FMU */
 	irqstate_t irq_flags = enter_critical_section();
 	const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
@@ -314,7 +305,13 @@ mixer_tick()
 		}
 
 		/* mix */
-		mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
+			mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+			r_mixer_limits = mixer_group.get_saturation_status();
+
+		} else {
+			mixed = 0;
+		}
 
 		/* the pwm limit call takes care of out of band errors */
 		output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
@@ -480,12 +477,7 @@ mixer_callback(uintptr_t handle,
 	}
 
 	/* limit output */
-	if (control > 1.0f) {
-		control = 1.0f;
-
-	} else if (control < -1.0f) {
-		control = -1.0f;
-	}
+	control = math::constrain(control, -1.f, 1.f);
 
 	/* motor spinup phase - lock throttle to zero */
 	if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
@@ -520,25 +512,26 @@ mixer_callback(uintptr_t handle,
 
 static char mixer_text[PX4IO_MAX_MIXER_LENGTH];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
-static bool mixer_update_pending = false;
+static volatile bool mixer_update_pending = false;
+static volatile bool mixer_reset_pending = false;
 
 int
 mixer_handle_text_create_mixer()
 {
-	/* only run on update */
-	if (!mixer_update_pending) {
-		return 0;
-	}
-
 	/* do not allow a mixer change while safety off and FMU armed */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 	    (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return 1;
 	}
 
-	/* abort if we're in the mixer - it will be tried again in the next iteration */
-	if (in_mixer) {
-		return 1;
+	if (mixer_reset_pending) {
+		mixer_group.reset();
+		mixer_reset_pending = false;
+	}
+
+	/* only run on update */
+	if (!mixer_update_pending || (mixer_text_length == 0)) {
+		return 0;
 	}
 
 	/* process the text buffer, adding new mixers as their descriptions can be parsed */
@@ -562,11 +555,13 @@ mixer_handle_text_create_mixer()
 
 	mixer_update_pending = false;
 
+	update_trims = true;
+	update_mc_thrust_param = true;
+
 	return 0;
 }
 
-int
-mixer_handle_text(const void *buffer, size_t length)
+int interrupt_mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while safety off and FMU armed */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
@@ -577,15 +572,6 @@ mixer_handle_text(const void *buffer, size_t length)
 	/* disable mixing, will be enabled once load is complete */
 	atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
 
-	/* set the update flags to dirty so we reload those values after a mixer change */
-	update_trims = true;
-	update_mc_thrust_param = true;
-
-	/* abort if we're in the mixer - the caller is expected to retry */
-	if (in_mixer) {
-		return 1;
-	}
-
 	px4io_mixdata *msg = (px4io_mixdata *)buffer;
 
 	isr_debug(2, "mix txt %u", length);
@@ -601,7 +587,7 @@ mixer_handle_text(const void *buffer, size_t length)
 		isr_debug(2, "reset");
 
 		/* THEN actually delete it */
-		mixer_group.reset();
+		mixer_reset_pending = true;
 		mixer_text_length = 0;
 
 	/* FALLTHROUGH */
@@ -634,14 +620,18 @@ mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
+void interrupt_mixer_set_failsafe()
+{
+	mix_failsafe = true;
+}
+
 void
-mixer_set_failsafe()
+mixer_mix_failsafe()
 {
 	/*
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
-
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 	    !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 		return;
@@ -670,19 +660,22 @@ mixer_set_failsafe()
 	}
 
 	/* mix */
-	mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
+		r_mixer_limits = mixer_group.get_saturation_status();
+
+	} else {
+		mixed = 0;
+	}
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
-
 		/* scale to servo output */
 		r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
-
 	}
 
 	/* disable the rest of the outputs */
 	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 		r_page_servo_failsafe[i] = 0;
 	}
-
 }

src/modules/px4iofirmware/px4io.h

@@ -193,10 +193,9 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
  * Mixer
  */
 extern void	mixer_tick(void);
-extern int	mixer_handle_text_create_mixer(void);
-extern int	mixer_handle_text(const void *buffer, size_t length);
+extern int	interrupt_mixer_handle_text(const void *buffer, size_t length);
 /* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
-extern void	mixer_set_failsafe(void);
+extern void	interrupt_mixer_set_failsafe(void);
 
 /**
  * Safety switch/LED.

src/modules/px4iofirmware/registers.c

@@ -461,7 +461,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		 * this state defines an active system. This check is done in the
 		 * text handling function.
 		 */
-		return mixer_handle_text(values, num_values * sizeof(*values));
+		return interrupt_mixer_handle_text(values, num_values * sizeof(*values));
 
 	default:
 
@@ -514,9 +514,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			}
 
 			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
-
 				/* update failsafe values, now that the mixer is set to ok */
-				mixer_set_failsafe();
+				interrupt_mixer_set_failsafe();
 			}
 
 			break;

src/modules/sensors/sensors.cpp

@@ -219,6 +219,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
+		(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
 		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
@@ -534,7 +535,6 @@ void Sensors::InitializeVehicleAirData()
 {
 	if (_param_sys_has_baro.get()) {
 		if (_vehicle_air_data == nullptr) {
-			if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
 			_vehicle_air_data = new VehicleAirData();
 
 			if (_vehicle_air_data) {
@@ -543,12 +543,11 @@ void Sensors::InitializeVehicleAirData()
 		}
 	}
 }
-}
 
 void Sensors::InitializeVehicleGPSPosition()
 {
+	if (_param_sys_has_gps.get()) {
 		if (_vehicle_gps_position == nullptr) {
-		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
@@ -602,7 +601,6 @@ void Sensors::InitializeVehicleMagnetometer()
 {
 	if (_param_sys_has_mag.get()) {
 		if (_vehicle_magnetometer == nullptr) {
-			if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
 			_vehicle_magnetometer = new VehicleMagnetometer();
 
 			if (_vehicle_magnetometer) {
@@ -611,7 +609,6 @@ void Sensors::InitializeVehicleMagnetometer()
 		}
 	}
 }
-}
 
 void Sensors::Run()
 {

src/modules/sensors/voted_sensors_update.cpp

@@ -187,6 +187,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	// find the best sensor
 	int accel_best_index = -1;
 	int gyro_best_index = -1;
+	_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
+	_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
 
 	if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) {
 		// use sensor_selection to find best
@@ -213,9 +215,6 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 
 	} else {
 		// use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published
-		_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
-		_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
-
 		checkFailover(_accel, "Accel");
 		checkFailover(_gyro, "Gyro");
 	}

upload_fmuv3.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v3_default upload
+endtime=`date +'%Y-%m-%d %H:%M:%S'`
+date -R
+start_seconds=$(date --date="$starttime" +%s);
+end_seconds=$(date --date="$endtime" +%s);
+echo "本次运行时间： "$((end_seconds-start_seconds))"s"