rc/dsm: remove system field check, add new validity checks

- unfortunately we can't depend on the system field due to potential external binding and non-genuine Spektrum equipment - reject any DSM frame with duplicate channels - add 16 channel mask - tighten valid PWM range 990-2010us (±100% travel is 1102-1898µs) - update RCTest rejected frame count
positionControl: add check on sign before sqrtf
2026-05-23 23:07:36 +08:00 · 2021-01-01 22:58:47 +01:00 · 2020-11-19 10:01:56 -05:00 · 2020-11-19 11:15:30 +01:00 · 2020-11-18 11:47:06 -05:00 · 2020-11-12 12:57:27 -05:00
81 changed files with 1719 additions and 519 deletions
@@ -34,7 +34,9 @@ pipeline {
                     "bitcraze_crazyflie_default",
                     "cuav_nora_default",
                     "cuav_x7pro_default",
+                     "cubepilot_cubeorange_console",
                     "cubepilot_cubeorange_default",
+                     "cubepilot_cubeyellow_console",
                     "cubepilot_cubeyellow_default",
                     "holybro_durandal-v1_default",
                     "holybro_durandal-v1_stackcheck",
@@ -20,7 +20,9 @@ jobs:
          bitcraze_crazyflie_default,
          cuav_nora_default,
          cuav_x7pro_default,
+          cubepilot_cubeorange_console,
          cubepilot_cubeorange_default,
+          cubepilot_cubeyellow_console,
          cubepilot_cubeyellow_default,
          holybro_durandal-v1_default,
          holybro_durandal-v1_stackcheck,
@@ -119,7 +119,7 @@ if [ $AUTOCNF = yes ]
 then
 	param set SYS_AUTOSTART $REQUESTED_AUTOSTART

-	param set BAT_N_CELLS 3
+	param set BAT_N_CELLS 4

 	param set CAL_ACC0_ID 1311244
 	param set CAL_ACC_PRIME 1311244
@@ -12,7 +12,6 @@
 #
 # @maintainer
 #
-# @board px4_fmu-v2 exclude
 # @board intel_aerofc-v1 exclude
 # @board bitcraze_crazyflie exclude
 #
@@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
 fi

 if [ $SYSTYPE = "Linux" ]; then
-SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,"
+SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,"
 fi

 if [[ $SYSTYPE = *"CYGWIN"* ]]; then
@@ -219,8 +219,6 @@ Syslink::update_params(bool force_set)
 		this->_params_update[2] = t;
 		this->_params_ack[2] = 0;
 	}
-
-
 }

 // 1M 8N1 serial connection to NRF51
@@ -363,7 +361,7 @@ Syslink::task_main()
 			}

 			if (fds[1].revents & POLLIN) {
-				struct parameter_update_s update;
+				parameter_update_s update;
 				orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 				update_params(false);
 			}
@@ -372,7 +370,6 @@ Syslink::task_main()
 	}

 	close(_fd);
-
 }

 void
@@ -497,7 +494,6 @@ Syslink::handle_message(syslink_message_t *msg)
 	} else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) {
 		set_address(_addr);
 	}
-
 }

 void
@@ -515,7 +511,6 @@ Syslink::handle_radio(syslink_message_t *sys)
 	} else if (sys->type == SYSLINK_RADIO_ADDRESS) {
 		_params_ack[2] = t;
 	}
-
 }

 void
@@ -602,7 +597,6 @@ Syslink::handle_bootloader(syslink_message_t *sys)
 		c->data[22] = 0x10; // Protocol version
 		send_message(sys);
 	}
-
 }

 void
@@ -797,7 +791,6 @@ void status()
 		}

 		printf("\n\n");
-
 	}

 	close(deckfd);
@@ -827,20 +820,13 @@ void attached(int pid)
 	exit(found ? 1 : 0);
 }

-
-
 void test()
 {
 	// TODO: Ensure battery messages are recent
 	// TODO: Read and write from memory to ensure it is working
 }

-
-
-
-}
-
-
+} // namespace syslink

 int syslink_main(int argc, char *argv[])
 {
@@ -849,7 +835,6 @@ int syslink_main(int argc, char *argv[])
 		exit(1);
 	}

-
 	const char *verb = argv[1];

 	if (!strcmp(verb, "start")) {
@@ -873,9 +858,6 @@ int syslink_main(int argc, char *argv[])
 		syslink::test();
 	}

-
-
-
 	syslink::usage();
 	exit(1);

@@ -52,7 +52,6 @@ using namespace time_literals;

 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))

-
 typedef enum {
 	BAT_DISCHARGING = 0,
 	BAT_CHARGING = 1,
@@ -82,7 +81,6 @@ public:
 	int txrate;

 private:
-
 	friend class SyslinkBridge;
 	friend class SyslinkMemory;

@@ -153,7 +151,6 @@ private:
 	static int task_main_trampoline(int argc, char *argv[]);

 	void task_main();
-
 };


@@ -174,11 +171,9 @@ public:
 	void pipe_message(crtp_message_t *msg);

 protected:
-
 	virtual pollevent_t poll_state(struct file *filp);

 private:
-
 	Syslink *_link;

 	// Stores data that was received from syslink but not yet read by another driver
@@ -186,7 +181,6 @@ private:

 	crtp_message_t _msg_to_send;
 	int _msg_to_send_size_remaining;
-
 };


@@ -219,5 +213,4 @@ private:
 	int write(int i, uint16_t addr, const char *buf, int length);

 	void sendAndWait();
-
 };
@@ -153,8 +153,7 @@
 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
 #define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
-#define STM32_RCC_APB1ENR  STM32_RCC_APB1LENR
-#define RCC_APB1ENR_TIM3EN RCC_APB1LENR_TIM3EN
+

 #define HRT_PPM_CHANNEL         /* T3C1 */  1  /* use capture/compare channel 1 */
 #define GPIO_PPM_IN             /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
@@ -200,8 +199,6 @@

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4};
-
 #define BOARD_ENABLE_CONSOLE_BUFFER

 #define PX4_GPIO_INIT_LIST { \
@@ -9,7 +9,8 @@ px4_add_board(
 	ROMFSROOT px4fmu_common
 	BUILD_BOOTLOADER
 	TESTING
-#	UAVCAN_INTERFACES 2  - No H7 or FD can support in UAVCAN
+	UAVCAN_INTERFACES 2
+	UAVCAN_TIMER_OVERRIDE	  2
 	SERIAL_PORTS
 		GPS1:/dev/ttyS0
 		TEL1:/dev/ttyS1
@@ -56,7 +57,7 @@ px4_add_board(
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
-#		uavcan - No H7 or FD can support in UAVCAN yet
+		uavcan
 	MODULES
 		airspeed_selector
 		attitude_estimator_q
@@ -193,6 +193,9 @@
 #define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2    /* SPI6 clock source */
 #define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3    /* USB 1 and 2 clock source */
 #define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2     /* ADC 1 2 3 clock source */
+#define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */
+
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY

 /* FLASH wait states */
 #define BOARD_FLASH_WAITSTATES 2
@@ -153,8 +153,7 @@
 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
 #define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
-#define STM32_RCC_APB1ENR  STM32_RCC_APB1LENR
-#define RCC_APB1ENR_TIM3EN RCC_APB1LENR_TIM3EN
+

 #define HRT_PPM_CHANNEL         /* T3C1 */  1  /* use capture/compare channel 1 */
 #define GPIO_PPM_IN             /* PB4 T3C1 */ GPIO_TIM3_CH1IN_2
@@ -200,8 +199,6 @@

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4};
-
 #define BOARD_ENABLE_CONSOLE_BUFFER

 #define PX4_GPIO_INIT_LIST { \
@@ -0,0 +1,129 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR cubepilot
+	MODEL cubeorange
+	LABEL console
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	BUILD_BOOTLOADER
+	IO cubepilot_io-v2_default
+	TESTING
+	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		TEL1:/dev/ttyS0
+		TEL2:/dev/ttyS1
+		GPS1:/dev/ttyS2
+		# PX4IO:/dev/ttyS3
+		# CONSOLE:/dev/ttyS4
+		GPS2:/dev/ttyS5
+	DRIVERS
+		adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/invensense/icm20602
+		imu/invensense/icm20649
+		imu/invensense/icm20948
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		power_monitor/ina226
+		#protocol_splitter
+		pwm_out_sim
+		pwm_out
+		px4io
+		roboclaw
+		rpm
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_magnetometer
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
@@ -16,7 +16,7 @@ px4_add_board(
 		TEL2:/dev/ttyS1
 		GPS1:/dev/ttyS2
 		# PX4IO:/dev/ttyS3
-		# CONSOLE:/dev/ttyS4
+		TEL3:/dev/ttyS4
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc
@@ -0,0 +1,238 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x1016
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MAX_TASKS=64
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NFILE_DESCRIPTORS=15
+CONFIG_NFILE_STREAMS=8
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_MB=y
+CONFIG_NSH_DISABLE_MH=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_DTCMEXCLUDE=y
+CONFIG_STM32H7_DTCM_PROCFS=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_SPI_DMATHRESHOLD=8
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TIME_EXTENDED=y
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -5,6 +5,7 @@
 # You can then do "make savedefconfig" to generate a new defconfig file that includes your
 # modifications.
 #
+# CONFIG_DEV_CONSOLE is not set
 # CONFIG_DISABLE_ENVIRON is not set
 # CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
 # CONFIG_MMCSD_HAVE_CARDDETECT is not set
@@ -212,7 +213,6 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
-CONFIG_UART7_SERIAL_CONSOLE=y
 CONFIG_UART7_TXBUFSIZE=1500
 CONFIG_UART8_BAUD=57600
 CONFIG_UART8_RXBUFSIZE=600
@@ -107,11 +107,6 @@
 #define GPIO_nVDD_5V_HIPOWER_OC         /* PE10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) // VDD_5V_HIPOWER_OC

 /* Tone alarm output */
-#define STM32_RCC_APB1ENR 		STM32_RCC_APB1LENR  /* This is stupid and only applies for H7 */
-#define STM32_RCC_APB1RSTR 		STM32_RCC_APB1LRSTR /* This is stupid and only applies for H7 */
-#define RCC_APB1ENR_TIM2EN		RCC_APB1LENR_TIM2EN /* This is stupid and only applies for H7 */
-#define RCC_APB1ENR_TIM5EN		RCC_APB1LENR_TIM5EN /* This is stupid and only applies for H7 */
-#define RCC_APB1RSTR_TIM5RST		RCC_APB1LRSTR_TIM5RST /* This is stupid and only applies for H7 */
 #define TONE_ALARM_TIMER        2  /* timer 2 */
 #define TONE_ALARM_CHANNEL      1  /* PA15 TIM2_CH1 */

@@ -150,7 +145,7 @@

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4};
+#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -0,0 +1,128 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	VENDOR cubepilot
+	MODEL cubeyellow
+	LABEL console
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	IO cubepilot_io-v2_default
+	TESTING
+	UAVCAN_INTERFACES 2
+	SERIAL_PORTS
+		TEL1:/dev/ttyS0
+		TEL2:/dev/ttyS1
+		GPS1:/dev/ttyS2
+		# PX4IO:/dev/ttyS3
+		# CONSOLE:/dev/ttyS4
+		GPS2:/dev/ttyS5
+	DRIVERS
+		adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		#heater
+		#imu # all available imu drivers
+		imu/adis16448
+		imu/adis16477
+		imu/adis16497
+		imu/invensense/icm20602
+		imu/invensense/icm20649
+		imu/invensense/icm20948
+		irlock
+		lights/blinkm
+		lights/rgbled
+		lights/rgbled_ncp5623c
+		magnetometer # all available magnetometer drivers
+		mkblctrl
+		optical_flow # all available optical flow drivers
+		#osd
+		pca9685
+		power_monitor/ina226
+		#protocol_splitter
+		pwm_out_sim
+		pwm_out
+		px4io
+		roboclaw
+		rpm
+		tap_esc
+		telemetry # all available telemetry drivers
+		test_ppm
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		fw_att_control
+		fw_pos_control_l1
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		tests # tests and test runner
+		top
+		topic_listener
+		tune_control
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_magnetometer
+		fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		hello
+		hwtest # Hardware test
+		#matlab_csv_serial
+		px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		rover_steering_control # Rover example app
+		uuv_example_app
+		work_item
+	)
@@ -15,7 +15,7 @@ px4_add_board(
 		TEL2:/dev/ttyS1
 		GPS1:/dev/ttyS2
 		# PX4IO:/dev/ttyS3
-		# CONSOLE:/dev/ttyS4
+		TEL3:/dev/ttyS4
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc
@@ -0,0 +1,240 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32f7"
+CONFIG_ARCH_CHIP_STM32F777VI=y
+CONFIG_ARCH_CHIP_STM32F7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=22114
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x1012
+CONFIG_CDCACM_PRODUCTSTR="CubeYellow"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MAX_TASKS=64
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_MULTIBLOCK_DISABLE=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NFILE_DESCRIPTORS=15
+CONFIG_NFILE_STREAMS=8
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_MB=y
+CONFIG_NSH_DISABLE_MH=y
+CONFIG_NSH_DISABLE_TELNETD=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32F7_ADC1=y
+CONFIG_STM32F7_BBSRAM=y
+CONFIG_STM32F7_BBSRAM_FILES=5
+CONFIG_STM32F7_BKPSRAM=y
+CONFIG_STM32F7_DMA1=y
+CONFIG_STM32F7_DMA2=y
+CONFIG_STM32F7_DMACAPABLE=y
+CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32F7_I2C1=y
+CONFIG_STM32F7_I2C2=y
+CONFIG_STM32F7_I2C_DYNTIMEO=y
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32F7_OTGFS=y
+CONFIG_STM32F7_PROGMEM=y
+CONFIG_STM32F7_PWR=y
+CONFIG_STM32F7_RTC=y
+CONFIG_STM32F7_RTC_HSECLOCK=y
+CONFIG_STM32F7_RTC_MAGIC_REG=1
+CONFIG_STM32F7_SAVE_CRASHDUMP=y
+CONFIG_STM32F7_SDMMC1=y
+CONFIG_STM32F7_SDMMC_DMA=y
+CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32F7_SPI1=y
+CONFIG_STM32F7_SPI1_DMA=y
+CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI2=y
+CONFIG_STM32F7_SPI4=y
+CONFIG_STM32F7_SPI4_DMA=y
+CONFIG_STM32F7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI_DMA=y
+CONFIG_STM32F7_SPI_DMATHRESHOLD=8
+CONFIG_STM32F7_TIM10=y
+CONFIG_STM32F7_TIM11=y
+CONFIG_STM32F7_UART4=y
+CONFIG_STM32F7_UART7=y
+CONFIG_STM32F7_UART8=y
+CONFIG_STM32F7_USART2=y
+CONFIG_STM32F7_USART3=y
+CONFIG_STM32F7_USART6=y
+CONFIG_STM32F7_USART_BREAKS=y
+CONFIG_STM32F7_USART_INVERT=y
+CONFIG_STM32F7_USART_SINGLEWIRE=y
+CONFIG_STM32F7_USART_SWAP=y
+CONFIG_STM32F7_WWDG=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TIME_EXTENDED=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_RXDMA=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_RXDMA=y
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -5,6 +5,7 @@
 # You can then do "make savedefconfig" to generate a new defconfig file that includes your
 # modifications.
 #
+# CONFIG_DEV_CONSOLE is not set
 # CONFIG_DISABLE_ENVIRON is not set
 # CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
 # CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set
@@ -209,7 +210,6 @@ CONFIG_UART4_RXDMA=y
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
-CONFIG_UART7_SERIAL_CONSOLE=y
 CONFIG_UART7_TXBUFSIZE=1500
 CONFIG_UART8_BAUD=57600
 CONFIG_UART8_RXBUFSIZE=600
@@ -145,7 +145,7 @@

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

-#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4};
+#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -41,12 +41,12 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
-	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
-	initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortC, GPIO::Pin9}),
-	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
-	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
 	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortC, GPIO::Pin9}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -6,7 +6,7 @@
 adc start

 # Start Digital power monitors
