增加两个改地址数组，报错增加延时打印

- 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/airframes/4073_ifo-s

@@ -71,6 +71,7 @@ param set-default SENS_BOARD_ROT 10
 param set-default SENS_FLOW_ROT 4
 # TFMini on TELEM3
 param set-default SENS_TFMINI_CFG 103
+param set-default SENS_TF_SER_ROT 25
 
 # Smart Battery
 param set-default SENS_EN_BATT 1

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -65,6 +65,11 @@ then
 	mb12xx start -X
 fi
 
+if param greater -s SENS_EN_TFI2C 0
+then
+	tfmini_i2c start -X
+fi
+
 # pga460 sonar sensor
 if param greater -s SENS_EN_PGA460 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

Tools/simulation-ignition

@@ -0,0 +1 @@
+Subproject commit 483193d9b8b89211c3b970c735b4fbb5f724b63a

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

@@ -24,8 +24,8 @@ px4_add_board(
 		#camera_capture
 		#camera_trigger
 		#differential_pressure # all available differential pressure drivers
-		differential_pressure/ms4525
-		#distance_sensor # all available distance sensor drivers
+		# differential_pressure/ms4525
+		distance_sensor # all available distance sensor drivers
 		#distance_sensor/ll40ls
 		#distance_sensor/lightware_laser_serial
 		#dshot
@@ -60,7 +60,7 @@ px4_add_board(
 		tone_alarm
 		#uavcan
 	MODULES
-		airspeed_selector
+		# airspeed_selector
 		#attitude_estimator_q
 		battery_status
 		#camera_feedback
@@ -70,13 +70,13 @@ px4_add_board(
 		#esc_battery
 		#events
 		flight_mode_manager
-		fw_att_control
-		fw_pos_control_l1
+		# fw_att_control
+		# fw_pos_control_l1
 		#gyro_calibration
 		#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,9 @@
+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"
+cp ./build/px4_fmu-v3_default/px4_fmu-v3_default.px4 /mnt/f/_02-work/100_data/Firmware/px4/px4_fmu-v3_tfiic-12.px4

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/mb12xx/parameters.c

@@ -291,4 +291,4 @@ PARAM_DEFINE_INT32(SENS_MB12_10_ROT, 0);
  * @value 6 Yaw 270°
  * @value 7 Yaw 315°
  */
-PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);
+PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);

src/drivers/distance_sensor/tfmini/TFMINI.cpp

@@ -73,6 +73,13 @@ int
 TFMINI::init()
 {
 	int32_t hw_model = 1; // only one model so far...
+	param_t _param_tf_rot = param_find("SENS_TF_S1_ROT");
+	param_t _param_dist_max = param_find("SENS_TF_S_MAXD");
+	param_t _param_dist_min = param_find("SENS_TF_S_MIND");
+	int32_t tf_rot,dist_max,dist_min;
+	param_get(_param_tf_rot, &tf_rot);
+	param_get(_param_dist_max, &dist_max);
+	param_get(_param_dist_min, &dist_min);
 
 	switch (hw_model) {
 	case 1: // TFMINI (12m, 100 Hz)
@@ -81,8 +88,9 @@ TFMINI::init()
 		// 0.3 is too close to minimum so chattering of invalid sensor decision
 		// is happening sometimes. this cause EKF to believe inconsistent range readings.
 		// So we set 0.4 as valid minimum.
-		_px4_rangefinder.set_min_distance(0.4f);
-		_px4_rangefinder.set_max_distance(12.0f);
+		_px4_rangefinder.set_min_distance(dist_min/100.0f);
+		_px4_rangefinder.set_max_distance(dist_max/100.0f);
+		_px4_rangefinder.set_orientation(tf_rot);
 		_px4_rangefinder.set_fov(math::radians(1.15f));
 
 		break;

src/drivers/distance_sensor/tfmini/TFMINI.hpp

@@ -54,6 +54,7 @@
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <uORB/topics/distance_sensor.h>
 
+#include <px4_platform_common/module_params.h>
 #include "tfmini_parser.h"
 
 #define TFMINI_DEFAULT_PORT	"/dev/ttyS3"
@@ -97,4 +98,6 @@ private:
 	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")};
 
+protected:
+
 };

src/drivers/distance_sensor/tfmini/module.yaml

@@ -1,7 +1,27 @@
 module_name: Benewake TFmini Rangefinder
 serial_config:
-    - command: tfmini start -d ${SERIAL_DEV}
+    - command: tfmini start -d ${SERIAL_DEV} -R SENS_TF_SER_ROT
       port_config_param:
         name: SENS_TFMINI_CFG
         group: Sensors
 
+
+parameters:
+    - group: Sensors
+      definitions:
+        SENS_TF_S1_ROT:
+            description:
+                short: Distance Sensor Rotation
+                long: |
+                    Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum
+
+            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
+            default: 25

src/drivers/distance_sensor/tfmini/parameters.c

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Benewake TFmini Sensor 0 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_S_ROT, 0);
+
+/**
+ * Benewake TFmini Sensor S Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_S_MAXD, 1200);
+
+/**
+ * Benewake TFmini Sensor S Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_S_MIND, 10);

src/drivers/distance_sensor/tfmini_i2c/CMakeLists.txt

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

src/drivers/distance_sensor/tfmini_i2c/parameters.c

@@ -0,0 +1,445 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Benewake TF Rangefinder Sonar (tf i2c)
+ *
+ * @reboot_required true
+ *
+ * @boolean
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0);
+
+/**
+ * Benewake TF Sensor 0 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_0_ADDR, 16);
+
+/**
+ * Benewake TF Sensor 0 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0);
+
+/**
+ * Benewake TF Sensor 0 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 0 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10);
+
+/**
+ * Benewake TF Sensor 1 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_1_ADDR, 17);
+
+/**
+ * Benewake TF Sensor 1 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0);
+
+/**
+ * Benewake TF Sensor 1 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 1 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10);
+
+/**
+ * Benewake TF Sensor 2 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_2_ADDR, 18);
+
+/**
+ * Benewake TF Sensor 2 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0);
+
+/**
+ * Benewake TF Sensor 2 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 2 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10);
+
+/**
+ * Benewake TF Sensor 3 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_3_ADDR, 19);
+
+/**
+ * Benewake TF Sensor 3 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0);
+
+/**
+ * Benewake TF Sensor 3 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 3 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10);
+
+/**
+ * Benewake TF Sensor 4 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_4_ADDR, 20);
+
+/**
+ * Benewake TF Sensor 4 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0);
+
+/**
+ * Benewake TF Sensor 4 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 4 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_4_MIND, 10);
+
+/**
+ * Benewake TF Sensor 5 Address
+ *
+ * This parameter defines the address of the TFi2c sensor.
+ *
+ * @reboot_required true
+ * @min 1
+ * @max 255
+ * @group Sensors
+ *
+ */
+
+PARAM_DEFINE_INT32(SENS_TF_5_ADDR, 21);
+
+/**
+ * Benewake TF Sensor 5 Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 No rotation
+ * @value 1 Yaw 45°
+ * @value 2 Yaw 90°
+ * @value 3 Yaw 135°
+ * @value 4 Yaw 180°
+ * @value 5 Yaw 225°
+ * @value 6 Yaw 270°
+ * @value 7 Yaw 315°
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0);
+
+/**
+ * Benewake TF Sensor 5 Max Distance
+ *
+ * This parameter defines maximum measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_5_MAXD, 1200);
+
+/**
+ * Benewake TF Sensor 5 Min Distance
+ *
+ * This parameter defines min measuring distance.
+ *
+ * @reboot_required false
+ * @min 5
+ * @max 20000
+ * @group Sensors
+ *
+ */
+PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10);
+

