FlightTasks: hotfix guard against accessing no task

Safety guard for null pointer exception when accessing the current task when none is running. This could only happen if the running task was changed during the update because of command handling failing. Note: This is fixed on master in a different, better way.
rc/dsm: remove system field check, add new validity checks
2026-05-23 19:07:35 +08:00 · 2021-01-26 20:34:31 +01: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
82 changed files with 1731 additions and 521 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]
@@ -150,14 +150,24 @@ public:
 	/**
 	 * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy.
 	 */
-	void setYawHandler(WeatherVane *ext_yaw_handler) {_current_task.task->setYawHandler(ext_yaw_handler);}
+	void setYawHandler(WeatherVane *ext_yaw_handler)
+	{
+		if (isAnyTaskActive()) {
+			_current_task.task->setYawHandler(ext_yaw_handler);
+		}
+	}

 	/**
 	 *   This method will re-activate current task.
 	 */
 	void reActivate();

-	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp)
+	{
+		if (isAnyTaskActive()) {
+			_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp);
+		}
+	}

 private:

@@ -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
Matthias Grob	21c82a4814	FlightTasks: hotfix guard against accessing no task Safety guard for null pointer exception when accessing the current task when none is running. This could only happen if the running task was changed during the update because of command handling failing. Note: This is fixed on master in a different, better way.	2021-01-26 20:34:31 +01:00
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