-voxlpm -X -b 3 -T VBATT start
+voxlpm -X -b 3 -K -T VBATT start
 voxlpm -X -b 3 -T P5VDC start

 # Internal SPI bus ICM-20602
@@ -6,14 +6,14 @@
 adc start

 # Internal ICM-20602
-icm20602 -s -R 8 start
+icm20602 -s -b 1 -R 8 start

 # Internal SPI bus BMI088 accel & gyro
-bmi088 -A -R 8 -s start
-bmi088 -G -R 8 -s start
+bmi088 -A -s -b 5 -R 8 start
+bmi088 -G -s -b 5 -R 8 start

 # Internal ICM-20948 (with magnetometer)
-icm20948 -s -R 8 -M start
+icm20948 -s -b 1 -R 8 -M start

 # Interal DPS310 (barometer)
-dps310 -s start
+dps310 -s -b 2 start
@@ -63,7 +63,6 @@
 *   HSI: 16 MHz RC factory-trimmed
 *   HSE: 24 MHz crystal for HSE
 */
-
 #define STM32_BOARD_XTAL        24000000ul

 #define STM32_HSI_FREQUENCY     16000000ul
@@ -102,7 +101,6 @@
 * SYSCLK  = 432 MHz / 2 = 216 MHz
 * SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz
 */
-
 #define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
 #define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(432)
 #define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
@@ -113,7 +111,6 @@
 #define STM32_OTGFS_FREQUENCY   (STM32_VCO_FREQUENCY / 9)

 /* Configure factors for  PLLSAI clock */
-
 #define CONFIG_STM32F7_PLLSAI 1
 #define STM32_RCC_PLLSAICFGR_PLLSAIN    RCC_PLLSAICFGR_PLLSAIN(192)
 #define STM32_RCC_PLLSAICFGR_PLLSAIP    RCC_PLLSAICFGR_PLLSAIP(8)
@@ -121,7 +118,6 @@
 #define STM32_RCC_PLLSAICFGR_PLLSAIR    RCC_PLLSAICFGR_PLLSAIR(2)

 /* Configure Dedicated Clock Configuration Register */
-
 #define STM32_RCC_DCKCFGR1_PLLI2SDIVQ  RCC_DCKCFGR1_PLLI2SDIVQ(1)
 #define STM32_RCC_DCKCFGR1_PLLSAIDIVQ  RCC_DCKCFGR1_PLLSAIDIVQ(1)
 #define STM32_RCC_DCKCFGR1_PLLSAIDIVR  RCC_DCKCFGR1_PLLSAIDIVR(0)
@@ -131,10 +127,7 @@
 #define STM32_RCC_DCKCFGR1_DFSDM1SRC   0
 #define STM32_RCC_DCKCFGR1_ADFSDM1SRC  0

-
-
 /* Configure factors for  PLLI2S clock */
-
 #define CONFIG_STM32F7_PLLI2S 1
 #define STM32_RCC_PLLI2SCFGR_PLLI2SN   RCC_PLLI2SCFGR_PLLI2SN(192)
 #define STM32_RCC_PLLI2SCFGR_PLLI2SP   RCC_PLLI2SCFGR_PLLI2SP(2)
@@ -142,7 +135,6 @@
 #define STM32_RCC_PLLI2SCFGR_PLLI2SR   RCC_PLLI2SCFGR_PLLI2SR(2)

 /* Configure Dedicated Clock Configuration Register 2 */
-
 #define STM32_RCC_DCKCFGR2_USART1SRC  RCC_DCKCFGR2_USART1SEL_APB
 #define STM32_RCC_DCKCFGR2_USART2SRC  RCC_DCKCFGR2_USART2SEL_APB
 #define STM32_RCC_DCKCFGR2_UART4SRC   RCC_DCKCFGR2_UART4SEL_APB
@@ -170,18 +162,15 @@
 */

 /* AHB clock (HCLK) is SYSCLK (216 MHz) */
-
 #define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
 #define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
 #define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */

 /* APB1 clock (PCLK1) is HCLK/4 (54 MHz) */
-
 #define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
 #define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)

 /* Timers driven from APB1 will be twice PCLK1 */
-
 #define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
 #define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
 #define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
@@ -193,12 +182,10 @@
 #define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)

 /* APB2 clock (PCLK2) is HCLK/2 (108MHz) */
-
 #define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
 #define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)

 /* Timers driven from APB2 will be twice PCLK2 */
-
 #define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
 #define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
 #define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
@@ -216,7 +203,6 @@
 /* Use the Falling edge of the SDIO_CLK clock to change the edge the
 * data and commands are change on
 */
-
 #define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE

 #define STM32_SDMMC_INIT_CLKDIV         (118 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
@@ -234,8 +220,6 @@
 /* DMA ON:  SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(1+2)=16 MHz
 * DMA OFF: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(2+2)=12 MHz
 */
-//TODO #warning "Check Freq for 24mHz"
-
 #ifdef CONFIG_STM32F7_SDMMC_DMA
 #  define STM32_SDMMC_SDXFR_CLKDIV      (1 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
@@ -256,8 +240,8 @@

 #define BOARD_FLASH_WAITSTATES 7

-/* Alternate function pin selections ************************************************/

+/* UART/USART */
 #define GPIO_USART2_RX   GPIO_USART2_RX_2   /* PD6 */
 #define GPIO_USART2_TX   GPIO_USART2_TX_2   /* PD5 */
 #define GPIO_USART2_RTS  GPIO_USART2_RTS_2  /* PD4 */
@@ -283,20 +267,13 @@
 *      GPIO_UART8_TX                          PE1
 */

-/* CAN
- *
- * CAN1 is routed to transceiver.
- */
+
+/* CAN */
 #define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
 #define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1 */

-/* SPI
- * SPI1 sensors 1
- * SPI2 FRAM + baro
- * SPI5 sensors 2
- * SPI6 Reserved
- *
- */
+
+/* SPI */
 #define GPIO_SPI1_SCK    GPIO_SPI1_SCK_1    /* PA5 */
 #define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1   /* PA6 */
 #define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1   /* PA7 */
@@ -313,19 +290,3 @@
 /* I2C */
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */
-
-#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
-
-/* SDMMC1
- *
- *      VDD 3.3
- *      GND
- *      SDMMC1_CK                           PC12
- *      SDMMC1_CMD                          PD2
- *      SDMMC1_D0                           PC8
- *      SDMMC1_D1                           PC9
- *      SDMMC1_D2                           PC10
- *      SDMMC1_D3                           PC11
- */
-
@@ -74,7 +74,7 @@
 | Channel 10 | SAI1_B           | SAI2_B           | SAI2_A           | -                | -                | -                | SAI1_A           | -                |
 | Channel 11 | SDMMC2           | -                | QUADSPI          | -                | -                | SDMMC2           | -                | -                |
 |            |                  |                  |                  |                  |                  |                  |                  |                  |
-| Usage      | SPI1_RX_1        |                  | USART6_RX_2      | SPI1_TX_1        | SPI5_TX_1        | SPI5_RX_2        | SDMMC1_2         | USART6_TX_2      |
+| Usage      | SPI1_RX_1        | USART6_RX_2      |                  | SPI1_TX_1        | SPI5_TX_1        | SPI5_RX_2        | SDMMC1_2         |                  |
 */

 // DMA1 Channel/Stream Selections
@@ -89,10 +89,10 @@
 //  DMA2 Channel/Stream Selections
 //--------------------------------------------//---------------------------//----------------
 #define DMAMAP_SPI1_RX    DMAMAP_SPI1_RX_1    // DMA2, Stream 0, Channel 3    (SPI1 sensors RX)
-//      AVAILABLE                             // DMA2, Stream 1
-#define DMAMAP_USART6_RX  DMAMAP_USART6_RX_2  // DMA2, Stream 2, Channel 5
+#define DMAMAP_USART6_RX  DMAMAP_USART6_RX_1  // DMA2, Stream 1, Channel 5
+//      AVAILABLE                             // DMA2, Stream 2
 #define DMAMAP_SPI1_TX    DMAMAP_SPI1_TX_1    // DMA2, Stream 3, Channel 3    (SPI1 sensors TX)
 #define DMAMAP_SPI5_TX    DMAMAP_SPI5_TX_1    // DMA2, Stream 4, Channel 3    (SPI5 sensors TX)
 #define DMAMAP_SPI5_RX    DMAMAP_SPI5_RX_2    // DMA2, Stream 5, Channel 3    (SPI5 sensors RX)
 #define DMAMAP_SDMMC1     DMAMAP_SDMMC1_2     // DMA2, Stream 6, Channel 4
-#define DMAMAP_USART6_TX  DMAMAP_USART6_TX_2  // DMA2, Stream 7, Channel 5
+//      AVAILABLE                             // DMA2, Stream 7, Channel 5
@@ -1,7 +1,7 @@
 /****************************************************************************
 * scripts/script.ld
 *
- *   Copyright (C) 2016 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2019-2020 Gregory Nutt. All rights reserved.
 *   Author: Gregory Nutt <gnutt@nuttx.org>
 *
 *
@@ -65,19 +65,19 @@
 * where the code expects to begin execution by jumping to the entry point in
 * the 0x0800:0000 address range.
 *
- * Bootloader reserves the first 32K bank (2 Mbytes Flash memory single bank)
+ * Bootloader reserves three 32K banks (2 Mbytes Flash memory single bank)
 * organization (256 bits read width)
 */

 MEMORY
 {
-    FLASH_ITCM (rx) : ORIGIN = 0x00218000, LENGTH = 1952K
-    FLASH_AXIM (rx) : ORIGIN = 0x08018000, LENGTH = 1952K /* start on 4th sector (1st sector for bootloader, 2 for extra storage) */
+	FLASH_ITCM (rx) : ORIGIN = 0x00218000, LENGTH = 1952K
+	FLASH_AXIM (rx) : ORIGIN = 0x08018000, LENGTH = 1952K /* start on 4th sector (1st sector for bootloader, 2 for extra storage) */

-    ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K
-    DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
-    SRAM1    (rwx) : ORIGIN = 0x20020000, LENGTH = 368K
-    SRAM2    (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K
+	ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K
+	DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
+	SRAM1    (rwx) : ORIGIN = 0x20020000, LENGTH = 368K
+	SRAM2    (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K
 }

 OUTPUT_ARCH(arm)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -105,9 +105,7 @@
 /* CAN Silence: Silent mode control \ ESC Mux select */
 #define GPIO_CAN1_SILENT_S0  /* PF5  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)

-/* PWM
- *
- */
+/* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS  8
 #define DIRECT_INPUT_TIMER_CHANNELS  8

@@ -118,12 +116,10 @@
 #define BOARD_NUMBER_BRICKS             1

 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN  /* PE4  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
-#define GPIO_VDD_3V3_SENSORS_EN         /* PE3  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3)
-

 /* Define True logic Power Control in arch agnostic form */
-#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true))
-#define READ_VDD_3V3_SPEKTRUM_POWER_EN()   px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN)
+#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
+#define READ_VDD_3V3_SPEKTRUM_POWER_EN()   (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)

 /* Tone alarm output */
 #define TONE_ALARM_TIMER        2  /* timer 2 */
@@ -134,22 +130,18 @@
 #define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
 #define GPIO_TONE_ALARM         GPIO_TIM2_CH1OUT_2

-/* USB OTG FS
- *
- * PA9  OTG_FS_VBUS VBUS sensing
- */
+/* USB OTG FS */
 #define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)

 /* High-resolution timer */
 #define HRT_TIMER               3  /* use timer3 for the HRT */
-#define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
+#define HRT_TIMER_CHANNEL       2  /* use capture/compare channel 2 */

-#define HRT_PPM_CHANNEL         /* T3C2 */  2  /* use capture/compare channel 1 */
-#define GPIO_PPM_IN             /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
+#define HRT_PPM_CHANNEL         /* T3C3 */  3  /* use capture/compare channel 3 */
+#define GPIO_PPM_IN             /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1

 /* RC Serial port */
 #define RC_SERIAL_PORT                     "/dev/ttyS3"
-#define RC_SERIAL_SINGLEWIRE

 #define GPIO_RSSI_IN                       /* PC1  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)

@@ -162,12 +154,11 @@
 #define SPEKTRUM_POWER(_on_true)           VDD_3V3_SPEKTRUM_POWER_EN(_on_true)

 /*
- * FMUv5 has a separate RC_IN
+ * Board has a separate RC_IN
 *
- * GPIO PPM_IN on PC7 T3CH2
- * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PG9 (NOT FMUv5 test HW ONLY)
- *   In version is possible in the UART
- * and can drive  GPIO PPM_IN as an output
+ * GPIO PPM_IN on PB0 T3CH3
+ * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
+ *   Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
 */
 #define GPIO_PPM_IN_AS_OUT             (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define SPEKTRUM_RX_AS_GPIO_OUTPUT()   px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
@@ -201,7 +192,6 @@
 		GPIO_CAN1_SILENT_S0,              \
 		GPIO_nPOWER_IN_A,                 \
 		GPIO_VDD_3V3_SPEKTRUM_POWER_EN,   \
-		GPIO_VDD_3V3_SENSORS_EN,          \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_SAFETY_SWITCH_IN,            \
 	}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -100,9 +100,6 @@ __END_DECLS
 ************************************************************************************/
 __EXPORT void board_peripheral_reset(int ms)
 {
-	/* set the peripheral rails off */
-	board_control_spi_sensors_power(false, 0xffff);
-
 	bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN();
 	/* Keep Spektum on to discharge rail*/
 	VDD_3V3_SPEKTRUM_POWER_EN(false);
@@ -115,7 +112,6 @@ __EXPORT void board_peripheral_reset(int ms)

 	/* switch the peripheral rail back on */
 	VDD_3V3_SPEKTRUM_POWER_EN(last);
-	board_control_spi_sensors_power(true, 0xffff);
 }

 /************************************************************************************
@@ -161,9 +157,7 @@ stm32_boardinitialize(void)
 	/* configure pins */
 	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
 	px4_gpio_init(gpio, arraySize(gpio));
-
-	/* configure SPI interfaces */
-	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+	board_control_spi_sensors_power_configgpio();

 	/* configure USB interfaces */
 	stm32_usbinitialize();
@@ -198,7 +192,6 @@ stm32_boardinitialize(void)
 __EXPORT int board_app_initialize(uintptr_t arg)
 {
 	/* Power on Interfaces */
-	px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
 	board_control_spi_sensors_power(true, 0xffff);
 	VDD_3V3_SPEKTRUM_POWER_EN(true);

@@ -39,7 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
-	}),
+	}, {GPIO::PortE, GPIO::Pin3}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}),
 		initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}),