src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp

@@ -0,0 +1,686 @@
+/****************************************************************************
+ *
+ *   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
+ * @author Greg Hulands
+ * @author Jon Verbeke <jon.verbeke@kuleuven.be>
+ *
+ * Driver for the Maxbotix sonar range finders connected via I2C.
+ */
+
+#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>
+using namespace time_literals;
+
+#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
+
+/* Configuration Constants */
+#define TFMINI_I2C_BASE_ADDR                        0x10   // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224.
+#define TFMINI_I2C_MAX_ADDR                         0x16   // 7-bit address is 0x5A = 90.  8-bit address is 0xB4 = 180.
+#define TFMINI_I2C_BUS_SPEED                        100000 // 100kHz bus speed.
+
+/* TF_I2Cxx Registers addresses */
+#define TFMINI_I2C_TAKE_RANGE_REG                   0x51 // Measure range Register.
+#define TFMINI_I2C_SET_ADDRESS_1                    0xAA // Change address 1 Register.
+#define TFMINI_I2C_SET_ADDRESS_2                    0xA5 // Change address 2 Register.
+
+/* Device limits */
+#define TFMINI_I2C_MIN_DISTANCE                     (0.10f)
+#define TFMINI_I2C_MAX_DISTANCE                     (12.00f)
+
+#define TFMINI_I2C_MEASURE_INTERVAL                 20_ms // 60ms minimum for one sonar.
+#define TFMINI_I2C_INTERVAL_BETWEEN_SUCCESIVE_FIRES 10_ms // 30ms minimum between each sonar measurement (watch out for interference!).
+
+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: 200-224 even addresses only.
+	 * @return Returns PX4_OK iff successful, PX4_ERROR otherwise.
+	 */
+	int set_address(const uint8_t address = TFMINI_I2C_BASE_ADDR);
+
+	/**
+	 * 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;
+
+private:
+
+	/**
+	 * Collects the most recent sensor measurement data from the i2c bus.
+	 */
+	int collect();
+
+	/**
+	 * Gets the current sensor rotation value.
+	 */
+	int get_sensor_rotation(const size_t index);
+
+	/**
+	 * 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();
+
+	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{TFMINI_I2C_MEASURE_INTERVAL};	// 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 max_min_dist
+	{
+		int32_t max;
+		int32_t min;
+	};
+	max_min_dist dist_range[6];
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_TFI2C>)   _p_sensor_enabled,
+		(ParamInt<px4::params::SENS_TF_0_ADDR>)  _p_sensor0_addr,
+		(ParamInt<px4::params::SENS_TF_1_ADDR>)  _p_sensor1_addr,
+		(ParamInt<px4::params::SENS_TF_2_ADDR>)  _p_sensor2_addr,
+		(ParamInt<px4::params::SENS_TF_3_ADDR>)  _p_sensor3_addr,
+		(ParamInt<px4::params::SENS_TF_4_ADDR>)  _p_sensor4_addr,
+		(ParamInt<px4::params::SENS_TF_5_ADDR>)  _p_sensor5_addr,
+		(ParamInt<px4::params::SENS_TF_0_ROT>)  _p_sensor0_rot,
+		(ParamInt<px4::params::SENS_TF_1_ROT>)  _p_sensor1_rot,
+		(ParamInt<px4::params::SENS_TF_2_ROT>)  _p_sensor2_rot,
+		(ParamInt<px4::params::SENS_TF_3_ROT>)  _p_sensor3_rot,
+		(ParamInt<px4::params::SENS_TF_4_ROT>)  _p_sensor4_rot,
+		(ParamInt<px4::params::SENS_TF_5_ROT>)  _p_sensor5_rot,
+		(ParamInt<px4::params::SENS_TF_0_MAXD>)  _p_tf_0_max_dist,
+		(ParamInt<px4::params::SENS_TF_1_MAXD>)  _p_tf_1_max_dist,
+		(ParamInt<px4::params::SENS_TF_2_MAXD>)  _p_tf_2_max_dist,
+		(ParamInt<px4::params::SENS_TF_3_MAXD>)  _p_tf_3_max_dist,
+		(ParamInt<px4::params::SENS_TF_4_MAXD>)  _p_tf_4_max_dist,
+		(ParamInt<px4::params::SENS_TF_5_MAXD>)  _p_tf_5_max_dist,
+		(ParamInt<px4::params::SENS_TF_0_MIND>)  _p_tf_0_min_dist,
+		(ParamInt<px4::params::SENS_TF_1_MIND>)  _p_tf_1_min_dist,
+		(ParamInt<px4::params::SENS_TF_2_MIND>)  _p_tf_2_min_dist,
+		(ParamInt<px4::params::SENS_TF_3_MIND>)  _p_tf_3_min_dist,
+		(ParamInt<px4::params::SENS_TF_4_MIND>)  _p_tf_4_min_dist,
+		(ParamInt<px4::params::SENS_TF_5_MIND>)  _p_tf_5_min_dist,
+		(ParamFloat<px4::params::CP_DIST>)   _p_cp_dist,
+        	(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
+
+	);
+    orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
+    float avoid_distance;
+    int entra_avoid_area{0};
+    uint8_t get_a_data;
+};
+
+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);
+}
+
+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);
+}
+
+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;
+	}
+	printf("\n--------------------\n");
+	const uint8_t param_total_addr[6] = { (uint8_t)_p_sensor0_addr.get(), (uint8_t)_p_sensor1_addr.get(), (uint8_t)_p_sensor2_addr.get(), \
+						(uint8_t)_p_sensor3_addr.get(),(uint8_t)_p_sensor4_addr.get(),(uint8_t)_p_sensor5_addr.get() };
+	const uint8_t param_total_rot[6] = { (uint8_t)_p_sensor0_rot.get(), (uint8_t)_p_sensor1_rot.get(), (uint8_t)_p_sensor2_rot.get(), \
+						(uint8_t)_p_sensor3_rot.get(),(uint8_t)_p_sensor4_rot.get(),(uint8_t)_p_sensor5_rot.get() };
+	// Check for connected rangefinders on each i2c port by decrementing from the base address,
+	// (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MAX_ADDR = 90).
+	uint8_t address;
+	for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) {
+		address = param_total_addr[index];
+		set_device_address(address);
+		px4_usleep(_measure_interval);
+		if (measure() == PX4_OK) {
+			px4_usleep(_measure_interval);
+			// Store I2C address
+			_sensor_addresses[_sensor_count] = address;
+			_sensor_rotations[_sensor_count] = param_total_rot[index];
+			_sensor_count++;
+            		int set_ret = set_param();
+			printf("tf[%i]: address 0x%02X ,rototion %d, set param:%d\n", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count],set_ret);
+			mavlink_log_info(&_mavlink_log_pub, "sensor %i at address 0x%02X ,rotation:%d", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count]);
+			if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) {
+				break;
+			}
+		}else{
+
+			printf("\nerr tf[%d]:address:%02x",_sensor_count,address);
+		}
+		px4_usleep(_measure_interval);
+	}
+	printf("\n--------------------\n");
+
+    // if(avoid_dist > 0.5f){
+
+
+        // _p_cp_dist.set(avoid_dist);
+        // _param_mpc_pos_mode.set(avoid_mode);
+        // printf("set avoid distance:%.2fm,mode:%d",(double)avoid_dist,avoid_mode);
+        avoid_distance = _p_cp_dist.get();
+        printf("get avoid distance:%.2fm,mode:%d\n",(double)avoid_distance,(uint8_t)_param_mpc_pos_mode.get());
+    // }
+
+	dist_range[0].max = _p_tf_0_max_dist.get();
+    dist_range[1].max = _p_tf_1_max_dist.get();
+    dist_range[2].max = _p_tf_2_max_dist.get();
+    dist_range[3].max = _p_tf_3_max_dist.get();
+    dist_range[4].max = _p_tf_4_max_dist.get();
+    dist_range[5].max = _p_tf_5_max_dist.get();
+    dist_range[0].min = _p_tf_0_min_dist.get();
+    dist_range[1].min = _p_tf_1_min_dist.get();
+    dist_range[2].min = _p_tf_2_min_dist.get();
+    dist_range[3].min = _p_tf_3_min_dist.get();
+    dist_range[4].min = _p_tf_4_min_dist.get();
+    dist_range[5].min = _p_tf_5_min_dist.get();
+
+	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 , 0x05 , 0x0B , 0x20 , 0x8a }; // 修改地址
+//     const uint8_t CMD_OUTPUT_FORMAT_CM[] =       { 0x5A, 0x04, 0x11, 0x6F };	// 保存配置
+    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_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    // const uint8_t CMD_FW_VERSION[] = { 0x5A, 0x04, 0x01, 0x5F };
+    const uint8_t CMD_READ_MEASUREMENT[] = {0x5A,0x05,0x00,0x01,0x60 };
+
+	// uint8_t cmd = TFMINI_I2C_TAKE_RANGE_REG;
+	int ret_val = transfer(CMD_READ_MEASUREMENT, 5, nullptr, 0);
+
+	return ret_val;
+}
+
+int
+TFMINI_I2C::collect()
+{
+
+    uint16_t distance = 0;
+    // uint16_t strength;
+    // uint32_t timestamp;
+    static uint32_t err_cnt;
+    const uint8_t Data_Len = 11;
+    static uint64_t last_us ;
+	perf_begin(_sample_perf);
+
+    static  uint8_t raw_data[Data_Len];
+
+	// 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_addresses[_sensor_index]);
+
+	// Transfer data from the bus.
+	// int ret_val = transfer(CMD_READ_MEASUREMENT, 5, raw_data, 11);
+	int ret_val;
+	if (1)
+	{
+    		uint8_t CMD_READ_MEASUREMENT[] = { 0x5A, 0x05, 0x00, 0x07, 0x66 };
+		ret_val = tfi2c_transfer(CMD_READ_MEASUREMENT, 5, raw_data, Data_Len);
+	}else
+	{
+		ret_val = tfi2c_transfer(nullptr, 0, raw_data, Data_Len);
+	}
+
+
+	if (ret_val < 0) {
+		// PX4_ERR("sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address());
+		perf_count(_comms_error);
+		perf_end(_sample_perf);
+        // if (measure() != PX4_OK) {
+        //     PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
+        //     perf_count(_comms_error);
+        //     perf_end(_sample_perf);
+        //     return ret_val;
+        // }
+		return ret_val;
+	}
+    /////////////////////
+
+    /**
+     *
+    uint8_t header1;
+    uint8_t header2;
+    le16_t distance;
+    le16_t strength;
+    le32_t timestamp;
+    uint8_t checksum;
+    */
+    if(raw_data[0] == 0x59 && raw_data[1] == 0x59)
+    {
+        if(check_checksum(raw_data,Data_Len)){
+            distance = ((uint16_t)raw_data[3] << 8) | raw_data[2];
+            // strength = ((uint16_t)raw_data[5] << 8) | raw_data[4];
+            // timestamp = ((uint32_t)raw_data[9] << 8) | ((uint32_t)raw_data[8] << 8) | ((uint32_t)raw_data[7] << 8) | raw_data[6];
+        }
+
+    }else{
+        err_cnt += 1;
+        if(hrt_absolute_time() - last_us > 2 * 1_s){
+            printf("{%d}err arr\n",__LINE__);
+            for (size_t i = 0; i < Data_Len; i++)
+            {
+                printf("%02x ",raw_data[i]);
+            }
+            printf("\n");
+
+            printf("{%d}errcnt:%d, head %02x,%02x,crc: %02x ,clc:%02x\n",__LINE__,err_cnt,raw_data[0],raw_data[1],raw_data[Data_Len - 1],crc_clc);
+            last_us = hrt_absolute_time();
+            err_cnt = 0;
+        }
+		// return -EAGAIN;
+    }
+///////////////////////
+	uint16_t distance_cm = distance;
+	if (distance_cm > dist_range[_sensor_index].max || distance_cm < dist_range[_sensor_index].min )	// 超出距离范围
+	{
+		distance_cm = dist_range[_sensor_index].max ;
+	}
+
+	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     = dist_range[_sensor_index].max / 100.0f;
+	report.min_distance     = dist_range[_sensor_index].min / 100.0f;
+	report.orientation      = _sensor_rotations[_sensor_index];
+	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;
+	}
+
+	// Begin the next measurement.
+	// if (measure() != PX4_OK) {
+	// 	PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
+	// 	perf_count(_comms_error);
+	// 	perf_end(_sample_perf);
+	// 	return ret_val;
+	// }
+//     if(avoid_distance - distance_m > 0.1f && !entra_avoid_area){
+//         mavlink_log_info(&_mavlink_log_pub, "(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+//         printf("(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+//         entra_avoid_area = 1;
+//     }else if(distance_m - avoid_distance > 0.1f && entra_avoid_area){
+//         entra_avoid_area = 0;
+//         printf("(%d)Out obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m);
+
+//     }
+
+	// 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:%.2f,%.2f,%.2f,%.2f,%.2f,%.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::get_sensor_rotation(const size_t index)
+{
+	switch (index) {
+	case 0: return _p_sensor0_rot.get();
+
+	case 1: return _p_sensor1_rot.get();
+
+	case 2: return _p_sensor2_rot.get();
+
+	case 3: return _p_sensor3_rot.get();
+
+	case 4: return _p_sensor4_rot.get();
+
+	case 5: return _p_sensor5_rot.get();
+
+
+	default: return PX4_ERROR;
+	}
+}
+
+
+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_addresses[i]);
+	}
+}
+
+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(0x70);
+	PRINT_MODULE_USAGE_COMMAND("set_address");
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x70);
+	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/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,21 +53,22 @@
 
 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 */
