NuttX Critical STM32H7 Interrupt Storm on I2C

- 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

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -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

boards/bitcraze/crazyflie/syslink/syslink_main.cpp

@@ -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);
 

boards/bitcraze/crazyflie/syslink/syslink_main.h

@@ -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();
-
 };

boards/cuav/nora/src/board_config.h

@@ -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

boards/cuav/x7pro/default.cmake

@@ -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

boards/cuav/x7pro/nuttx-config/include/board.h

@@ -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

boards/cuav/x7pro/src/board_config.h

@@ -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

boards/cubepilot/cubeorange/src/board_config.h

@@ -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 */
 

boards/modalai/fc-v1/init/rc.board_sensors

@@ -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

boards/spracing/h7extreme/src/board_config.h

@@ -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 */
 

cmake/px4_add_board.cmake

@@ -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)

platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/micro_hal.h

@@ -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

src/drivers/distance_sensor/ll40ls/ll40ls.cpp

@@ -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();
 }
@@ -99,6 +99,7 @@ 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) {

src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp

@@ -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);

src/drivers/power_monitor/ina226/ina226.cpp

@@ -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();
 	}
 

src/drivers/power_monitor/ina226/ina226.h

@@ -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);

src/drivers/power_monitor/voxlpm/voxlpm.cpp

@@ -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);
+}

src/drivers/power_monitor/voxlpm/voxlpm.hpp

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

src/drivers/power_monitor/voxlpm/voxlpm_main.cpp

@@ -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;
 		}
 	}
 

src/drivers/power_monitor/voxlpm/voxlpm_params.c

@@ -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);

src/drivers/uavcan/CMakeLists.txt

@@ -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

src/lib/battery/battery.cpp

@@ -105,11 +105,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us)
 	updateParams();
 }
 
-Battery::~Battery()
-{
-	orb_unadvertise(_orb_advert);
-}
-
 void
 Battery::reset()
 {
@@ -179,6 +174,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 	const bool should_publish = (source == _params.source);
 
 	if (should_publish) {
+		_battery_status_pub.publish(_battery_status);
 		publish();
 	}
 }
@@ -186,7 +182,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
 void
 Battery::publish()
 {
-	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
+	_battery_status_pub.publish(_battery_status);
 }
 
 void

src/lib/battery/battery.h

@@ -43,12 +43,11 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.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>
@@ -65,8 +64,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.
@@ -157,67 +155,53 @@ protected:
 
 	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
+	 * 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 (!isFloatEqual(*old_val, *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 || !isFloatEqual(*old_val, 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 (!isFloatEqual(*new_val, 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;
 	}
 
+	bool isFloatEqual(float a, float b) { return fabsf(a - b) > FLT_EPSILON; }
+
 private:
 	void sumDischarged(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();
 
+	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;
@@ -229,7 +213,4 @@ private:
 	float _scale = 1.f;
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
-
-	orb_advert_t _orb_advert{nullptr};
-	int _orb_instance;
 };

src/lib/battery/battery_params_deprecated.c

@@ -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);

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -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;
 	}

src/modules/battery_status/analog_battery.cpp

@@ -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;

src/modules/battery_status/analog_battery.h

@@ -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

src/modules/commander/Commander.cpp

@@ -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) {

src/modules/commander/commander_params.c

@@ -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
  */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -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());

src/modules/simulator/battery_simulator/BatterySimulator.cpp

@@ -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;
 }
 

src/modules/simulator/battery_simulator/BatterySimulator.hpp

@@ -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};