@@ -111,8 +111,7 @@
 /* High-resolution timer */
 #define HRT_TIMER               2  /* use timer2 for the HRT */
 #define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
-#define STM32_RCC_APB1ENR       STM32_RCC_APB1LENR
-#define RCC_APB1ENR_TIM2EN      RCC_APB1LENR_TIM2EN /* This is stupid and only applies for H7 */
+

 /* RC Serial port */

@@ -142,6 +142,7 @@ function(px4_add_board)
 			IO
 			BOOTLOADER
 			UAVCAN_INTERFACES
+			UAVCAN_TIMER_OVERRIDE
 			LINKER_PREFIX
 		MULTI_VALUE
 			DRIVERS
@@ -221,6 +222,10 @@ function(px4_add_board)
 		set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
 	endif()

+	if(UAVCAN_TIMER_OVERRIDE)
+		set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
+	endif()
+
 	# OPTIONS

 	if(CONSTRAINED_FLASH)
@@ -89,6 +89,7 @@ function(px4_add_common_flags)
 		-Wno-missing-field-initializers
 		-Wno-missing-include-dirs # TODO: fix and enable
 		-Wno-unused-parameter
+		-Wno-logical-op # to compile 1.11 release on GCC 10 see https://github.com/PX4/PX4-Matrix/pull/146
 		)

 	# compiler specific flags
@@ -57,6 +57,7 @@ if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux")
 		/dev/serial/by-id/usb-Gumstix*
 		/dev/serial/by-id/usb-Hex_ProfiCNC*
 		/dev/serial/by-id/usb-UVify*
+		/dev/serial/by-id/usb-ArduPilot*
 		)

 elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin")
@@ -129,7 +129,7 @@ int board_hardfault_init(int display_to_console, bool allow_prompt)

 		/* Also end the misery for a user that holds for a key down on the console */

-		int bytesWaiting;
+		int bytesWaiting = 0;
 		ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting));

 		if (reboots > display_to_console || bytesWaiting != 0) {
@@ -38,6 +38,19 @@
 __BEGIN_DECLS

 #define PX4_SOC_ARCH_ID             PX4_SOC_ARCH_ID_STM32H7
+
+
+#define STM32_RCC_APB1ENR     STM32_RCC_APB1LENR
+#define STM32_RCC_APB1RSTR    STM32_RCC_APB1LRSTR
+#define RCC_APB1ENR_TIM2EN    RCC_APB1LENR_TIM2EN
+#define RCC_APB1ENR_TIM3EN    RCC_APB1LENR_TIM3EN
+#define RCC_APB1ENR_TIM5EN    RCC_APB1LENR_TIM5EN
+#define RCC_APB1ENR_TIM14EN   RCC_APB1LENR_TIM14EN
+#define RCC_APB1RSTR_TIM2RST  RCC_APB1LRSTR_TIM2RST
+#define RCC_APB1RSTR_TIM5RST  RCC_APB1LRSTR_TIM5RST
+
+
+
 #include <chip.h>
 #include <hardware/stm32_flash.h>
 #include <up_internal.h> //include up_systemreset() which is included on stm32.h
@@ -11,7 +11,6 @@ param set SYS_RESTART_TYPE 0

 dataman start

-battery_simulator start
 simulator start
 tone_alarm start
 pwm_out_sim start
@@ -135,7 +135,7 @@ DPS310::reset()
 	getTwosComplement(_calibration.c01, 16);

 	// 0x1A c11 [15:8] + 0x1B c11 [7:0]
-	_calibration.c11 = ((uint32_t)coef[8] << 8) | (uint32_t)coef[9];
+	_calibration.c11 = ((uint32_t)coef[10] << 8) | (uint32_t)coef[11];
 	getTwosComplement(_calibration.c11, 16);

 	// 0x1C c20 [15:8] + 0x1D c20 [7:0]
@@ -41,11 +41,11 @@

 #include "LidarLiteI2C.h"

-LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
+LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
 			   const int address) :
 	I2C(DRV_RNG_DEVTYPE_LL40LS, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, rotation)
+	_px4_rangefinder(get_device_id(), ORB_PRIO_DEFAULT, orientation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE);
@@ -94,7 +94,7 @@ static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms};
 class LidarLiteI2C : public device::I2C, public I2CSPIDriver<LidarLiteI2C>
 {
 public:
-	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
+	LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency,
 		     const int address = LL40LS_BASEADDR);
 	virtual ~LidarLiteI2C();

@@ -64,7 +64,7 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
 	PRINT_MODULE_USAGE_COMMAND("regdump");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }
@@ -72,7 +72,7 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html
 I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				      int runtime_instance)
 {
-	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);
+	LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency);

 	if (instance == nullptr) {
 		PX4_ERR("alloc failed");
@@ -99,12 +99,13 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
 	int ch;
 	using ThisDriver = LidarLiteI2C;
 	BusCLIArguments cli{true, false};
+	cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	cli.default_i2c_frequency = 100000;

 	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
 		switch (ch) {
 		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optarg());
+			cli.orientation = (enum Rotation)atoi(cli.optarg());
 			break;
 		}
 	}
@@ -455,7 +455,8 @@ void BMI055_Accelerometer::FIFOReset()
 void BMI055_Accelerometer::UpdateTemperature()
 {
 	// The slope of the temperature sensor is 0.5K/LSB, its center temperature is 23°C [(ACC 0x08) temp = 0x00].
-	float temperature = RegisterRead(Register::ACCD_TEMP) * 0.5f + 23.f;
+	// The register contains the current chip temperature represented in two’s complement format.
+	float temperature = static_cast<int8_t>(RegisterRead(Register::ACCD_TEMP)) * 0.5f + 23.f;

 	if (PX4_ISFINITE(temperature)) {
 		_px4_accel.set_temperature(temperature);
@@ -210,9 +210,11 @@ INA226::collect()
 {
 	perf_begin(_sample_perf);

-	parameter_update_s param_update;
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);

-	if (_parameters_sub.copy(&param_update)) {
 		updateParams();
 	}

@@ -169,7 +169,7 @@ private:

 	Battery 		  _battery;
 	uORB::Subscription  _actuators_sub{ORB_ID(actuator_controls_0)};
-	uORB::Subscription  _parameters_sub{ORB_ID(parameter_update)};
+	uORB::Subscription  _parameter_update_sub{ORB_ID(parameter_update)};

 	int read(uint8_t address, int16_t &data);
 	int write(uint8_t address, uint16_t data);
@@ -33,54 +33,217 @@

 /**
 * @file voxlpm.cpp
- * Driver for the VOXL Power Management unit
+ * Driver for the VOXL Power Module unit
 */

 #include "voxlpm.hpp"

 /*
- * The VOXLPM has two LTC2946 ICs on it.
+ * The VOXLPM v2 has two LTC2946 ICs on it.
 * Address 0x6A - measures battery voltage and current with a 0.0005 ohm sense resistor
- * Address 0x6B - measures 5VDC ouptut voltage and current
+ * Address 0x6B - measures 5VDC ouptut voltage and current with a 0.005 ohm sense resistor
+ *
+ * The VOXLPM v3 has two INA231 ICs on it.
+ * Address 0x44 - measures battery voltage and current with a 0.0005 ohm sense resistor
+ * Address 0x45 - measures 5VDC/12VDC ouptut voltage and current with a 0.005 ohm sense resistor
 */
 VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type) :
-	I2C(DRV_POWER_DEVTYPE_VOXLPM, MODULE_NAME, bus,
-	    (ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_ADDR_VBATT : VOXLPM_LTC2946_ADDR_P5VD, bus_frequency),
+	I2C(DRV_POWER_DEVTYPE_VOXLPM, MODULE_NAME, bus, VOXLPM_INA231_ADDR_VBATT, bus_frequency),
 	ModuleParams(nullptr),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
 	_ch_type(ch_type),
 	_battery(1, this, _meas_interval_us)
 {
-	if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
-		_rsense = VOXLPM_RSENSE_VBATT;
-
-	} else {
-		_rsense = VOXLPM_RSENSE_5VOUT;
-	}
 }

 VOXLPM::~VOXLPM()
 {
 	perf_free(_sample_perf);
+	perf_free(_comms_errors);
 }

 int
 VOXLPM::init()
 {
+	_initialized = false;
 	int ret = PX4_ERROR;

-	/* do I2C init (and probe) first */
+	if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+		_battery.updateBatteryStatus(
+			hrt_absolute_time(),
+			0.0,
+			0.0,
+			false,
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+			0,
+			0.0
+		);
+	}
+
+	/* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */
 	if (I2C::init() != OK) {
 		return ret;
 	}

-	write_reg(DEFAULT_CTRLA_REG_VAL, VOXLPM_LTC2946_CTRLA_REG);
-	write_reg(DEFAULT_CTRLB_REG_VAL, VOXLPM_LTC2946_CTRLB_REG);
+	/* If we've probed and succeeded we'll have an accurate address here for the VBat addr */
+	uint8_t addr = get_device_address();

-	_battery.reset();
+	if (addr == VOXLPM_LTC2946_ADDR_VBATT || addr == VOXLPM_LTC2946_ADDR_P5VD) {
+		_pm_type = VOXLPM_TYPE_V2_LTC;
+		load_params(_pm_type, _ch_type);
+		ret = init_ltc2946();

+	} else if (addr == VOXLPM_INA231_ADDR_VBATT || addr == VOXLPM_INA231_ADDR_P5_12VDC) {
+		_pm_type = VOXLPM_TYPE_V3_INA;
+		load_params(_pm_type, _ch_type);
+		ret = init_ina231();
+
+	} else {
+		PX4_ERR("Unkown device address");
+		ret = PX4_ERROR;
+	}
+
+	if (ret == PX4_OK) {
+		_initialized = true;
+		start();
+	}
+
+	return ret;
+}
+
+int
+VOXLPM::force_init()
+{
+	int ret = init();
 	start();
+	return ret;
+}
+
+int
+VOXLPM::probe()
+{
+	int ret = PX4_ERROR;
+	uint8_t data[2];
+
+	uint8_t addr;
+
+	/* Try LTC2946 first */
+	if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+		addr = VOXLPM_LTC2946_ADDR_VBATT;
+
+	} else {
+		addr = VOXLPM_LTC2946_ADDR_P5VD;
+	}
+
+	set_device_address(addr);
+
+	/* Check status register */
+	ret = read_reg_buf(VOXLPM_LTC2946_STATUS_REG, data, sizeof(data));
+
+	if (ret) {
+		/* Try INA231 next */
+		if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+			addr = VOXLPM_INA231_ADDR_VBATT;
+
+		} else {
+			addr = VOXLPM_INA231_ADDR_P5_12VDC;
+		}
+
+		set_device_address(addr);
+
+		/* Check config register */
+		ret = read_reg_buf(INA231_REG_CONFIG, data, sizeof(data));
+	}
+
+	return ret;
+}
+
+int
+VOXLPM::load_params(VOXLPM_TYPE pm_type, VOXLPM_CH_TYPE ch_type)
+{
+	if (pm_type == VOXLPM_TYPE_V2_LTC) {
+		/* No configuration needed */
+		_rshunt = (ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_LTC2946_VBAT_SHUNT : VOXLPM_LTC2946_VREG_SHUNT;
+
+	} else if (pm_type == VOXLPM_TYPE_V3_INA) {
+
+		_rshunt = -1.0f;
+		float fvalue = -1.0f;
+		param_t ph;
+
+		/* Allow for configuration */
+		if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+			ph = param_find("VOXLPM_SHUNT_BAT");
+
+			if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+				_rshunt = fvalue;
+			}
+
+		} else {
+			ph = param_find("VOXLPM_SHUNT_REG");
+
+			if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+				_rshunt = fvalue;
+			}
+		}
+
+		if (_rshunt < 0) {
+			_rshunt = (_ch_type == VOXLPM_CH_TYPE_VBATT) ? VOXLPM_INA231_VBAT_SHUNT : VOXLPM_INA231_VREG_SHUNT;
+		}
+
+	}
+
+	return PX4_OK;
+}
+
+int
+VOXLPM::init_ltc2946()
+{
+	write_reg(VOXLPM_LTC2946_CTRLA_REG, DEFAULT_LTC2946_CTRLA_REG_VAL);
+	write_reg(VOXLPM_LTC2946_CTRLB_REG, DEFAULT_LTC2946_CTRLB_REG_VAL);
+	return PX4_OK;
+}
+
+int
+VOXLPM::init_ina231()
+{
+	int ret = PX4_OK;
+	uint16_t cmd;
+
+	/* Reset */
+	cmd = INA231_RST_BIT;
+	ret = write_word_swapped(INA231_REG_CONFIG, cmd);
+
+	if (ret) {
+		PX4_ERR("Failed to reset INA231");
+		return ret;
+	}
+
+	/* Config */
+	cmd = INA231_CONFIG;
+	ret = write_word_swapped(INA231_REG_CONFIG, cmd);
+
+	if (ret) {
+		PX4_ERR("Failed to config INA231");
+		return ret;
+	}
+
+	if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+		_cal = (INA231_CONST / (VOXLPM_INA231_VBAT_I_LSB * _rshunt));
+
+	} else {
+		_cal = (INA231_CONST / (VOXLPM_INA231_VREG_I_LSB * _rshunt));
+	}
+
+	/* Set calibration */
+	ret = write_word_swapped(INA231_REG_CALIBRATION, _cal);
+
+	if (ret) {
+		PX4_ERR("Failed to calibrate INA231");
+		return ret;
+	}

 	return PX4_OK;
 }