-#define NCP5623_LED_PWM1	0x60	/**< pwm1 register */
-#define NCP5623_LED_PWM2	0x80	/**< pwm2 register */
+#define NCP5623_LED_CURRENT 0x20  /**< Current register */
+#define NCP5623_LED_PWM0    0x40  /**< pwm0 register */
+#define NCP5623_LED_PWM1    0x60  /**< pwm1 register */
+#define NCP5623_LED_PWM2    0x80  /**< pwm2 register */
 
-#define NCP5623_LED_BRIGHT	0x1f	/**< full brightness */
-#define NCP5623_LED_OFF		0x00	/**< off */
+#define NCP5623_LED_BRIGHT  0x1f  /**< full brightness */
+#define NCP5623_LED_OFF     0x00  /**< off */
 
 
 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,15 +79,20 @@ 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};
 
-	uint8_t			_r{0};
-	uint8_t			_g{0};
-	uint8_t			_b{0};
+	uint8_t		_r{0};
+	uint8_t		_g{0};
+	uint8_t		_b{0};
 	volatile bool		_running{false};
 	volatile bool		_should_run{true};
 	bool			_leds_enabled{true};
@@ -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/drivers/uavcan/sensors/rangefinder.cpp

@@ -52,6 +52,7 @@ int UavcanRangefinderBridge::init()
 	// Initialize min/max range from params
 	param_get(param_find("UAVCAN_RNG_MIN"), &_range_min_m);
 	param_get(param_find("UAVCAN_RNG_MAX"), &_range_max_m);
+	param_get(param_find("UAVCAN_RNG_ROT"), &_range_rot);
 
 	int res = _sub_range_data.start(RangeCbBinder(this, &UavcanRangefinderBridge::range_sub_cb));
 
@@ -104,6 +105,7 @@ void UavcanRangefinderBridge::range_sub_cb(const
 		rangefinder->set_fov(msg.field_of_view);
 		rangefinder->set_min_distance(_range_min_m);
 		rangefinder->set_max_distance(_range_max_m);
+		rangefinder->set_orientation((uint8_t)_range_rot);
 
 		_inited = true;
 	}

src/drivers/uavcan/sensors/rangefinder.hpp

@@ -69,6 +69,7 @@ private:
 
 	float _range_min_m{0.0f};
 	float _range_max_m{0.0f};
+	int32_t _range_rot{25};
 
 	bool _inited{false};
 

src/drivers/uavcan/uavcan_params.c

@@ -106,6 +106,25 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f);
  */
 PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f);
 
+/**
+ * UAVCAN rangefinder  Rotation
+ *
+ * This parameter defines the rotation of the sensor relative to the platform.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 25
+ * @group Sensors
+ *
+ * @value 0 ROTATION_FORWARD_FACING
+ * @value 2 ROTATION_RIGHT_FACING
+ * @value 4 ROTATION_BACKWARD_FACING
+ * @value 6 ROTATION_LEFT_FACING
+ * @value 24 ROTATION_UPWARD_FACING
+ * @value 25 ROTATION_DOWNWARD_FACING
+ */
+PARAM_DEFINE_INT32(UAVCAN_RNG_ROT, 25);
+
 /**
  * UAVCAN ANTI_COLLISION light operating mode
  *

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include <drivers/drv_hrt.h>
 
 using namespace matrix;
 using namespace time_literals;
@@ -531,6 +532,154 @@ 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)
+{
+
+	static uint64_t delay_1s = hrt_absolute_time();
+	bool need_prt = false;
+	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	{
+		// need_prt = true;
+		delay_1s = hrt_absolute_time();
+	}
+
+	//calculate movement constraints based on range data
+	float setpoint = original_setpoint;
+	/******************************************************************/
+
+	// _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
+		// read parameters
+	static float min_distance;
+	static float max_distance;
+	static float up_distance;
+    static uint64_t _data_h_timestamps;
+    static uint64_t _dist_timestamps;
+
+    // 获取测距距离值相关
+    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 (need_prt)
+		// 	printf("0.1:ts:%lld,rot:%d\n",distance_sensor.timestamp,distance_sensor.orientation);
+            if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
+                (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING)) {
+
+                // update message description
+                _dist_timestamps = distance_sensor.timestamp;
+                max_distance = math::max(max_distance,(distance_sensor.max_distance ));
+                min_distance = math::min(min_distance,(distance_sensor.min_distance ));
+
+                up_distance = distance_sensor.current_distance;
+		// if (need_prt)
+		// 	printf("1:dist up:%.2f,max:%.2f,min:%.2f\n",(double)up_distance,(double)max_distance,(double)min_distance);
+
+            }
+        }
+
+	// if (need_prt)
+	// 	printf("1.1:update:%d,max:%.2f,min:%.2fv\n",dist_sens_sub.update(&distance_sensor),(double)max_distance,(double)min_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 ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US) {
+		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 (need_prt)
+			printf("2:age:%lld,dist:%.2f,max:%.2f\n",data_age,(double)distance,(double)max_range);
+            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);
+                    }
+
+
+		if (need_prt)
+			printf("3:parallel:%.2f,posctrl:%.2f,stop_d:%.2f,vmax:%.2f\n",(double)curr_vel_parallel,(double)vel_max_posctrl,(double)stop_distance,(double)vel_max);
+
+            }else if (distance >= UINT16_MAX * 0.01f ) {
+			if (!move_no_data || (move_no_data)) {
+					vel_max = 0.f;
+				}
+		}
+
+            		setpoint = -vel_max;
+			if (need_prt)
+				printf("4:vmax:%.2f\n",(double)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();
+		}
+
+		if (need_prt)
+			printf("5:setpoint:%.2f,vmax:%.2f\n", (double)setpoint,(double)vel_max);
+
+
+	}
+    _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 (need_prt)
+		printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint);
+}
+
 void CollisionPrevention::_publishVehicleCmdDoLoiter()
 {
 	vehicle_command_s command{};

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -81,6 +81,8 @@ 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:
 

src/lib/crypto/libtomcrypt

@@ -0,0 +1 @@
+Subproject commit 673f5ce29015a9bba3c96792920a10601b5b0718

src/lib/crypto/libtommath

@@ -0,0 +1 @@
+Subproject commit fd73d7630b9d3ed5a79d613ff680a549e9780de7

src/lib/ecl

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

src/lib/events/libevents

@@ -0,0 +1 @@
+Subproject commit 6329c909a7b16bb6f97c94cbb598815635b94982

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/Commander.cpp

@@ -737,6 +737,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					/* POSCTL */
 					reset_posvel_validity();
 					main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
+					mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d,lo:%d,go:%d\n",custom_main_mode,main_ret,_status_flags.condition_local_altitude_valid,_status_flags.condition_global_position_valid);
+					// main_ret = TRANSITION_CHANGED;
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */

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

@@ -39,11 +39,13 @@
 #include <float.h>
 #include <mathlib/mathlib.h>
 #include <ecl/geo/geo.h>
+#include <drivers/drv_hrt.h>
 
 using namespace matrix;
 
 FlightTaskManualAltitude::FlightTaskManualAltitude() :
-	_sticks(this)
+	_sticks(this),
+	_collision_prevention(this)
 {}
 
 bool FlightTaskManualAltitude::updateInitialize()
@@ -97,13 +99,40 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
+	static uint64_t delay_1s = hrt_absolute_time();
+	bool need_prt = false;
+	if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	{
+		// need_prt = true;
+		delay_1s = hrt_absolute_time();
+	}
+
 	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
 	const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
 	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
 
 	// 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);
+	// _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
+
+	float vel_z = vel_max_z * _sticks.getPositionExpo()(2);
+	// collision prevention
+	if (_collision_prevention.is_active()) {
+		if (need_prt)
+			printf("\n1:vel_max_z:%.2f\n",(double)vel_z);
+		_collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2));
+		if (need_prt)
+			printf("2:vel_max_z:%.2f\n",(double)vel_z);
+	}
+
+	_velocity_setpoint(2) = vel_z;
+	if (need_prt)
+	{
+		printf(" vf:%.3f,vb:%.3f\n",(double)_velocity_setpoint(2), (double)vel_z);
+		// printf("vel_max_z:%.2f,alt:%.2f,vz:%.2fv\n",(double)vel_max_z,(double)_position(2),(double)_velocity(2));
+		printf("active:%d,vel_max_z:%.2f,alt:%.2f,vz:%.2fv\n",_collision_prevention.is_active(),(double)vel_max_z,(double)_position(2),(double)_velocity(2));
+	}
+
 }
 
 float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -376,5 +405,13 @@ bool FlightTaskManualAltitude::update()
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
 