@@ -90,16 +253,41 @@ VOXLPM::print_status()
 {
 	perf_print_counter(_sample_perf);

-	if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
-		printf("- type: BATT\n");
+	switch (_pm_type) {
+	case VOXLPM_TYPE_V2_LTC:
+		printf("- V2 (LTC2964)\n");
+		break;

-	} else {
-		printf("- type: P5VDC\n");
+	case VOXLPM_TYPE_V3_INA:
+		printf("- V3 (INA231)\n");
+		break;
+
+	default:
+		break;
 	}

-	printf("  - voltage: %9.2f VDC \n", (double)_voltage);
-	printf("  - current: %9.2f ADC \n", (double)_amperage);
-	printf("  - rsense: %9.6f ohm \n", (double)_rsense);
+	switch (_ch_type) {
+	case VOXLPM_CH_TYPE_VBATT:
+		printf("- type: BATT\n");
+		break;
+
+	case VOXLPM_CH_TYPE_P5VDC:
+		printf("- type: P5VDC\n");
+		break;
+
+	case VOXLPM_CH_TYPE_P12VDC:
+		printf("- type: P12VDC\n");
+		break;
+
+	default:
+		printf("- type: UNKOWN\n");
+		break;
+	}
+
+	printf("  - voltage: %9.4f VDC \n", (double)_voltage);
+	printf("  - current: %9.4f ADC \n", (double)_amperage);
+	printf("  - shunt: %9.4f mV, %9.4f mA\n", (double)_vshunt * 1000, (double)_vshuntamps * 1000);
+	printf("  - rsense: %9.6f ohm, cal: %i\n", (double)_rshunt, _cal);
 	printf("  - meas interval:  %u us \n", _meas_interval_us);
 }

@@ -118,44 +306,60 @@ VOXLPM::RunImpl()
 int
 VOXLPM::measure()
 {
-	parameter_update_s update;
+	int ret = PX4_ERROR;
+
+	if (!_initialized) {
+		ret = init();
+
+		if (ret) {
+			return ret;
+		}
+	}
+
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);

-	if (_parameter_sub.update(&update)) {
 		updateParams();
 	}

+	perf_begin(_sample_perf);

 	_voltage = 0.0f;
 	_amperage = 0.0f;
-
-	uint8_t vraw[2];
-	uint8_t iraw[2];
-
-	perf_begin(_sample_perf);
-
 	hrt_abstime tnow = hrt_absolute_time();