+	// static uint64_t delay_1s = hrt_absolute_time();
+	// static uint64_t delt_time;
+	// if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL)
+	// {
+	// 	printf("update,delt:%dms \n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL) );
+        // delay_1s = hrt_absolute_time();
+	// }
+    	// delt_time = hrt_absolute_time();
 	return ret;
 }

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/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -114,6 +114,7 @@ void FlightTaskManualPosition::_scaleSticks()
 	}
 
 	_velocity_setpoint.xy() = vel_sp_xy;
+
 }
 
 void FlightTaskManualPosition::_updateXYlock()

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -76,4 +76,5 @@ private:
 		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 
 	CollisionPrevention _collision_prevention;	/**< collision avoidance setpoint amendment */
+	orb_advert_t		_mavlink_log_pub{nullptr};	///< mavlink log pub
 };

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

@@ -0,0 +1 @@
+Subproject commit b568a60fca42599d9998434e606f6e38e0b5e298

src/modules/mavlink/mavlink_main.cpp

@@ -1521,7 +1521,8 @@ 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", 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,16 +572,7 @@ 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;
+	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,12 +535,10 @@ 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();
+			_vehicle_air_data = new VehicleAirData();
 
-				if (_vehicle_air_data) {
-					_vehicle_air_data->Start();
-				}
+			if (_vehicle_air_data) {
+				_vehicle_air_data->Start();
 			}
 		}
 	}
@@ -547,8 +546,8 @@ void Sensors::InitializeVehicleAirData()
 
 void Sensors::InitializeVehicleGPSPosition()
 {
-	if (_vehicle_gps_position == nullptr) {
-		if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
+	if (_param_sys_has_gps.get()) {
+		if (_vehicle_gps_position == nullptr) {
 			_vehicle_gps_position = new VehicleGPSPosition();
 
 			if (_vehicle_gps_position) {
@@ -602,12 +601,10 @@ 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();
+			_vehicle_magnetometer = new VehicleMagnetometer();
 
-				if (_vehicle_magnetometer) {
-					_vehicle_magnetometer->Start();
-				}
+			if (_vehicle_magnetometer) {
+				_vehicle_magnetometer->Start();
 			}
 		}
 	}

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

v2_fmu.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v2_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"

v5_build.sh

@@ -0,0 +1,8 @@
+date -R
+starttime=`date +'%Y-%m-%d %H:%M:%S'`
+make px4_fmu-v5_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"