-	int curr_read_ret = read_reg_buf(VOXLPM_LTC2946_DELTA_SENSE_MSB_REG, iraw, sizeof(iraw)); // 0x14
-	int volt_read_ret = read_reg_buf(VOXLPM_LTC2946_VIN_MSB_REG, vraw, sizeof(vraw));         // 0x1E
+	switch (_pm_type) {
+	case VOXLPM_TYPE_V2_LTC:
+		ret = measure_ltc2946();
+		break;

-	if ((volt_read_ret == 0) && (curr_read_ret == 0)) {
-		uint16_t volt16 = (((uint16_t)vraw[0]) << 8) | vraw[1];   // MSB first
-		volt16        >>= 4;                                      // data is 12 bit and left-aligned
-		_voltage        = (volt16 / VOXLPM_LTC2946_RESOLUTION) * VOXLPM_LTC2946_VFS_SENSE;
+	case VOXLPM_TYPE_V3_INA:
+		ret = measure_ina231();
+		break;

-		uint16_t curr16 = (((uint16_t)iraw[0]) << 8) | iraw[1];   // MSB first
-		curr16        >>= 4;                                      // data is 12 bit and left-aligned
-		_amperage       = curr16 / VOXLPM_LTC2946_RESOLUTION * VOXLPM_LTC2946_VFS_DELTA_SENSE / _rsense;
+	default:
+		break;
+	}

+	if (ret == PX4_OK) {
 		switch (_ch_type) {
 		case VOXLPM_CH_TYPE_VBATT: {
-				_battery.updateBatteryStatus(tnow, _voltage, _amperage, true,
-							     battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
+				_actuators_sub.copy(&_actuator_controls);
+
+				_battery.updateBatteryStatus(tnow,
+							     _voltage,
+							     _amperage,
+							     true,
+							     battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+							     0,
+							     _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]);
 			}

 		// fallthrough
-
-		case VOXLPM_CH_TYPE_P5VDC: {
+		case VOXLPM_CH_TYPE_P5VDC:
+		case VOXLPM_CH_TYPE_P12VDC: {
 				memset(&_pm_status, 0x00, sizeof(_pm_status));
 				_pm_status.timestamp = tnow;
 				_pm_status.voltage_v = (float) _voltage;
@@ -169,10 +373,17 @@ VOXLPM::measure()
 		}

 	} else {
+		perf_count(_comms_errors);
+
 		switch (_ch_type) {
 		case VOXLPM_CH_TYPE_VBATT: {
-				_battery.updateBatteryStatus(tnow, 0.0, 0.0, true,
-							     battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0);
+				_battery.updateBatteryStatus(tnow,
+							     0.0,
+							     0.0,
+							     true,
+							     battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+							     0,
+							     0.0);
 			}
 			break;

@@ -183,7 +394,73 @@ VOXLPM::measure()

 	perf_end(_sample_perf);

-	return OK;
+	return ret;
+}
+
+int
+VOXLPM::measure_ltc2946()
+{
+	int ret = PX4_ERROR;
+	uint8_t vraw[2];
+	uint8_t iraw[2];
+
+	int amp_ret = read_reg_buf(VOXLPM_LTC2946_DELTA_SENSE_MSB_REG, iraw, sizeof(iraw));  // 0x14
+	int volt_ret = read_reg_buf(VOXLPM_LTC2946_VIN_MSB_REG, vraw, sizeof(vraw));         // 0x1E
+
+	if ((amp_ret == 0) && (volt_ret == 0)) {
+		uint16_t volt16 = (((uint16_t)vraw[0]) << 8) | vraw[1];   // MSB first
+		volt16        >>= 4;                                      // data is 12 bit and left-aligned
+		_voltage        = (volt16 / VOXLPM_LTC2946_RESOLUTION) * VOXLPM_LTC2946_VFS_SENSE;
+
+		uint16_t curr16 = (((uint16_t)iraw[0]) << 8) | iraw[1];   // MSB first
+		curr16        >>= 4;                                      // data is 12 bit and left-aligned
+		_amperage       = curr16 / VOXLPM_LTC2946_RESOLUTION * VOXLPM_LTC2946_VFS_DELTA_SENSE / _rshunt;
+		ret = PX4_OK;
+	}
+
+	return ret;
+}
+
+int
+VOXLPM::measure_ina231()
+{
+	int ret = PX4_ERROR;
+
+	uint8_t raw_vshunt[2];
+	uint8_t raw_vbus[2];
+	uint8_t raw_amps[2];
+
+	int16_t vshunt = -1;
+	uint16_t vbus = -1;
+	uint16_t amps = 0;
+
+	int vshunt_ret = read_reg_buf(INA231_REG_SHUNTVOLTAGE, raw_vshunt, sizeof(raw_vshunt));
+	int vbus_ret = read_reg_buf(INA231_REG_BUSVOLTAGE, raw_vbus, sizeof(raw_vbus));
+	int amp_ret = read_reg_buf(INA231_REG_CURRENT, raw_amps, sizeof(raw_amps));
+
+	if ((vshunt_ret == 0) && (vbus_ret == 0) && (amp_ret == 0)) {
+		vshunt = (((int16_t)raw_vshunt[0]) << 8) | raw_vshunt[1];  // MSB first
+		vbus   = (((uint16_t)raw_vbus[0]) << 8) | raw_vbus[1];  // MSB first
+		amps   = (((uint16_t)raw_amps[0]) << 8) | raw_amps[1];  // MSB first
+
+		_voltage = (float) vbus * INA231_VBUSSCALE;
+		_vshunt = (float) vshunt * INA231_VSHUNTSCALE;
+
+		if (_ch_type == VOXLPM_CH_TYPE_VBATT) {
+			/* vshunt is in microvolts, convert to AMPs */
+			_vshuntamps = ((float) _vshunt / VOXLPM_INA231_VBAT_SHUNT);
+			_amperage = (float) amps * VOXLPM_INA231_VBAT_I_LSB;
+
+		} else {
+			/* vshunt is in microvolts, convert to AMPs */
+			_vshuntamps = ((float) _vshunt / VOXLPM_INA231_VREG_SHUNT);
+			_amperage = (float) amps * VOXLPM_INA231_VREG_I_LSB;
+		}
+
+		ret = PX4_OK;
+	}
+
+	return ret;
 }

 uint8_t
@@ -191,7 +468,6 @@ VOXLPM::read_reg(uint8_t addr)
 {
 	uint8_t cmd[2] = { (uint8_t)(addr), 0};
 	transfer(&cmd[0], 1, &cmd[1], 1);
-
 	return cmd[1];
 }

@@ -203,8 +479,18 @@ VOXLPM::read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
 }

 int
-VOXLPM::write_reg(uint8_t value, uint8_t addr)
+VOXLPM::write_reg(uint8_t addr, uint8_t value)
 {
 	uint8_t cmd[2] = { (uint8_t)(addr), value};
 	return transfer(cmd, sizeof(cmd), nullptr, 0);
 }
+
+int
+VOXLPM::write_word_swapped(uint8_t addr, uint16_t value)
+{
+	uint8_t data[3] = {};
+	data[0] = addr;
+	data[1] = (value & 0xFF00) >> 8;
+	data[2] = (value & 0x00FF);
+	return transfer(data, sizeof(data), nullptr, 0);
+}
@@ -34,10 +34,12 @@
 /**
 * @file voxlpm.hpp
 *
- * Shared defines for the voxlpm (QTY2 LTC2946) driver.
+ * Shared defines for the voxlpm driver.
 *
 * This is roughly what's goin on:
 *
+ * - VOXLPM v2 (QTY2 LTC2946) -
+ *
 *             +~~~~~~~~~~~~~~+
 *  VBATT -----| RSENSE_VBATT | ----------+---------------------> VBATT TO ESCS
 *     |       +~~~~~~~~~~~~~~+           |
@@ -52,6 +54,24 @@
 *     #################              #################
 *     # LTC2946, 0x6a #              # LTC2946, 0x6b #
 *     #################              #################
+ *
+  * - VOXLPM v3 (QTY2 INA231) -
+ *
+ *             +~~~~~~~~~~~~~~+
+ *  VBATT -----| RSENSE_VBATT | ----------+---------------------> VBATT TO ESCS
+ *     |       +~~~~~~~~~~~~~~+           |
+ *     |              |          +--------+------+
+ *     +----+    +----+          | 5/12V REGULATOR  |
+ *          |    |               +--------+------+
+ *          |    |                        |   +~~~~~~~~~~~~~~+
+ *          |    |                        +---| RSENSE_5VOUT |---> 5/12VDC TO COMPUTE/PERIPHERAL
+ *          |    |                        |   +~~~~~~~~~~~~~~+
+ *          |    |                        |         |
+ *         V|    |A                      V|         |A
+ *     #################              #################
+ *     # INA231, 0x44  #              # INA231, 0x45  #
+ *     #################              #################
+ *
 *
 *     Publishes:                     Publishes:
 *     - ORB_ID(battery_status)
@@ -69,6 +89,7 @@

 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/power_monitor.h>
 #include <uORB/topics/parameter_update.h>
@@ -76,7 +97,7 @@
 using namespace time_literals;

 /*
- * Note that these are unshifted addresses.
+ * VOXLPM v2 - Note that these are unshifted addresses.
 */
 #define VOXLPM_LTC2946_ADDR_VBATT		0x6a // 0x6a  = 0xd4 >> 1
 #define VOXLPM_LTC2946_ADDR_P5VD		0x6b // 0x6b  = 0xd6 >> 1
@@ -87,6 +108,7 @@ using namespace time_literals;
 #define VOXLPM_LTC2946_POWER_MSB2_REG		0x05
 #define VOXLPM_LTC2946_CTRLB_MSG1_REG		0x06
 #define VOXLPM_LTC2946_CTRLB_LSB_REG		0x07
+#define VOXLPM_LTC2946_STATUS_REG		0x80

 #define VOXLPM_LTC2946_DELTA_SENSE_MSB_REG	0x14
 #define VOXLPM_LTC2946_DELTA_SENSE_LSB_REG	0x15
@@ -106,7 +128,7 @@ using namespace time_literals;
 * 2:0 - [Channel Configuration]
 *       000 --> Alternate Voltage, Current Measurement
 */
-#define DEFAULT_CTRLA_REG_VAL			0x18
+#define DEFAULT_LTC2946_CTRLA_REG_VAL		0x18

 /*
 * CTRLB (Address 0x01 - LTC2946_CTRLA_REG)
@@ -124,7 +146,7 @@ using namespace time_literals;
 * 1:0 - [Auto-Reset Mode/Reset]
 *       01 --> Enable Auto-Reset
 */
-#define DEFAULT_CTRLB_REG_VAL			0x01
+#define DEFAULT_LTC2946_CTRLB_REG_VAL		0x01

 /* 12 bits */
 #define VOXLPM_LTC2946_RESOLUTION 		4095.0f
@@ -136,14 +158,67 @@ using namespace time_literals;
 #define VOXLPM_LTC2946_VFS_DELTA_SENSE 		0.1024f

 /* Power sense resistor for battery current */
-#define VOXLPM_RSENSE_VBATT			0.0005f
+#define VOXLPM_LTC2946_VBAT_SHUNT		0.0005f

 /* Power sense resistor for 5VDC output current */
-#define VOXLPM_RSENSE_5VOUT			0.005f
+#define VOXLPM_LTC2946_VREG_SHUNT		0.005f
+
+/*
+ * VOXLPM v3 - Coniguration from SBOS644C –FEBRUARY 2013–REVISED MARCH 2018
+ *             http://www.ti.com/lit/ds/symlink/ina231.pdf
+ */
+#define VOXLPM_INA231_ADDR_VBATT		0x44
+#define VOXLPM_INA231_ADDR_P5_12VDC		0x45
+
+/* INA231 Registers addresses */
+#define INA231_REG_CONFIG			0x00
+#define INA231_REG_SHUNTVOLTAGE			0x01
+#define INA231_REG_BUSVOLTAGE			0x02
+#define INA231_REG_POWER			0x03
+#define INA231_REG_CURRENT			0x04
+#define INA231_REG_CALIBRATION			0x05
+#define INA231_REG_MASKENABLE			0x06
+#define INA231_REG_ALERTLIMIT			0x07
+
+/* [0:2] Mode - Shunt and bus, 111, continuous (INA231A default) */
+#define INA231_CONFIG_MODE			(0x07 << 0)
+/* [5:3] Shunt Voltage Conversion Time, 100, 1.1ms (INA231A default) */
+#define INA231_CONFIG_SHUNT_CT			(0x04 << 3)
+/* [8:6] Shunt Voltage Conversion Time, 100, 1.1ms (INA231A default) */
+#define INA231_CONFIG_BUS_CT			(0x04 << 6)
+/* [11:9] Averaging Mode, 010, 16 */
+#define INA231_CONFIG_AVG			(0x02 << 9)
+/* [1] Reset bit */
+#define INA231_RST_BIT				(0x01 << 15)
+/* Configuration register settings */
+#define INA231_CONFIG				(INA231_CONFIG_MODE+INA231_CONFIG_SHUNT_CT+INA231_CONFIG_BUS_CT+INA231_CONFIG_AVG)
+
+#define INA231_CONST				0.00512f  /* is an internal fixed value used to ensure scaling is maintained properly  */
+#define INA231_VBUSSCALE			0.00125f  /* LSB of bus voltage is 1.25 mV  */
+#define INA231_VSHUNTSCALE			0.0000025f /* LSB of shunt voltage is 2.5 uV  */
+
+/* From SCH-M00041 REVB */
+#define VOXLPM_INA231_VBAT_SHUNT		0.0005f   /* VBAT shunt is 500 micro-ohm */
+#define VOXLPM_INA231_VREG_SHUNT		0.005f    /* VREG output shunt is 5 milli-ohm */
+#define VOXLPM_INA231_VBAT_MAX_AMPS		90.0f     /* 90.0 Amps max through VBAT sense resistor */
+#define VOXLPM_INA231_VREG_MAX_AMPS		6.0f      /* 6.0 Amps max through VREG sense resistor */
+
+/* ina231.pdf section 8.5 */
+#define VOXLPM_INA231_VBAT_I_LSB		(VOXLPM_INA231_VBAT_MAX_AMPS/32768.0f)
+#define VOXLPM_INA231_VREG_I_LSB		(VOXLPM_INA231_VREG_MAX_AMPS/32768.0f)
+
+#define swap16(w)				__builtin_bswap16((w))
+
+enum VOXLPM_TYPE {
+	VOXLPM_UNKOWN,
+	VOXLPM_TYPE_V2_LTC,
+	VOXLPM_TYPE_V3_INA
+};

 enum VOXLPM_CH_TYPE {
 	VOXLPM_CH_TYPE_VBATT = 0,
-	VOXLPM_CH_TYPE_P5VDC
+	VOXLPM_CH_TYPE_P5VDC,
+	VOXLPM_CH_TYPE_P12VDC
 };

 class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver<VOXLPM>
@@ -157,29 +232,46 @@ public:
 	static void print_usage();

 	virtual int		init();
+	int			force_init();
 	void			print_status() override;

 	void 			RunImpl();
+
 private:
+	int			probe() override;
 	void 			start();
 	int 			measure();
+	int 			load_params(VOXLPM_TYPE pm_type, VOXLPM_CH_TYPE ch_type);
+	int 			init_ltc2946();
+	int 			init_ina231();
+	int 			measure_ltc2946();
+	int 			measure_ina231();

+	bool			_initialized;
 	static constexpr unsigned _meas_interval_us{100_ms};
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;

 	uORB::PublicationMulti<power_monitor_s>		_pm_pub_topic{ORB_ID(power_monitor)};
-	uORB::Subscription _parameter_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

 	power_monitor_s 	_pm_status{};

+	VOXLPM_TYPE		_pm_type{VOXLPM_UNKOWN};
 	const VOXLPM_CH_TYPE	_ch_type;
 	float			_voltage{0.0f};
 	float			_amperage{0.0f};
-	float			_rsense{0.0f};
+	float			_rshunt{0.0005f};
+	float			_vshunt{0.0f};
+	float			_vshuntamps{0.0f};
+	int16_t			_cal{0};

 	Battery 		_battery;
+	uORB::Subscription	_actuators_sub{ORB_ID(actuator_controls_0)};
+	actuator_controls_s	_actuator_controls{};

 	uint8_t 		read_reg(uint8_t addr);
 	int 			read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
-	int 			write_reg(uint8_t value, uint8_t addr);
+	int 			write_reg(uint8_t addr, uint8_t value);
+	int 			write_word_swapped(uint8_t addr, uint16_t value);
 };
@@ -47,7 +47,13 @@ I2CSPIDriverBase *VOXLPM::instantiate(const BusCLIArguments &cli, const BusInsta
 		return nullptr;
 	}

-	if (OK != instance->init()) {
+	if (cli.custom1 == 1) {
+		if (OK != instance->force_init()) {
+			PX4_INFO("Failed to init voxlpm type: %d on bus: %d, but will try again periodically.", (VOXLPM_CH_TYPE)cli.type,
+				 iterator.bus());
+		}
+
+	} else if (OK != instance->init()) {
 		delete instance;
 		return nullptr;
 	}
@@ -62,7 +68,8 @@ VOXLPM::print_usage()

 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAM_STRING('T', "VBATT", "VBATT|P5VDC", "Type", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('T', "VBATT", "VBATT|P5VDC|P12VDC", "Type", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('K', "if initialization (probing) fails, keep retrying periodically", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 }

@@ -75,7 +82,7 @@ voxlpm_main(int argc, char *argv[])
 	cli.default_i2c_frequency = 400000;
 	cli.type = VOXLPM_CH_TYPE_VBATT;

-	while ((ch = cli.getopt(argc, argv, "T:")) != EOF) {
+	while ((ch = cli.getopt(argc, argv, "KT:")) != EOF) {
 		switch (ch) {
 		case 'T':
 			if (strcmp(cli.optarg(), "VBATT") == 0) {
@@ -84,12 +91,19 @@ voxlpm_main(int argc, char *argv[])
 			} else if (strcmp(cli.optarg(), "P5VDC") == 0) {
 				cli.type = VOXLPM_CH_TYPE_P5VDC;

+			} else if (strcmp(cli.optarg(), "P12VDC") == 0) {
+				cli.type = VOXLPM_CH_TYPE_P12VDC; //  same as P5VDC
+
 			} else {
 				PX4_ERR("unknown type");
 				return -1;
 			}

 			break;
+
+		case 'K': // keep retrying
+			cli.custom1 = 1;
+			break;
 		}
 	}

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * VOXL Power Monitor Shunt, Battery
+ *
+ * @reboot_required true
+ *
+ * @min 0.000000001
+ * @max 0.1
+ * @decimal 10
+ * @increment .000000001
+ * @group Sensors
+ */
+PARAM_DEFINE_FLOAT(VOXLPM_SHUNT_BAT, 0.00063f);
+
+/**
+ * VOXL Power Monitor Shunt, Regulator
+ *
+ * @reboot_required true
+ *
+ * @min 0.000000001
+ * @max 0.1
+ * @decimal 10
+ * @increment .000000001
+ * @group Sensors
+ */
+PARAM_DEFINE_FLOAT(VOXLPM_SHUNT_REG, 0.0056f);
@@ -46,6 +46,9 @@ if(CONFIG_ARCH_CHIP)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
 		set(UAVCAN_DRIVER "stm32h7")
 		set(UAVCAN_TIMER 5) # The default timer is TIM5
+		if (DEFINED config_uavcan_timer_override)
+			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
+		endif()
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
 		set(UAVCAN_DRIVER "stm32")
 		set(UAVCAN_TIMER 5) # The default timer is TIM5
@@ -110,6 +113,7 @@ add_custom_target(px4_uavcan_dsdlc DEPENDS px4_uavcan_dsdlc_run.stamp)
 px4_add_module(
 	MODULE drivers__uavcan
 	MAIN uavcan
+	STACK_MAIN 4096
 	INCLUDES
 		${DSDLC_OUTPUT}
 		${LIBUAVCAN_DIR}/libuavcan/include
@@ -49,9 +49,7 @@ using namespace time_literals;

 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) :
 	ModuleParams(parent),
-	_index(index < 1 || index > 9 ? 1 : index),
-	_warning(battery_status_s::BATTERY_WARNING_NONE),
-	_last_timestamp(0)
+	_index(index < 1 || index > 9 ? 1 : index)
 {
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
 	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
@@ -105,13 +103,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
 	updateParams();
 }

-Battery::~Battery()
-{
-	orb_unadvertise(_orb_advert);
-}
-
-void
-Battery::reset()
+void Battery::reset()
 {
 	memset(&_battery_status, 0, sizeof(_battery_status));
 	_battery_status.current_a = -1.f;
@@ -126,13 +118,10 @@ Battery::reset()
 	_battery_status.id = (uint8_t) _index;
 }

-void
-Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
-			     bool connected, int source, int priority,
-			     float throttle_normalized)
+void Battery::updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
+				  int source, int priority, float throttle_normalized)
 {
 	reset();
-	_battery_status.timestamp = timestamp;

 	if (!_battery_initialized) {
 		_voltage_filter_v.reset(voltage_v);
@@ -174,23 +163,18 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 		}
 	}

-	_battery_status.timestamp = timestamp;
-
-	const bool should_publish = (source == _params.source);
-
-	if (should_publish) {
+	if (source == _params.source) {
 		publish();
 	}
 }

-void
-Battery::publish()
+void Battery::publish()
 {
-	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
+	_battery_status.timestamp = hrt_absolute_time();
+	_battery_status_pub.publish(_battery_status);
 }

-void
-Battery::sumDischarged(hrt_abstime timestamp, float current_a)
+void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)
 {
 	// Not a valid measurement
 	if (current_a < 0.f) {
@@ -211,8 +195,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
 	_last_timestamp = timestamp;
 }

-void
-Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
+void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
 {
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;
@@ -250,8 +233,7 @@ Battery::estimateRemaining(const float voltage_v, const float current_a, const f
 	}
 }

-void
-Battery::determineWarning(bool connected)
+void Battery::determineWarning(bool connected)
 {
 	if (connected) {
 		// propagate warning state only if the state is higher, otherwise remain in current warning state
@@ -270,8 +252,7 @@ Battery::determineWarning(bool connected)
 	}
 }

-void
-Battery::computeScale()
+void Battery::computeScale()
 {
 	const float voltage_range = (_params.v_charged - _params.v_empty);

@@ -42,18 +42,19 @@

 #pragma once

-#include <uORB/uORB.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/battery_status.h>
-#include <drivers/drv_hrt.h>
-#include <px4_platform_common/module_params.h>
-#include <parameters/param.h>
-#include <drivers/drv_adc.h>
-#include <board_config.h>
-#include <px4_platform_common/board_common.h>
 #include <math.h>
 #include <float.h>
+
+#include <board_config.h>
+#include <px4_platform_common/board_common.h>
+#include <px4_platform_common/module_params.h>
+#include <matrix/math.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/parameters/param.h>
 #include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/battery_status.h>

 /**
 * BatteryBase is a base class for any type of battery.
@@ -65,8 +66,7 @@ class Battery : public ModuleParams
 {
 public:
 	Battery(int index, ModuleParams *parent, const int sample_interval_us);
-
-	~Battery();
+	~Battery() = default;

 	/**
 	 * Reset all battery stats and report invalid/nothing.
@@ -98,14 +98,9 @@ public:
 	 * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417
 	 * @param throttle_normalized: Throttle of the vehicle, between 0 and 1
 	 */
-	void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected,
+	void updateBatteryStatus(const hrt_abstime &timestamp, float voltage_v, float current_a, bool connected,
 				 int source, int priority, float throttle_normalized);

-	/**
-	 * Publishes the uORB battery_status message with the most recently-updated data.
-	 */
-	void publish();
-
 protected:
 	struct {
 		param_t v_empty;
@@ -128,7 +123,7 @@ protected:
 		param_t v_load_drop_old;
 		param_t r_internal_old;
 		param_t source_old;
-	} _param_handles;
+	} _param_handles{};

 	struct {
 		float v_empty;
@@ -151,85 +146,71 @@ protected:
 		float v_load_drop_old;
 		float r_internal_old;
 		int source_old;
-	} _params;
+	} _params{};

-	battery_status_s _battery_status;
+	battery_status_s _battery_status{};

 	const int _index;

-	bool _first_parameter_update{false};
-	virtual void updateParams() override;
+	bool _first_parameter_update{true};
+	void updateParams() override;

 	/**
-	 * This function helps with migrating to new parameters. It performs several tasks:
-	 *  - Update both the old and new parameter values using `param_get(...)`
-	 *  - Check if either parameter changed just now
-	 *    - If so, display a warning if the deprecated parameter was used
-	 *    - Copy the new value over to the other parameter
-	 *  - If this is the first time the parameters are fetched, check if they are equal
-	 *    - If not, display a warning and copy the value of the deprecated parameter over to the new one
+	 * Publishes the uORB battery_status message with the most recently-updated data.
+	 */
+	void publish();
+
+	/**
+	 * This function helps migrating and syncing from/to deprecated parameters. BAT_* BAT1_*
 	 * @tparam T Type of the parameter (int or float)
-	 * @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS")
-	 * @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS")
+	 * @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS"))
+	 * @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS"))
 	 * @param old_val Pointer to the value of the old deprecated parameter
 	 * @param new_val Pointer to the value of the new replacement parameter
-	 * @param firstcall If true, then this function will not check to see if the values have changed
-	 * 					  (Since the old values are uninitialized)
-	 * @return True iff either of these parameters changed just now and the migration was done.
+	 * @param firstcall If true, this function prefers migrating old to new
 	 */
 	template<typename T>
-	bool migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall)
+	void migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall)
 	{
-
 		T previous_old_val = *old_val;
 		T previous_new_val = *new_val;

+		// Update both the old and new parameter values
 		param_get(old_param, old_val);
 		param_get(new_param, new_val);

-		if (!firstcall) {
-			if ((float) fabs((float) *old_val - (float) previous_old_val) > FLT_EPSILON
-			    && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
+		// Check if the parameter values are different
+		if (!matrix::isEqualF((float)*old_val, (float)*new_val)) {
+			// If so, copy the new value over to the unchanged parameter
+			// Note: If they differ from the beginning we migrate old to new
+			if (firstcall || !matrix::isEqualF((float)*old_val, (float)previous_old_val)) {
 				param_set_no_notification(new_param, old_val);
 				param_get(new_param, new_val);
-				return true;

-			} else if ((float) fabs((float) *new_val - (float) previous_new_val) > FLT_EPSILON
-				   && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
+			} else if (!matrix::isEqualF((float)*new_val, (float)previous_new_val)) {
 				param_set_no_notification(old_param, new_val);
 				param_get(old_param, old_val);
-				return true;
-			}
-
-		} else {
-			if ((float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) {
-				param_set_no_notification(new_param, old_val);
-				param_get(new_param, new_val);
-				return true;
 			}
 		}
-
-		return false;
 	}

 private:
-	void sumDischarged(hrt_abstime timestamp, float current_a);
+	void sumDischarged(const hrt_abstime &timestamp, float current_a);
 	void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
 	void determineWarning(bool connected);
 	void computeScale();

-	bool _battery_initialized = false;
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
+	bool _battery_initialized{false};
 	AlphaFilter<float> _voltage_filter_v;
 	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float> _throttle_filter;
-	float _discharged_mah = 0.f;
-	float _discharged_mah_loop = 0.f;
-	float _remaining_voltage = -1.f;		///< normalized battery charge level remaining based on voltage
-	float _remaining = -1.f;			///< normalized battery charge level, selected based on config param
-	float _scale = 1.f;
-	uint8_t _warning;
-	hrt_abstime _last_timestamp;
-
-	orb_advert_t _orb_advert{nullptr};
-	int _orb_instance;
+	float _discharged_mah{0.f};
+	float _discharged_mah_loop{0.f};
+	float _remaining_voltage{-1.f};		///< normalized battery charge level remaining based on voltage
+	float _remaining{-1.f};			///< normalized battery charge level, selected based on config param
+	float _scale{1.f};
+	uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
+	hrt_abstime _last_timestamp{0};
 };
@@ -32,11 +32,11 @@
 ****************************************************************************/

 /**
- * @file battery_params_1.c
+ * @file battery_params_deprecated.c
 * @author Timothy Scott <timothy@auterion.com>
 *
- * Defines parameters for Battery 1. For backwards compatibility, the
- * parameter names do not have a "1" in them.
+ * Defines the deprcated single battery configuration which are temporarily kept for backwards compatibility with QGC.
+ * The new parameter set has a number after "BAT" e.g. BAT1_V_EMPTY.
 */

 /**
@@ -160,4 +160,4 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
 * @value 1 External
 * @group Battery Calibration
 */
-PARAM_DEFINE_INT32(BAT_SOURCE, 0);
+PARAM_DEFINE_INT32(BAT_SOURCE, 0);
@@ -133,10 +133,11 @@ parameters:
                    This requires the ESC to provide both voltage as well as current.
            type: enum
            values:
+                -1: Disabled
                0: Power Module
                1: External
                2: ESCs
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [0, 0]
+            default: [0, -1]
@@ -297,7 +297,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 	State previous_state = _current_state;
 	_current_state = _getCurrentState();

-	if (triplet_update || (_current_state != previous_state)) {
+	if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) {
 		_updateInternalWaypoints();
 		_mission_gear = _sub_triplet_setpoint.get().current.landing_gear;
 	}
@@ -40,6 +40,7 @@
 #include "Landingslope.hpp"

 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>

 void
 Landingslope::update(float landing_slope_angle_rad_new,
@@ -77,7 +78,7 @@ Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, flo
 		float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
+	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
 		return getLandingSlopeRelativeAltitude(wp_landing_distance);

 	}
@@ -90,7 +91,7 @@ Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float
 		float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
-	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) {
+	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
 		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;

 	}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -44,6 +44,7 @@
 #include <px4_platform_common/defines.h>

 #include <fcntl.h>
+#include <math.h>
 #include <unistd.h>
 #include <termios.h>
 #include <string.h>
@@ -53,6 +54,8 @@
 #include "common_rc.h"
 #include <drivers/drv_hrt.h>

+#include <include/containers/Bitset.hpp>
+
 #if defined(__PX4_NUTTX)
 #include <nuttx/arch.h>
 #define dsm_udelay(arg)    up_udelay(arg)
@@ -106,7 +109,7 @@ static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
 */
 static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, uint16_t &value)
 {
-	if (raw == 0xffff) {
+	if (raw == 0 || raw == 0xffff) {
 		return false;
 	}

@@ -118,8 +121,26 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		static constexpr uint16_t MASK_1024_SXPOS = 0x03FF;

 		channel = (raw & MASK_1024_CHANID) >> 10; // 6 bits
-		uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
-		value = servo_position * 2; // scale to be equivalent to 2048 mode
+
+		const uint16_t servo_position = (raw & MASK_1024_SXPOS); // 10 bits
+
+		if (channel > DSM_MAX_CHANNEL_COUNT) {
+			PX4_DEBUG("invalid channel: %d\n", channel);
+			return false;
+		}
+
+		// PWM_OUT = (ServoPosition x 1.166μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 1.166f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;

 	} else if (shift == 11) {
@@ -127,32 +148,54 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 		//  Bits 15    Servo Phase
 		//  Bits 14-11 Channel ID
 		//  Bits 10-0  Servo Position
-		static constexpr uint16_t MASK_2048_CHANID = 0x7800;
-		static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
+
+		uint16_t servo_position = 0;

 		// from Spektrum Remote Receiver Interfacing Rev G Page 6
-		uint8_t chan = (raw & MASK_2048_CHANID) >> 11;
-		uint16_t servo_position = 0;
+		const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
+		uint8_t chan = (raw >> 11) & 0x0F;

 		if (chan < 12) {
 			// Normal channels
+			static constexpr uint16_t MASK_2048_SXPOS = 0x07FF;
 			servo_position = (raw & MASK_2048_SXPOS);

 		} else if (chan == 12) {
 			// XPlus channels
 			chan += ((raw >> 9) & 0x03);

-			const bool phase = raw & (2 >> 15); // the phase is part of the X-Plus address (bit 15)
-
 			if (phase) {
 				chan += 4;
 			}

+			if (chan > DSM_MAX_CHANNEL_COUNT) {
+				PX4_DEBUG("invalid channel: %d\n", chan);
+				return false;
+			}
+
 			servo_position = (raw & 0x01FF) << 2;
+
+			channel = chan;
+
+		} else {
+			PX4_DEBUG("invalid channel: %d\n", chan);
+			return false;
 		}

 		channel = chan;
-		value = servo_position;
+
+		// PWM_OUT = (ServoPosition x 0.583μs) + Offset
+		static constexpr uint16_t offset = 903; // microseconds
+		value = roundf(servo_position * 0.583f) + offset;
+
+		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
+		//  ±100% travel is 1102µs to 1898 µs
+		if (value < 990 || value > 2010) {
+			// if the value is unrealistic, fail the parsing entirely
+			PX4_DEBUG("channel %d invalid range %d", channel, value);
+			return false;
+		}
+
 		return true;
 	}

@@ -180,6 +223,12 @@ dsm_guess_format(bool reset)
 		return false;
 	}

+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_10;
+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found_11;
+
+	bool cs10_frame_valid = true;
+	bool cs11_frame_valid = true;
+
 	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {

@@ -190,15 +239,43 @@ dsm_guess_format(bool reset)
 		uint16_t value = 0;

 		/* if the channel decodes, remember the assigned number */
-		if (dsm_decode_channel(raw, 10, channel, value) && (channel < 31)) {
-			cs10 |= (1 << channel);
+		if (dsm_decode_channel(raw, 10, channel, value)) {
+			// invalidate entire frame (for 1024) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_10[channel]) {
+				cs10_frame_valid = false;
+
+			} else {
+				channels_found_10.set(channel);
+			}
 		}

-		if (dsm_decode_channel(raw, 11, channel, value) && (channel < 31)) {
-			cs11 |= (1 << channel);
-		}
+		if (dsm_decode_channel(raw, 11, channel, value)) {
+			// invalidate entire frame (for 2048) if channel already found, no duplicate channels per DSM frame
+			if (channels_found_11[channel]) {
+				cs11_frame_valid = false;

-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
+			} else {
+				channels_found_11.set(channel);
+			}
+		}
+	}
+
+	// add valid cs10 channels
+	if (cs10_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_10[channel]) {
+				cs10 |= 1 << channel;
+			}
+		}
+	}
+
+	// add valid cs11 channels
+	if (cs11_frame_valid) {
+		for (unsigned channel = 0; channel < DSM_FRAME_CHANNELS; channel++) {
+			if (channels_found_11[channel]) {
+				cs11 |= 1 << channel;
+			}
+		}
 	}

 	samples++;
@@ -229,8 +306,10 @@ dsm_guess_format(bool reset)
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
 		0x1fff,	/* 13 channels (DX10t) */
-		0x3fff	/* 18 channels (DX10) */
+		0xffff,	/* 16 channels */
+		0x3ffff,/* 18 channels (DX10) */
 	};
+
 	unsigned votes10 = 0;
 	unsigned votes11 = 0;

@@ -470,91 +549,6 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 		}
 	}

-	/*
-	 * The second byte indicates the protocol and frame rate. We have a
-	 * guessing state machine, so we don't need to use this. At any rate,
-	 * these are the allowable values:
-	 *
-	 * 0x01 22MS 1024 DSM2
-	 * 0x12 11MS 2048 DSM2
-	 * 0xa2 22MS 2048 DSMX
-	 * 0xb2 11MS 2048 DSMX
-	 */
-	const uint8_t system = dsm_frame[1];
-
-	switch (system) {
-	case 0x00: // SURFACE DSM1
-		// unsupported
-		PX4_DEBUG("ERROR system: SURFACE DSM1 (%X) unsupported\n", system);
-		return false;
-
-	case 0x01: // DSM2 1024 22ms
-		if (dsm_channel_shift != 10) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0x02: // DSM2 1024 (MC24)
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM2 1024 (MC24) (%X) unsupported\n", system);
-		return false;
-
-	case 0x12: // DSM2 2048 11ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0x23: // SURFACE DSM2 16.5ms
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM2 16.5ms (%X) unsupported\n", system);
-		return false;
-
-	case 0x50: // DSM MARINE
-		// unsupported
-		PX4_DEBUG("ERROR system: DSM MARINE (%X) unsupported\n", system);
-		return false;
-
-	case 0x92: // DSMJ
-		// unsupported
-		PX4_DEBUG("ERROR system: DSMJ (%X) unsupported\n", system);
-		return false;
-
-	case 0xA2: // DSMX 22ms OR DSMR 11ms or DSMR 22ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	case 0xA4: // DSMR 5.5ms
-		// unsupported
-		PX4_DEBUG("ERROR system: DSMR 5.5ms (%X) unsupported\n", system);
-		return false;
-
-	case 0xAE: // NOT_BOUND
-		PX4_DEBUG("ERROR system: NOT_BOUND (%X) unsupported\n", system);
-		return false;
-
-	case 0xB2: // DSMX 11ms
-		if (dsm_channel_shift != 11) {
-			dsm_guess_format(true);
-			return false;
-		}
-
-		break;
-
-	default:
-		// ERROR
-		PX4_DEBUG("ERROR system: %X unsupported\n", system);
-		return false;
-	}
-
 	/*
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
@@ -563,18 +557,42 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	 * seven channels are being transmitted.
 	 */

+	px4::Bitset<DSM_MAX_CHANNEL_COUNT> channels_found;
+
+	unsigned channel_decode_failures = 0;
+
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {

 		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];

+		// ignore
+		if (raw == 0 || raw == 0xffff) {
+			continue;
+		}
+
 		uint8_t channel = 0;
 		uint16_t value = 0;

 		if (!dsm_decode_channel(raw, dsm_channel_shift, channel, value)) {
+			channel_decode_failures++;
 			continue;
 		}

+		// discard entire frame if at least half of it (4 channels) failed to decode
+		if (channel_decode_failures >= 4) {
+			return false;
+		}
+
+		// abort if channel already found, no duplicate channels per DSM frame
+		if (channels_found[channel]) {
+			PX4_DEBUG("duplicate channel %d\n\n", channel);
+			return false;
+
+		} else {
+			channels_found.set(channel);
+		}
+
 		/* reset bit guessing state machine if the channel index is out of bounds */
 		if (channel > DSM_MAX_CHANNEL_COUNT) {
 			dsm_guess_format(true);
@@ -614,11 +632,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 			break;
 		}

-		// Scaling
-		//  See Specification for Spektrum Remote Receiver Interfacing Rev G 2019 January 22
-		//  2048 Mode Scaling: PWM_OUT = (ServoPosition x 0.583μs) + Offset (903μs)
-		//  scaled integer for decent accuracy while staying efficient (0.583us ~= 1194/2048)
-		values[channel] = (value * 1194) / 2048 + 903;
+		values[channel] = value;
 	}

 	/*
@@ -32,7 +32,7 @@ private:
 	bool crsfTest();
 	bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0);
 	bool dsmTest10Ch();
-	bool dsmTest12Ch();
+	bool dsmTest16Ch();
 	bool sbus2Test();
 	bool st24Test();
 	bool sumdTest();
@@ -42,7 +42,7 @@ bool RCTest::run_tests()
 {
 	ut_run_test(crsfTest);
 	ut_run_test(dsmTest10Ch);
-	ut_run_test(dsmTest12Ch);
+	ut_run_test(dsmTest16Ch);
 	ut_run_test(sbus2Test);
 	ut_run_test(st24Test);
 	ut_run_test(sumdTest);
@@ -138,12 +138,12 @@ bool RCTest::crsfTest()

 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 64, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
 }

-bool RCTest::dsmTest12Ch()
+bool RCTest::dsmTest16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 12, 454, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500);
 }

 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -172,7 +172,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 	unsigned dsm_frame_drops = 0;
 	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);

-	int rate_limiter = 0;
+	int count = 0;
 	unsigned last_drop = 0;

 	dsm_proto_init();
@@ -192,7 +192,9 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);

 		if (result) {
-			ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+			if (count > (16 * 10)) { // need to process enough data to have full channel count
+				ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+			}

 			ut_test(abs((int)chan0 - (int)rc_values[0]) < 30);

@@ -208,13 +210,13 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 			last_drop = dsm_frame_drops;
 		}

-		rate_limiter++;
+		count++;
 	}

 	fclose(fp);

 	ut_test(ret == EOF);
-	PX4_INFO("drop: %d", (int)last_drop);
+	//PX4_INFO("drop: %d", (int)last_drop);
 	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);

 	return true;
@@ -1,3 +1,37 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdio.h>
 #include <lib/battery/battery.h>
 #include "analog_battery.h"

@@ -116,7 +150,7 @@ AnalogBattery::updateParams()
 		}
 	}

-	if (_analog_params.a_per_v <= 0.0f) {
+	if (_analog_params.a_per_v < 0.0f) {
 		/* apply scaling according to defaults if set to default */

 		_analog_params.a_per_v = BOARD_BATTERY1_A_PER_V;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -57,6 +57,7 @@
 #include <lib/battery/battery.h>
 #include <lib/conversion/rotation.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -101,7 +102,7 @@ private:

 	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
-	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
+	uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};

 	static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
 	static constexpr uint32_t SAMPLE_INTERVAL_US  = 1_s / SAMPLE_FREQUENCY_HZ;
@@ -280,9 +281,7 @@ BatteryStatus::task_spawn(int argc, char *argv[])
 bool
 BatteryStatus::init()
 {
-	ScheduleOnInterval(SAMPLE_INTERVAL_US);
-
-	return true;
+	return _adc_report_sub.registerCallback();
 }

 int BatteryStatus::custom_command(int argc, char *argv[])
@@ -46,11 +46,28 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 {
 	bool success = true; // start with a pass and change to a fail if any test fails
 	bool ahrs_present = true;
-	float test_limit = 1.0f; // pass limit re-used for each test

 	int32_t mag_strength_check_enabled = 1;
 	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);

+	float hgt_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
+
+	float vel_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
+
+	float pos_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
+
+	float mag_test_ratio_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
+
+	float ekf_ab_test_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit);
+
+	float ekf_gb_test_limit = 1.f;
+	param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);
+
 	bool gps_success = true;
 	bool gps_present = true;

@@ -98,9 +115,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check vertical position innovation test ratio
-	param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
-
-	if (status.hgt_test_ratio > test_limit) {
+	if (status.hgt_test_ratio > hgt_test_ratio_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
 		}
@@ -110,9 +125,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check velocity innovation test ratio
-	param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
-
-	if (status.vel_test_ratio > test_limit) {
+	if (status.vel_test_ratio > vel_test_ratio_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
 		}
@@ -122,9 +135,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check horizontal position innovation test ratio
-	param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
-
-	if (status.pos_test_ratio > test_limit) {
+	if (status.pos_test_ratio > pos_test_ratio_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
 		}
@@ -134,9 +145,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check magnetometer innovation test ratio
-	param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
-
-	if (status.mag_test_ratio > test_limit) {
+	if (status.mag_test_ratio > mag_test_ratio_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
 		}
@@ -146,17 +155,17 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check accelerometer delta velocity bias estimates
-	param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
+	param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit);

 	for (uint8_t index = 13; index < 16; index++) {
 		// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
 		// adjust test threshold by 3-sigma
 		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));

-		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
+		if (fabsf(status.states[index]) > ekf_ab_test_limit + test_uncertainty) {

 			if (report_fail) {
-				PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit,
+				PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)ekf_ab_test_limit,
 					(double)test_uncertainty);
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
 			}
@@ -167,10 +176,10 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}

 	// check gyro delta angle bias estimates
-	param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
+	param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);

-	if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
-	    || fabsf(status.states[12]) > test_limit) {
+	if (fabsf(status.states[10]) > ekf_gb_test_limit || fabsf(status.states[11]) > ekf_gb_test_limit
+	    || fabsf(status.states[12]) > ekf_gb_test_limit) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
 		}
@@ -68,7 +68,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st

 		if (!calibration_valid) {
 			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u %u uncalibrated", instance, device_id);
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
 			}
 		}

@@ -68,6 +68,9 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
 	const system_power_s &system_power = system_power_sub.get();

 	if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
+		int32_t required_power_module_count = 0;
+		param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
+
 		// Check avionics rail voltages (if USB isn't connected)
 		if (!system_power.usb_connected) {
 			float avionics_power_rail_voltage = system_power.voltage5v_v;
@@ -94,9 +97,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta

 			const int power_module_count = countSetBits(system_power.brick_valid);

-			int32_t required_power_module_count = 0;
-			param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
-
 			if (power_module_count < required_power_module_count) {
 				success = false;

@@ -105,7 +105,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
 							     power_module_count, required_power_module_count);
 				}
 			}
-
 		}

 	} else {
@@ -1667,7 +1667,15 @@ Commander::run()
 			}

 			// Auto disarm after 5 seconds if kill switch is engaged
-			_auto_disarm_killed.set_state_and_update(armed.manual_lockdown || armed.lockdown, hrt_absolute_time());
+			bool auto_disarm = armed.manual_lockdown;
+
+			// auto disarm if locked down to avoid user confusion
+			//  skipped in HITL where lockdown is enabled for safety
+			if (status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+				auto_disarm |= armed.lockdown;
+			}
+
+			_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());

 			if (_auto_disarm_killed.get_state()) {
 				if (armed.manual_lockdown) {
@@ -2164,7 +2172,7 @@ Commander::run()
 		 * as finished even though we only just started with the takeoff. Therefore, we also
 		 * check the timestamp of the mission_result topic. */
 		if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
-		    && (_mission_result_sub.get().timestamp > _internal_state.timestamp)
+		    && (_mission_result_sub.get().timestamp >= _internal_state.timestamp)
 		    && _mission_result_sub.get().finished) {

 			const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp)
@@ -283,7 +283,7 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 * @unit s
 * @decimal 2
 */
-PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT,  10.0f);
+PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);


 /**
@@ -641,8 +641,8 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);
 *
 * @min 0
 * @max 3
- * @bit 0  Enable override in auto modes
- * @bit 1  Enable override in offboard mode
+ * @bit 0 Enable override in auto modes
+ * @bit 1 Enable override in offboard mode
 * @group Commander
 */
 PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
@@ -664,7 +664,7 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
 * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes.
 *
 * @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
- * @value 1 Land/Terminate.  Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
+ * @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
 *
 * @group Commander
 */
@@ -113,7 +113,6 @@ EscBattery::Run()
 			battery_status_s::BATTERY_SOURCE_ESCS,
 			priority,
 			ctrl.control[actuator_controls_s::INDEX_THROTTLE]);
-		_battery.publish();
 	}
 }

@@ -98,8 +98,8 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get());
 	_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
-	_tecs.set_time_const_throt(_param_fw_t_time_const.get());
-	_tecs.set_time_const(_param_fw_t_thro_const.get());
+	_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
+	_tecs.set_time_const(_param_fw_t_time_const.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain(_param_fw_t_integ_gain.get());
@@ -164,7 +164,12 @@ void PositionControl::_velocityControl(const float dt)
 	// Get allowed horizontal thrust after prioritizing vertical control
 	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
 	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
-	float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);
+	const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
+	float thrust_max_xy = 0;
+
+	if (thrust_max_xy_squared > 0) {
+		thrust_max_xy = sqrtf(thrust_max_xy_squared);
+	}

 	// Saturate thrust in horizontal direction
 	const Vector2f thrust_sp_xy(_thr_sp);
@@ -481,7 +481,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
 			_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting);
 		}

-		_states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2));
+		_states.acceleration(2) = _vel_z_deriv.update(_states.velocity(2));

 	} else {
 		_states.velocity(2) = _states.acceleration(2) = NAN;
@@ -76,6 +76,10 @@ void RTL::find_RTL_destination()
 		return;
 	}

+	if (!_navigator->home_position_valid()) {
+		return;
+	}
+
 	_destination_check_time = hrt_absolute_time();

 	// get home position:
@@ -160,7 +160,7 @@ extern output_limit_t pwm_limit;
 * GPIO handling.
 */
 /* HEX Cube Orange and Cube Yellow uses an inverted signal to control the IMU heater */
-#ifdef CONFIG_ARCH_BOARD_HEX_IO_V2
+#ifdef CONFIG_ARCH_BOARD_CUBEPILOT_IO_V2
 #define LED_BLUE(_s)			px4_arch_gpiowrite(GPIO_LED1, (_s))
 #else
 #define LED_BLUE(_s)			px4_arch_gpiowrite(GPIO_LED1, !(_s))
@@ -405,6 +405,11 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer)

 		if (_mag.enabled[uorb_index] && _mag.subscription[uorb_index].update(&mag_report)) {

+			// force parameter update (loads calibration) if device id still isn't set
+			if (_mag_device_id[uorb_index] == 0) {
+				parametersUpdate();
+			}
+
 			// First publication with data
 			if (_mag.priority[uorb_index] == 0) {
 				_mag.priority[uorb_index] = _mag.subscription[uorb_index].get_priority();
@@ -35,7 +35,8 @@

 BatterySimulator::BatterySimulator() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US)
 {
 }

@@ -46,7 +47,7 @@ BatterySimulator::~BatterySimulator()

 bool BatterySimulator::init()
 {
-	ScheduleOnInterval(SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US);
+	ScheduleOnInterval(BATTERY_SIMLATOR_SAMPLE_INTERVAL_US);
 	return true;
 }

@@ -67,34 +67,15 @@ public:
 private:
 	void Run() override;

+	static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz
+	static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ;
+
 	uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};

 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	class SimulatorBattery : public Battery
-	{
-	public:
-		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz
-		static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ;
-
-		SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {}
-
-		virtual void updateParams() override
-		{
-			Battery::updateParams();
-			_params.v_empty = 3.5f;
-			_params.v_charged = 4.05f;
-			_params.n_cells = 4;
-			_params.capacity = 10.0f;
-			_params.v_load_drop = 0.0f;
-			_params.r_internal = 0.0f;
-			_params.low_thr = 0.15f;
-			_params.crit_thr = 0.07f;
-			_params.emergen_thr = 0.05f;
-			_params.source = 0;
-		}
-	} _battery;
+	Battery _battery;

 	uint64_t _last_integration_us{0};
 	float _battery_percentage{1.f};
@@ -67,7 +67,7 @@ void TemperatureCompensationModule::parameters_update()
 			int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index);

 			if (temp < 0) {
-				PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index);
+				PX4_INFO("No temperature calibration available for gyro %i (device id %u)", uorb_index, report.device_id);
 				_corrections.gyro_device_ids[uorb_index] = 0;

 			} else {
@@ -84,8 +84,7 @@ void TemperatureCompensationModule::parameters_update()
 			int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);

 			if (temp < 0) {
-				PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index);
-
+				PX4_INFO("No temperature calibration available for accel %i (device id %u)", uorb_index, report.device_id);
 				_corrections.accel_device_ids[uorb_index] = 0;

 			} else {
@@ -102,7 +101,7 @@ void TemperatureCompensationModule::parameters_update()
 			int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index);

 			if (temp < 0) {
-				PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index);
+				PX4_INFO("No temperature calibration available for baro %i (device id %u)", uorb_index, report.device_id);
 				_corrections.baro_device_ids[uorb_index] = 0;

 			} else {
@@ -87,6 +87,13 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int
 		return 0;
 	}

+	if (PX4_ISFINITE(accel_data.temperature)) {
+		data.has_valid_temperature = true;
+
+	} else {
+		return 0;
+	}
+
 	data.device_id = accel_data.device_id;

 	data.sensor_sample_filt[0] = accel_data.x;
@@ -167,6 +174,14 @@ int TemperatureCalibrationAccel::finish()

 int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
 {
+	if (!data.has_valid_temperature) {
+		PX4_WARN("Result Accel %d does not have a valid temperature sensor", sensor_index);
+
+		uint32_t param = 0;
+		set_parameter("TC_A%d_ID", sensor_index, &param);
+		return 0;
+	}
+
 	if (!data.hot_soaked || data.tempcal_complete) {
 		return 0;
 	}
@@ -87,6 +87,13 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int
 		return 0;
 	}

+	if (PX4_ISFINITE(baro_data.temperature)) {
+		data.has_valid_temperature = true;
+
+	} else {
+		return 0;
+	}
+
 	data.device_id = baro_data.device_id;

 	data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
@@ -163,6 +170,14 @@ int TemperatureCalibrationBaro::finish()

 int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
 {
+	if (!data.has_valid_temperature) {
+		PX4_WARN("Result baro %d does not have a valid temperature sensor", sensor_index);
+
+		uint32_t param = 0;
+		set_parameter("TC_B%d_ID", sensor_index, &param);
+		return 0;
+	}
+
 	if (!data.hot_soaked || data.tempcal_complete) {
 		return 0;
 	}
@@ -146,6 +146,10 @@ public:
 			float min_diff = _min_temperature_rise;

 			for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+				if (!_data[uorb_index].has_valid_temperature) {
+					return 110;
+				}
+
 				float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;

 				if (cur_diff < min_diff) {
@@ -171,6 +175,7 @@ protected:
 		/// verified and the starting temperature set
 		bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase
 		bool tempcal_complete = false; ///< true when the calibration has been completed
+		bool has_valid_temperature = false; ///< true if this sensor has temperature sensor
 		float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C)
 		float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C)
 		float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the
@@ -74,6 +74,13 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int
 		return 0;
 	}

+	if (PX4_ISFINITE(gyro_data.temperature)) {
+		data.has_valid_temperature = true;
+
+	} else {
+		return 0;
+	}
+
 	data.device_id = gyro_data.device_id;

 	data.sensor_sample_filt[0] = gyro_data.x;
@@ -154,6 +161,15 @@ int TemperatureCalibrationGyro::finish()

 int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index)
 {
+	if (!data.has_valid_temperature) {
+		PX4_WARN("Result Gyro %d does not have a valid temperature sensor", sensor_index);
+		data.tempcal_complete = true;
+
+		uint32_t param = 0;
+		set_parameter("TC_G%d_ID", sensor_index, &param);
+		return 0;
+	}
+
 	if (!data.hot_soaked || data.tempcal_complete) {
 		return 0;
 	}
Author	SHA1	Message	Date
Daniel Agar	a6274bc5ed	rc/dsm: remove system field check, add new validity checks - unfortunately we can't depend on the system field due to potential external binding and non-genuine Spektrum equipment - reject any DSM frame with duplicate channels - add 16 channel mask - tighten valid PWM range 990-2010us (±100% travel is 1102-1898µs) - update RCTest rejected frame count	2021-01-01 22:58:47 +01:00
Nicolas Martin	4b0038bd43	positionControl: add check on sign before sqrtf	2020-11-19 10:01:56 -05:00
Beat Küng	5b2f41de12	ll40ls: fix rotation -> orientation	2020-11-19 11:15:30 +01:00
Matthias Grob	02c6f207ce	PX4 compile flags: disable Wlogical-op for GCC 10 There is a proper fix for this already on master but it's a rabbit hole to cherry-pick it: -> update matrix -> dependency on changing ecl -> dependency on a lot of autopilot changes	2020-11-18 11:47:06 -05:00
Daniel Agar	97ab8277ed	ROMFS: 50000_generic_ground_vehicle remove fmu-v2 exclude - this allows px4_fmu-v2_rover to function until we have a better mechanism for including or excluding ROMFS dependencies #15711	2020-11-12 12:57:27 -05:00
AlexanderAurora	8068ed89f6	landing_slope: added wrap_pi to getLandingSlope and getFlareCurve - fixes https://github.com/PX4/PX4-Autopilot/issues/16132	2020-11-12 12:57:27 -05:00
Alex Mikhalev	a0a4b10885	Fix non-determinstic boot hang with crashdumps On boot, if board_hardfault_init finds a hardfault stored in BBSRAM, it checks if there is any data available on stdin to see if there is somebody there to respond to a prompt. But on boards such as cubeorange where there is not a serial console by default, the ioctl fails and bytesWaiting is uninitialized. So it will non-deterministally hang the boot process with no outside feedback if that value is not zero. Signed-off-by: Alex Mikhalev <alexmikhalevalex@gmail.com>	2020-11-12 12:57:27 -05:00
Julian Kent	377dc909ed	Check that home position is valid in RTL (#15964 )	2020-11-12 12:57:27 -05:00
Daniel Agar	8a516e5488	commander: preflight check don't read mag device_id	2020-11-12 12:57:27 -05:00
CUAVcaijie	d8778ac8df	Add a USB ardupilot * type	2020-11-12 12:57:27 -05:00
Jacob Dahl	92a9278a6a	fixed dps310 calibration coeffecient for c11	2020-11-12 12:57:27 -05:00
Julian Oes	508c09d610	commander: fix switch to loiter Sometimes, the mission_result timestamp is the same as the internal_state timestamp which would meant that we would not switch to LOITER even though the takeoff is clearly done at that point.	2020-11-12 12:57:27 -05:00
Daniel Agar	efabca854f	ekf2: update ecl with bug fixes - ecl/EKF: Improve robustness of yaw reset to bad inertial data (https://github.com/PX4/PX4-ECL/pull/914) - ecl/EKF: Use strength in Gauss (bug fix) (https://github.com/PX4/PX4-ECL/pull/933)	2020-11-12 11:27:56 -05:00
Daniel Agar	bdcccb1723	sensors: force parameter update if mag device id still isn't set	2020-10-22 09:36:00 -04:00
Beat Küng	8592fe2358	tests: don't run battery_simulator The battery migration interferes with the tests.	2020-10-20 11:43:41 -04:00
Beat Küng	5faf8bd52c	battery: disable secondary module by default Avoid a GCS showing 2 battery indicators. Alternatively we could also check the 'connected' flag, but this is more explicit.	2020-10-20 11:43:41 -04:00
Beat Küng	977479c370	battery: fix isFloatEqual(), use matrix::isEqualF Fixes param migration, e.g. if BAT_N_CELLS is set, migrates to BAT1_N_CELLS.	2020-10-20 11:43:41 -04:00
Beat Küng	6da456e7f5	fix EscBattery: remove _battery.publish(); updateBatteryStatus() already publishes	2020-10-20 11:43:41 -04:00
Daniel Agar	9fcbf18d82	battery: fix duplicate uORB publish and minor cleanup - run battery_status module on adc_report publications rather than a fixed schedule	2020-10-05 14:49:54 -04:00
Daniel Agar	3188408d81	boards: mRo Control Zero F7 fix RC input and cleanup sensors init	2020-10-05 14:49:54 -04:00
Daniel Agar	d3148f2656	commander: PreFlightCheck param_find all parameters immediately - this ensures the relevant parameters are marked active immediately before parameter sync - fixes https://github.com/PX4/Firmware/issues/15872	2020-10-03 17:52:46 -04:00
Beat Küng	665effca3b	kakutef7: fix output ordering Regression from `d2254c2e44`, I overlooked that they were defined in reverse order.	2020-10-03 17:52:46 -04:00
Alex Mikhalev	db37c632b4	uavcan: Increase uavcan main stack size I observed stack overflows when executing `uavcan params list`, so the stack size probably needs to be increased. Signed-off-by: Alex Mikhalev <alexmikhalevalex@gmail.com>	2020-10-03 17:52:46 -04:00
Nicolas Martin	50d768c79d	mc_pos_control: fix acc Z sign	2020-09-29 09:33:43 -04:00
Daniel Agar	740c471021	boards: CUAV Nora/X7pro fix BOARD_DSHOT_MOTOR_ASSIGNMENT - BOARD_DSHOT_MOTOR_ASSIGNMENT isn't needed if there's no remapping	2020-09-24 12:04:28 -04:00
Daniel Agar	8398e80fa5	boards: cubepilot orange/yellow fix BOARD_DSHOT_MOTOR_ASSIGNMENT	2020-09-24 12:04:28 -04:00
Daniel Agar	4f8bcec1d7	boards: cubeorange and cubeyellow disable serial console (used for ADSB on new carrier boards) - add console builds (cubepilot_cubeorange_console) for hardware test rack and developers	2020-09-21 13:44:54 -04:00
Mirko Denecke	f01fbdf8b5	CubeOrange and CubeYellow IMU heating fix	2020-09-16 10:28:33 -04:00
Nicolas MARTIN	db529aa013	temperature compensation: change sensor id not found message	2020-09-16 10:28:33 -04:00
Nicolas MARTIN	cd3db45d27	thermal calibrtation: do not calibrate sensor without temperature sensor	2020-09-16 10:28:33 -04:00
David Sidrane	71db0903a9	NuttX Critical STM32H7 Interrupt Storm on I2C	2020-09-07 15:36:11 -04:00
Dusan Zivkovic	bfc59f6d88	FlightTaskAuto: update waypoints on every iteration when in offtrack state	2020-09-07 15:25:06 -04:00
Daniel Agar	2bc4a5a44b	commander: HITL skip auto disarm if lockdown - auto disarm when locked down was added in #14766 to prevent user confusion in regular usage, but also breaks HITL where lockdown is enabled for safety - fixes #15686	2020-09-06 19:30:02 -04:00
Jonathan Hahn	33cabba185	fw_pos_control_l1: fix swapped TECS time parameters (#15685 ) Co-authored-by: Jonathan Hahn <hahn@wingcopter.com>	2020-09-04 15:14:00 -04:00
Matthias Grob	4cd7d44b4a	battery: switch to PublicationMulti for battery_status	2020-09-01 22:08:28 -04:00
Matthias Grob	7da7ebad3d	analog_battery: fix missing stdio include This was not a problem before because battery.h included the adc driver and implicitly snprintf was defined through there.	2020-09-01 22:08:28 -04:00
Matthias Grob	a809e4cfc8	commander_params: remove some double spaces	2020-09-01 22:08:28 -04:00
Matthias Grob	4db9d7131e	syslink_main: remove empty lines and struct keyword	2020-09-01 22:08:28 -04:00
Matthias Grob	f92c5aa688	ina226/voxlpm: make sure parameter sub is reset The subscription to parameter updates has to get copied otherwise the change detection will not get reset for next time.	2020-09-01 22:08:28 -04:00
Matthias Grob	165c8b23bf	battery: fix parameter migration and clarify	2020-09-01 22:08:28 -04:00
Matthias Grob	226f5f875d	BatterySimulator: remove SimulatorBattery It loads the battery parameters but then overwrites them with hardcoded values and it breaks the ModuleParams parent/child hierarchy. Both is undesired.	2020-09-01 22:08:28 -04:00
Oleg	af9c6e6fce	batterry_status: fix checking default a_per_v	2020-08-28 16:28:54 -04:00
bresch	c3a410e19c	ll40ls: set default rotation to downwards facing All the other distance sensors have their default rotation to downwards facing as well	2020-08-28 16:28:54 -04:00
Daniel Agar	427b17d8a1	bosch/bmi055: fix accel temperature reading - single register output is in 2's complement	2020-08-28 16:28:29 -04:00
CUAVcaijie	1672fc646f	boards: add UAVCAN timer override mechanism and CUAV X7 add CAN (#15348 ) * X7Pro adds CAN driver * UAVCAN timer selection moved to default.cmake * Modify some details about @CUAVcaijie UAVCAN timer selection moved to default.cmake * Put some timer parameters to micro_hal.h from board_config.h. Fix all h7 boards Co-authored-by: honglang <honglang@cuav.net>	2020-08-28 16:28:29 -04:00
modaltb	c915f0dca9	modalai_fc-v1 - power_monitor/voxlpm: add support for voxlpm v3 (INA231 sensors)	2020-08-28 16:28:01 -04:00