Disable multicopter attitude autotuner for fmu-v5 test build.

Scope VEHICLE_MODE_FLAG to the Commander class.

.github/PULL_REQUEST_TEMPLATE.md

@@ -23,7 +23,7 @@ For release notes:
 ```
 Feature/Bugfix XYZ
 New parameter: XYZ_Z
-Documentation: Need to clarfiy page ... / done, read docs.px4.io/...
+Documentation: Need to clarify page ... / done, read docs.px4.io/...
 ```
 
 ### Alternatives

.github/workflows/stale.yml

@@ -0,0 +1,20 @@
+name: 'Close stale issues and PRs'
+on:
+  schedule:
+    - cron: '30 1 * * *'
+
+jobs:
+  stale:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/stale@v4.1.1
+        with:
+          stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.'
+          stale-pr-message: 'This PR is stale because it has been open 45 days with no activity. Remove stale label or comment or this will be closed in 10 days.'
+          close-issue-message: 'This issue was closed because it has been stalled for 5 days with no activity.'
+          close-pr-message: 'This PR was closed because it has been stalled for 10 days with no activity.'
+          days-before-issue-stale: 30
+          days-before-pr-stale: 45
+          days-before-issue-close: 5
+          days-before-pr-close: 10
+          debug-only: true

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -17,6 +17,7 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1

ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter

@@ -48,35 +48,32 @@ param set-default PWM_MAIN_REV 96 # invert both elevons
 param set-default EKF2_MULTI_IMU 0
 param set-default SENS_IMU_MODE 1
 
-param set-default NPFG_PERIOD 12
-param set-default FW_PR_I 0.2
-param set-default FW_PR_P 0.2
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_FF 0.1
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_P 0.2
-param set-default FW_THR_TRIM 0.33
-param set-default FW_THR_MAX 0.6
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.2
+param set-default FW_RR_P 0.3
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
 param set-default FW_THR_MIN 0.05
-param set-default FW_T_ALT_TC 2
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-param set-default FW_T_TAS_TC 2
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
 
 param set-default MC_AIRMODE 1
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
 param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
 
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-
+param set-default VT_ARSP_TRANS 10
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_S_Y 0.5
+param set-default VT_FW_DIFTHR_S_Y 1
 param set-default VT_F_TRANS_DUR 1.5
-param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
 
 param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter

@@ -0,0 +1,74 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 0
+
+parm set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -60,6 +60,7 @@ px4_add_romfs_files(
 	1042_gazebo-classic_tiltrotor
 	1043_gazebo-classic_standard_vtol_drop
 	1044_gazebo-classic_plane_lidar
+	1045_gazebo-classic_quadtailsitter
 	1060_gazebo-classic_rover
 	1061_gazebo-classic_r1_rover
 	1062_flightgear_tf-r1

ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil

@@ -20,6 +20,7 @@
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
 
 param set UAVCAN_ENABLE 0
+param set-default VT_B_TRANS_DUR 5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -31,3 +31,4 @@ param set-default CA_SV_CS1_TYPE 6
 param set-default MAV_TYPE 19
 param set-default VT_TYPE 0
 param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_B_TRANS_DUR 5

ROMFS/px4fmu_common/init.d/rcS

@@ -254,6 +254,7 @@ else
 	#
 	rgbled start -X -q
 	rgbled_ncp5623c start -X -q
+	rgbled_lp5562 start -X -q
 
 	#
 	# Override parameters from user configuration file.

Tools/simulation/sitl_multiple_run.sh

@@ -10,7 +10,7 @@ sitl_num=2
 [ -n "$1" ] && sitl_num="$1"
 
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-src_path="$SCRIPT_DIR/.."
+src_path="$SCRIPT_DIR/../../"
 
 build_path=${src_path}/build/px4_sitl_default
 

boards/ark/cannode/default.px4board

@@ -20,6 +20,7 @@ CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_COMMON_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y

boards/modalai/fc-v2/default.px4board

@@ -14,10 +14,10 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+# CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
+# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
+# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
@@ -29,11 +29,13 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
+CONFIG_DRIVERS_RPM=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+# CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
@@ -52,6 +54,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+# CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -70,6 +73,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_GPIO=y

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

@@ -20,8 +20,8 @@ icm42688p -s -b 1 -R 12 start
 # Internal SPI2 ICM-42688
 icm42688p -s -b 2 -R 12 start
 
-# Internal I2C mag
-bmm150 -I start
+# Don't start Internal I2C mag
+# bmm150 -I start
 
 # Internal I2C baro
 icp201xx -I start

boards/modalai/fc-v2/src/CMakeLists.txt

@@ -68,4 +68,18 @@ else()
 			nuttx_drivers # sdio
 			px4_layer
 	)
+
+	set(COMMON_MODALAI_SRC_DIR ${PX4_SOURCE_DIR}/boards/modalai/src)
+	set(MODALAI_SYSTEMCMD_SRC_DIR ${COMMON_MODALAI_SRC_DIR}/systemcmds/modalai)
+
+	px4_add_module(
+		MODULE systemcmds__modalai
+		MAIN modalai
+		COMPILE_FLAGS
+		SRCS
+			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai_fc-v2.c
+			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai_fc-v1.c
+			${MODALAI_SYSTEMCMD_SRC_DIR}/modalai.c
+		DEPENDS
+	)
 endif()

boards/modalai/fc-v2/src/board_config.h

@@ -91,6 +91,12 @@
 #  define GPIO_nLED_GREEN      /* PE4 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
 #  define GPIO_nLED_BLUE       /* PE5 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
 
+// GPIO_nLED_2_RED/ GPIO_nLED_2_GREEN /GPIO_nLED_2_BLUE are for v1 LED tests
+
+#  define GPIO_nLED_2_RED      /* PI0 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN0)
+#  define GPIO_nLED_2_GREEN    /* PH11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN11)
+#  define GPIO_nLED_2_BLUE     /* PA2 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN2)
+
 #  define BOARD_HAS_CONTROL_STATUS_LEDS      1
 #  define BOARD_OVERLOAD_LED     LED_RED
 #  define BOARD_ARMED_STATE_LED  LED_BLUE
@@ -208,6 +214,12 @@
 
 #define CAN1_SILENT                       /* PD15 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
 
+/* For primary/backup signaling with VOXL, 2 pins on J4 are exposed */
+// GPIO_VOXL_STATUS_OUT/ GPIO_VOXL_STATUS_IN are for v1 Spare MSS Communications Interface and J4 tests
+
+#define GPIO_VOXL_STATUS_OUT            /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
+#define GPIO_VOXL_STATUS_IN             /* PE3 */ (GPIO_INPUT|GPIO_FLOAT|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))
@@ -338,8 +350,8 @@
 
 #define BOARD_NUM_IO_TIMERS 5
 
-// J1 / TELEM1 / USART7
-#define MODAL_IO_DEFAULT_PORT                     "/dev/ttyS6"
+// J5 USART5 TELEM2 Port next to PWM connector
+#define MODAL_IO_DEFAULT_PORT                     "/dev/ttyS4"
 
 __BEGIN_DECLS
 

boards/modalai/src/systemcmds/modalai/modalai.c

@@ -0,0 +1,131 @@
+#include <px4_platform_common/module.h>
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+// v2
+#ifdef CONFIG_ARCH_CHIP_STM32H753II		// chip on M0087
+#include "modalai_fc-v2.h"
+#define MODALAI_FC_V2 1
+#else
+#include "modalai_fc-v1.h"
+#endif
+
+__EXPORT int modalai_main(int argc, char *argv[]);
+
+int modalai_main(int argc, char *argv[])
+{
+	int hw_rev = board_get_hw_revision();
+	int hw_ver = board_get_hw_version();
+
+	eHW_TYPE hw_type = eHwNone;
+
+#ifdef MODALAI_FC_V2
+
+	if (hw_rev == 0 && hw_ver == 3) {	// (should be hw_rev == 1 && hw_ver == 3) eventually...
+		hw_type = eM0087;
+
+	} else if (hw_rev == 0 && hw_ver == 3) {
+		hw_type = eM0079;
+
+	} else {
+		return -1;
+
+	}
+
+#else
+
+	if (hw_rev == 6 && hw_ver == 0) {
+		hw_type = eM0018;
+
+	} else if (hw_rev == 0 && hw_ver == 1) {
+		hw_type = eM0019;
+
+	} else if (hw_rev == 0 && hw_ver == 2) {
+		hw_type = eM0051;
+
+	} else {
+		return -1;
+
+	}
+
+#endif
+
+	if (argc <= 1) {
+#ifdef MODALAI_FC_V2
+		modalai_print_usage_v2();
+#else
+		modalai_print_usage_v1();
+#endif
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "led")) {
+#ifdef MODALAI_FC_V2
+		return modalai_led_test_v2();
+#else
+		return modalai_led_test_v1();
+#endif
+
+	} else if (!strcmp(argv[1], "con")) {
+		if (argc <= 2) {
+			PRINT_MODULE_USAGE_COMMAND("con");
+			PRINT_MODULE_USAGE_ARG("<1,4,5,6,7,9,10,12,13>", "Connector ID", false);
+			PRINT_MODULE_USAGE_ARG("<uint>", "Pin Number", false);
+			PRINT_MODULE_USAGE_ARG("0 | 1", "<output state> (defaults to 0)", false);
+			return 1;
+		}
+
+		uint8_t con = 0;
+		uint8_t pin = 0;
+		bool state = false;
+
+		if (argc > 2) {
+			con = atoi(argv[2]);
+		}
+
+		if (argc > 3) {
+			pin = atoi(argv[3]);
+		}
+
+		if (argc > 4) {
+			state = atoi(argv[4]);
+		}
+
+#ifdef MODALAI_FC_V2
+		return modalai_con_gpio_test_v2(con, pin, state);
+#else
+		return modalai_con_gpio_test_v1(con, pin, state);
+#endif
+
+	} else if (!strcmp(argv[1], "buzz")) {
+
+#ifdef MODALAI_FC_V2
+		return modalai_buzz_test_v2(hw_type);
+#else
+		return modalai_buzz_test_v1(hw_type);
+#endif
+
+
+
+	} else if (!strcmp(argv[1], "detect")) {
+#ifdef MODALAI_FC_V2
+		modalai_hw_detect_v2(hw_type);
+#else
+		modalai_hw_detect_v1(hw_type);
+#endif
+
+		return 0;
+	}
+
+#ifdef MODALAI_FC_V2
+	modalai_print_usage_v2();
+#else
+	modalai_print_usage_v1();
+#endif
+	return -EINVAL;
+}

boards/modalai/src/systemcmds/modalai/modalai_fc-v1.c

@@ -0,0 +1,913 @@
+#include <px4_platform_common/module.h>
+
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+// v1
+#ifndef CONFIG_ARCH_CHIP_STM32H743ZI
+
+#include "modalai_fc-v1.h"
+
+
+
+
+void modalai_print_usage_v1(void)
+{
+	PRINT_MODULE_DESCRIPTION("ModalAI Test utility\n");
+
+	PRINT_MODULE_USAGE_NAME_SIMPLE("modalai", "command");
+
+	PRINT_MODULE_USAGE_COMMAND_DESCR("led", "LED Test");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("con", "Connector Output Test (as GPIO)");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("buzz", "Automated buzz out test");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("detect", "Detect board type");
+}
+
+void modalai_print_usage_con_gpio_test_v1(void)
+{
+	PRINT_MODULE_USAGE_NAME_SIMPLE("modalai con", "command");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("1",  "W<3,6> R<2,6>, <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("4",  "W<2-4,6-7> R<8>, <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("5",  "W<2-5>,   <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("6",  "W<2-5>,   <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("7",  "W<2-9>,   <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("9",  "R<2>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("10", "W<2-5>,   <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("12", "W<1-3>,   <0-1>");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("13", "W<3-5>,   <0-1>");
+}
+
+int modalai_led_test_v1(void)
+{
+	PX4_INFO("Running led test");
+
+	stm32_configgpio(GPIO_nLED_RED);
+	stm32_configgpio(GPIO_nLED_GREEN);
+	stm32_configgpio(GPIO_nLED_BLUE);
+
+	int i = 0;
+
+	stm32_configgpio(GPIO_nLED_2_RED);
+	stm32_configgpio(GPIO_nLED_2_GREEN);
+	stm32_configgpio(GPIO_nLED_2_BLUE);
+
+	for (i = 0; i < 3; i++) {
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_RED, false);
+
+		stm32_gpiowrite(GPIO_nLED_2_RED, false);
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_RED, true);
+		stm32_gpiowrite(GPIO_nLED_2_RED, true);
+
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_GREEN, false);
+		stm32_gpiowrite(GPIO_nLED_2_GREEN, false);
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_GREEN, true);
+		stm32_gpiowrite(GPIO_nLED_2_GREEN, true);
+
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_BLUE, false);
+		stm32_gpiowrite(GPIO_nLED_2_BLUE, false);
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_BLUE, true);
+		stm32_gpiowrite(GPIO_nLED_2_BLUE, true);
+	}
+
+	return OK;
+}
+
+int modalai_con_gpio_test_v1(uint8_t con, uint8_t pin, bool state)
+{
+	// validate
+	switch (con) {
+	// Primary MSS Communications Interface
+	case 1:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J1_PIN2_IN);
+			state = stm32_gpioread(J1_PIN2_IN);
+			break;
+
+		case 3:
+			stm32_configgpio(J1_PIN3);
+			stm32_gpiowrite(J1_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J1_PIN4);
+			stm32_gpiowrite(J1_PIN4, state);
+			break;
+
+		case 6:
+			stm32_configgpio(J1_PIN6_IN);
+			state = stm32_gpioread(J1_PIN6_IN);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// STM JTAG Programming Header
+	case 2:
+		modalai_print_usage_con_gpio_test_v1();
+		return -1;
+
+	// USB 2.0 Full-Speed Downstream Device Port
+	case 3:
+		modalai_print_usage_con_gpio_test_v1();
+		return -1;
+
+	// Spare MSS Communications Interface
+	case 4:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J4_PIN2);
+			stm32_gpiowrite(J4_PIN2, state);
+			break;
+
+		case 3:
+			stm32_configgpio(J4_PIN3);
+			stm32_gpiowrite(J4_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J4_PIN4);
+			stm32_gpiowrite(J4_PIN4, state);
+			break;
+
+		case 6:
+			stm32_configgpio(J4_PIN6);
+			stm32_gpiowrite(J4_PIN6, state);
+			break;
+
+		case 7:
+			stm32_configgpio(J4_PIN7);
+			stm32_gpiowrite(J4_PIN7, state);
+			break;
+
+		case 8:
+			stm32_configgpio(J4_PIN8);
+			state = stm32_gpioread(J4_PIN8);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// TELEMETRY CONNECTOR
+	case 5:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J5_PIN2);
+			stm32_gpiowrite(J5_PIN2, state);
+			break;
+
+		case 3:
+			stm32_configgpio(J5_PIN3);
+			stm32_gpiowrite(J5_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J5_PIN4);
+			stm32_gpiowrite(J5_PIN4, state);
+			break;
+
+		case 5:
+			stm32_configgpio(J5_PIN5);
+			stm32_gpiowrite(J5_PIN5, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// EXPANSION CONNECTOR
+	case 6:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J6_PIN2);
+			stm32_gpiowrite(J6_PIN2, state);
+			break;
+
+		case 3:
+			stm32_configgpio(J6_PIN3);
+			stm32_gpiowrite(J6_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J6_PIN4);
+			stm32_gpiowrite(J6_PIN4, state);
+			break;
+
+		case 5:
+			stm32_configgpio(J6_PIN5);
+			stm32_gpiowrite(J6_PIN5, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// PWM Output Connector
+	case 7:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J7_PIN2);
+			stm32_gpiowrite(J7_PIN2, state);
+			break;
+
+		case 3:
+			stm32_configgpio(J7_PIN3);
+			stm32_gpiowrite(J7_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J7_PIN4);
+			stm32_gpiowrite(J7_PIN4, state);
+			break;
+
+		case 5:
+			stm32_configgpio(J7_PIN5);
+			stm32_gpiowrite(J7_PIN5, state);
+			break;
+
+		case 6:
+			stm32_configgpio(J7_PIN6);
+			stm32_gpiowrite(J7_PIN6, state);
+			break;
+
+		case 7:
+			stm32_configgpio(J7_PIN7);
+			stm32_gpiowrite(J7_PIN7, state);
+			break;
+
+		case 8:
+			stm32_configgpio(J7_PIN8);
+			stm32_gpiowrite(J7_PIN8, state);
+			break;
+
+		case 9:
+			stm32_configgpio(J7_PIN9);
+			stm32_gpiowrite(J7_PIN9, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// CAN 1 Peripheral Connector
+	case 8:
+		modalai_print_usage_con_gpio_test_v1();
+		return -1;
+
+	// PPM (RC) IN
+	case 9:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J9_PIN2_IN);
+			state = stm32_gpioread(J9_PIN2_IN);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// GPS CONNECTOR
+	case 10:
+		switch (pin) {
+		case 2:
+			stm32_configgpio(J10_PIN2);
+			stm32_gpiowrite(J10_PIN2, state);
+			break;
+
+		case 3:
+			stm32_configgpio(J10_PIN3);
+			stm32_gpiowrite(J10_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J10_PIN4);
+			stm32_gpiowrite(J10_PIN4, state);
+			break;
+
+		case 5:
+			stm32_configgpio(J10_PIN5);
+			stm32_gpiowrite(J10_PIN5, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// Micro SD Card Slot
+	case 11:
+		modalai_print_usage_con_gpio_test_v1();
+		return -1;
+
+	// Spektrum RC Input Connector
+	case 12:
+		switch (pin) {
+		case 1:
+			VDD_3V3_SPEKTRUM_POWER_EN(state);
+			break;
+
+		case 2:
+			__asm("nop");
+			stm32_configgpio(J12_PIN2);
+			stm32_gpiowrite(J12_PIN2, state);
+			//state = stm32_gpioread(J12_PIN2);
+			__asm("nop");
+			break;
+
+		case 3:
+			stm32_configgpio(J12_PIN3);
+			stm32_gpiowrite(J12_PIN3, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+
+	// I2C DISPLAY / SPARE SENSOR CONNECTOR
+	case 13:
+		switch (pin) {
+		case 3:
+			stm32_configgpio(J13_PIN3);
+			stm32_gpiowrite(J13_PIN3, state);
+			break;
+
+		case 4:
+			stm32_configgpio(J13_PIN4);
+			stm32_gpiowrite(J13_PIN4, state);
+			break;
+
+		case 5:
+			stm32_configgpio(J13_PIN5);
+			stm32_gpiowrite(J13_PIN5, state);
+			break;
+
+		default:
+			modalai_print_usage_con_gpio_test_v1();
+			return -1;
+		}
+
+		break;
+	}
+
+	printf("GPIO - Con: %d, Pin: %d, State: %d\n", con, pin, state);
+	return OK;
+}
+
+bool test_pair(uint32_t output_pin, uint32_t input_pin)
+{
+
+	bool state = false;
+
+	stm32_gpiowrite(output_pin, true);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != true) {
+		return false;
+	}
+
+	usleep(1000 * 10);
+
+	stm32_gpiowrite(output_pin, false);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != false) {
+		return false;
+	}
+
+	return true;
+}
+
+bool modalai_test_pair(uint32_t output_pin, uint32_t input_pin)
+{
+
+	bool state = false;
+
+	stm32_gpiowrite(output_pin, true);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != true) {
+		return false;
+	}
+
+	usleep(1000 * 10);
+
+	stm32_gpiowrite(output_pin, false);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != false) {
+		return false;
+	}
+
+	return true;
+}
+
+int modalai_buzz_test_v1(eHW_TYPE hw_type)
+{
+	PX4_INFO("test: buzz");
+	usleep(1000 * 100 * 10);
+
+	if (hw_type == eM0018) {
+		PX4_INFO("Using Flight Core Config");
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO("Using VOXL-Flight Config");
+
+	} else if (hw_type == eM0051) {
+		PX4_INFO("Using M0051 Config");
+
+	} else {
+		return -1;
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J1");
+		stm32_configgpio(J1_PIN2_IN); // 2 [in] to 4 [out]
+		stm32_configgpio(J1_PIN3); // 3 [out] to 6 [in]
+		stm32_configgpio(J1_PIN4); // 4 [out] to 2 [in]
+		stm32_configgpio(J1_PIN6_IN); // 6 [in] to 3 [out]
+
+		if (test_pair(J1_PIN4, J1_PIN2_IN)) {
+			PX4_INFO("PASS: J1P4-J1P2");
+
+		} else {
+			PX4_ERR("FAIL: J1P4-J1P2 ----------------------------------------");
+		}
+
+		if (test_pair(J1_PIN3, J1_PIN6_IN)) {
+			PX4_INFO("PASS: J1P3-J1P6");
+
+		} else {
+			PX4_ERR("FAIL: J1P3-J1P6 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		// NA on VOXL-Flight (internally routed)
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J4");
+		stm32_configgpio(J4_PIN2);    // 2 [out] 6 [in]
+		stm32_configgpio(J4_PIN3);    // 3 [out] 7 [in]
+		stm32_configgpio(J4_PIN4);    // 4 [out] 8 [in]
+		stm32_configgpio(J4_PIN6_IN); // 2 [out] 6 [in]
+		stm32_configgpio(J4_PIN7_IN); // 3 [out] 7 [in]
+		stm32_configgpio(J4_PIN8_IN); // 4 [out] 8 [in]
+
+		if (test_pair(J4_PIN2, J4_PIN6_IN)) {
+			PX4_INFO("PASS: J4P2-J4P6");
+
+		} else {
+			PX4_ERR("FAIL: J4P2-J4P6 ----------------------------------------");
+		}
+
+		if (test_pair(J4_PIN3, J4_PIN7_IN)) {
+			PX4_INFO("PASS: J4P3-J4P7");
+
+		} else {
+			PX4_ERR("FAIL: J4P3-J4P7 ----------------------------------------");
+		}
+
+		if (test_pair(J4_PIN4, J4_PIN8_IN)) {
+			PX4_INFO("PASS: J4P4-J4P8");
+
+		} else {
+			PX4_ERR("FAIL: J4P4-J4P8 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1002");
+		stm32_configgpio(J1002_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J1002_PIN3);    // 3 [out] 6 [in]
+		stm32_configgpio(J1002_PIN4_IN); // 2 [out] 4 [in]
+		stm32_configgpio(J1002_PIN6_IN); // 3 [out] 6 [in]
+
+		if (test_pair(J1002_PIN2, J1002_PIN4_IN)) {
+			PX4_INFO("PASS: J1002P2-J1002P4");
+
+		} else {
+			PX4_ERR("FAIL: J1002P2-J1002P4 ----------------------------------------");
+		}
+
+		if (test_pair(J1002_PIN3, J1002_PIN6_IN)) {
+			PX4_INFO("PASS: J1002P3-J1002P6");
+
+		} else {
+			PX4_ERR("FAIL: J1002P3-J1002P6 ----------------------------------------");
+		}
+
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J5");
+		stm32_configgpio(J5_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J5_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J5_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J5_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J5_PIN2, J5_PIN4_IN)) {
+			PX4_INFO("PASS: J5P2-J5P4");
+
+		} else {
+			PX4_ERR("FAIL: J5P2-J5P4 ----------------------------------------");
+		}
+
+		if (test_pair(J5_PIN3, J5_PIN5_IN)) {
+			PX4_INFO("PASS: J5P3-J5P5");
+
+		} else {
+			PX4_ERR("FAIL: J5P3-J5P5 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1010");
+		stm32_configgpio(J1010_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J1010_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J1010_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J1010_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J1010_PIN2, J1010_PIN4_IN)) {
+			PX4_INFO("PASS: J1010P2-J1010P4");
+
+		} else {
+			PX4_ERR("FAIL: J1010P2-J1010P4 ----------------------------------------");
+		}
+
+		if (test_pair(J1010_PIN3, J1010_PIN5_IN)) {
+			PX4_INFO("PASS: J1010P3-J1010P5");
+
+		} else {
+			PX4_ERR("FAIL: J1010P3-J1010P5 ----------------------------------------");
+		}
+
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J6");
+		stm32_configgpio(J6_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J6_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J6_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J6_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J6_PIN2, J6_PIN4_IN)) {
+			PX4_INFO("PASS: J6P2-J6P4");
+
+		} else {
+			PX4_ERR("FAIL: J6P2-J6P4 ----------------------------------------");
+		}
+
+		if (test_pair(J6_PIN3, J6_PIN5_IN)) {
+			PX4_INFO("PASS: J6P3-J6P5");
+
+		} else {
+			PX4_ERR("FAIL: J6P3-J6P5 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1009");
+		stm32_configgpio(J1009_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J1009_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J1009_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J1009_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J1009_PIN2, J1009_PIN4_IN)) {
+			PX4_INFO("PASS: J1009P2-J1009P4");
+
+		} else {
+			PX4_ERR("FAIL: J1009P2-J1009P4 ----------------------------------------");
+		}
+
+		if (test_pair(J1009_PIN3, J1009_PIN5_IN)) {
+			PX4_INFO("PASS: J1009P3-J1009P5");
+
+		} else {
+			PX4_ERR("FAIL: J1009P3-J1009P5 ----------------------------------------");
+		}
+	}
+
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J7");
+		stm32_configgpio(J7_PIN2);    // 2 [out] 6 [in]
+		stm32_configgpio(J7_PIN3);    // 3 [out] 7 [in]
+		stm32_configgpio(J7_PIN4);    // 4 [out] 8 [in]
+		stm32_configgpio(J7_PIN5);    // 5 [out] 9 [in]
+		stm32_configgpio(J7_PIN6_IN); // 6 [in] 2 [out]
+		stm32_configgpio(J7_PIN7_IN); // 7 [in] 3 [out]
+		stm32_configgpio(J7_PIN8_IN); // 8 [in] 4 [out]
+		stm32_configgpio(J7_PIN9_IN); // 9 [in] 5 [out]
+
+		if (test_pair(J7_PIN2, J7_PIN6_IN)) {
+			PX4_INFO("PASS: J7P2-J7P6");
+
+		} else {
+			PX4_ERR("FAIL: J7P2-J7P6 ----------------------------------------");
+		}
+
+		if (test_pair(J7_PIN3, J7_PIN7_IN)) {
+			PX4_INFO("PASS: J7P3-J7P7");
+
+		} else {
+			PX4_ERR("FAIL: J7P3-J7P7 ----------------------------------------");
+		}
+
+		if (test_pair(J7_PIN4, J7_PIN8_IN)) {
+			PX4_INFO("PASS: J7P4-J7P8");
+
+		} else {
+			PX4_ERR("FAIL: J7P4-J7P8 ----------------------------------------");
+		}
+
+		if (test_pair(J7_PIN5, J7_PIN9_IN)) {
+			PX4_INFO("PASS: J7P5-J7P9");
+
+		} else {
+			PX4_ERR("FAIL: J7P5-J7P9 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1007");
+		stm32_configgpio(J1007_PIN2);    // 2 [out] 6 [in]
+		stm32_configgpio(J1007_PIN3);    // 3 [out] 7 [in]
+		stm32_configgpio(J1007_PIN4);    // 4 [out] 8 [in]
+		stm32_configgpio(J1007_PIN5);    // 5 [out] 9 [in]
+		stm32_configgpio(J1007_PIN6_IN); // 6 [in] 2 [out]
+		stm32_configgpio(J1007_PIN7_IN); // 7 [in] 3 [out]
+		stm32_configgpio(J1007_PIN8_IN); // 8 [in] 4 [out]
+		stm32_configgpio(J1007_PIN9_IN); // 9 [in] 5 [out]
+
+		if (test_pair(J1007_PIN2, J1007_PIN6_IN)) {
+			PX4_INFO("PASS: J1007P2-J1007P6");
+
+		} else {
+			PX4_ERR("FAIL: J1007P2-J1007P6 ----------------------------------------");
+		}
+
+		if (test_pair(J1007_PIN3, J1007_PIN7_IN)) {
+			PX4_INFO("PASS: J1007P3-J1007P7");
+
+		} else {
+			PX4_ERR("FAIL: J1007P3-J1007P7 ----------------------------------------");
+		}
+
+		if (test_pair(J1007_PIN4, J1007_PIN8_IN)) {
+			PX4_INFO("PASS: J1007P4-J1007P8");
+
+		} else {
+			PX4_ERR("FAIL: J1007P4-J1007P8 ----------------------------------------");
+		}
+
+		if (test_pair(J1007_PIN5, J1007_PIN9_IN)) {
+			PX4_INFO("PASS: J1007P5-J1007P9");
+
+		} else {
+			PX4_ERR("FAIL: J1007P5-J1007P9 ----------------------------------------");
+		}
+
+	} else if (hw_type == eM0051) {
+		PX4_INFO(">> Testing M0051 J13");
+		stm32_configgpio(M0051J13_PIN2);    // 2 [out] 6 [in]
+		stm32_configgpio(M0051J13_PIN3);    // 3 [out] 7 [in]
+		stm32_configgpio(M0051J13_PIN4);    // 4 [out] 8 [in]
+		stm32_configgpio(M0051J13_PIN5);    // 5 [out] 9 [in]
+		stm32_configgpio(M0051J13_PIN6_IN); // 6 [in] 2 [out]
+		stm32_configgpio(M0051J13_PIN7_IN); // 7 [in] 3 [out]
+		stm32_configgpio(M0051J13_PIN8_IN); // 8 [in] 4 [out]
+		stm32_configgpio(M0051J13_PIN9_IN); // 9 [in] 5 [out]
+
+		if (test_pair(M0051J13_PIN2, M0051J13_PIN6_IN)) {
+			PX4_INFO("PASS: J13_P2-J13_P6");
+
+		} else {
+			PX4_ERR("FAIL: J13_P2-J13_P6 ----------------------------------------");
+		}
+
+		if (test_pair(M0051J13_PIN3, M0051J13_PIN7_IN)) {
+			PX4_INFO("PASS: JJ13_P3-J13_P7");
+
+		} else {
+			PX4_ERR("FAIL: J13_P3-J13_7P7 ----------------------------------------");
+		}
+
+		if (test_pair(M0051J13_PIN4, M0051J13_PIN8_IN)) {
+			PX4_INFO("PASS: J13_P4-J13_P8");
+
+		} else {
+			PX4_ERR("FAIL: J13_P4-J13_P8 ----------------------------------------");
+		}
+
+		if (test_pair(M0051J13_PIN5, M0051J13_PIN9_IN)) {
+			PX4_INFO("PASS: J13_P5-J13_P9");
+
+		} else {
+			PX4_ERR("FAIL: J13_P5-J13_P9 ----------------------------------------");
+		}
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J10");
+		stm32_configgpio(J10_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J10_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J10_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J10_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J10_PIN2, J10_PIN4_IN)) {
+			PX4_INFO("PASS: J10P2-J10P4");
+
+		} else {
+			PX4_ERR("FAIL: J10P2-J10P4 --------------------------------------");
+		}
+
+		if (test_pair(J10_PIN3, J10_PIN5_IN)) {
+			PX4_INFO("PASS: J10P3-J10P5");
+
+		} else {
+			PX4_ERR("FAIL: J10P3-J10P5 --------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1012");
+		stm32_configgpio(J1012_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(J1012_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(J1012_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(J1012_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(J1012_PIN2, J1012_PIN4_IN)) {
+			PX4_INFO("PASS: J1012P2-J1120P4");
+
+		} else {
+			PX4_ERR("FAIL: J1012P2-J1012P4 --------------------------------------");
+		}
+
+		if (test_pair(J1012_PIN3, J1012_PIN5_IN)) {
+			PX4_INFO("PASS: J1012P3-J1012P5");
+
+		} else {
+			PX4_ERR("FAIL: J1012P3-J1012P5 --------------------------------------");
+		}
+
+	} else if (hw_type == eM0051) {
+		PX4_INFO(">> Testing M0051 J15");
+		stm32_configgpio(M0051J15_PIN2);    // 2 [out] 4 [in]
+		stm32_configgpio(M0051J15_PIN3);    // 3 [out] 5 [in]
+		stm32_configgpio(M0051J15_PIN4_IN); // 4 [in] 2 [out]
+		stm32_configgpio(M0051J15_PIN5_IN); // 5 [in] 3 [out]
+
+		if (test_pair(M0051J15_PIN2, M0051J15_PIN4_IN)) {
+			PX4_INFO("PASS: J15_P2-J15_P4");
+
+		} else {
+			PX4_ERR("FAIL: J15_P2-JJ15_P4 --------------------------------------");
+		}
+
+		if (test_pair(M0051J15_PIN3, M0051J15_PIN5_IN)) {
+			PX4_INFO("PASS: J15_P3-J15_P5");
+
+		} else {
+			PX4_ERR("FAIL: J15_P3-J15_P5 --------------------------------------");
+		}
+
+	}
+
+	if (hw_type == eM0018) {
+		PX4_INFO(">> Testing J9/J12/J13");
+		stm32_configgpio(J9_PIN2_IN);  // J9-2 [in] J13-5 [out]
+
+		stm32_configgpio(J12_PIN2_IN); // J12-2 [in] J13-3 [out]
+		stm32_configgpio(J12_PIN3_IN); // J12-3 [in] J13-4 [out]
+
+		stm32_configgpio(J13_PIN3);    // J13-3 [out] J12-2 [in]
+		stm32_configgpio(J13_PIN4);    // J13-4 [out] J12-3 [in]
+		stm32_configgpio(J13_PIN5);    // J13-5 [out] J9-2 [in]
+
+		if (test_pair(J13_PIN3, J12_PIN2_IN)) {
+			PX4_INFO("PASS: J13P3-J12P2");
+
+		} else {
+			PX4_ERR("FAIL: J13P3-J12P2 --------------------------------------");
+		}
+
+		if (test_pair(J13_PIN4, J12_PIN3_IN)) {
+			PX4_INFO("PASS: J13P4-J12P3");
+
+		} else {
+			PX4_ERR("FAIL: J13P4-J12P3 --------------------------------------");
+		}
+
+		if (test_pair(J13_PIN5, J9_PIN2_IN)) {
+			PX4_INFO("PASS: J13P5-J9P2");
+
+		} else {
+			PX4_ERR("FAIL: J13P5-J9P2 --------------------------------------");
+		}
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO(">> Testing J1003/J1004/J1011");
+		stm32_configgpio(J1003_PIN2_IN);  // J1003-2 [in] J13-5 [out]
+
+		stm32_configgpio(J1004_PIN2_IN); // J1004-2 [in] J13-3 [out]
+		stm32_configgpio(J1004_PIN3_IN); // J1004-3 [in] J13-4 [out]
+
+		stm32_configgpio(J1011_PIN3);    // J1011-3 [out] J12-2 [in]
+		stm32_configgpio(J1011_PIN4);    // J1011-4 [out] J12-3 [in]
+		stm32_configgpio(J1011_PIN5);    // J1011-5 [out] J9-2 [in]
+
+		if (test_pair(J1011_PIN3, J1004_PIN2_IN)) {
+			PX4_INFO("PASS: J1011P3-J1004P2");
+
+		} else {
+			PX4_ERR("FAIL: J1011P3-J1004P2 --------------------------------------");
+		}
+
+		if (test_pair(J1011_PIN4, J1004_PIN3_IN)) {
+			PX4_INFO("PASS: J1011P4-J1004P3");
+
+		} else {
+			PX4_ERR("FAIL: J1011P4-J1004P3 --------------------------------------");
+		}
+
+		if (test_pair(J1011_PIN5, J1003_PIN2_IN)) {
+			PX4_INFO("PASS: J1011P5-J1011P5");
+
+		} else {
+			PX4_ERR("FAIL: J1011P5-J1011P5 --------------------------------------");
+		}
+
+	} else if (hw_type == eM0051) {
+		PX4_INFO(">> Testing M0051 J14");
+		stm32_configgpio(M0051J14_PIN2);  // J14-2 [out] J14-3 [in]
+		stm32_configgpio(M0051J14_PIN3_IN); // J14-3 [in] J14-2 [out]
+
+		if (test_pair(M0051J14_PIN2, M0051J14_PIN3_IN)) {
+			PX4_INFO("PASS: J14_P2-J14_P3");
+
+		} else {
+			PX4_ERR("FAIL: J14_P2-J14_P3 --------------------------------------");
+		}
+
+	}
+
+	return 0;
+}
+
+int modalai_hw_detect_v1(eHW_TYPE hw_type)
+{
+	int result = 0;
+
+	if (hw_type == eM0018) {
+		PX4_INFO("V106 - Flight Core");
+
+	} else if (hw_type == eM0019) {
+		PX4_INFO("V110 - VOXL-Flight");
+
+	} else if (hw_type == eM0051) {
+		PX4_INFO("V120 - M0051");
+
+	} else {
+		PX4_ERR("Unknown hardware");
+		result = -1;
+	}
+
+	return result;
+}
+
+#endif //CONFIG_ARCH_CHIP_STM32H743ZI

boards/modalai/src/systemcmds/modalai/modalai_fc-v1.h

@@ -0,0 +1,218 @@
+#ifndef MODALAI_FC_V1_H_
+#define MODALAI_FC_V1_H_
+
+typedef enum {
+	eHwUnknown = -1,
+	eHwNone = 0,
+	eM0018,           // Flight Core
+	eM0019,           // VOXL Flight
+	eM0051
+} eHW_TYPE;
+
+#define _MK_GPIO_INPUT(def)  (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP))
+#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR))
+
+
+//
+// Flight Core - J1 - Primary MSS Communications Interface
+// VOXL Flight - NA
+//
+#define J1_PIN2_IN  _MK_GPIO_INPUT(GPIO_UART5_RX)
+#define J1_PIN3     _MK_GPIO_OUTPUT(GPIO_UART5_TX)
+#define J1_PIN4     _MK_GPIO_OUTPUT(GPIO_UART5_RTS)
+#define J1_PIN6_IN  _MK_GPIO_INPUT(GPIO_UART5_CTS)
+
+//
+// STM JTAG Programming Header
+// Flight Core - J2
+// VOXL Flight - J1001
+//
+
+//
+// USB 2.0 Full-Speed Downstream Device Port
+// Flight Core - J
+// VOXL Flight - J1006
+//
+
+//
+// Spare MSS Comms
+// Flight Core - J4
+// VOXL Flight - J1002
+//
+#define J4_PIN2       _MK_GPIO_OUTPUT(GPIO_USART2_RX)
+#define J1002_PIN2    J4_PIN2
+#define J4_PIN3       _MK_GPIO_OUTPUT(GPIO_USART2_TX)
+#define J1002_PIN3    J4_PIN3
+#define J4_PIN4       _MK_GPIO_OUTPUT(GPIO_USART2_RTS)
+#define J1002_PIN4    J4_PIN4
+#define J4_PIN4_IN    _MK_GPIO_INPUT(GPIO_USART2_RTS)
+#define J1002_PIN4_IN J4_PIN4_IN
+#define J4_PIN6       _MK_GPIO_OUTPUT(GPIO_USART2_CTS)
+#define J4_PIN6_IN    _MK_GPIO_INPUT(GPIO_USART2_CTS)
+#define J1002_PIN6_IN J4_PIN6_IN
+#define J4_PIN7       _MK_GPIO_OUTPUT(GPIO_VOXL_STATUS_OUT)
+#define J4_PIN7_IN    _MK_GPIO_INPUT(GPIO_VOXL_STATUS_OUT)
+#define J4_PIN8       _MK_GPIO_OUTPUT(GPIO_VOXL_STATUS_IN)
+#define J4_PIN8_IN    _MK_GPIO_INPUT(GPIO_VOXL_STATUS_IN)
+
+//
+// TELEMETRY CONNECTOR
+// Flight Core - J5
+// VOXL Flight - J1010
+//
+#define J5_PIN2       _MK_GPIO_OUTPUT(GPIO_UART7_TX)
+#define J1010_PIN2    J5_PIN2
+#define J5_PIN3       _MK_GPIO_OUTPUT(GPIO_UART7_RX)
+#define J1010_PIN3    J5_PIN3
+#define J5_PIN4       _MK_GPIO_OUTPUT(GPIO_UART7_CTS)
+#define J1010_PIN4    J5_PIN4
+#define J5_PIN4_IN    _MK_GPIO_INPUT(GPIO_UART7_CTS)
+#define J1010_PIN4_IN J5_PIN4_IN
+#define J5_PIN5       _MK_GPIO_OUTPUT(GPIO_UART7_RTS)
+#define J1010_PIN5    J5_PIN5
+#define J5_PIN5_IN    _MK_GPIO_INPUT(GPIO_UART7_RTS)
+#define J1010_PIN5_IN J5_PIN5_IN
+
+//
+// EXPANSION CONNECTOR
+// Flight Core - J6
+// VOXL Flight - J1009
+//
+#define J6_PIN2        _MK_GPIO_OUTPUT(GPIO_UART4_TX_5)
+#define J1009_PIN2     J6_PIN2
+#define J6_PIN3        _MK_GPIO_OUTPUT(GPIO_UART4_RX_5)
+#define J1009_PIN3     J6_PIN3
+#define J6_PIN4        _MK_GPIO_OUTPUT(GPIO_I2C3_SCL_2)
+#define J1009_PIN4     J6_PIN4
+#define J6_PIN4_IN     _MK_GPIO_INPUT(GPIO_I2C3_SCL_2)
+#define J1009_PIN4_IN  J6_PIN4_IN
+#define J6_PIN5        _MK_GPIO_OUTPUT(GPIO_I2C3_SDA_2)
+#define J1009_PIN5     J6_PIN5
+#define J6_PIN5_IN     _MK_GPIO_INPUT(GPIO_I2C3_SDA_2)
+#define J1009_PIN5_IN  J6_PIN5_IN
+
+//
+// Flight Core - J7 - PWM Output Connector
+// VOXL Flight - J1007
+// M0051       - J13
+//
+#define J7_PIN2          _MK_GPIO_OUTPUT(GPIO_TIM1_CH4OUT_2)
+#define J1007_PIN2       J7_PIN2
+#define M0051J13_PIN2    J7_PIN2
+#define J7_PIN3          _MK_GPIO_OUTPUT(GPIO_TIM1_CH3OUT_1)
+#define J1007_PIN3       J7_PIN3
+#define M0051J13_PIN3    J7_PIN3
+#define J7_PIN4          _MK_GPIO_OUTPUT(GPIO_TIM1_CH2OUT_2)
+#define J1007_PIN4       J7_PIN4
+#define M0051J13_PIN4    J7_PIN4
+#define J7_PIN5          _MK_GPIO_OUTPUT(GPIO_TIM1_CH1OUT_1)
+#define J1007_PIN5       J7_PIN5
+#define M0051J13_PIN5    J7_PIN5
+#define J7_PIN6          _MK_GPIO_OUTPUT(GPIO_TIM4_CH2OUT_2)
+#define J1007_PIN6       J7_PIN6
+#define M0051J13_PIN6    J7_PIN6
+#define J7_PIN6_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH2OUT_2)
+#define J1007_PIN6_IN    J7_PIN6_IN
+#define M0051J13_PIN6_IN J7_PIN6_IN
+#define J7_PIN7          _MK_GPIO_OUTPUT(GPIO_TIM4_CH3OUT_2)
+#define J1007_PIN7       J7_PIN7
+#define M0051J13_PIN7    J7_PIN7
+#define J7_PIN7_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH3OUT_2)
+#define J1007_PIN7_IN    J7_PIN7_IN
+#define M0051J13_PIN7_IN J7_PIN7_IN
+#define J7_PIN8          _MK_GPIO_OUTPUT(GPIO_TIM4_CH1OUT_2)
+#define J1007_PIN8       J7_PIN8
+#define M0051J13_PIN8    J7_PIN8
+#define J7_PIN8_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH1OUT_2)
+#define J1007_PIN8_IN    J7_PIN8_IN
+#define M0051J13_PIN8_IN J7_PIN8_IN
+#define J7_PIN9          _MK_GPIO_OUTPUT(GPIO_TIM4_CH4OUT_2)
+#define J1007_PIN9       J7_PIN9
+#define M0051J13_PIN9    J7_PIN9
+#define J7_PIN9_IN       _MK_GPIO_INPUT(GPIO_TIM4_CH4OUT_2)
+#define J1007_PIN9_IN    J7_PIN9_IN
+#define M0051J13_PIN9_IN J7_PIN9_IN
+
+//
+// CAN 1 Peripheral Connector
+// Flight Core - J8
+// VOXL Flight - J1008
+//
+//#define J8_PIN2     _MK_GPIO_OUTPUT()
+//#define J8_PIN3     _MK_GPIO_OUTPUT()
+
+// PPM (RC) IN
+// Flight Core - J9
+// VOXL Flight - J1003
+//
+#define J9_PIN2_IN    _MK_GPIO_INPUT(GPIO_TIM8_CH1IN_2)
+#define J1003_PIN2_IN J9_PIN2_IN
+
+//
+// GPS CONNECTOR
+// Flight Core - J10
+// VOXL Flight - J1012
+// M0051       - J15
+//
+#define J10_PIN2         _MK_GPIO_OUTPUT(GPIO_USART1_TX_3)
+#define J1012_PIN2       J10_PIN2
+#define M0051J15_PIN2    J10_PIN2
+#define J10_PIN3         _MK_GPIO_OUTPUT(GPIO_USART1_RX_3)
+#define J1012_PIN3       J10_PIN3
+#define M0051J15_PIN3    J10_PIN3
+#define J10_PIN4         _MK_GPIO_OUTPUT(GPIO_I2C1_SCL_2)
+#define J1012_PIN4       J10_PIN4
+#define M0051J15_PIN4    J10_PIN4
+#define J10_PIN4_IN      _MK_GPIO_INPUT(GPIO_I2C1_SCL_2)
+#define J1012_PIN4_IN    J10_PIN4_IN
+#define M0051J15_PIN4_IN J10_PIN4_IN
+#define J10_PIN5         _MK_GPIO_OUTPUT(GPIO_I2C1_SDA_1)
+#define J1012_PIN5       J10_PIN5
+#define M0051J15_PIN5    J10_PIN5
+#define J10_PIN5_IN      _MK_GPIO_INPUT(GPIO_I2C1_SDA_1)
+#define J1012_PIN5_IN    J10_PIN5_IN
+#define M0051J15_PIN5_IN J10_PIN5_IN
+
+//
+// Spektrum RC Input Connector
+// Flight Core - J12
+// VOXL Flight - J1004
+// M0051       - J14
+//
+#define J12_PIN1      GPIO_VDD_3V3_SPEKTRUM_POWER_EN
+#define J1004_PIN1       J12_PIN1
+#define M0051J14_PIN1    J12_PIN1
+#define J12_PIN2         _MK_GPIO_OUTPUT(GPIO_USART6_TX_1)
+#define J1004_PIN2       J12_PIN2
+#define M0051J14_PIN2    J12_PIN2
+#define J12_PIN2_IN      _MK_GPIO_INPUT(GPIO_USART6_TX_1)
+#define J1004_PIN2_IN    J12_PIN2_IN
+#define M0051J14_PIN2_IN J12_PIN2_IN
+#define J12_PIN3         _MK_GPIO_OUTPUT(GPIO_USART6_RX_1)
+#define J1004_PIN3       J12_PIN3
+#define M0051J14_PIN3    J12_PIN3
+#define J12_PIN3_IN      _MK_GPIO_INPUT(GPIO_USART6_RX_1)
+#define J1004_PIN3_IN    J12_PIN3_IN
+#define M0051J14_PIN3_IN J12_PIN3_IN
+
+//
+// I2C Display / Spare Sensor Connector
+// Flight Core - J13
+// VOXL Flight - J1011
+//
+#define J13_PIN3   _MK_GPIO_OUTPUT(GPIO_I2C2_SDA_2)
+#define J1011_PIN3 J13_PIN3
+#define J13_PIN4   _MK_GPIO_OUTPUT(GPIO_I2C2_SCL_2)
+#define J1011_PIN4 J13_PIN4
+#define J13_PIN5   _MK_GPIO_OUTPUT(GPIO_PF3_EVENTOUT)
+#define J1011_PIN5 J13_PIN5
+
+
+void 	modalai_print_usage_v1(void);
+void 	modalai_print_usage_con_gpio_test_v1(void);
+int  	modalai_con_gpio_test_v1(uint8_t con, uint8_t pin, bool state);
+int  	modalai_led_test_v1(void);
+int  	modalai_buzz_test_v1(eHW_TYPE type);
+int	modalai_hw_detect_v1(eHW_TYPE type);
+
+#endif //MODALAI_FC_V1_H_

boards/modalai/src/systemcmds/modalai/modalai_fc-v2.c

@@ -0,0 +1,437 @@
+#include <px4_platform_common/module.h>
+
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+// v2
+#ifdef CONFIG_ARCH_CHIP_STM32H753II		// chip on M0087
+
+
+#include "modalai_fc-v2.h"
+
+void modalai_print_usage_v2(void)
+{
+	PRINT_MODULE_DESCRIPTION("ModalAI Test utility\n");
+
+	PRINT_MODULE_USAGE_NAME_SIMPLE("modalai", "command");
+
+	PRINT_MODULE_USAGE_COMMAND_DESCR("led", "LED Test");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("con", "Connector Output Test (as GPIO)");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("buzz", "Automated buzz out test");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("detect", "Detect board type");
+	return;
+}
+void modalai_print_usage_con_gpio_test_v2(void)
+{
+	return;
+}
+int modalai_con_gpio_test_v2(uint8_t con, uint8_t pin, bool state)
+{
+	return 0;
+}
+int modalai_led_test_v2(void)
+{
+	PX4_INFO("Running led test");
+
+	stm32_configgpio(GPIO_nLED_RED);
+	stm32_configgpio(GPIO_nLED_GREEN);
+	stm32_configgpio(GPIO_nLED_BLUE);
+
+	int i = 0;
+
+	for (i = 0; i < 3; i++) {
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_RED, false);
+
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_RED, true);
+
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_GREEN, false);
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_GREEN, true);
+
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_BLUE, false);
+		usleep(1000 * 100);
+		stm32_gpiowrite(GPIO_nLED_BLUE, true);
+	}
+
+	return 0;
+}
+
+bool test_pair(uint32_t output_pin, uint32_t input_pin)
+{
+
+	bool state = false;
+
+	stm32_gpiowrite(output_pin, true);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != true) {
+		return false;
+	}
+
+	usleep(1000 * 10);
+
+	stm32_gpiowrite(output_pin, false);
+	usleep(1000 * 10);
+	state = stm32_gpioread(input_pin);
+
+	if (state != false) {
+		return false;
+	}
+
+	return true;
+}
+
+int modalai_buzz_test_v2(eHW_TYPE hw_type)
+{
+	PX4_INFO("test: buzz");
+	usleep(1000 * 100 * 10);
+
+	if (hw_type == eM0079) {
+		PX4_INFO("Using M0079 config");
+
+	} else if (hw_type == eM0087) {
+		PX4_INFO("Using M0087 config");
+
+	} else {
+		return -1;
+
+	}
+
+	if (hw_type == eM0079) {
+		//
+		//
+		//
+		PX4_INFO(">> Testing J1");
+		stm32_configgpio(M0079_J1_PIN_2_OUT); // 2-3
+		stm32_configgpio(M0079_J1_PIN_3_IN);  // 3-2
+		stm32_configgpio(M0079_J1_PIN_4_OUT); // 4-5
+		stm32_configgpio(M0079_J1_PIN_5_IN);  // 5-4
+
+		if (test_pair(M0079_J1_PIN_2_OUT, M0079_J1_PIN_3_IN)) {
+			PX4_INFO("PASS: M0079_J1_PIN_2_OUT M0079_J1_PIN_3_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J1_PIN_2_OUT M0079_J1_PIN_3_IN");
+		}
+
+		if (test_pair(M0079_J1_PIN_4_OUT, M0079_J1_PIN_5_IN)) {
+			PX4_INFO("PASS: M0079_J1_PIN_4_OUT M0079_J1_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J1_PIN_4_OUT M0079_J1_PIN_5_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J5");
+		stm32_configgpio(M0079_J5_PIN_2_OUT); // 2-4
+		stm32_configgpio(M0079_J5_PIN_3_OUT); // 3-5
+		stm32_configgpio(M0079_J5_PIN_4_IN);  // 4-2
+		stm32_configgpio(M0079_J5_PIN_5_IN);  // 5-3
+
+		if (test_pair(M0079_J5_PIN_2_OUT, M0079_J5_PIN_4_IN)) {
+			PX4_INFO("PASS: M0079_J5_PIN_2_OUT M0079_J5_PIN_4_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J5_PIN_2_OUT M0079_J5_PIN_4_IN");
+		}
+
+		if (test_pair(M0079_J5_PIN_3_OUT, M0079_J5_PIN_5_IN)) {
+			PX4_INFO("PASS: M0079_J5_PIN_3_OUT M0079_J5_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J5_PIN_3_OUT M0079_J5_PIN_5_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J7");
+		stm32_configgpio(M0079_J7_PIN_2_OUT); // 2-6
+		stm32_configgpio(M0079_J7_PIN_3_OUT); // 3-7
+		stm32_configgpio(M0079_J7_PIN_4_OUT); // 4-8
+		stm32_configgpio(M0079_J7_PIN_5_OUT); // 5-9
+		stm32_configgpio(M0079_J7_PIN_6_IN); // 6-2
+		stm32_configgpio(M0079_J7_PIN_7_IN); // 7-3
+		stm32_configgpio(M0079_J7_PIN_8_IN); // 8-4
+		stm32_configgpio(M0079_J7_PIN_9_IN); // 9-5
+
+		if (test_pair(M0079_J7_PIN_2_OUT, M0079_J7_PIN_6_IN)) {
+			PX4_INFO("PASS: M0079_J7_PIN_2_OUT M0079_J7_PIN_6_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J7_PIN_2_OUT M0079_J7_PIN_6_IN");
+		}
+
+		if (test_pair(M0079_J7_PIN_3_OUT, M0079_J7_PIN_7_IN)) {
+			PX4_INFO("PASS: M0079_J7_PIN_3_OUT M0079_J7_PIN_7_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J7_PIN_3_OUT M0079_J7_PIN_7_IN");
+		}
+
+		if (test_pair(M0079_J7_PIN_4_OUT, M0079_J7_PIN_8_IN)) {
+			PX4_INFO("PASS: M0079_J7_PIN_4_OUT M0079_J7_PIN_8_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J7_PIN_4_OUT M0079_J7_PIN_8_IN");
+		}
+
+		if (test_pair(M0079_J7_PIN_5_OUT, M0079_J7_PIN_9_IN)) {
+			PX4_INFO("PASS: M0079_J7_PIN_5_OUT M0079_J7_PIN_9_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J7_PIN_5_OUT M0079_J7_PIN_9_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J10");
+		stm32_configgpio(M0079_J10_PIN_2_OUT); // 2-4
+		stm32_configgpio(M0079_J10_PIN_3_OUT); // 3-5
+		stm32_configgpio(M0079_J10_PIN_4_IN);  // 4-2
+		stm32_configgpio(M0079_J10_PIN_5_IN);  // 5-3
+
+		if (test_pair(M0079_J10_PIN_2_OUT, M0079_J10_PIN_4_IN)) {
+			PX4_INFO("PASS: M0079_J10_PIN_2_OUT M0079_J10_PIN_4_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J10_PIN_2_OUT M0079_J10_PIN_4_IN");
+		}
+
+		if (test_pair(M0079_J10_PIN_3_OUT, M0079_J10_PIN_5_IN)) {
+			PX4_INFO("PASS: M0079_J10_PIN_3_OUT M0079_J10_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J10_PIN_3_OUT M0079_J10_PIN_5_IN");
+		}
+
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J13");
+		stm32_configgpio(M0079_J12_PIN_2_OUT); // 2-3
+		stm32_configgpio(M0079_J12_PIN_3_IN);  // 3-2
+
+		if (test_pair(M0079_J12_PIN_2_OUT, M0079_J12_PIN_3_IN)) {
+			PX4_INFO("PASS: M0079_J12_PIN_2 M0079_J12_PIN_3");
+
+		} else {
+			PX4_ERR("FAIL: M0079_J12_PIN_2 M0079_J12_PIN_3");
+		}
+
+
+	} else if (hw_type == eM0087) {
+		//
+		//
+		//
+		PX4_INFO(">> Testing J1");
+		stm32_configgpio(M0087_J1_PIN_2_IN);    // 2-4
+		stm32_configgpio(M0087_J1_PIN_3_OUT);   // 3-5
+		stm32_configgpio(M0087_J1_PIN_4_OUT);   // 4-2
+		stm32_configgpio(M0087_J1_PIN_5_IN);    // 5-3
+
+		if (test_pair(M0087_J1_PIN_4_OUT, M0087_J1_PIN_2_IN)) {
+			PX4_INFO("PASS: M0087_J1_PIN_4_OUT M0087_J1_PIN_2_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J1_PIN_4_OUT M0087_J1_PIN_2_IN");
+		}
+
+		if (test_pair(M0087_J1_PIN_3_OUT, M0087_J1_PIN_5_IN)) {
+			PX4_INFO("PASS: M0087_J1_PIN_3_OUT M0087_J1_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J1_PIN_3_OUT M0087_J1_PIN_5_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J5");
+		stm32_configgpio(M0087_J5_PIN_2_OUT); // 2-4
+		stm32_configgpio(M0087_J5_PIN_3_OUT); // 3-5
+		stm32_configgpio(M0087_J5_PIN_4_IN);  // 4-2
+		stm32_configgpio(M0087_J5_PIN_5_IN);  // 5-3
+
+		if (test_pair(M0087_J5_PIN_2_OUT, M0087_J5_PIN_4_IN)) {
+			PX4_INFO("PASS: M0087_J5_PIN_2_OUT M0087_J5_PIN_4_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J5_PIN_2_OUT M0087_J5_PIN_4_IN");
+		}
+
+		if (test_pair(M0087_J5_PIN_3_OUT, M0087_J5_PIN_5_IN)) {
+			PX4_INFO("PASS: M0087_J5_PIN_3_OUT M0087_J5_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J5_PIN_3_OUT M0087_J5_PIN_5_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J7");
+		stm32_configgpio(M0087_J7_PIN_2_OUT); // 2-6
+		stm32_configgpio(M0087_J7_PIN_3_OUT); // 3-7
+		stm32_configgpio(M0087_J7_PIN_4_OUT); // 4-8
+		stm32_configgpio(M0087_J7_PIN_5_OUT); // 5-9
+		stm32_configgpio(M0087_J7_PIN_6_IN); // 6-2
+		stm32_configgpio(M0087_J7_PIN_7_IN); // 7-3
+		stm32_configgpio(M0087_J7_PIN_8_IN); // 8-4
+		stm32_configgpio(M0087_J7_PIN_9_IN); // 9-5
+
+		if (test_pair(M0087_J7_PIN_2_OUT, M0087_J7_PIN_6_IN)) {
+			PX4_INFO("PASS: M0087_J7_PIN_2_OUT M0087_J7_PIN_6_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J7_PIN_2_OUT M0087_J7_PIN_6_IN");
+		}
+
+		if (test_pair(M0087_J7_PIN_3_OUT, M0087_J7_PIN_7_IN)) {
+			PX4_INFO("PASS: M0087_J7_PIN_3_OUT M0087_J7_PIN_7_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J7_PIN_3_OUT M0087_J7_PIN_7_IN");
+		}
+
+		if (test_pair(M0087_J7_PIN_4_OUT, M0087_J7_PIN_8_IN)) {
+			PX4_INFO("PASS: M0087_J7_PIN_4_OUT M0087_J7_PIN_8_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J7_PIN_4_OUT M0087_J7_PIN_8_IN");
+		}
+
+		if (test_pair(M0087_J7_PIN_5_OUT, M0087_J7_PIN_9_IN)) {
+			PX4_INFO("PASS: M0087_J7_PIN_5_OUT M0087_J7_PIN_9_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J7_PIN_5_OUT M0087_J7_PIN_9_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J10");
+		stm32_configgpio(M0087_J10_PIN_2_OUT); // 2-4
+		stm32_configgpio(M0087_J10_PIN_3_OUT); // 3-5
+		stm32_configgpio(M0087_J10_PIN_4_IN);  // 4-2
+		stm32_configgpio(M0087_J10_PIN_5_IN);  // 5-3
+
+		if (test_pair(M0087_J10_PIN_2_OUT, M0087_J10_PIN_4_IN)) {
+			PX4_INFO("PASS: M0087_J10_PIN_2_OUT M0087_J10_PIN_4_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J10_PIN_2_OUT M0087_J10_PIN_4_IN");
+		}
+
+		if (test_pair(M0087_J10_PIN_3_OUT, M0087_J10_PIN_5_IN)) {
+			PX4_INFO("PASS: M0087_J10_PIN_3_OUT M0087_J10_PIN_5_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J10_PIN_3_OUT M0087_J10_PIN_5_IN");
+		}
+
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J12");
+		stm32_configgpio(M0087_J12_PIN_2_OUT); // 2-3
+		stm32_configgpio(M0087_J12_PIN_3_IN);  // 3-2
+
+		if (test_pair(M0087_J12_PIN_2_OUT, M0087_J12_PIN_3_IN)) {
+			PX4_INFO("PASS: M0087_J12_PIN_2_OUT M0087_J12_PIN_3_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J12_PIN_2_OUT M0087_J12_PIN_3_IN");
+		}
+
+		//
+		//
+		//
+		PX4_INFO(">> Testing J14");
+		stm32_configgpio(M0087_J14_PIN_2_OUT);
+		stm32_configgpio(M0087_J14_PIN_3_OUT);
+		stm32_configgpio(M0087_J14_PIN_4_OUT);
+		stm32_configgpio(M0087_J14_PIN_5_OUT);
+		stm32_configgpio(M0087_J14_PIN_6_OUT);
+		stm32_configgpio(M0087_J14_PIN_7_IN);
+		stm32_configgpio(M0087_J14_PIN_8_IN);
+		stm32_configgpio(M0087_J14_PIN_9_IN);
+		stm32_configgpio(M0087_J14_PIN_10_IN);
+		stm32_configgpio(M0087_J14_PIN_11_IN);
+
+		if (test_pair(M0087_J14_PIN_2_OUT, M0087_J14_PIN_7_IN)) {
+			PX4_INFO("PASS: M0087_J14_PIN_2_OUT M0087_J14_PIN_7_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J14_PIN_2_OUT M0087_J14_PIN_7_IN");
+		}
+
+		if (test_pair(M0087_J14_PIN_3_OUT, M0087_J14_PIN_8_IN)) {
+			PX4_INFO("PASS: M0087_J14_PIN_3_OUT M0087_J14_PIN_8_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J14_PIN_3_OUT M0087_J14_PIN_8_IN");
+		}
+
+		if (test_pair(M0087_J14_PIN_4_OUT, M0087_J14_PIN_9_IN)) {
+			PX4_INFO("PASS: M0087_J14_PIN_4_OUT M0087_J14_PIN_9_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J14_PIN_4_OUT M0087_J14_PIN_9_IN");
+		}
+
+		if (test_pair(M0087_J14_PIN_5_OUT, M0087_J14_PIN_10_IN)) {
+			PX4_INFO("PASS: M0087_J14_PIN_5_OUT M0087_J14_PIN_10_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J14_PIN_5_OUT M0087_J14_PIN_10_IN");
+		}
+
+		if (test_pair(M0087_J14_PIN_6_OUT, M0087_J14_PIN_11_IN)) {
+			PX4_INFO("PASS: M0087_J14_PIN_6_OUT M0087_J14_PIN_11_IN");
+
+		} else {
+			PX4_ERR("FAIL: M0087_J14_PIN_6_OUT M0087_J14_PIN_11_IN");
+		}
+
+	}
+
+	return 0;
+}
+
+int modalai_hw_detect_v2(eHW_TYPE hw_type)
+{
+	int result = -1;
+
+	if (hw_type == eM0079) {
+		PX4_INFO("V230 - M0079");
+		result = 0;
+
+	} else if (hw_type == eM0087) {
+		PX4_INFO("V230 - M0087");
+		result = 0;
+
+	} else {
+		PX4_ERR("Unknown hardware");
+	}
+
+	return result;
+}
+
+#endif //CONFIG_ARCH_CHIP_STM32H753II

boards/modalai/src/systemcmds/modalai/modalai_fc-v2.h

@@ -0,0 +1,183 @@
+#ifndef MODALAI_FC_V2_H_
+#define MODALAI_FC_V2_H_
+
+typedef enum {
+	eHwUnknown = -1,
+	eHwNone = 0,
+	eM0079,		//FCv2
+	eM0087		//FCv2 Pro
+} eHW_TYPE;
+
+#define _MK_GPIO_INPUT(def)  (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP))
+#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR))
+
+/*	M0079 Pins	*/
+
+//
+// TELEM1
+//   M0079- J1
+//             PF6   PIN2 - out
+//             PE8   PIN3 - in
+//	       PF8   PIN4 - out
+//	       PE10  PIN4 - in
+//
+#define M0079_J1_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN6)
+#define M0079_J1_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN8)
+#define M0079_J1_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN8)
+#define M0079_J1_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN10)
+
+//
+// TELEM2
+//   M0079- J5
+//             PC12 PIN2 - out
+//             PD2  PIN3 - out
+//	       PC9  PIN4 - in
+//	       PC8  PIN4 - in
+//
+#define M0079_J5_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN12)
+#define M0079_J5_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTD|GPIO_PIN2)
+#define M0079_J5_PIN_4_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN9)
+#define M0079_J5_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN8)
+
+//
+// PWM Output
+//   M0079- J7
+//             PI0  PIN2 - out
+//             PH12 PIN3 - out
+//             PH11 PIN4 - out
+//             PH10 PIN5 - out
+//
+//             PD13 PIN6 - in
+//             PD14 PIN7 - in
+//             PH6  PIN8 - in
+//             PH9  PIN9 - in
+//
+#define M0079_J7_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN0)
+#define M0079_J7_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN12)
+#define M0079_J7_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN11)
+#define M0079_J7_PIN_5_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN10)
+#define M0079_J7_PIN_6_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN13)
+#define M0079_J7_PIN_7_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN14)
+#define M0079_J7_PIN_8_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN6)
+#define M0079_J7_PIN_9_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN9)
+
+//
+// GPS/Mag
+//   M0079- J10
+//             PB6 PIN2 - out
+//             PB7 PIN3 - out
+//	       PB8 PIN4 - in
+//	       PB9 PIN4 - in
+//
+#define M0079_J10_PIN_2_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN6)
+#define M0079_J10_PIN_3_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN7)
+#define M0079_J10_PIN_4_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN8)
+#define M0079_J10_PIN_5_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN9)
+
+//
+// Spektrum RC Input Connector
+//   M0079- J12
+//             PC6 PIN2 - out
+//             PC7 PIN3 - in
+//
+#define M0079_J12_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN6)
+#define M0079_J12_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN7)
+
+
+/*	M0087 Pins 	*/
+
+//
+// TELEM1
+//   M0087- J1
+//             PF6   PIN2 - in
+//             PE8   PIN3 - out
+//	       PF8   PIN4 - out
+//	       PE10  PIN5 - in
+//
+#define M0087_J1_PIN_2_IN             	_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN6)
+#define M0087_J1_PIN_3_OUT              _MK_GPIO_OUTPUT(GPIO_PORTE|GPIO_PIN8)
+#define M0087_J1_PIN_4_OUT             	_MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN8)
+#define M0087_J1_PIN_5_IN              	_MK_GPIO_INPUT(GPIO_PORTE|GPIO_PIN10)
+
+//
+// TELEM2
+//   M0087- J5
+//             PC12 PIN2 - out
+//             PD2  PIN3 - out
+//	       PC9  PIN4 - in
+//	       PC8  PIN5 - in
+//
+#define M0087_J5_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN12)
+#define M0087_J5_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTD|GPIO_PIN2)
+#define M0087_J5_PIN_4_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN9)
+#define M0087_J5_PIN_5_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN8)
+
+//
+// PWM Output
+//   M0087- J7
+//             PI0  PIN2 - out
+//             PH12 PIN3 - out
+//             PH11 PIN4 - out
+//             PH10 PIN5 - out
+//
+//             PD13 PIN6 - in
+//             PD14 PIN7 - in
+//             PH6  PIN8 - in
+//             PH9  PIN9 - in
+//
+#define M0087_J7_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN0)
+#define M0087_J7_PIN_3_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN12)
+#define M0087_J7_PIN_4_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN11)
+#define M0087_J7_PIN_5_OUT             _MK_GPIO_OUTPUT(GPIO_PORTH|GPIO_PIN10)
+#define M0087_J7_PIN_6_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN13)
+#define M0087_J7_PIN_7_IN              _MK_GPIO_INPUT(GPIO_PORTD|GPIO_PIN14)
+#define M0087_J7_PIN_8_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN6)
+#define M0087_J7_PIN_9_IN              _MK_GPIO_INPUT(GPIO_PORTH|GPIO_PIN9)
+
+//
+// GPS/Mag
+//   M0087- J10
+//             PB6 PIN2 - out
+//             PB7 PIN3 - out
+//	       PB8 PIN4 - in
+//	       PB9 PIN5 - in
+//
+#define M0087_J10_PIN_2_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN6)
+#define M0087_J10_PIN_3_OUT            _MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN7)
+#define M0087_J10_PIN_4_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN8)
+#define M0087_J10_PIN_5_IN             _MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN9)
+
+//
+// Spektrum RC Input Connector
+//   M0087- J12
+//             PC6 PIN2 - out
+//             PC7 PIN3 - in
+//
+#define M0087_J12_PIN_2_OUT             _MK_GPIO_OUTPUT(GPIO_PORTC|GPIO_PIN6)
+#define M0087_J12_PIN_3_IN              _MK_GPIO_INPUT(GPIO_PORTC|GPIO_PIN7)
+
+//
+// AVIATOR SPI/I2C/ADC EXPANSION CONNECTOR
+//   M0087- J14
+//             PC6 PIN2 - out
+//             PC7 PIN3 - in
+//
+#define  M0087_J14_PIN_2_OUT				_MK_GPIO_OUTPUT(GPIO_PORTA|GPIO_PIN6)
+#define  M0087_J14_PIN_3_OUT				_MK_GPIO_OUTPUT(GPIO_PORTG|GPIO_PIN14)
+#define  M0087_J14_PIN_4_OUT				_MK_GPIO_OUTPUT(GPIO_PORTB|GPIO_PIN3)
+#define  M0087_J14_PIN_5_OUT				_MK_GPIO_OUTPUT(GPIO_PORTI|GPIO_PIN10)
+#define  M0087_J14_PIN_6_OUT				_MK_GPIO_OUTPUT(GPIO_PORTF|GPIO_PIN0)
+#define  M0087_J14_PIN_7_IN				_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN1)
+#define  M0087_J14_PIN_8_IN				_MK_GPIO_INPUT(GPIO_PORTF|GPIO_PIN12)
+#define  M0087_J14_PIN_9_IN				_MK_GPIO_INPUT(GPIO_PORTB|GPIO_PIN0)
+#define  M0087_J14_PIN_10_IN				_MK_GPIO_INPUT(GPIO_PORTA|GPIO_PIN0)
+#define  M0087_J14_PIN_11_IN				_MK_GPIO_INPUT(GPIO_PORTA|GPIO_PIN4)
+
+void	modalai_print_usage_v2(void);
+void	modalai_print_usage_con_gpio_test_v2(void);
+int 	modalai_con_gpio_test_v2(uint8_t con, uint8_t pin, bool state);
+int 	modalai_led_test_v2(void);
+int 	modalai_buzz_test_v2(eHW_TYPE type);
+int 	modalai_hw_detect_v2(eHW_TYPE type);
+
+#endif //MODALAI_FC_V2_H_

boards/modalai/voxl2-io/default.px4board

@@ -1,5 +0,0 @@
-CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
-CONFIG_BOARD_ARCHITECTURE="cortex-m3"
-CONFIG_BOARD_ROMFSROOT=""
-CONFIG_BOARD_CONSTRAINED_FLASH=y
-CONFIG_MODULES_PX4IOFIRMWARE=y

boards/modalai/voxl2-io/firmware.prototype

@@ -1,13 +0,0 @@
-{
-    "board_id": 41777,
-    "magic": "PX4FWv2",
-    "description": "Firmware for the voxl2-io board",
-    "image": "",
-    "build_time": 0,
-    "summary": "voxl2io",
-    "version": "2.0",
-    "image_size": 0,
-    "image_maxsize": 61440,
-    "git_identity": "",
-    "board_revision": 0
-}

boards/modalai/voxl2-io/nuttx-config/include/board.h

@@ -1,141 +0,0 @@
-/************************************************************************************
- * nuttx-configs/px4io/include/board.h
- * include/arch/board/board.h
- *
- *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
- *   Copyright (C) 2012 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 NuttX 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.
- *
- ************************************************************************************/
-
-#ifndef __ARCH_BOARD_BOARD_H
-#define __ARCH_BOARD_BOARD_H
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-#ifndef __ASSEMBLY__
-# include <stdint.h>
-#endif
-#include <stm32.h>
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/* Clocking *************************************************************************/
-
-/* On-board crystal frequency is 24MHz (HSE) */
-
-#define STM32_BOARD_XTAL        16000000ul
-
-/* Use the HSE output as the system clock */
-
-#define STM32_SYSCLK_SW         RCC_CFGR_SW_HSE
-#define STM32_SYSCLK_SWS        RCC_CFGR_SWS_HSE
-#define STM32_SYSCLK_FREQUENCY  STM32_BOARD_XTAL
-
-/* AHB clock (HCLK) is SYSCLK (24MHz) */
-
-#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK
-#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
-#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY    /* same as above, to satisfy compiler */
-
-/* APB2 clock (PCLK2) is HCLK (24MHz) */
-
-#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK
-#define STM32_PCLK2_FREQUENCY   STM32_HCLK_FREQUENCY
-#define STM32_APB2_CLKIN        (STM32_PCLK2_FREQUENCY)   /* Timers 2-4 */
-
-/* APB2 timer 1 will receive PCLK2. */
-
-#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
-#define STM32_APB2_TIM15_CLKIN  (STM32_PCLK2_FREQUENCY)
-#define STM32_APB2_TIM16_CLKIN  (STM32_PCLK2_FREQUENCY)
-#define STM32_APB2_TIM17_CLKIN  (STM32_PCLK2_FREQUENCY)
-
-/* APB1 clock (PCLK1) is HCLK (24MHz) */
-
-#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLK
-#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY)
-
-/* All timers run off PCLK */
-
-
-#define STM32_APB1_TIM2_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM3_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM4_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM5_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM6_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM7_CLKIN   (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM12_CLKIN  (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM13_CLKIN  (STM32_PCLK1_FREQUENCY)
-#define STM32_APB1_TIM14_CLKIN  (STM32_PCLK1_FREQUENCY)
-
-/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
- * otherwise frequency is 2xAPBx.
- * Note: TIM1, 15-17 are on APB2, others on APB1
- */
-
-#define BOARD_TIM1_FREQUENCY    STM32_APB2_TIM1_CLKIN
-#define BOARD_TIM2_FREQUENCY    STM32_APB1_TIM2_CLKIN
-#define BOARD_TIM3_FREQUENCY    STM32_APB1_TIM3_CLKIN
-#define BOARD_TIM4_FREQUENCY    STM32_APB1_TIM4_CLKIN
-#define BOARD_TIM5_FREQUENCY    STM32_APB1_TIM5_CLKIN
-#define BOARD_TIM6_FREQUENCY    STM32_APB1_TIM6_CLKIN
-#define BOARD_TIM7_FREQUENCY    STM32_APB1_TIM7_CLKIN
-#define BOARD_TIM12_FREQUENCY   STM32_APB1_TIM12_CLKIN
-#define BOARD_TIM13_FREQUENCY   STM32_APB1_TIM13_CLKIN
-#define BOARD_TIM14_FREQUENCY   STM32_APB1_TIM14_CLKIN
-#define BOARD_TIM15_FREQUENCY   STM32_APB2_TIM15_CLKIN
-#define BOARD_TIM16_FREQUENCY   STM32_APB2_TIM16_CLKIN
-#define BOARD_TIM17_FREQUENCY   STM32_APB2_TIM17_CLKIN
-
-
-/*
- * Some of the USART pins are not available; override the GPIO
- * definitions with an invalid pin configuration.
- */
-#undef GPIO_USART2_CTS
-#define GPIO_USART2_CTS	0xffffffff
-#undef GPIO_USART2_RTS
-#define GPIO_USART2_RTS	0xffffffff
-#undef GPIO_USART2_CK
-#define GPIO_USART2_CK 	0xffffffff
-#undef GPIO_USART3_CK
-#define GPIO_USART3_CK 	0xffffffff
-#undef GPIO_USART3_CTS
-#define GPIO_USART3_CTS	0xffffffff
-#undef GPIO_USART3_RTS
-#define GPIO_USART3_RTS	0xffffffff
-
-#endif  /* __ARCH_BOARD_BOARD_H */

boards/modalai/voxl2-io/nuttx-config/nsh/defconfig

@@ -1,61 +0,0 @@
-#
-# 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_DEV_NULL is not set
-CONFIG_ARCH="arm"
-CONFIG_ARCH_BOARD_CUSTOM=y
-CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/voxl2-io/nuttx-config"
-CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
-CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
-CONFIG_ARCH_CHIP="stm32"
-CONFIG_ARCH_CHIP_STM32=y
-CONFIG_ARCH_CHIP_STM32F100C8=y
-CONFIG_ARMV7M_USEBASEPRI=y
-CONFIG_ARM_MPU_EARLY_RESET=y
-CONFIG_BOARD_LOOPSPERMSEC=2000
-CONFIG_DEBUG_FULLOPT=y
-CONFIG_DEBUG_SYMBOLS=y
-CONFIG_DEFAULT_SMALL=y
-CONFIG_DISABLE_MOUNTPOINT=y
-CONFIG_FDCLONE_DISABLE=y
-CONFIG_FDCLONE_STDIO=y
-CONFIG_HAVE_CXX=y
-CONFIG_HAVE_CXXINITIALIZE=y
-CONFIG_IDLETHREAD_STACKSIZE=316
-CONFIG_INIT_ENTRYPOINT="user_start"
-CONFIG_INIT_STACKSIZE=1100
-CONFIG_MM_FILL_ALLOCATIONS=y
-CONFIG_MM_SMALL=y
-CONFIG_NAME_MAX=12
-CONFIG_PREALLOC_TIMERS=0
-CONFIG_RAM_SIZE=8192
-CONFIG_RAM_START=0x20000000
-CONFIG_RAW_BINARY=y
-CONFIG_SERIAL_TERMIOS=y
-CONFIG_STACK_COLORATION=y
-CONFIG_START_DAY=30
-CONFIG_START_MONTH=11
-CONFIG_STDIO_DISABLE_BUFFERING=y
-CONFIG_STM32_ADC1=y
-CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
-CONFIG_STM32_DMA1=y
-CONFIG_STM32_JTAG_SW_ENABLE=y
-CONFIG_STM32_USART1=y
-CONFIG_STM32_USART2=y
-CONFIG_STM32_USART3=y
-CONFIG_STM32_USART_SINGLEWIRE=y
-CONFIG_TASK_NAME_SIZE=0
-CONFIG_USART1_RXBUFSIZE=64
-CONFIG_USART1_RXDMA=y
-CONFIG_USART1_SERIAL_CONSOLE=y
-CONFIG_USART1_TXBUFSIZE=32
-CONFIG_USART2_RXBUFSIZE=64
-CONFIG_USART2_TXBUFSIZE=64
-CONFIG_USART3_RXBUFSIZE=64
-CONFIG_USART3_RXDMA=y
-CONFIG_USART3_TXBUFSIZE=64
-CONFIG_USEC_PER_TICK=1000

boards/modalai/voxl2-io/nuttx-config/scripts/script.ld

@@ -1,131 +0,0 @@
-/****************************************************************************
- * configs/px4io-v2/scripts/ld.script
- *
- *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
- *
- * 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 NuttX 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.
- *
- ****************************************************************************/
-
-/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and
- * 8Kb of SRAM beginning at address 0x2000:0000.  When booting from FLASH,
- * FLASH memory is aliased to address 0x0000:0000 where the code expects to
- * begin execution by jumping to the entry point in the 0x0800:0000 address
- * range.
- */
-
-MEMORY
-{
-    flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K
-    sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K
-}
-
-OUTPUT_ARCH(arm)
-ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
-EXTERN(_vectors)	/* force the vectors to be included in the output */
-
-/*
- * Ensure that abort() is present in the final object.  The exception handling
- * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
- */
-EXTERN(abort)
-
-SECTIONS
-{
-	.text : {
-		_stext = ABSOLUTE(.);
-		*(.vectors)
-		*(.text .text.*)
-		*(.fixup)
-		*(.gnu.warning)
-		*(.rodata .rodata.*)
-		*(.gnu.linkonce.t.*)
-		*(.glue_7)
-		*(.glue_7t)
-		*(.got)
-		*(.gcc_except_table)
-		*(.gnu.linkonce.r.*)
-		_etext = ABSOLUTE(.);
-	} > flash
-
-	/*
-	 * Init functions (static constructors and the like)
-	 */
-        .init_section : {
-                _sinit = ABSOLUTE(.);
-                KEEP(*(.init_array .init_array.*))
-                _einit = ABSOLUTE(.);
-        } > flash
-
-	.ARM.extab : {
-		*(.ARM.extab*)
-	} > flash
-
-	__exidx_start = ABSOLUTE(.);
-	.ARM.exidx : {
-		*(.ARM.exidx*)
-	} > flash
-	__exidx_end = ABSOLUTE(.);
-
-	_eronly = ABSOLUTE(.);
-
-	/* The STM32F100CB has 8Kb of SRAM beginning at the following address */
-
-	.data : {
-		_sdata = ABSOLUTE(.);
-		*(.data .data.*)
-		*(.gnu.linkonce.d.*)
-		CONSTRUCTORS
-		_edata = ABSOLUTE(.);
-		. = ALIGN(4);
-	} > sram AT > flash
-
-	.bss : {
-		_sbss = ABSOLUTE(.);
-		*(.bss .bss.*)
-		*(.gnu.linkonce.b.*)
-		*(COMMON)
-		. = ALIGN(4);
-		_ebss = ABSOLUTE(.);
-	} > sram
-
-	/* Stabs debugging sections. */
-	.stab 0 : { *(.stab) }
-	.stabstr 0 : { *(.stabstr) }
-	.stab.excl 0 : { *(.stab.excl) }
-	.stab.exclstr 0 : { *(.stab.exclstr) }
-	.stab.index 0 : { *(.stab.index) }
-	.stab.indexstr 0 : { *(.stab.indexstr) }
-	.comment 0 : { *(.comment) }
-	.debug_abbrev 0 : { *(.debug_abbrev) }
-	.debug_info 0 : { *(.debug_info) }
-	.debug_line 0 : { *(.debug_line) }
-	.debug_pubnames 0 : { *(.debug_pubnames) }
-	.debug_aranges 0 : { *(.debug_aranges) }
-}

boards/modalai/voxl2-io/src/board_config.h

@@ -1,157 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file board_config.h
- *
- * PX4IOV2 internal definitions
- */
-
-#pragma once
-
-/******************************************************************************
- * Included Files
- ******************************************************************************/
-
-#include <px4_platform_common/px4_config.h>
-#include <nuttx/compiler.h>
-#include <stdint.h>
-
-/******************************************************************************
- * Definitions
- ******************************************************************************/
-/* Configuration **************************************************************/
-
-/******************************************************************************
- * Serial
- ******************************************************************************/
-#define PX4FMU_SERIAL_BASE	STM32_USART2_BASE
-#define PX4FMU_SERIAL_VECTOR	STM32_IRQ_USART2
-#define PX4FMU_SERIAL_TX_GPIO	GPIO_USART2_TX
-#define PX4FMU_SERIAL_RX_GPIO	GPIO_USART2_RX
-#define PX4FMU_SERIAL_TX_DMA	DMACHAN_USART2_TX
-#define PX4FMU_SERIAL_RX_DMA	DMACHAN_USART2_RX
-#define PX4FMU_SERIAL_CLOCK	STM32_PCLK1_FREQUENCY
-#define PX4FMU_SERIAL_BITRATE	921600
-
-/******************************************************************************
- * GPIOS
- ******************************************************************************/
-
-/* LEDS  **********************************************************************/
-
-#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
-
-#define LED_BLUE(on_true)   stm32_gpiowrite(GPIO_LED_BLUE, !(on_true))
-#define LED_AMBER(on_true)  stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
-#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
-
-//#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
-
-//#define GPIO_HEATER_OFF 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-
-#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
-
-#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
-# define SENSE_PH1 0b10 /* Floating pulled as set */
-# define SENSE_PH2 0b01 /* Driven as tied */
-
-#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
-
-/* Safety switch button *******************************************************/
-
-/* CONNECTED TO TP8 - pulled down via SW */
-
-#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
-
-/* Power switch controls ******************************************************/
-
-#define GPIO_SPEKTRUM_PWR_EN         (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
-#define SPEKTRUM_POWER(_on_true)     px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_on_true))
-
-#define SPEKTRUM_OUT(_one_true)      px4_arch_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_one_true))
-#define SPEKTRUM_RX_AS_UART()        px4_arch_configgpio(GPIO_USART1_RX)
-#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_USART1_RX_SPEKTRUM)
-
-#define GPIO_SERVO_FAULT_DETECT 0 // (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
-
-/* Analog inputs **************************************************************/
-
-/* PWM pins  **************************************************************/
-
-#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
-
-#define DIRECT_PWM_OUTPUT_CHANNELS 8
-#define BOARD_HAS_NO_CAPTURE
-
-/* SBUS pins  *************************************************************/
-
-/* XXX these should be UART pins */
-#define GPIO_SBUS_INPUT   (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
-#define GPIO_SBUS_OUTPUT  (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define GPIO_SBUS_OENABLE 0 // (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
-
-/*
- * High-resolution timer
- */
-#define HRT_TIMER		1	/* use timer1 for the HRT */
-#define HRT_TIMER_CHANNEL	2	/* use capture/compare channel 2 */
-#define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 PA8 */
-#define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
-
-/* LED definitions ******************************************************************/
-/* PX4 has two LEDs that we will encode as: */
-
-#define LED_STARTED       0  /* LED? */
-#define LED_HEAPALLOCATE  1  /* LED? */
-#define LED_IRQSENABLED   2  /* LED? + LED? */
-#define LED_STACKCREATED  3  /* LED? */
-#define LED_INIRQ         4  /* LED? + LED? */
-#define LED_SIGNAL        5  /* LED? + LED? */
-#define LED_ASSERTION     6  /* LED? + LED? + LED? */
-#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
-
-#define BOARD_NUM_IO_TIMERS 3
-
-#include <px4_platform_common/board_common.h>

boards/modalai/voxl2-io/src/init.c

@@ -1,133 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file init.c
- *
- * PX4FMU-specific early startup code.  This file implements the
- * stm32_boardinitialize() function that is called during cpu startup.
- *
- * Code here is run before the rcS script is invoked; it should start required
- * subsystems and perform board-specific initialization.
- */
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <px4_platform_common/px4_config.h>
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <debug.h>
-#include <errno.h>
-#include <syslog.h>
-
-#include <nuttx/board.h>
-
-#include <stm32.h>
-#include "board_config.h"
-
-#include <arch/board/board.h>
-
-/****************************************************************************
- * Pre-Processor Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Protected Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- *   All STM32 architectures must provide the following entry point.  This entry point
- *   is called early in the intitialization -- after all memory has been configured
- *   and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-__EXPORT void stm32_boardinitialize(void)
-{
-	/* configure GPIOs */
-
-	/* Set up for sensing HW */
-	stm32_configgpio(GPIO_SENSE_PC14_DN);
-	stm32_configgpio(GPIO_SENSE_PC15_UP);
-
-	/* LEDS - default to off */
-	stm32_configgpio(GPIO_LED_BLUE);
-	stm32_configgpio(GPIO_LED_AMBER);
-	stm32_configgpio(GPIO_LED_SAFETY);
-
-	stm32_configgpio(GPIO_PC14);
-	stm32_configgpio(GPIO_PC15);
-
-
-	stm32_configgpio(GPIO_BTN_SAFETY);
-
-	/* spektrum power enable is active high - enable it by default */
-	stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
-
-	stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
-	stm32_configgpio(GPIO_SBUS_OUTPUT);
-
-	stm32_gpiowrite(GPIO_PWM1, true);
-	stm32_configgpio(GPIO_PWM1);
-
-	stm32_gpiowrite(GPIO_PWM2, true);
-	stm32_configgpio(GPIO_PWM2);
-
-	stm32_gpiowrite(GPIO_PWM3, true);
-	stm32_configgpio(GPIO_PWM3);
-
-	stm32_gpiowrite(GPIO_PWM4, true);
-	stm32_configgpio(GPIO_PWM4);
-
-	stm32_gpiowrite(GPIO_PWM5, true);
-	stm32_configgpio(GPIO_PWM5);
-
-	stm32_gpiowrite(GPIO_PWM6, true);
-	stm32_configgpio(GPIO_PWM6);
-
-	stm32_gpiowrite(GPIO_PWM7, true);
-	stm32_configgpio(GPIO_PWM7);
-
-	stm32_gpiowrite(GPIO_PWM8, true);
-	stm32_configgpio(GPIO_PWM8);
-}

boards/px4/fmu-v5/test.px4board

@@ -4,6 +4,8 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n

boards/px4/fmu-v5x/test.px4board

@@ -7,6 +7,7 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_DRIVERS_TEST_PPM=y

boards/px4/fmu-v6x/nuttx-config/nsh/defconfig

@@ -291,8 +291,9 @@ CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
 CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=1500
 CONFIG_UART5_RXDMA=y
-CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y

msg/GeofenceResult.msg

@@ -1,14 +1,14 @@
-uint64 timestamp                        # time since system start (microseconds)
-uint8 GF_ACTION_NONE = 0                # no action on geofence violation
-uint8 GF_ACTION_WARN = 1                # critical mavlink message
-uint8 GF_ACTION_LOITER = 2              # switch to AUTO|LOITER
-uint8 GF_ACTION_RTL = 3                 # switch to AUTO|RTL
-uint8 GF_ACTION_TERMINATE = 4           # flight termination
-uint8 GF_ACTION_LAND = 5                # switch to AUTO|LAND
+uint64 timestamp                # time since system start (microseconds)
+uint8 GF_ACTION_NONE      = 0   # no action on geofence violation
+uint8 GF_ACTION_WARN      = 1   # critical mavlink message
+uint8 GF_ACTION_LOITER    = 2   # switch to AUTO|LOITER
+uint8 GF_ACTION_RTL       = 3   # switch to AUTO|RTL
+uint8 GF_ACTION_TERMINATE = 4   # flight termination
+uint8 GF_ACTION_LAND      = 5   # switch to AUTO|LAND
 
-uint8 geofence_violation_reason         # one of geofence_violation_reason_t::*
+uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
 
-bool primary_geofence_breached          # true if the primary geofence is breached
-uint8 primary_geofence_action           # action to take when the primary geofence is breached
+bool primary_geofence_breached  # true if the primary geofence is breached
+uint8 primary_geofence_action   # action to take when the primary geofence is breached
 
-bool home_required                      # true if the geofence requires a valid home position
+bool home_required              # true if the geofence requires a valid home position

msg/SensorCorrection.msg

@@ -4,15 +4,6 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
-# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-uint32[4] gyro_device_ids
-float32[4] gyro_temperature
-float32[3] gyro_offset_0	# gyro 0 XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_offset_1	# gyro 1 XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_offset_2	# gyro 2 XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_offset_3	# gyro 3 XYZ offsets in the sensor frame in rad/s
-
 # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] accel_device_ids
@@ -22,6 +13,24 @@ float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-a
 float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
 float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
 
+# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
+# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
+uint32[4] gyro_device_ids
+float32[4] gyro_temperature
+float32[3] gyro_offset_0	# gyro 0 XYZ offsets in the sensor frame in rad/s
+float32[3] gyro_offset_1	# gyro 1 XYZ offsets in the sensor frame in rad/s
+float32[3] gyro_offset_2	# gyro 2 XYZ offsets in the sensor frame in rad/s
+float32[3] gyro_offset_3	# gyro 3 XYZ offsets in the sensor frame in rad/s
+
+# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset
+# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
+uint32[4] mag_device_ids
+float32[4] mag_temperature
+float32[3] mag_offset_0	# magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_1	# magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_2	# magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_3	# magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
+
 # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] baro_device_ids

platforms/common/px4_log.cpp

@@ -103,7 +103,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
+			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -138,12 +138,12 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1);
-			pos += sprintf(buf + sz, "%s\n", PX4_ANSI_COLOR_RESET);
+			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET);
 
 		} else
 #endif // PX4_LOG_COLORIZED_OUTPUT
 		{
-			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
+			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
 		}
 
 		// ensure NULL termination (buffer is max_length + 1)
@@ -162,7 +162,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
 			va_start(argptr, fmt);
 			pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr);
 			va_end(argptr);
-			pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n");
+			pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n");
 			buf[max_length] = 0; // ensure NULL termination
 		}
 
@@ -220,7 +220,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 #if defined(PX4_LOG_COLORIZED_OUTPUT)
 
 		if (use_color) {
-			pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]);
+			pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT
@@ -235,7 +235,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
 		if (use_color) {
 			// alway reset color
 			const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET));
-			pos += sprintf(buf + sz, "%s", PX4_ANSI_COLOR_RESET);
+			pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s", PX4_ANSI_COLOR_RESET);
 		}
 
 #endif // PX4_LOG_COLORIZED_OUTPUT

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -61,12 +61,13 @@ static BlockingList<WorkQueue *> *_wq_manager_wqs_list{nullptr};
 static BlockingQueue<const wq_config_t *, 1> *_wq_manager_create_queue{nullptr};
 
 static px4::atomic_bool _wq_manager_should_exit{true};
+static px4::atomic_bool _wq_manager_running{false};
 
 
 static WorkQueue *
 FindWorkQueueByName(const char *name)
 {
-	if (_wq_manager_wqs_list == nullptr) {
+	if (!_wq_manager_running.load()) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -86,7 +87,7 @@ FindWorkQueueByName(const char *name)
 WorkQueue *
 WorkQueueFindOrCreate(const wq_config_t &new_wq)
 {
-	if (_wq_manager_create_queue == nullptr) {
+	if (!_wq_manager_running.load()) {
 		PX4_ERR("not running");
 		return nullptr;
 	}
@@ -258,6 +259,7 @@ WorkQueueManagerRun(int, char **)
 {
 	_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
 	_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
+	_wq_manager_running.store(true);
 
 	while (!_wq_manager_should_exit.load()) {
 		// create new work queues as needed
@@ -361,13 +363,15 @@ WorkQueueManagerRun(int, char **)
 		}
 	}
 
+	_wq_manager_running.store(false);
+
 	return 0;
 }
 
 int
 WorkQueueManagerStart()
 {
-	if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) {
+	if (_wq_manager_should_exit.load() && !_wq_manager_running.load()) {
 
 		_wq_manager_should_exit.store(false);
 
@@ -384,6 +388,18 @@ WorkQueueManagerStart()
 			return -errno;
 		}
 
+		// Wait until initialized
+		int max_tries = 1000;
+
+		while (!_wq_manager_running.load() && --max_tries > 0) {
+			px4_usleep(1000);
+		}
+
+		if (max_tries <= 0) {
+			PX4_ERR("failed to wait for task to start");
+			return PX4_ERROR;
+		}
+
 	} else {
 		PX4_WARN("already running");
 		return PX4_ERROR;
@@ -398,7 +414,7 @@ WorkQueueManagerStop()
 	if (!_wq_manager_should_exit.load()) {
 
 		// error can't shutdown until all WorkItems are removed/stopped
-		if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) {
+		if (_wq_manager_running.load() && (_wq_manager_wqs_list->size() > 0)) {
 			PX4_ERR("can't shutdown with active WQs");
 			WorkQueueManagerStatus();
 			return PX4_ERROR;
@@ -422,6 +438,7 @@ WorkQueueManagerStop()
 			}
 
 			delete _wq_manager_wqs_list;
+			_wq_manager_wqs_list = nullptr;
 		}
 
 		_wq_manager_should_exit.store(true);
@@ -433,6 +450,7 @@ WorkQueueManagerStop()
 			px4_usleep(10000);
 
 			delete _wq_manager_create_queue;
+			_wq_manager_create_queue = nullptr;
 		}
 
 	} else {
@@ -446,7 +464,7 @@ WorkQueueManagerStop()
 int
 WorkQueueManagerStatus()
 {
-	if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {
+	if (!_wq_manager_should_exit.load() && _wq_manager_running.load()) {
 
 		const size_t num_wqs = _wq_manager_wqs_list->size();
 		PX4_INFO_RAW("\nWork Queue: %-2zu threads                          RATE        INTERVAL\n", num_wqs);

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -111,7 +111,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 
 	if (pubsubtest_print && timings) {
 		char fname[32] {};
-		sprintf(fname, PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
+		snprintf(fname, sizeof(fname), PX4_STORAGEDIR"/uorb_timings%u.txt", timingsgroup);
 		FILE *f = fopen(fname, "w");
 
 		if (f == nullptr) {

platforms/nuttx/src/px4/common/px4_init.cpp

@@ -127,8 +127,6 @@ int px4_platform_init()
 	hrt_ioctl_init();
 #endif
 
-	param_init();
-
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 	cpuload_initialize_once();
@@ -180,9 +178,10 @@ int px4_platform_init()
 
 #endif // CONFIG_FS_BINFS
 
-
 	px4::WorkQueueManagerStart();
 
+	param_init();
+
 	uorb_start();
 
 	px4_log_initialize();

platforms/posix/src/px4/common/px4_init.cpp

@@ -44,10 +44,10 @@ int px4_platform_init(void)
 {
 	hrt_init();
 
-	param_init();
-
 	px4::WorkQueueManagerStart();
 
+	param_init();
+
 	uorb_start();
 
 	px4_log_initialize();

src/drivers/drv_sensor.h

@@ -157,6 +157,8 @@
 #define DRV_LED_DEVTYPE_RGBLED        0x7a
 #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
 #define DRV_LED_DEVTYPE_RGBLED_IS31FL3195    0xbf
+#define DRV_LED_DEVTYPE_RGBLED_LP5562    0xc0
+
 #define DRV_BAT_DEVTYPE_SMBUS         0x7c
 #define DRV_SENS_DEVTYPE_IRLOCK       0x7d
 #define DRV_SENS_DEVTYPE_PCF8583      0x7e

src/drivers/lights/CMakeLists.txt

@@ -35,4 +35,5 @@
 add_subdirectory(rgbled)
 add_subdirectory(rgbled_ncp5623c)
 add_subdirectory(rgbled_is31fl3195)
+add_subdirectory(rgbled_lp5562)
 #add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)

src/drivers/lights/Kconfig

@@ -5,6 +5,7 @@ menu "Lights"
         select DRIVERS_LIGHTS_RGBLED
         select DRIVERS_LIGHTS_RGBLED_NCP5623C
         select DRIVERS_LIGHTS_RGBLED_IS31FL3195
+        select DRIVERS_LIGHTS_RGBLED_LP5562
         ---help---
             Enable default set of light drivers
     rsource "*/Kconfig"

src/drivers/lights/rgbled_lp5562/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023 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
@@ -31,12 +31,12 @@
 #
 ############################################################################
 
-add_library(drivers_board
-	init.c
-	timer_config.cpp
-)
-
-target_link_libraries(drivers_board
-	PRIVATE
-		nuttx_arch
-)
+px4_add_module(
+	MODULE drivers__rgbled_lp5562
+	MAIN rgbled_lp5562
+	SRCS
+		rgbled_lp5562.cpp
+	DEPENDS
+		drivers__device
+		led
+	)

src/drivers/lights/rgbled_lp5562/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_LIGHTS_RGBLED_LP5562
+	bool "rgbled lp5562"
+	default n
+	---help---
+		Enable support for RGBLED LP5562 driver

src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp

@@ -0,0 +1,329 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rgbled_lp5562.cpp
+ *
+ * Driver for the RGB LED controller Texas Instruments LP5562 connected via I2C.
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/device/i2c.h>
+#include <lib/led/led.h>
+#include <lib/parameters/param.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/module.h>
+
+
+using namespace time_literals;
+
+// The addresses are 0x60, 0x62, 0x64, 0x66 according to the datasheet page 27.
+// We specify 7bit addresses, hence 0x60 becomes 0x30.
+#define I2C_ADDR 0x30
+
+// Unfortunately, there is no WHO_AM_I or device id register, so
+// instead we query the W_CURRENT which has a certain pattern
+// after reset, and we don't use it or change it, so we don't have
+// to reset it and therefore don't mess with a device that we're
+// not sure what it is.
+static constexpr uint8_t LED_MAP_ADDR = 0x70;
+static constexpr uint8_t LED_MAP_ALL_PWM = 0b00000000;
+
+static constexpr uint8_t ENABLE_ADDR = 0x00;
+static constexpr uint8_t ENABLE_CHIP_EN = 0b01000000;
+
+static constexpr uint8_t CONFIG_ADDR = 0x08;
+static constexpr uint8_t CONFIG_ENABLE_INTERNAL_CLOCK = 0b00000001;
+
+static constexpr uint8_t RESET_ADDR = 0x0D;
+static constexpr uint8_t RESET_DO_RESET = 0xFF;
+
+static constexpr uint8_t B_PWM_ADDR = 0x02;
+
+static constexpr uint8_t B_CURRENT_ADDR = 0x05;
+
+static constexpr uint8_t W_CURRENT_ADDR = 0x0F;
+static constexpr uint8_t W_CURRENT_DEFAULT = 0b10101111;
+
+
+class RGBLED_LP5562: public device::I2C, public I2CSPIDriver<RGBLED_LP5562>
+{
+public:
+	RGBLED_LP5562(const I2CSPIDriverConfig &config);
+	virtual ~RGBLED_LP5562() = default;
+
+	static void print_usage();
+
+	int init() override;
+	int probe() override;
+
+	void RunImpl();
+
+private:
+	int read(uint8_t address, uint8_t *data, unsigned count);
+	int write(uint8_t address, uint8_t *data, unsigned count);
+	int send_led_rgb(uint8_t r, uint8_t g, uint8_t b);
+
+	LedController _led_controller;
+	uint8_t _current = 175; // matching default current of 17.5mA
+};
+
+RGBLED_LP5562::RGBLED_LP5562(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config)
+{
+	_current = config.custom1;
+}
+
+int
+RGBLED_LP5562::init()
+{
+	int ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	uint8_t command[1] = {ENABLE_CHIP_EN};
+	ret = write(ENABLE_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// We have to wait 500us after enable.
+	px4_usleep(500);
+
+	command[0] = CONFIG_ENABLE_INTERNAL_CLOCK;
+	ret = write(CONFIG_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	command[0] = LED_MAP_ALL_PWM;
+	ret = write(LED_MAP_ADDR, command, sizeof(command));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// Write all 3 colors at once.
+	uint8_t currents[3] = {_current, _current, _current};
+	ret = write(B_CURRENT_ADDR, currents, sizeof(currents));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ScheduleNow();
+
+	return OK;
+}
+
+int
+RGBLED_LP5562::probe()
+{
+	uint8_t result[1] = {0};
+	int ret = read(W_CURRENT_ADDR, result, sizeof(result));
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	_retries = 1;
+
+	return (result[0] == W_CURRENT_DEFAULT) ? OK : ERROR;
+}
+
+int
+RGBLED_LP5562::read(uint8_t address, uint8_t *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
+int
+RGBLED_LP5562::write(uint8_t address, uint8_t *data, unsigned count)
+{
+	uint8_t buf[4];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], count + 1, nullptr, 0);
+}
+
+void
+RGBLED_LP5562::RunImpl()
+{
+	if (should_exit()) {
+		send_led_rgb(0, 0, 0);
+		return;
+	}
+
+	LedControlData led_control_data;
+
+	if (_led_controller.update(led_control_data) == 1) {
+
+		const uint8_t on = led_control_data.leds[0].brightness;
+
+		switch (led_control_data.leds[0].color) {
+		case led_control_s::COLOR_RED:
+			send_led_rgb(on, 0, 0);
+			break;
+
+		case led_control_s::COLOR_GREEN:
+			send_led_rgb(0, on, 0);
+			break;
+
+		case led_control_s::COLOR_BLUE:
+			send_led_rgb(0, 0, on);
+			break;
+
+		case led_control_s::COLOR_AMBER: // same as yellow
+		case led_control_s::COLOR_YELLOW:
+			send_led_rgb(on, on, 0);
+			break;
+
+		case led_control_s::COLOR_PURPLE:
+			send_led_rgb(on, 0, on);
+			break;
+
+		case led_control_s::COLOR_CYAN:
+			send_led_rgb(0, on, on);
+			break;
+
+		case led_control_s::COLOR_WHITE:
+			send_led_rgb(on, on, on);
+			break;
+
+		case led_control_s::COLOR_OFF:
+		default:
+			send_led_rgb(0, 0, 0);
+			break;
+		}
+
+	}
+
+	ScheduleDelayed(_led_controller.maximum_update_interval());
+}
+
+int
+RGBLED_LP5562::send_led_rgb(uint8_t r, uint8_t g, uint8_t b)
+{
+	uint8_t leds[3] = {b, g, r};
+	return write(B_PWM_ADDR, leds, sizeof(leds));
+}
+
+void
+RGBLED_LP5562::print_usage()
+{
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for [LP5562](https://www.ti.com/product/LP5562) LED driver connected via I2C.
+
+This used in some GPS modules by Holybro for [PX4 status notification](../getting_started/led_meanings.md)
+
+The driver is included by default in firmware (KConfig key DRIVERS_LIGHTS_RGBLED_LP5562) and is always enabled.
+)DESCR_STR");
+	PRINT_MODULE_USAGE_NAME("rgbled_lp5562", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(I2C_ADDR);
+	PRINT_MODULE_USAGE_PARAM_FLOAT('u', 17.5f, 0.1f, 25.5f, "Current in mA", true);
+
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int rgbled_lp5562_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = RGBLED_LP5562;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = I2C_ADDR;
+	cli.custom1 = 175;
+
+	while ((ch = cli.getOpt(argc, argv, "u:")) != EOF) {
+		switch (ch) {
+		case 'u':
+			float v = atof(cli.optArg());
+
+			if (v >= 0.1f && v <= 25.5f) {
+				cli.custom1 = ((uint8_t)(v * 10.f));
+
+			} else {
+				PX4_ERR("current out of range");
+				return -1;
+			}
+
+			break;
+		}
+	}
+
+
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP5562);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/px4io/px4io.cpp

@@ -209,7 +209,7 @@ private:
 	bool			_timer_rates_configured{false};
 
 	/* advertised topics */
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<input_rc_s>	_input_rc_pub{ORB_ID(input_rc)};
 	uORB::Publication<px4io_status_s>	_px4io_status_pub{ORB_ID(px4io_status)};
 
 	ButtonPublisher	_button_publisher;
@@ -1144,7 +1144,7 @@ int PX4IO::io_publish_raw_rc()
 		input_rc.link_quality = -1;
 		input_rc.rssi_dbm = NAN;
 
-		_to_input_rc.publish(input_rc);
+		_input_rc_pub.publish(input_rc);
 	}
 
 	return ret;

src/drivers/rc_input/RCInput.cpp

@@ -192,16 +192,16 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 		    unsigned frame_drops, int rssi = -1)
 {
 	// fill rc_in struct for publishing
-	_rc_in.channel_count = raw_rc_count_local;
+	_input_rc.channel_count = raw_rc_count_local;
 
-	if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
-		_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+	if (_input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		_input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
 
 	unsigned valid_chans = 0;
 
-	for (unsigned i = 0; i < _rc_in.channel_count; i++) {
-		_rc_in.values[i] = raw_rc_values_local[i];
+	for (unsigned i = 0; i < _input_rc.channel_count; i++) {
+		_input_rc.values[i] = raw_rc_values_local[i];
 
 		if (raw_rc_values_local[i] != UINT16_MAX) {
 			valid_chans++;
@@ -211,20 +211,20 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 		_raw_rc_values[i] = UINT16_MAX;
 	}
 
-	_rc_in.timestamp = now;
-	_rc_in.timestamp_last_signal = _rc_in.timestamp;
-	_rc_in.rc_ppm_frame_length = 0;
+	_input_rc.timestamp = now;
+	_input_rc.timestamp_last_signal = _input_rc.timestamp;
+	_input_rc.rc_ppm_frame_length = 0;
 
 	/* fake rssi if no value was provided */
 	if (rssi == -1) {
-		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) {
+		if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _input_rc.channel_count)) {
 			const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get();
 			const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get();
 			const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get();
 
 			// get RSSI from input channel
-			int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
-			_rc_in.rssi = math::constrain(rc_rssi, 0, 100);
+			int rc_rssi = ((_input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min);
+			_input_rc.rssi = math::constrain(rc_rssi, 0, 100);
 
 		} else if (_analog_rc_rssi_stable) {
 			// set RSSI if analog RSSI input is present
@@ -238,24 +238,24 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 				rssi_analog = 0.0f;
 			}
 
-			_rc_in.rssi = rssi_analog;
+			_input_rc.rssi = rssi_analog;
 
 		} else {
-			_rc_in.rssi = 255;
+			_input_rc.rssi = 255;
 		}
 
 	} else {
-		_rc_in.rssi = rssi;
+		_input_rc.rssi = rssi;
 	}
 
 	if (valid_chans == 0) {
-		_rc_in.rssi = 0;
+		_input_rc.rssi = 0;
 	}
 
-	_rc_in.rc_failsafe = failsafe;
-	_rc_in.rc_lost = (valid_chans == 0);
-	_rc_in.rc_lost_frame_count = frame_drops;
-	_rc_in.rc_total_frame_count = 0;
+	_input_rc.rc_failsafe = failsafe;
+	_input_rc.rc_lost = (valid_chans == 0);
+	_input_rc.rc_lost_frame_count = frame_drops;
+	_input_rc.rc_total_frame_count = 0;
 }
 
 void RCInput::set_rc_scan_state(RC_SCAN newState)
@@ -468,7 +468,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new SBUS frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   sbus_frame_drop, sbus_failsafe, frame_drops);
 						_rc_scan_locked = true;
@@ -506,7 +506,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new DSM frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   false, false, frame_drops, dsm_rssi);
 						_rc_scan_locked = true;
@@ -552,14 +552,14 @@ void RCInput::Run()
 					if (rc_updated) {
 						if (lost_count == 0) {
 							// we have a new ST24 frame. Publish it.
-							_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
+							_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24;
 							fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 								   false, false, frame_drops, st24_rssi);
 							_rc_scan_locked = true;
 
 						} else {
 							// if the lost count > 0 means that there is an RC loss
-							_rc_in.rc_lost = true;
+							_input_rc.rc_lost = true;
 						}
 					}
 				}
@@ -600,7 +600,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new SUMD frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
 							   false, sumd_failsafe, frame_drops, sumd_rssi);
 						_rc_scan_locked = true;
@@ -625,14 +625,14 @@ void RCInput::Run()
 			} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
 				// see if we have new PPM input data
-				if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
+				if ((ppm_last_valid_decode != _input_rc.timestamp_last_signal) && ppm_decoded_channels > 3) {
 					// we have a new PPM frame. Publish it.
 					rc_updated = true;
-					_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
+					_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
 					fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
 					_rc_scan_locked = true;
-					_rc_in.rc_ppm_frame_length = ppm_frame_length;
-					_rc_in.timestamp_last_signal = ppm_last_valid_decode;
+					_input_rc.rc_ppm_frame_length = ppm_frame_length;
+					_input_rc.timestamp_last_signal = ppm_last_valid_decode;
 				}
 
 			} else {
@@ -669,7 +669,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new CRSF frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0);
 
 						// on Pixhawk (-related) boards we cannot write to the RC UART
@@ -718,7 +718,7 @@ void RCInput::Run()
 
 					if (rc_updated) {
 						// we have a new GHST frame. Publish it.
-						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
+						_input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
 
 						// ghst telemetry works on fmu-v5
@@ -753,12 +753,12 @@ void RCInput::Run()
 		if (rc_updated) {
 			perf_count(_publish_interval_perf);
 
-			_rc_in.link_quality = -1;
-			_rc_in.rssi_dbm = NAN;
+			_input_rc.link_quality = -1;
+			_input_rc.rssi_dbm = NAN;
 
-			_to_input_rc.publish(_rc_in);
+			_input_rc_pub.publish(_input_rc);
 
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_input_rc.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -907,8 +907,8 @@ int RCInput::print_status()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_publish_interval_perf);
 
-	if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) {
-		print_message(ORB_ID(input_rc), _rc_in);
+	if (hrt_elapsed_time(&_input_rc.timestamp) < 1_s) {
+		print_message(ORB_ID(input_rc), _input_rc);
 	}
 
 	return 0;

src/drivers/rc_input/RCInput.hpp

@@ -127,6 +127,7 @@ private:
 
 	void rc_io_invert(bool invert);
 
+	input_rc_s _input_rc{};
 	hrt_abstime _rc_scan_begin{0};
 
 	bool _initialized{false};
@@ -140,16 +141,13 @@ private:
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	input_rc_s	_rc_in{};
+	uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
 
 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};
 
 	bool _armed{false};
 
-
-	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
-
 	int		_rcs_fd{-1};
 	char		_device[20] {};					///< device / serial port path
 

src/drivers/uavcan/arming_status.cpp

@@ -69,7 +69,7 @@ void UavcanArmingStatus::periodic_update(const uavcan::TimerEvent &)
 		if (actuator_armed.lockdown || actuator_armed.manual_lockdown) {
 			cmd.status = cmd.STATUS_DISARMED;
 
-		} else if (actuator_armed.armed || actuator_armed.prearmed) {
+		} else if (actuator_armed.armed) {
 			cmd.status = cmd.STATUS_FULLY_ARMED;
 
 		} else {

src/drivers/uavcannode/Publishers/GnssAuxiliary.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+#include "UavcanPublisherBase.hpp"
+
+#include <uavcan/equipment/gnss/Auxiliary.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+namespace uavcannode
+{
+
+class GnssAuxiliary :
+	public UavcanPublisherBase,
+	public uORB::SubscriptionCallbackWorkItem,
+	private uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>
+{
+public:
+	GnssAuxiliary(px4::WorkItem *work_item, uavcan::INode &node) :
+		UavcanPublisherBase(uavcan::equipment::gnss::Auxiliary::DefaultDataTypeID),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)),
+		uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>(node)
+	{
+		this->setPriority(uavcan::TransferPriority::Default);
+	}
+
+	void PrintInfo() override
+	{
+		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       uavcan::equipment::gnss::Auxiliary::getDataTypeFullName(),
+			       id());
+		}
+	}
+
+	void BroadcastAnyUpdates() override
+	{
+		using uavcan::equipment::gnss::Auxiliary;
+
+		// sensor_gps -> uavcan::equipment::gnss::Auxiliary
+		sensor_gps_s gps;
+
+		if (uORB::SubscriptionCallbackWorkItem::update(&gps)) {
+			uavcan::equipment::gnss::Auxiliary auxiliary{};
+
+			//auxiliary.gdop = gps.gdop;
+			//auxiliary.pdop = gps.pdop;
+			auxiliary.hdop = gps.hdop;
+			auxiliary.vdop = gps.vdop;
+			//auxiliary.tdop = gps.tdop;
+			//auxiliary.ndop = gps.ndop;
+			//auxiliary.edop = gps.edop;
+
+			auxiliary.sats_visible = gps.satellites_used;
+			auxiliary.sats_used = gps.satellites_used;
+
+			uavcan::Publisher<uavcan::equipment::gnss::Auxiliary>::broadcast(auxiliary);
+
+			// ensure callback is registered
+			uORB::SubscriptionCallbackWorkItem::registerCallback();
+		}
+	}
+};
+} // namespace uavcannode

src/drivers/uavcannode/Subscribers/ArmingStatus.hpp

@@ -89,6 +89,7 @@ private:
 			actuator_armed.armed = false;
 		}
 
+		actuator_armed.prearmed = true;
 		actuator_armed.timestamp = hrt_absolute_time();
 		_actuator_armed_pub.publish(actuator_armed);
 	}

src/drivers/uavcannode/UavcanNode.cpp

@@ -58,6 +58,7 @@
 
 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 #include "Publishers/GnssFix2.hpp"
+#include "Publishers/GnssAuxiliary.hpp"
 #endif // CONFIG_UAVCANNODE_GNSS_FIX
 
 #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)
@@ -366,6 +367,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 
 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 	_publisher_list.add(new GnssFix2(this, _node));
+	_publisher_list.add(new GnssAuxiliary(this, _node));
 #endif // CONFIG_UAVCANNODE_GNSS_FIX
 
 #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH)

src/lib/events/enums.json

@@ -240,6 +240,10 @@
                         "536870912": {
                             "name": "gyro",
                             "description": "Gyroscope"
+                        },
+                        "1073741824": {
+                            "name": "open_drone_id",
+                            "description": "Open Drone ID system"
                         }
                     }
                 },

src/lib/matrix/matrix/Dcm.hpp

@@ -90,7 +90,6 @@ public:
 		const Type b = q(1);
 		const Type c = q(2);
 		const Type d = q(3);
-		const Type aa = a * a;
 		const Type ab = a * b;
 		const Type ac = a * c;
 		const Type ad = a * d;
@@ -100,15 +99,15 @@ public:
 		const Type cc = c * c;
 		const Type cd = c * d;
 		const Type dd = d * d;
-		dcm(0, 0) = aa + bb - cc - dd;
+		dcm(0, 0) = Type(1) - Type(2) * (cc + dd);
 		dcm(0, 1) = Type(2) * (bc - ad);
 		dcm(0, 2) = Type(2) * (ac + bd);
 		dcm(1, 0) = Type(2) * (bc + ad);
-		dcm(1, 1) = aa - bb + cc - dd;
+		dcm(1, 1) = Type(1) - Type(2) * (bb + dd);
 		dcm(1, 2) = Type(2) * (cd - ab);
 		dcm(2, 0) = Type(2) * (bd - ac);
 		dcm(2, 1) = Type(2) * (ab + cd);
-		dcm(2, 2) = aa - bb - cc + dd;
+		dcm(2, 2) = Type(1) - Type(2) * (bb + cc);
 	}
 
 	/**

src/lib/parameters/CMakeLists.txt

@@ -156,7 +156,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
 
 set(SRCS)
 
-list(APPEND SRCS parameters.cpp)
+list(APPEND SRCS parameters.cpp atomic_transaction.cpp autosave.cpp)
 
 if(BUILD_TESTING)
 	list(APPEND SRCS param_translation_unit_tests.cpp)

src/lib/parameters/ConstLayer.h

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ParamLayer.h"
+
+class ConstLayer : public ParamLayer
+{
+public:
+
+	ConstLayer() = default;
+
+	bool store(param_t param, param_value_u value) override
+	{
+		return false;
+	}
+
+	bool contains(param_t param) const override
+	{
+		return param < PARAM_COUNT;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+
+		for (int i = 0; i < PARAM_COUNT; i++) {
+			set.set(i);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		if (param >= PARAM_COUNT) {
+			return {0};
+		}
+
+		return px4::parameters[param].val;
+	}
+
+	void reset(param_t param) override
+	{
+		// Do nothing
+	}
+
+	void refresh(param_t param) override
+	{
+		// Do nothing
+	}
+
+	int size() const override
+	{
+		return PARAM_COUNT;
+	}
+
+	int byteSize() const override
+	{
+		return PARAM_COUNT * sizeof(param_info_s);
+	}
+};

src/lib/parameters/DynamicSparseLayer.h

@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "ParamLayer.h"
+
+#include <px4_platform_common/atomic.h>
+
+class DynamicSparseLayer : public ParamLayer
+{
+public:
+	DynamicSparseLayer(ParamLayer *parent, int n_prealloc = 32, int n_grow = 4) : ParamLayer(parent),
+		_n_slots(n_prealloc), _n_grow(n_grow)
+	{
+		Slot *slots = (Slot *)malloc(sizeof(Slot) * n_prealloc);
+
+		if (slots == nullptr) {
+			PX4_ERR("Failed to allocate memory for dynamic sparse layer");
+			_n_slots = 0;
+			return;
+		}
+
+		for (int i = 0; i < _n_slots; i++) {
+			slots[i] = {UINT16_MAX, param_value_u{}};
+		}
+
+		_slots.store(slots);
+	}
+
+	virtual ~DynamicSparseLayer()
+	{
+		if (_slots.load()) {
+			free(_slots.load());
+		}
+	}
+
+	bool store(param_t param, param_value_u value) override
+	{
+		AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // already exists
+			slots[index].value = value;
+
+		} else if (_next_slot < _n_slots) {
+			slots[_next_slot++] = {param, value};
+			_sort();
+
+		} else {
+			if (!_grow(transaction)) {
+				return false;
+			}
+
+			_slots.load()[_next_slot++] = {param, value};
+			_sort();
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		return _getIndex(param) < _next_slot;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+		const AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		for (int i = 0; i < _next_slot; i++) {
+			set.set(slots[i].param);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		Slot *slots = _slots.load();
+
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // exists in our data structure
+			return slots[index].value;
+		}
+
+		return _parent->get(param);
+	}
+
+	void reset(param_t param) override
+	{
+		const AtomicTransaction transaction;
+		int index = _getIndex(param);
+		Slot *slots = _slots.load();
+
+		if (index < _next_slot) {
+			slots[index] = {UINT16_MAX, param_value_u{}};
+			_sort();
+			_next_slot--;
+		}
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+	}
+
+	int size() const override
+	{
+		return _next_slot;
+	}
+
+	int byteSize() const override
+	{
+		return _n_slots * sizeof(Slot);
+	}
+
+private:
+	struct Slot {
+		param_t param;
+		param_value_u value;
+	};
+
+	static int _slotCompare(const void *a, const void *b)
+	{
+		return ((int)((Slot *)a)->param) - ((int)((Slot *)b)->param);
+	}
+
+	void _sort()
+	{
+		qsort(_slots.load(), _n_slots, sizeof(Slot), _slotCompare);
+	}
+
+	int _getIndex(param_t param) const
+	{
+		int left = 0;
+		int right = _next_slot - 1;
+		Slot *slots = _slots.load();
+
+		while (left <= right) {
+			int mid = (left + right) / 2;
+
+			if (slots[mid].param == param) {
+				return mid;
+
+			} else if (slots[mid].param < param) {
+				left = mid + 1;
+
+			} else {
+				right = mid - 1;
+			}
+		}
+
+		return _next_slot;
+	}
+
+	bool _grow(AtomicTransaction &transaction)
+	{
+		if (_n_slots == 0) {
+			return false;
+		}
+
+		int max_retries = 5;
+
+		// As malloc uses locking, so we need to re-enable IRQ's during malloc/free and
+		// then atomically exchange the buffer
+		while (_next_slot >= _n_slots && max_retries-- > 0) {
+			Slot *previous_slots = nullptr;
+			Slot *new_slots = nullptr;
+
+			do {
+				previous_slots = _slots.load();
+				transaction.unlock();
+
+				if (new_slots) {
+					free(new_slots);
+				}
+
+				new_slots = (Slot *) malloc(sizeof(Slot) * (_n_slots + _n_grow));
+				transaction.lock();
+
+				if (new_slots == nullptr) {
+					return false;
+				}
+
+			} while (!_slots.compare_exchange(&previous_slots, new_slots));
+
+			memcpy(new_slots, previous_slots, sizeof(Slot) * _n_slots);
+
+			for (int i = _n_slots; i < _n_slots + _n_grow; i++) {
+				new_slots[i] = {UINT16_MAX, param_value_u{}};
+			}
+
+			_n_slots += _n_grow;
+
+			transaction.unlock();
+			free(previous_slots);
+			transaction.lock();
+		}
+
+		return _next_slot < _n_slots;
+	}
+
+	int _next_slot = 0;
+	int _n_slots = 0;
+	const int _n_grow;
+	px4::atomic<Slot *> _slots{nullptr};
+};

src/lib/parameters/ExhaustiveLayer.h

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/atomic_bitset.h>
+#include "ParamLayer.h"
+
+class ExhaustiveLayer : public ParamLayer
+{
+public:
+	ExhaustiveLayer(ParamLayer *parent) : ParamLayer(parent)
+	{
+		// refresh all values
+		for (param_t i = 0; i < PARAM_COUNT; i++) {
+			_values[i] = parent->get(i);
+		}
+	}
+
+	bool store(param_t param, param_value_u value) override
+	{
+		if (param >= PARAM_COUNT) {
+			return false;
+		}
+
+		{
+			const AtomicTransaction transaction;
+			_values[param] = value;
+			_ownership_set.set(param);
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		return param < PARAM_COUNT && _ownership_set[param];
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		return _ownership_set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		if (param >= PARAM_COUNT) {
+			return {0};
+		}
+
+		const AtomicTransaction transaction;
+		// We assume to have the correct values for all params, even without ownership.
+		// We expect that refresh was called when underlying defaults changed
+		return _values[param];
+	}
+
+	void reset(param_t param) override
+	{
+		if (param >= PARAM_COUNT) {
+			return;
+		}
+
+		const AtomicTransaction transaction;
+		_values[param] = _parent->get(param);
+		_ownership_set.set(param, false);
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+		// in case we don't have ownership, and it changed below, we have to refresh our cache.
+		{
+			const AtomicTransaction transaction;
+
+			if (!contains(param)) {
+				_values[param] = _parent->get(param);
+			}
+		}
+	}
+
+	int size() const override
+	{
+		return _ownership_set.count();
+	}
+
+	int byteSize() const override
+	{
+		return PARAM_COUNT * sizeof(param_value_u);
+	}
+
+private:
+	param_value_u _values[PARAM_COUNT];
+	px4::AtomicBitset<PARAM_COUNT> _ownership_set;
+};

src/lib/parameters/ParamLayer.h

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/atomic_bitset.h>
+#include "atomic_transaction.h"
+#include "param.h"
+#include <stdlib.h>
+
+class ParamLayer
+{
+public:
+	static constexpr param_t PARAM_COUNT = sizeof(px4::parameters) / sizeof(param_info_s);
+
+	ParamLayer(ParamLayer *parent = nullptr) : _parent(parent) {}
+
+	ParamLayer(const ParamLayer &) = delete;
+	ParamLayer &operator=(const ParamLayer &) = delete;
+	ParamLayer(ParamLayer &&) = delete;
+	ParamLayer &operator=(ParamLayer &&) = delete;
+
+
+	virtual bool store(param_t param, param_value_u value) = 0;
+
+	virtual bool contains(param_t param) const = 0;
+
+	virtual px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const = 0;
+
+	virtual param_value_u get(param_t param) const = 0;
+
+	virtual void reset(param_t param) = 0;
+
+	virtual void refresh(param_t param) = 0;
+
+	virtual int size() const = 0;
+
+	virtual int byteSize() const = 0;
+
+protected:
+	ParamLayer *const _parent;
+};

src/lib/parameters/StaticSparseLayer.h

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdlib.h>
+#include "ParamLayer.h"
+
+template <int N_SLOTS>
+class StaticSparseLayer : public ParamLayer
+{
+public:
+	StaticSparseLayer(ParamLayer *parent) : ParamLayer(parent)
+	{
+		for (int i = 0; i < N_SLOTS; i++) {
+			_slots[i] = {UINT16_MAX, param_value_u{}};
+		}
+	}
+
+	virtual ~StaticSparseLayer() = default;
+
+	bool store(param_t param, param_value_u value) override
+	{
+		const AtomicTransaction transaction;
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // already exists
+			_slots[index].value = value;
+
+		} else if (_next_slot < N_SLOTS) {
+			_slots[_next_slot++] = {param, value};
+			_sort();
+
+		} else {
+			return false;
+		}
+
+		return true;
+	}
+
+	bool contains(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		return _getIndex(param) < _next_slot;
+	}
+
+	px4::AtomicBitset<PARAM_COUNT> containedAsBitset() const override
+	{
+		px4::AtomicBitset<PARAM_COUNT> set;
+		const AtomicTransaction transaction;
+
+		for (int i = 0; i < _next_slot; i++) {
+			set.set(_slots[i].param);
+		}
+
+		return set;
+	}
+
+	param_value_u get(param_t param) const override
+	{
+		const AtomicTransaction transaction;
+		const int index = _getIndex(param);
+
+		if (index < _next_slot) { // exists in this layer
+			return _slots[index].value;
+		}
+
+		return _parent->get(param);
+	}
+
+	void reset(param_t param) override
+	{
+		const AtomicTransaction transaction;
+		int index = _getIndex(param);
+
+		if (index < _next_slot) {
+			_slots[index] = {UINT16_MAX, param_value_u{}};
+			_sort();
+			_next_slot--;
+		}
+	}
+
+	void refresh(param_t param) override
+	{
+		_parent->refresh(param);
+	}
+
+	int size() const override
+	{
+		return _next_slot;
+	}
+
+	int byteSize() const override
+	{
+		return N_SLOTS * sizeof(Slot);
+	}
+
+private:
+	struct Slot {
+		param_t param;
+		param_value_u value;
+	};
+
+	static int _slotCompare(const void *a, const void *b)
+	{
+		return ((int)((Slot *)a)->param) - ((int)((Slot *)b)->param);
+	}
+
+	void _sort()
+	{
+		qsort(_slots, N_SLOTS, sizeof(Slot), &_slotCompare);
+	}
+
+	int _getIndex(param_t param) const
+	{
+		int left = 0;
+		int right = _next_slot - 1;
+
+		while (left <= right) {
+			int mid = (left + right) / 2;
+
+			if (_slots[mid].param == param) {
+				return mid;
+
+			} else if (_slots[mid].param < param) {
+				left = mid + 1;
+
+			} else {
+				right = mid - 1;
+			}
+		}
+
+		return _next_slot;
+	}
+
+	Slot _slots[N_SLOTS];
+	int _next_slot = 0;
+};

src/lib/parameters/atomic_transaction.cpp

@@ -0,0 +1,38 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "atomic_transaction.h"
+
+#ifdef __PX4_POSIX
+_MutexHolder  AtomicTransaction::_mutex_holder = _MutexHolder {};
+#endif

src/lib/parameters/atomic_transaction.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *  Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023 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
@@ -31,24 +31,75 @@
  *
  ****************************************************************************/
 
-#include <px4_arch/io_timer_hw_description.h>
+#pragma once
 
-constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer2),
-	initIOTimer(Timer::Timer3),
-	initIOTimer(Timer::Timer4),
+#ifdef __PX4_NUTTX
+#include "px4_platform_common/micro_hal.h"
+#endif
+
+#ifdef __PX4_POSIX
+#include <pthread.h>
+
+class _MutexHolder
+{
+public:
+	pthread_mutex_t _mutex;
+	pthread_mutexattr_t _mutex_attr;
+
+	_MutexHolder()
+	{
+		pthread_mutexattr_init(&_mutex_attr);
+		pthread_mutexattr_settype(&_mutex_attr, PTHREAD_MUTEX_RECURSIVE);
+		pthread_mutex_init(&_mutex, &_mutex_attr);
+	}
+
+	~_MutexHolder()
+	{
+		pthread_mutex_destroy(&_mutex);
+	}
 };
+#endif
 
-constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
-	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
-	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
-	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortB, GPIO::Pin8}),
-	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortB, GPIO::Pin9}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortA, GPIO::Pin6}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortA, GPIO::Pin7}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
-	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+
+class AtomicTransaction
+{
+private:
+#ifdef __PX4_NUTTX
+	irqstate_t _irq_state;
+#endif
+
+#ifdef __PX4_POSIX
+	static _MutexHolder _mutex_holder;
+#endif
+
+public:
+	AtomicTransaction()
+	{
+		lock();
+	}
+
+	~AtomicTransaction()
+	{
+		unlock();
+	}
+
+	void lock()
+	{
+#ifdef __PX4_NUTTX
+		_irq_state = px4_enter_critical_section();
+#endif
+#ifdef __PX4_POSIX
+		pthread_mutex_lock(&_mutex_holder._mutex);
+#endif
+	}
+
+	void unlock()
+	{
+#ifdef __PX4_NUTTX
+		px4_leave_critical_section(_irq_state);
+#endif
+#ifdef __PX4_POSIX
+		pthread_mutex_unlock(&_mutex_holder._mutex);
+#endif
+	}
 };
-
-constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
-	initIOTimerChannelMapping(io_timers, timer_io_channels);

src/lib/parameters/autosave.cpp

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "autosave.h"
+
+#include <px4_platform_common/log.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/Subscription.hpp>
+
+#include "param.h"
+#include "atomic_transaction.h"
+
+using namespace time_literals;
+
+ParamAutosave::ParamAutosave()
+	: ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+}
+
+void ParamAutosave::request()
+{
+	if (_scheduled.load() || _disabled) {
+		return;
+	}
+
+	// wait at least 300ms before saving, because:
+	// - tasks often call param_set() for multiple params, so this avoids unnecessary save calls
+	// - the logger stores changed params. He gets notified on a param change via uORB and then
+	//   looks at all unsaved params.
+	hrt_abstime delay = 300_ms;
+
+	static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds
+	const hrt_abstime last_save_elapsed = hrt_elapsed_time(&_last_timestamp);
+
+	if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) {
+		delay = rate_limit - last_save_elapsed;
+	}
+
+	_scheduled.store(true);
+	ScheduleDelayed(delay);
+}
+
+void ParamAutosave::enable(bool enable)
+{
+	AtomicTransaction transaction;
+	_disabled = !enable;
+
+	if (!enable && _scheduled.load()) {
+		_scheduled.store(false);
+		px4::ScheduledWorkItem::ScheduleClear();
+	}
+}
+
+void ParamAutosave::Run()
+{
+	bool disabled = false;
+
+	if (!param_get_default_file()) {
+		// In case we save to FLASH, defer param writes until disarmed,
+		// as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7)
+		uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
+
+		if (armed_sub.get().armed) {
+			ScheduleDelayed(1_s);
+			return;
+		}
+	}
+
+	{
+		const AtomicTransaction transaction;
+		_last_timestamp = hrt_absolute_time();
+		// Note that the order is important here: we first clear _scheduled, then save the parameters, as during export,
+		// more parameter changes could happen.
+		_scheduled.store(false);
+		disabled = _disabled;
+	}
+
+	if (disabled) {
+		return;
+	}
+
+	PX4_DEBUG("Autosaving params");
+	int ret = param_save_default();
+
+	if (ret != 0) {
+		PX4_ERR("param auto save failed (%i)", ret);
+	}
+}
+

src/lib/parameters/autosave.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/atomic.h>
+#include <drivers/drv_hrt.h>
+
+class ParamAutosave : public px4::ScheduledWorkItem
+{
+public:
+
+	ParamAutosave();
+
+	/**
+	 * Automatically save the parameters after a timeout and at limited rate.
+	 */
+	void request();
+
+	void enable(bool enable);
+
+	bool enabled() const { return !_disabled; }
+
+	hrt_abstime lastAutosave() const { return _last_timestamp; }
+
+	void Run() override;
+
+private:
+	hrt_abstime _last_timestamp{0};
+	px4::atomic_bool _scheduled{false};
+	bool _disabled{false};
+};

src/lib/parameters/flashparams/CMakeLists.txt

@@ -42,7 +42,7 @@ add_library(flashparams
 	flashparams.cpp
 )
 
-add_dependencies(flashparams prebuild_targets)
+add_dependencies(flashparams prebuild_targets parameters_header)
 target_compile_definitions(flashparams PRIVATE -DMODULE_NAME="flashparams")
 target_compile_options(flashparams
 	PRIVATE

src/lib/parameters/flashparams/flashparams.cpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/shutdown.h>
+#include <parameters/px4_parameters.hpp>
 
 #include <string.h>
 #include <stdbool.h>
@@ -54,7 +55,6 @@
 
 #include <parameters/param.h>
 
-#include "../uthash/utarray.h"
 #include <lib/tinybson/tinybson.h>
 #include "flashparams.h"
 #include "flashfs.h"
@@ -78,36 +78,30 @@ struct param_wbuf_s {
 static int
 param_export_internal(param_filter_func filter)
 {
-	struct param_wbuf_s *s = nullptr;
 	bson_encoder_s encoder{};
 	int     result = -1;
 
 	/* Use realloc */
 
 	bson_encoder_init_buf(&encoder, nullptr, 0);
+	auto changed_params = user_config.containedAsBitset();
 
-	/* no modified parameters -> we are done */
-	if (param_values == nullptr) {
-		result = 0;
-		goto out;
-	}
+	for (param_t param = 0; param < user_config.PARAM_COUNT; param++) {
 
-	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
+		if (!changed_params[param] || (filter && !filter(param))) {
+			continue;
+		}
 
 		int32_t i;
 		float   f;
 
-		if (filter && !filter(s->param)) {
-			continue;
-		}
-
 		/* append the appropriate BSON type object */
 
-		switch (param_type(s->param)) {
+		switch (param_type(param)) {
 		case PARAM_TYPE_INT32:
-			i = s->val.i;
+			i = user_config.get(param).i;
 
-			if (bson_encoder_append_int32(&encoder, param_name(s->param), i)) {
+			if (bson_encoder_append_int32(&encoder, param_name(param), i)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}
@@ -115,9 +109,9 @@ param_export_internal(param_filter_func filter)
 			break;
 
 		case PARAM_TYPE_FLOAT:
-			f = s->val.f;
+			f = user_config.get(param).f;
 
-			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
+			if (bson_encoder_append_double(&encoder, param_name(param), (double)f)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
 				goto out;
 			}

src/lib/parameters/flashparams/flashparams.h

@@ -49,6 +49,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include <sys/types.h>
+#include "../DynamicSparseLayer.h"
 
 __BEGIN_DECLS
 
@@ -57,9 +58,9 @@ __BEGIN_DECLS
  * the param_values and 2 functions to be global
  */
 
-__EXPORT extern UT_array        *param_values;
+__EXPORT extern DynamicSparseLayer user_config;
 __EXPORT int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes);
-__EXPORT const void *param_get_value_ptr_external(param_t param);
+__EXPORT void param_get_external(param_t param, void *val);
 
 /* The interface hooks to the Flash based storage. The caller is responsible for locking */
 __EXPORT int flash_param_save(param_filter_func filter);

src/lib/parameters/uthash/doc/utarray.txt

@@ -1,376 +0,0 @@
-utarray: dynamic array macros for C 
-===================================
-Troy D. Hanson <thanson@users.sourceforge.net>
-v1.9.5, November 2011
-
-include::sflogo.txt[]
-include::topnav_utarray.txt[]
-
-Introduction
-------------
-include::toc.txt[]
-
-A set of general-purpose dynamic array macros for C structures are included with
-uthash in `utarray.h`.  To use these macros in your own C program, just
-copy `utarray.h` into your source directory and use it in your programs.
-
-  #include "utarray.h"
-
-The dynamic array supports basic operations such as push, pop, and erase on the
-array elements. These array elements can be any simple datatype or structure.
-The array <<operations,operations>> are based loosely on the C++ STL vector methods.
-
-Internally the dynamic array contains a contiguous memory region into which
-the elements are copied. This buffer is grown as needed using `realloc` to
-accomodate all the data that is pushed into it.
-
-Download
-~~~~~~~~
-To download the `utarray.h` header file, follow the link on the
-http://uthash.sourceforge.net[uthash home page]. 
-
-BSD licensed
-~~~~~~~~~~~~
-This software is made available under the 
-link:license.html[revised BSD license]. 
-It is free and open source. 
-
-Platforms
-~~~~~~~~~
-The 'utarray' macros have been tested on:
-
- * Linux, 
- * Mac OS X, 
- * Windows, using Visual Studio 2008 and Visual Studio 2010
-
-Usage 
------
-
-Declaration
-~~~~~~~~~~~
-
-The array itself has the data type `UT_array`, regardless of the type of
-elements to be stored in it. It is declared like,
-
-  UT_array *nums;
-
-New and free
-~~~~~~~~~~~~
-The next step is to create the array using `utarray_new`. Later when you're
-done with the array, `utarray_free` will free it and all its elements. 
-
-Push, pop, etc
-~~~~~~~~~~~~~~
-The central features of the utarray involve putting elements into it, taking
-them out, and iterating over them. There are several <<operations,operations>>
-to pick from that deal with either single elements or ranges of elements at a
-time. In the examples below we will use only the push operation to insert
-elements. 
-
-Elements
---------
-
-Support for dynamic arrays of integers or strings is especially easy. These are
-best shown by example:
-
-Integers
-~~~~~~~~
-This example makes a utarray of integers, pushes 0-9 into it, then prints it.
-Lastly it frees it.
-
-.Integer elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-int main() {
-  UT_array *nums;
-  int i, *p;
-
-  utarray_new(nums,&ut_int_icd);  
-  for(i=0; i < 10; i++) utarray_push_back(nums,&i);
-
-  for(p=(int*)utarray_front(nums); 
-      p!=NULL; 
-      p=(int*)utarray_next(nums,p)) {
-    printf("%d\n",*p);
-  }
-
-  utarray_free(nums);
-
-  return 0;
-}
--------------------------------------------------------------------------------
-
-The second argument to `utarray_push_back` is always a 'pointer' to the type
-(so a literal cannot be used). So for integers, it is an `int*`.
-
-Strings
-~~~~~~~
-In this example we make a utarray of strings, push two strings into it, print
-it and free it.
-
-.String elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-int main() {
-  UT_array *strs;
-  char *s, **p;
-
-  utarray_new(strs,&ut_str_icd);
-
-  s = "hello"; utarray_push_back(strs, &s);
-  s = "world"; utarray_push_back(strs, &s);
-  p = NULL;
-  while ( (p=(char**)utarray_next(strs,p))) {
-    printf("%s\n",*p);
-  }
-
-  utarray_free(strs);
-
-  return 0;
-}
--------------------------------------------------------------------------------
-
-In this example, since the element is a `char*`, we pass a pointer to it
-(`char**`) as the second argument to `utarray_push_back`. Note that "push" makes
-a copy of the source string and pushes that copy into the array.
-
-About UT_icd
-~~~~~~~~~~~~
-
-Arrays be made of any type of element, not just integers and strings.  The
-elements can be basic types or structures. Unless you're dealing with integers
-and strings (which use pre-defined `ut_int_icd` and `ut_str_icd`), you'll need
-to define a `UT_icd` helper structure. This structure contains everything that
-utarray needs to initialize, copy or destruct elements.
-
-  typedef struct {
-      size_t sz;
-      init_f *init;
-      ctor_f *copy;
-      dtor_f *dtor;
-  } UT_icd;
-
-The three function pointers `init`, `copy`, and `dtor` have these prototypes:
-
-  typedef void (ctor_f)(void *dst, const void *src);
-  typedef void (dtor_f)(void *elt);
-  typedef void (init_f)(void *elt);
-
-The `sz` is just the size of the element being stored in the array.
-
-The `init` function will be invoked whenever utarray needs to initialize an
-empty element. This only happens as a byproduct of `utarray_resize` or
-`utarray_extend_back`. If `init` is `NULL`, it defaults to zero filling the
-new element using memset.
-
-The `copy` function is used whenever an element is copied into the array.
-It is invoked during `utarray_push_back`, `utarray_insert`, `utarray_inserta`,
-or `utarray_concat`. If `copy` is `NULL`, it defaults to a bitwise copy using
-memcpy.
-
-The `dtor` function is used to clean up an element that is being removed from
-the array. It may be invoked due to `utarray_resize`, `utarray_pop_back`,
-`utarray_erase`, `utarray_clear`, `utarray_done` or `utarray_free`. If the
-elements need no cleanup upon destruction, `dtor` may be `NULL`.
-
-Scalar types
-~~~~~~~~~~~~
-
-The next example uses `UT_icd` with all its defaults to make a utarray of
-`long` elements. This example pushes two longs, prints them, and frees the
-array.
-
-.long elements
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-UT_icd long_icd = {sizeof(long), NULL, NULL, NULL };
-
-int main() {
-  UT_array *nums;
-  long l, *p;
-  utarray_new(nums, &long_icd);
-
-  l=1; utarray_push_back(nums, &l);
-  l=2; utarray_push_back(nums, &l);
-
-  p=NULL;
-  while( (p=(long*)utarray_next(nums,p))) printf("%ld\n", *p);
-
-  utarray_free(nums);
-  return 0;
-}
--------------------------------------------------------------------------------
-
-Structures
-~~~~~~~~~~
-
-Structures can be used as utarray elements. If the structure requires no
-special effort to initialize, copy or destruct, we can use `UT_icd` with all
-its defaults. This example shows a structure that consists of two integers. Here
-we push two values, print them and free the array.
-
-.Structure (simple)
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include "utarray.h"
-
-typedef struct {
-    int a;
-    int b;
-} intpair_t;
-
-UT_icd intpair_icd = {sizeof(intpair_t), NULL, NULL, NULL};
-
-int main() {
-
-  UT_array *pairs;
-  intpair_t ip, *p;
-  utarray_new(pairs,&intpair_icd);
-
-  ip.a=1;  ip.b=2;  utarray_push_back(pairs, &ip);
-  ip.a=10; ip.b=20; utarray_push_back(pairs, &ip);
-
-  for(p=(intpair_t*)utarray_front(pairs);
-      p!=NULL;
-      p=(intpair_t*)utarray_next(pairs,p)) {
-    printf("%d %d\n", p->a, p->b);
-  }
-
-  utarray_free(pairs);
-  return 0;
-}
--------------------------------------------------------------------------------
-
-The real utility of `UT_icd` is apparent when the elements of the utarray are
-structures that require special work to initialize, copy or destruct. 
-
-For example, when a structure contains pointers to related memory areas that
-need to be copied when the structure is copied (and freed when the structure is
-freed), we can use custom `init`, `copy`, and `dtor` members in the `UT_icd`.
-
-Here we take an example of a structure that contains an integer and a string.
-When this element is copied (such as when an element is pushed into the array),
-we want to "deep copy" the `s` pointer (so the original element and the new
-element point to their own copies of `s`). When an element is destructed, we
-want to "deep free" its copy of `s`. Lastly, this example is written to work
-even if `s` has the value `NULL`.
-
-.Structure (complex)
--------------------------------------------------------------------------------
-#include <stdio.h>
-#include <stdlib.h>
-#include "utarray.h"
-
-typedef struct {
-    int a;
-    char *s;
-} intchar_t;
-
-void intchar_copy(void *_dst, const void *_src) {
-  intchar_t *dst = (intchar_t*)_dst, *src = (intchar_t*)_src;
-  dst->a = src->a;
-  dst->s = src->s ? strdup(src->s) : NULL;
-}
-
-void intchar_dtor(void *_elt) {
-  intchar_t *elt = (intchar_t*)_elt;
-  if (elt->s) free(elt->s);
-}
-
-UT_icd intchar_icd = {sizeof(intchar_t), NULL, intchar_copy, intchar_dtor};
-
-int main() {
-  UT_array *intchars;
-  intchar_t ic, *p;
-  utarray_new(intchars, &intchar_icd);
-
-  ic.a=1; ic.s="hello"; utarray_push_back(intchars, &ic);
-  ic.a=2; ic.s="world"; utarray_push_back(intchars, &ic);
-
-  p=NULL;
-  while( (p=(intchar_t*)utarray_next(intchars,p))) {
-    printf("%d %s\n", p->a, (p->s ? p->s : "null"));
-  }
-
-  utarray_free(intchars);
-  return 0;
-}
-
--------------------------------------------------------------------------------
-
-[[operations]]
-Reference
----------
-This table lists all the utarray operations. These are loosely based on the C++
-vector class.
-
-Operations
-~~~~~~~~~~
-
-[width="100%",cols="50<m,40<",grid="none",options="none"]
-|===============================================================================
-| utarray_new(UT_array *a, UT_icd *icd)| allocate a new array 
-| utarray_free(UT_array *a)            | free an allocated array
-| utarray_init(UT_array *a,UT_icd *icd)| init an array (non-alloc)
-| utarray_done(UT_array *a)            | dispose of an array (non-allocd)
-| utarray_reserve(UT_array *a,int n)  | ensure space available for 'n' more elements
-| utarray_push_back(UT_array *a,void *p) | push element p onto a
-| utarray_pop_back(UT_array *a)        | pop last element from a
-| utarray_extend_back(UT_array *a)     | push empty element onto a
-| utarray_len(UT_array *a)             | get length of a 
-| utarray_eltptr(UT_array *a,int j)    | get pointer of element from index 
-| utarray_eltidx(UT_array *a,void *e)  | get index of element from pointer
-| utarray_insert(UT_array *a,void *p, int j) | insert element p to index j
-| utarray_inserta(UT_array *a,UT_array *w, int j) | insert array w into array a at index j
-| utarray_resize(UT_array *dst,int num)  | extend or shrink array to num elements
-| utarray_concat(UT_array *dst,UT_array *src) | copy src to end of dst array
-| utarray_erase(UT_array *a,int pos,int len) | remove len elements from a[pos]..a[pos+len-1]
-| utarray_clear(UT_array *a) | clear all elements from a, setting its length to zero
-| utarray_sort(UT_array *a,cmpfcn *cmp) | sort elements of a using comparison function 
-| utarray_find(UT_array *a,void *v, cmpfcn *cmp) | find element v in utarray (must be sorted)
-| utarray_front(UT_array *a) | get first element of a
-| utarray_next(UT_array *a,void *e) | get element of a following e (front if e is NULL)
-| utarray_prev(UT_array *a,void *e) | get element of a before e (back if e is NULL)
-| utarray_back(UT_array *a) | get last element of a
-|===============================================================================
-
-Notes
-~~~~~
-
-1. `utarray_new` and `utarray_free` are used to allocate a new array and free it,
-   while `utarray_init` and `utarray_done` can be used if the UT_array is already
-   allocated and just needs to be initialized or have its internal resources
-   freed.
-2. `utarray_reserve` takes the "delta" of elements to reserve (not the total 
-   desired capacity of the array-- this differs from the C++ STL "reserve" notion)
-3. `utarray_sort` expects a comparison function having the usual `strcmp` -like
-   convention where it accepts two elements (a and b) and returns a negative 
-   value if a precedes b, 0 if a and b sort equally, and positive if b precedes a.
-   This is an example of a comparison function:
-
-  int intsort(const void *a,const void*b) {
-      int _a = *(int*)a;
-      int _b = *(int*)b;
-      return _a - _b;
-  }
-
-4. `utarray_find` uses a binary search to locate an element having a certain value
-   according to the given comparison function. The utarray must be first sorted
-   using the same comparison function. An example of using `utarray_find` with
-   a utarray of strings is included in `tests/test61.c`.
-
-5. A 'pointer' to a particular element (obtained using `utarray_eltptr` or
-   `utarray_front`, `utarray_next`, `utarray_prev`, `utarray_back`) becomes invalid whenever
-   another element is inserted into the utarray. This is because the internal
-   memory management may need to `realloc` the element storage to a new address.
-   For this reason, it's usually better to refer to an element by its integer
-   'index' in code whose duration may include element insertion.
-
-// vim: set nowrap syntax=asciidoc: 
-

src/lib/parameters/uthash/utarray.h

@@ -1,234 +0,0 @@
-/*
-Copyright (c) 2008-2012, Troy D. Hanson   http://uthash.sourceforge.net
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-
-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.
-*/
-
-/* a dynamic array implementation using macros 
- * see http://uthash.sourceforge.net/utarray
- */
-#ifndef UTARRAY_H
-#define UTARRAY_H
-
-#define UTARRAY_VERSION 1.9.6
-
-#ifdef __GNUC__
-#define _UNUSED_ __attribute__ ((__unused__)) 
-#else
-#define _UNUSED_ 
-#endif
-
-#include <stddef.h>  /* size_t */
-#include <string.h>  /* memset, etc */
-#include <stdlib.h>  /* exit */
-
-// FIXME: this needs to be checked: we need to handle OOM properly instead of just exiting
-#define oom() system_exit(-1)
-
-typedef void (ctor_f)(void *dst, const void *src);
-typedef void (dtor_f)(void *elt);
-typedef void (init_f)(void *elt);
-typedef struct {
-    size_t sz;
-    init_f *init;
-    ctor_f *copy;
-    dtor_f *dtor;
-} UT_icd;
-
-typedef struct {
-    unsigned i,n;/* i: index of next available slot, n: num slots */
-    UT_icd icd;  /* initializer, copy and destructor functions */
-    char *d;     /* n slots of size icd->sz*/
-} UT_array;
-
-#define utarray_init(a,_icd) do {                                             \
-  memset(a,0,sizeof(UT_array));                                               \
-  (a)->icd=*_icd;                                                             \
-} while(0)
-
-#define utarray_done(a) do {                                                  \
-  if ((a)->n) {                                                               \
-    if ((a)->icd.dtor) {                                                      \
-      size_t _ut_i;                                                           \
-      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
-        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
-      }                                                                       \
-    }                                                                         \
-    free((a)->d);                                                             \
-  }                                                                           \
-  (a)->n=0;                                                                   \
-} while(0)
-
-#define utarray_new(a,_icd) do {                                              \
-  a=(UT_array*)malloc(sizeof(UT_array));                                      \
-  utarray_init(a,_icd);                                                       \
-} while(0)
-
-#define utarray_free(a) do {                                                  \
-  utarray_done(a);                                                            \
-  free(a);                                                                    \
-} while(0)
-
-#define utarray_reserve(a,by) do {                                            \
-  if (((a)->i+by) > ((a)->n)) {                                               \
-    while(((a)->i+by) > ((a)->n)) { (a)->n = ((a)->n ? (2*(a)->n) : 8); }     \
-    if ( ((a)->d=(char*)realloc((a)->d, (a)->n*(a)->icd.sz)) == NULL) oom();  \
-  }                                                                           \
-} while(0)
-
-#define utarray_push_back(a,p) do {                                           \
-  utarray_reserve(a,1);                                                       \
-  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,(a)->i++), p); }      \
-  else { memcpy(_utarray_eltptr(a,(a)->i++), p, (a)->icd.sz); };              \
-} while(0)
-
-#define utarray_pop_back(a) do {                                              \
-  if ((a)->icd.dtor) { (a)->icd.dtor( _utarray_eltptr(a,--((a)->i))); }       \
-  else { (a)->i--; }                                                          \
-} while(0)
-
-#define utarray_extend_back(a) do {                                           \
-  utarray_reserve(a,1);                                                       \
-  if ((a)->icd.init) { (a)->icd.init(_utarray_eltptr(a,(a)->i)); }            \
-  else { memset(_utarray_eltptr(a,(a)->i),0,(a)->icd.sz); }                   \
-  (a)->i++;                                                                   \
-} while(0)
-
-#define utarray_len(a) ((a)->i)
-
-#define utarray_eltptr(a,j) (((j) < (a)->i) ? _utarray_eltptr(a,j) : NULL)
-#define _utarray_eltptr(a,j) ((char*)((a)->d + ((a)->icd.sz*(j) )))
-
-#define utarray_insert(a,p,j) do {                                            \
-  utarray_reserve(a,1);                                                       \
-  if (j > (a)->i) break;                                                      \
-  if ((j) < (a)->i) {                                                         \
-    memmove( _utarray_eltptr(a,(j)+1), _utarray_eltptr(a,j),                  \
-             ((a)->i - (j))*((a)->icd.sz));                                   \
-  }                                                                           \
-  if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,j), p); }             \
-  else { memcpy(_utarray_eltptr(a,j), p, (a)->icd.sz); };                     \
-  (a)->i++;                                                                   \
-} while(0)
-
-#define utarray_inserta(a,w,j) do {                                           \
-  if (utarray_len(w) == 0) break;                                             \
-  if (j > (a)->i) break;                                                      \
-  utarray_reserve(a,utarray_len(w));                                          \
-  if ((j) < (a)->i) {                                                         \
-    memmove(_utarray_eltptr(a,(j)+utarray_len(w)),                            \
-            _utarray_eltptr(a,j),                                             \
-            ((a)->i - (j))*((a)->icd.sz));                                    \
-  }                                                                           \
-  if ((a)->icd.copy) {                                                        \
-    size_t _ut_i;                                                             \
-    for(_ut_i=0;_ut_i<(w)->i;_ut_i++) {                                       \
-      (a)->icd.copy(_utarray_eltptr(a,j+_ut_i), _utarray_eltptr(w,_ut_i));    \
-    }                                                                         \
-  } else {                                                                    \
-    memcpy(_utarray_eltptr(a,j), _utarray_eltptr(w,0),                        \
-           utarray_len(w)*((a)->icd.sz));                                     \
-  }                                                                           \
-  (a)->i += utarray_len(w);                                                   \
-} while(0)
-
-#define utarray_resize(dst,num) do {                                          \
-  size_t _ut_i;                                                               \
-  if (dst->i > (size_t)(num)) {                                               \
-    if ((dst)->icd.dtor) {                                                    \
-      for(_ut_i=num; _ut_i < dst->i; _ut_i++) {                               \
-        (dst)->icd.dtor(utarray_eltptr(dst,_ut_i));                           \
-      }                                                                       \
-    }                                                                         \
-  } else if (dst->i < (size_t)(num)) {                                        \
-    utarray_reserve(dst,num-dst->i);                                          \
-    if ((dst)->icd.init) {                                                    \
-      for(_ut_i=dst->i; _ut_i < num; _ut_i++) {                               \
-        (dst)->icd.init(utarray_eltptr(dst,_ut_i));                           \
-      }                                                                       \
-    } else {                                                                  \
-      memset(_utarray_eltptr(dst,dst->i),0,(dst)->icd.sz*(num-dst->i));       \
-    }                                                                         \
-  }                                                                           \
-  dst->i = num;                                                               \
-} while(0)
-
-#define utarray_concat(dst,src) do {                                          \
-  utarray_inserta((dst),(src),utarray_len(dst));                              \
-} while(0)
-
-#define utarray_erase(a,pos,len) do {                                         \
-  if ((a)->icd.dtor) {                                                        \
-    size_t _ut_i;                                                             \
-    for(_ut_i=0; _ut_i < len; _ut_i++) {                                      \
-      (a)->icd.dtor(utarray_eltptr((a),pos+_ut_i));                           \
-    }                                                                         \
-  }                                                                           \
-  if ((a)->i > (pos+len)) {                                                   \
-    memmove( _utarray_eltptr((a),pos), _utarray_eltptr((a),pos+len),          \
-            (((a)->i)-(pos+len))*((a)->icd.sz));                              \
-  }                                                                           \
-  (a)->i -= (len);                                                            \
-} while(0)
-
-#define utarray_renew(a,u) do {                                               \
-  if (a) utarray_clear(a); \
-  else utarray_new((a),(u));   \
-} while(0) 
-
-#define utarray_clear(a) do {                                                 \
-  if ((a)->i > 0) {                                                           \
-    if ((a)->icd.dtor) {                                                      \
-      size_t _ut_i;                                                           \
-      for(_ut_i=0; _ut_i < (a)->i; _ut_i++) {                                 \
-        (a)->icd.dtor(utarray_eltptr(a,_ut_i));                               \
-      }                                                                       \
-    }                                                                         \
-    (a)->i = 0;                                                               \
-  }                                                                           \
-} while(0)
-
-#define utarray_sort(a,cmp) do {                                              \
-  qsort((a)->d, (a)->i, (a)->icd.sz, cmp);                                    \
-} while(0)
-
-#define utarray_find(a,v,cmp) bsearch((v),(a)->d,(a)->i,(a)->icd.sz,cmp)
-
-#define utarray_front(a) (((a)->i) ? (_utarray_eltptr(a,0)) : NULL)
-#define utarray_next(a,e) (((e)==NULL) ? utarray_front(a) : ((((a)->i) > (utarray_eltidx(a,e)+1)) ? _utarray_eltptr(a,utarray_eltidx(a,e)+1) : NULL))
-#define utarray_prev(a,e) (((e)==NULL) ? utarray_back(a) : ((utarray_eltidx(a,e) > 0) ? _utarray_eltptr(a,utarray_eltidx(a,e)-1) : NULL))
-#define utarray_back(a) (((a)->i) ? (_utarray_eltptr(a,(a)->i-1)) : NULL)
-#define utarray_eltidx(a,e) (((char*)(e) >= (char*)((a)->d)) ? (((char*)(e) - (char*)((a)->d))/(a)->icd.sz) : -1)
-
-/* last we pre-define a few icd for common utarrays of ints and strings */
-static void utarray_str_cpy(void *dst, const void *src) {
-  char **_src = (char**)src, **_dst = (char**)dst;
-  *_dst = (*_src == NULL) ? NULL : strdup(*_src);
-}
-static void utarray_str_dtor(void *elt) {
-  char **eltc = (char**)elt;
-  if (*eltc) free(*eltc);
-}
-static const UT_icd ut_str_icd _UNUSED_ = {sizeof(char*),NULL,utarray_str_cpy,utarray_str_dtor};
-static const UT_icd ut_int_icd _UNUSED_ = {sizeof(int),NULL,NULL,NULL};
-static const UT_icd ut_ptr_icd _UNUSED_ = {sizeof(void*),NULL,NULL,NULL};
-
-
-#endif /* UTARRAY_H */

src/lib/rate_control/rate_control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -40,7 +40,7 @@
 
 using namespace matrix;
 
-void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
+void RateControl::setPidGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
 {
 	_gain_p = P;
 	_gain_i = I;

src/lib/rate_control/rate_control.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -51,12 +51,12 @@ public:
 	~RateControl() = default;
 
 	/**
-	 * Set the rate control gains
+	 * Set the rate control PID gains
 	 * @param P 3D vector of proportional gains for body x,y,z axis
 	 * @param I 3D vector of integral gains
 	 * @param D 3D vector of derivative gains
 	 */
-	void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
+	void setPidGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
 
 	/**
 	 * Set the mximum absolute value of the integrator for all axes
@@ -94,6 +94,18 @@ public:
 	 */
 	void resetIntegral() { _rate_int.zero(); }
 
+	/**
+	 * Set the integral term to 0 for specific axes
+	 * @param  axis roll 0 / pitch 1 / yaw 2
+	 * @see _rate_int
+	 */
+	void resetIntegral(size_t axis)
+	{
+		if (axis < 3) {
+			_rate_int(axis) = 0.f;
+		}
+	}
+
 	/**
 	 * Get status message of controller for logging/debugging
 	 * @param rate_ctrl_status status message to fill with internal states

src/lib/sensor_calibration/Magnetometer.cpp

@@ -65,6 +65,46 @@ void Magnetometer::set_device_id(uint32_t device_id)
 		Reset();
 
 		ParametersUpdate();
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+void Magnetometer::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.mag_device_ids[i] == _device_id) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.mag_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.mag_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.mag_offset_2};
+						return;
+					case 3:
+						_thermal_offset = Vector3f{corrections.mag_offset_3};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
 	}
 }
 

src/lib/sensor_calibration/Magnetometer.hpp

@@ -39,6 +39,7 @@
 #include <px4_platform_common/log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_correction.h>
 
 namespace calibration
 {
@@ -98,15 +99,21 @@ public:
 
 	void Reset();
 
+	void SensorCorrectionsUpdate(bool force = false);
+
 	void UpdatePower(float power) { _power = power; }
 
 private:
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
+
 	Rotation _rotation_enum{ROTATION_NONE};
 
 	matrix::Dcmf _rotation;
 	matrix::Vector3f _offset;
 	matrix::Matrix3f _scale;
+	matrix::Vector3f _thermal_offset;
 	matrix::Vector3f _power_compensation;
+
 	float _power{0.f};
 
 	int8_t _calibration_index{-1};

src/lib/sensor_calibration/Utilities.cpp

@@ -64,7 +64,7 @@ int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id)
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
 		char str[20] {};
-		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 
 		int32_t device_id_val = 0;
 
@@ -103,7 +103,7 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
 
 	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
 		char str[20] {};
-		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+		snprintf(str, sizeof(str), "CAL_%s%u_ID", sensor_type, i);
 		int32_t device_id_val = 0;
 
 		if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
@@ -138,7 +138,7 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
 {
 	// eg CAL_MAGn_ID/CAL_MAGn_ROT
 	char str[20] {};
-	sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	int32_t value = 0;
 
@@ -153,7 +153,7 @@ float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, ui
 {
 	// eg CAL_BAROn_OFF
 	char str[20] {};
-	sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
 
 	float value = NAN;
 
@@ -174,7 +174,7 @@ Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_t
 		char axis_char = 'X' + axis;
 
 		// eg CAL_MAGn_{X,Y,Z}OFF
-		sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
+		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
 
 		if (param_get(param_find(str), &values(axis)) != 0) {
 			PX4_ERR("failed to get %s", str);
@@ -193,7 +193,7 @@ bool SetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type,
 		char axis_char = 'X' + axis;
 
 		// eg CAL_MAGn_{X,Y,Z}OFF
-		sprintf(str, "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
+		snprintf(str, sizeof(str), "CAL_%s%" PRIu8 "_%c%s", sensor_type, instance, axis_char, cal_type);
 
 		if (param_set_no_notification(param_find(str), &values(axis)) != 0) {
 			PX4_ERR("failed to set %s = %.4f", str, (double)values(axis));

src/lib/sensor_calibration/Utilities.hpp

@@ -88,7 +88,7 @@ bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t
 	char str[20] {};
 
 	// eg CAL_MAGn_ID/CAL_MAGn_ROT
-	sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type);
+	snprintf(str, sizeof(str), "CAL_%s%u_%s", sensor_type, instance, cal_type);
 
 	int ret = param_set_no_notification(param_find(str), &value);
 

src/lib/version/px_update_git_header.py

@@ -119,21 +119,6 @@ header += f"""
 """
 
 
-# ECL
-if (os.path.exists('src/lib/ecl/.git')):
-    ecl_git_tag = subprocess.check_output('git describe --always --tags --dirty'.split(),
-                                  cwd='src/lib/ecl', stderr=subprocess.STDOUT).decode('utf-8')
-
-    ecl_git_version = subprocess.check_output('git rev-parse --verify HEAD'.split(),
-                                      cwd='src/lib/ecl', stderr=subprocess.STDOUT).decode('utf-8').strip()
-    ecl_git_version_short = ecl_git_version[0:16]
-
-    header += f"""
-#define ECL_LIB_GIT_VERSION_STR  "{ecl_git_version}"
-#define ECL_LIB_GIT_VERSION_BINARY 0x{ecl_git_version_short}
-"""
-
-
 # Mavlink
 if (os.path.exists('src/modules/mavlink/mavlink/.git')):
     mavlink_git_version = subprocess.check_output('git rev-parse --verify HEAD'.split(),

src/lib/version/version.c

@@ -345,14 +345,6 @@ uint64_t px4_firmware_version_binary(void)
 	return PX4_GIT_VERSION_BINARY;
 }
 
-const char *px4_ecl_lib_version_string(void)
-{
-#ifdef ECL_LIB_GIT_VERSION_STRING
-	return ECL_LIB_GIT_VERSION_STRING;
-#else
-	return NULL;
-#endif
-}
 
 #ifdef MAVLINK_LIB_GIT_VERSION_BINARY
 uint64_t px4_mavlink_lib_version_binary(void)

src/lib/version/version.h

@@ -179,11 +179,6 @@ __EXPORT const char *px4_firmware_git_branch(void);
  */
 __EXPORT uint64_t px4_firmware_version_binary(void);
 
-/**
- * ECL lib version as human readable string (git tag)
- */
-__EXPORT const char *px4_ecl_lib_version_string(void);
-
 /**
  * MAVLink lib version in binary form (first part of the git tag)
  */

src/modules/commander/Commander.cpp

@@ -32,23 +32,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.cpp
- *
- * Main state machine / business logic
- *
+ * @file Commander.cpp
+ * Primary state machine and flight stack logic
  */
 
 #include "Commander.hpp"
 
-/* commander module headers */
+/* Commander module headers */
 #include "Arming/ArmAuthorization/ArmAuthorization.h"
 #include "commander_helper.h"
 #include "esc_calibration.h"
-#define DEFINE_GET_PX4_CUSTOM_MODE
-#include "px4_custom_mode.h"
 #include "ModeUtil/control_mode.hpp"
 #include "ModeUtil/conversions.hpp"
 
+#define DEFINE_GET_PX4_CUSTOM_MODE
+#include "px4_custom_mode.h"
+
 /* PX4 headers */
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
@@ -69,23 +68,8 @@
 #include <cstring>
 #include <matrix/math.hpp>
 
-#include <uORB/topics/mavlink_log.h>
-#include <uORB/topics/tune_control.h>
-
-typedef enum VEHICLE_MODE_FLAG {
-	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED  = 1,   /* 0b00000001 Reserved for future use. | */
-	VEHICLE_MODE_FLAG_TEST_ENABLED         = 2,   /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
-	VEHICLE_MODE_FLAG_AUTO_ENABLED         = 4,   /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
-	VEHICLE_MODE_FLAG_GUIDED_ENABLED       = 8,   /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
-	VEHICLE_MODE_FLAG_STABILIZE_ENABLED    = 16,  /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
-	VEHICLE_MODE_FLAG_HIL_ENABLED          = 32,  /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
-	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,  /* 0b01000000 remote control input is enabled. | */
-	VEHICLE_MODE_FLAG_SAFETY_ARMED         = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
-	VEHICLE_MODE_FLAG_ENUM_END             = 129, /*  | */
-} VEHICLE_MODE_FLAG;
-
 // TODO: generate
-static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b)
+static constexpr bool operator == (const actuator_armed_s &a, const actuator_armed_s &b)
 {
 	return (a.armed == b.armed &&
 		a.prearmed == b.prearmed &&
@@ -95,9 +79,11 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
 		a.force_failsafe == b.force_failsafe &&
 		a.in_esc_calibration_mode == b.in_esc_calibration_mode);
 }
+
 static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
 
 #if defined(BOARD_HAS_POWER_CONTROL)
+static orb_advert_t power_button_state_pub = nullptr;
 static orb_advert_t tune_control_pub = nullptr;
 
 static void play_power_button_down_tune()
@@ -114,7 +100,6 @@ static void stop_tune()
 	orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
 }
 
-static orb_advert_t power_button_state_pub = nullptr;
 static int power_button_state_notification_cb(board_power_button_state_notification_e request)
 {
 	// Note: this can be called from IRQ handlers, so we publish a message that will be handled
@@ -228,6 +213,56 @@ static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = N
 }
 #endif
 
+Commander::Commander() :
+	ModuleParams(nullptr)
+{
+	_vehicle_land_detected.landed = true;
+
+	_vehicle_status.system_id = 1;
+	_vehicle_status.component_id = 1;
+
+	_vehicle_status.system_type = 0;
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
+
+	_vehicle_status.nav_state = _user_mode_intention.get();
+	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
+	_vehicle_status.nav_state_timestamp = hrt_absolute_time();
+
+	/* mark all signals lost as long as they haven't been found */
+	_vehicle_status.gcs_connection_lost = true;
+
+	_vehicle_status.power_input_valid = true;
+
+	// default for vtol is rotary wing
+	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+
+	_param_mav_comp_id = param_find("MAV_COMP_ID");
+	_param_mav_sys_id = param_find("MAV_SYS_ID");
+	_param_mav_type = param_find("MAV_TYPE");
+	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
+
+	updateParameters();
+}
+
+Commander::~Commander()
+{
+	perf_free(_loop_perf);
+	perf_free(_preflight_check_perf);
+}
+
+Commander *Commander::instantiate(int argc, char *argv[])
+{
+	Commander *instance = new Commander();
+
+	if (instance) {
+		if (argc >= 2 && !strcmp(argv[1], "-h")) {
+			instance->enableHIL();
+		}
+	}
+
+	return instance;
+}
+
 int Commander::custom_command(int argc, char *argv[])
 {
 	if (!is_running()) {
@@ -471,22 +506,6 @@ int Commander::custom_command(int argc, char *argv[])
 	return print_usage("unknown command");
 }
 
-int Commander::print_status()
-{
-	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
-	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
-	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
-	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
-	perf_print_counter(_loop_perf);
-	perf_print_counter(_preflight_check_perf);
-	return 0;
-}
-
-extern "C" __EXPORT int commander_main(int argc, char *argv[])
-{
-	return Commander::main(argc, argv);
-}
-
 bool Commander::shutdownIfAllowed()
 {
 	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
@@ -638,45 +657,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 	return arming_res;
 }
 
-Commander::Commander() :
-	ModuleParams(nullptr)
-{
-	_vehicle_land_detected.landed = true;
-
-	_vehicle_status.system_id = 1;
-	_vehicle_status.component_id = 1;
-
-	_vehicle_status.system_type = 0;
-	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
-
-	_vehicle_status.nav_state = _user_mode_intention.get();
-	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
-	_vehicle_status.nav_state_timestamp = hrt_absolute_time();
-
-	/* mark all signals lost as long as they haven't been found */
-	_vehicle_status.gcs_connection_lost = true;
-
-	_vehicle_status.power_input_valid = true;
-
-	// default for vtol is rotary wing
-	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
-
-	_param_mav_comp_id = param_find("MAV_COMP_ID");
-	_param_mav_sys_id = param_find("MAV_SYS_ID");
-	_param_mav_type = param_find("MAV_TYPE");
-	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
-
-	updateParameters();
-}
-
-Commander::~Commander()
-{
-	perf_free(_loop_perf);
-	perf_free(_preflight_check_perf);
-}
-
-bool
-Commander::handle_command(const vehicle_command_s &cmd)
+bool Commander::handleCommand(const vehicle_command_s &cmd)
 {
 	/* only handle commands that are meant to be handled by this system and component, or broadcast */
 	if (((cmd.target_system != _vehicle_status.system_id) && (cmd.target_system != 0))
@@ -738,7 +719,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-			if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+			if (base_mode == VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
@@ -805,14 +786,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else {
 				/* use base mode */
-				if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+				if (base_mode == VEHICLE_MODE_FLAG_AUTO_ENABLED) {
 					desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
 
-				} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-					if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+				} else if (base_mode == VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode == VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
 						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 
-					} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
+					} else if (base_mode == VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
 						desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 
 					} else {
@@ -913,7 +894,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					_actuator_armed.force_failsafe = true;
 					_flight_termination_triggered = true;
 					PX4_WARN("forcing failsafe (termination)");
-					send_parachute_command();
+					sendParachuteCommand();
 				}
 
 			} else {
@@ -1126,13 +1107,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			if (param1 == 0) {
 				// 0: Do nothing for autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
 			} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
 				// 1: Reboot autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1142,7 +1123,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
 				// 2: Shutdown autopilot
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1152,14 +1133,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
 				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
 #endif // CONFIG_BOARDCTL_RESET
 
 			} else {
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 			}
 		}
 
@@ -1170,7 +1151,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
@@ -1181,14 +1162,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
 				   ) {
 
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					break;
 
 				}
 
 				if ((int)(cmd.param1) == 1) {
 					/* gyro calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
 
@@ -1200,19 +1181,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param2) == 1) {
 					/* magnetometer calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::MagCalibration);
 
 				} else if ((int)(cmd.param3) == 1) {
 					/* baro calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					/* disable RC control input completely */
 					_vehicle_status.rc_calibration_in_progress = true;
 					mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
@@ -1221,39 +1202,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param4) == 2) {
 					/* RC trim calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
 
 				} else if ((int)(cmd.param5) == 2) {
 					// board offset calibration
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
 
 				} else if ((int)(cmd.param5) == 4) {
 					// accelerometer quick calibration
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
 
 				} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
 					// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
 					/* airspeed calibration */
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
 
 				} else if ((int)(cmd.param7) == 1) {
 					/* do esc calibration */
 					if (check_battery_disconnected(&_mavlink_log_pub)) {
-						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+						answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 						if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
 							mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
@@ -1267,7 +1248,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						}
 
 					} else {
-						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
+						answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					}
 
 				} else if ((int)(cmd.param4) == 0) {
@@ -1280,10 +1261,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							     "Calibration: Restoring RC input");
 					}
 
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				} else {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 				}
 			}
 
@@ -1295,10 +1276,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 				// parameter 1: Heading   (degrees)
 				// parameter 3: Latitude  (degrees)
 				// parameter 4: Longitude (degrees)
@@ -1330,28 +1311,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
 				if (((int)(cmd.param1)) == 0) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
 
 				} else if (((int)(cmd.param1)) == 1) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
 
 				} else if (((int)(cmd.param1)) == 2) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig);
 
 				} else if (((int)(cmd.param1)) == 3) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
 
 				} else if (((int)(cmd.param1)) == 4) {
-					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
 				}
 			}
@@ -1361,7 +1342,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
 		_health_and_arming_checks.update(true);
-		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
@@ -1410,13 +1391,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	default:
 		/* Warn about unsupported commands, this makes sense because only commands
 		 * to this component ID (or all) are passed by mavlink. */
-		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+		answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
 
 	if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(cmd, cmd_result);
+		answerCommand(cmd, cmd_result);
 	}
 
 	return true;
@@ -1540,7 +1521,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 			_status_changed = true;
 			_actuator_armed.manual_lockdown = true;
-			send_parachute_command();
+			sendParachuteCommand();
 		}
 
 		break;
@@ -1678,7 +1659,7 @@ void Commander::run()
 
 		handleAutoDisarm();
 
-		battery_status_check();
+		batteryStatusCheck();
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
@@ -1735,7 +1716,7 @@ void Commander::run()
 					PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
 				}
 
-				if (handle_command(cmd)) {
+				if (handleCommand(cmd)) {
 					_status_changed = true;
 				}
 			}
@@ -1818,7 +1799,7 @@ void Commander::run()
 		checkWorkerThread();
 
 		updateTunes();
-		control_status_leds(_status_changed, _battery_warning);
+		controlStatusLEDs(_status_changed, _battery_warning);
 
 		_status_changed = false;
 
@@ -2214,7 +2195,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 
 		if (!_flight_termination_triggered) {
 			_flight_termination_triggered = true;
-			send_parachute_command();
+			sendParachuteCommand();
 		}
 
 		break;
@@ -2250,7 +2231,7 @@ void Commander::checkAndInformReadyForTakeoff()
 #endif // CONFIG_ARCH_BOARD_PX4_SITL
 }
 
-void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
+void Commander::controlStatusLEDs(bool changed, const uint8_t battery_warning)
 {
 	switch (blink_msg_state()) {
 	case 1:
@@ -2432,7 +2413,7 @@ void Commander::printRejectMode(uint8_t nav_state)
 	}
 }
 
-void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
+void Commander::answerCommand(const vehicle_command_s &cmd, uint8_t result)
 {
 	switch (result) {
 	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
@@ -2468,43 +2449,7 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
 	_vehicle_command_ack_pub.publish(command_ack);
 }
 
-int Commander::task_spawn(int argc, char *argv[])
-{
-	_task_id = px4_task_spawn_cmd("commander",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_DEFAULT + 40,
-				      3250,
-				      (px4_main_t)&run_trampoline,
-				      (char *const *)argv);
-
-	if (_task_id < 0) {
-		_task_id = -1;
-		return -errno;
-	}
-
-	// wait until task is up & running
-	if (wait_until_running() < 0) {
-		_task_id = -1;
-		return -1;
-	}
-
-	return 0;
-}
-
-Commander *Commander::instantiate(int argc, char *argv[])
-{
-	Commander *instance = new Commander();
-
-	if (instance) {
-		if (argc >= 2 && !strcmp(argv[1], "-h")) {
-			instance->enable_hil();
-		}
-	}
-
-	return instance;
-}
-
-void Commander::enable_hil()
+void Commander::enableHIL()
 {
 	_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
 }
@@ -2690,7 +2635,7 @@ void Commander::dataLinkCheck()
 	}
 }
 
-void Commander::battery_status_check()
+void Commander::batteryStatusCheck()
 {
 	// Handle shutdown request from emergency battery action
 	if (_battery_warning != _failsafe_flags.battery_warning) {
@@ -2788,7 +2733,7 @@ void Commander::offboardControlCheck()
 	}
 }
 
-void Commander::send_parachute_command()
+void Commander::sendParachuteCommand()
 {
 	vehicle_command_s vcmd{};
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
@@ -2807,6 +2752,17 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }
 
+int Commander::print_status()
+{
+	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
+	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
+	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_preflight_check_perf);
+	return 0;
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {
@@ -2849,3 +2805,31 @@ The commander module contains the state machine for mode switching and failsafe
 
 	return 1;
 }
+
+int Commander::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("commander",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_DEFAULT + 40,
+				      3250,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	// wait until task is up & running
+	if (wait_until_running() < 0) {
+		_task_id = -1;
+		return -1;
+	}
+
+	return 0;
+}
+
+extern "C" __EXPORT int commander_main(int argc, char *argv[])
+{
+	return Commander::main(argc, argv);
+}

src/modules/commander/Commander.hpp

@@ -50,25 +50,21 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
-// publications
 #include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/actuator_test.h>
-#include <uORB/topics/failure_detector_status.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
-
-// subscriptions
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
+
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_test.h>
+#include <uORB/topics/failure_detector_status.h>
 #include <uORB/topics/action_request.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
@@ -78,10 +74,14 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/tune_control.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
 using math::constrain;
@@ -96,13 +96,13 @@ public:
 	~Commander();
 
 	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
+	static int custom_command(int argc, char *argv[]);
 
 	/** @see ModuleBase */
 	static Commander *instantiate(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
 
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
@@ -110,79 +110,12 @@ public:
 	/** @see ModuleBase::run() */
 	void run() override;
 
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
 
-	void enable_hil();
+	void enableHIL();
 
 private:
-	void answer_command(const vehicle_command_s &cmd, uint8_t result);
-
-	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
-
-	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
-
-	void battery_status_check();
-
-	void control_status_leds(bool changed, const uint8_t battery_warning);
-
-	/**
-	 * Checks the status of all available data links and handles switching between different system telemetry states.
-	 */
-	void dataLinkCheck();
-
-	void manualControlCheck();
-
-	void offboardControlCheck();
-
-	/**
-	 * @brief Handle incoming vehicle command relavant to Commander
-	 *
-	 * It ignores irrelevant vehicle commands defined inside the switch case statement
-	 * in the function.
-	 *
-	 * @param cmd 		Vehicle command to handle
-	 */
-	bool handle_command(const vehicle_command_s &cmd);
-
-	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
-
-	void executeActionRequest(const action_request_s &action_request);
-
-	void printRejectMode(uint8_t nav_state);
-
-	void updateControlMode();
-
-	bool shutdownIfAllowed();
-
-	void send_parachute_command();
-
-	void checkForMissionUpdate();
-
-	void handlePowerButtonState();
-
-	void systemPowerUpdate();
-
-	void landDetectorUpdate();
-
-	void safetyButtonUpdate();
-
-	void vtolStatusUpdate();
-
-	void updateTunes();
-
-	void checkWorkerThread();
-
-	bool getPrearmState() const;
-
-	void handleAutoDisarm();
-
-	bool handleModeIntentionAndFailsafe();
-
-	void updateParameters();
-
-	void checkAndInformReadyForTakeoff();
-
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -194,108 +127,177 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
+	enum VEHICLE_MODE_FLAG {
+		VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED  = 1,   /* 0b00000001 Reserved for future use. | */
+		VEHICLE_MODE_FLAG_TEST_ENABLED         = 2,   /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+		VEHICLE_MODE_FLAG_AUTO_ENABLED         = 4,   /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+		VEHICLE_MODE_FLAG_GUIDED_ENABLED       = 8,   /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+		VEHICLE_MODE_FLAG_STABILIZE_ENABLED    = 16,  /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+		VEHICLE_MODE_FLAG_HIL_ENABLED          = 32,  /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+		VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,  /* 0b01000000 remote control input is enabled. | */
+		VEHICLE_MODE_FLAG_SAFETY_ARMED         = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
+		VEHICLE_MODE_FLAG_ENUM_END             = 129, /*  | */
+	};
+
+	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
+
+	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
+
+	void answerCommand(const vehicle_command_s &cmd, uint8_t result);
+
+	void batteryStatusCheck();
+
+	void checkAndInformReadyForTakeoff();
+
+	void checkForMissionUpdate();
+
+	void checkWorkerThread();
+
+	void controlStatusLEDs(bool changed, const uint8_t battery_warning);
+
+	/** Checks the status of all available data links and handles switching between different system telemetry states. */
+	void dataLinkCheck();
+
+	void executeActionRequest(const action_request_s &action_request);
+
+	bool getPrearmState() const;
+
+	void handleAutoDisarm();
+
+	bool handleCommand(const vehicle_command_s &cmd);
+
+	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
+
+	bool handleModeIntentionAndFailsafe();
+
+	void handlePowerButtonState();
+
+	void landDetectorUpdate();
+
+	void manualControlCheck();
+
+	void offboardControlCheck();
+
+	void printRejectMode(uint8_t nav_state);
+
+	void safetyButtonUpdate();
+
+	void sendParachuteCommand();
+
+	bool shutdownIfAllowed();
+
+	void systemPowerUpdate();
+
+	void updateControlMode();
+
+	void updateParameters();
+
+	void updateTunes();
+
+	void vtolStatusUpdate();
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
-	vehicle_status_s        _vehicle_status{};
-
-	ArmStateMachine		_arm_state_machine{};
-	Failsafe		_failsafe_instance{this};
-	FailsafeBase		&_failsafe{_failsafe_instance};
-	FailureDetector		_failure_detector{this};
-	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
-	Safety			_safety{};
-	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
-	WorkerThread 		_worker_thread{};
+	ArmStateMachine       _arm_state_machine{};
+	Failsafe              _failsafe_instance{this};
+	FailsafeBase          &_failsafe{_failsafe_instance};
+	FailureDetector       _failure_detector{this};
+	HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
+	Safety                _safety{};
+	UserModeIntention     _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
+	WorkerThread          _worker_thread{};
 
 	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
-	HomePosition 		_home_position{_failsafe_flags};
 
+	HomePosition            _home_position{_failsafe_flags};
 
 	Hysteresis _auto_disarm_landed{false};
 	Hysteresis _auto_disarm_killed{false};
 
+	hrt_abstime _boot_timestamp{0};
+
 	hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
 	hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
 	hrt_abstime _datalink_last_heartbeat_gcs{0};
 	hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
 	hrt_abstime _datalink_last_heartbeat_parachute_system{0};
 
-	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent
-
 	hrt_abstime _high_latency_datalink_heartbeat{0};
 	hrt_abstime _high_latency_datalink_lost{0};
 
-	hrt_abstime _boot_timestamp{0};
 	hrt_abstime _last_disarmed_timestamp{0};
-	hrt_abstime _overload_start{0};		///< time when CPU overload started
+	hrt_abstime _last_health_and_arming_check{0};
+	hrt_abstime _last_print_mode_reject_time{0};    ///< To remember when last notification was sent
+
+	hrt_abstime _led_overload_toggle{0};
+
+	hrt_abstime _overload_start{0};         ///< time when CPU overload started
 
 #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
 	hrt_abstime _led_armed_state_toggle {0};
 #endif
-	hrt_abstime _led_overload_toggle {0};
 
-	hrt_abstime _last_health_and_arming_check{0};
+	actuator_armed_s        _actuator_armed{};
+	vehicle_control_mode_s  _vehicle_control_mode{};
+	vehicle_land_detected_s _vehicle_land_detected{};
+	vehicle_status_s        _vehicle_status{};
+	vtol_vehicle_status_s   _vtol_vehicle_status{};
 
-	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 
-	bool _failsafe_user_override_request{false}; ///< override request due to stick movements
-
-	bool _flight_termination_triggered{false};
-	bool _lockdown_triggered{false};
-
-	bool _open_drone_id_system_lost{true};
+	bool _arm_tune_played{false};
 	bool _avoidance_system_lost{false};
-	bool _onboard_controller_lost{false};
-	bool _parachute_system_lost{true};
 
-	bool _last_overload{false};
-	bool _mode_switch_mapped{false};
+	bool _failsafe_user_override_request{false}; // override request due to stick movements
+	bool _flight_termination_triggered{false};
+
+	bool _have_taken_off_since_arming{false};
 
 	bool _is_throttle_above_center{false};
 	bool _is_throttle_low{false};
 
-	bool _arm_tune_played{false};
-	bool _was_armed{false};
-	bool _have_taken_off_since_arming{false};
+	bool _last_overload{false};
+	bool _lockdown_triggered{false};
+
+	bool _mode_switch_mapped{false};
+
+	bool _onboard_controller_lost{false};
+	bool _open_drone_id_system_lost{true};
+
+	bool _parachute_system_lost{true};
+
 	bool _status_changed{true};
-
-	vehicle_land_detected_s	_vehicle_land_detected{};
-
-	// commander publications
-	actuator_armed_s        _actuator_armed{};
-	vehicle_control_mode_s  _vehicle_control_mode{};
-	vtol_vehicle_status_s	_vtol_vehicle_status{};
+	bool _was_armed{false};
 
 	// Subscriptions
-	uORB::Subscription					_action_request_sub{ORB_ID(action_request)};
-	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
-	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
-	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
-	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
-
-	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	uORB::SubscriptionMultiArray<telemetry_status_s>	_telemetry_status_subs{ORB_ID::telemetry_status};
+	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
+	uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _system_power_sub{ORB_ID(system_power)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 #if defined(BOARD_HAS_POWER_CONTROL)
-	uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
+	uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
 #endif // BOARD_HAS_POWER_CONTROL
 
-	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
-	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+
+	uORB::SubscriptionData<mission_result_s>        _mission_result_sub{ORB_ID(mission_result)};
+	uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 
 	// Publications
-	uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
-	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
-	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
-	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
-	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
+	uORB::Publication<actuator_armed_s>          _actuator_armed_pub{ORB_ID(actuator_armed)};
+	uORB::Publication<actuator_test_s>           _actuator_test_pub{ORB_ID(actuator_test)};
+	uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
+	uORB::Publication<vehicle_command_ack_s>     _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_control_mode_s>    _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_status_s>          _vehicle_status_pub{ORB_ID(vehicle_status)};
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -61,6 +61,7 @@ px4_add_library(health_and_arming_checks
 	checks/rcAndDataLinkCheck.cpp
 	checks/vtolCheck.cpp
 	checks/offboardCheck.cpp
+	checks/openDroneIDCheck.cpp
 )
 add_dependencies(health_and_arming_checks mode_util)
 

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -67,6 +67,7 @@
 #include "checks/rcAndDataLinkCheck.hpp"
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"
+#include "checks/openDroneIDCheck.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -126,6 +127,7 @@ private:
 	ManualControlChecks _manual_control_checks;
 	HomePositionChecks _home_position_checks;
 	ModeChecks _mode_checks;
+	OpenDroneIDChecks _open_drone_id_checks;
 	ParachuteChecks _parachute_checks;
 	PowerChecks _power_checks;
 	RcCalibrationChecks _rc_calibration_checks;
@@ -140,7 +142,7 @@ private:
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
 
-	HealthAndArmingCheckBase *_checks[30] = {
+	HealthAndArmingCheckBase *_checks[31] = {
 		&_accelerometer_checks,
 		&_airspeed_checks,
 		&_baro_checks,
@@ -157,6 +159,7 @@ private:
 		&_mission_checks,
 		&_offboard_checks, // must be after _estimator_checks
 		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
+		&_open_drone_id_checks,
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "openDroneIDCheck.hpp"
+
+
+void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	// Check to see if the check has been disabled
+	if (!_param_com_arm_odid.get()) {
+		return;
+	}
+
+	NavModes affected_modes{NavModes::None};
+
+	if (_param_com_arm_odid.get() == 2) {
+		// disallow arming without the Open Drone ID system
+		affected_modes = NavModes::All;
+	}
+
+	if (!context.status().open_drone_id_system_present) {
+		/* EVENT
+		 * @description
+		 * Open Drone ID system failed to report. Make sure it is setup and installed properly.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+					    events::ID("check_open_drone_id_missing"),
+					    events::Log::Error, "Open Drone ID system missing");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
+		}
+
+	} else if (!context.status().open_drone_id_system_healthy) {
+		/* EVENT
+		 * @description
+		 * Open Drone ID system reported being unhealthy.
+		 *
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_ODID</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+					    events::ID("check_open_drone_id_unhealthy"),
+					    events::Log::Error, "Open Drone ID system not ready");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system not ready");
+		}
+
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+
+class OpenDroneIDChecks : public HealthAndArmingCheckBase
+{
+public:
+	OpenDroneIDChecks() = default;
+	~OpenDroneIDChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::COM_ARM_ODID>) _param_com_arm_odid
+				       )
+};

src/modules/commander/HealthAndArmingChecks/checks/rcCalibrationCheck.cpp

@@ -53,16 +53,16 @@ RcCalibrationChecks::RcCalibrationChecks()
 	char nbuf[20];
 
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
-		sprintf(nbuf, "RC%d_MIN", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MIN", i + 1);
 		_param_handles[i].min = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_TRIM", i + 1);
 		_param_handles[i].trim = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_MAX", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_MAX", i + 1);
 		_param_handles[i].max = param_find(nbuf);
 
-		sprintf(nbuf, "RC%d_DZ", i + 1);
+		snprintf(nbuf, sizeof(nbuf), "RC%d_DZ", i + 1);
 		_param_handles[i].dz = param_find(nbuf);
 	}
 

src/modules/commander/commander_params.c

@@ -50,8 +50,8 @@
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -66,8 +66,8 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -82,8 +82,8 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
  * copying them using the GCS.
  *
  * @group Radio Calibration
- * @min -0.25
- * @max 0.25
+ * @min -0.5
+ * @max 0.5
  * @decimal 2
  * @increment 0.01
  */
@@ -1003,6 +1003,20 @@ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
 
+/**
+ * Enable Drone ID system detection and health check
+ *
+ * This check detects if the Open Drone ID system is missing.
+ * Depending on the value of the parameter, the check can be
+ * disabled, warn only or deny arming.
+ *
+ * @group Commander
+ * @value 0 Disabled
+ * @value 1 Warning only
+ * @value 2 Enforce Open Drone ID system presence
+ */
+PARAM_DEFINE_INT32(COM_ARM_ODID, 0);
+
 /**
  * Enforced delay between arming and further navigation
  *

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -167,6 +167,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
 			if (_control_status.flags.synthetic_mag_z) {
+				fused[2] = true;
 				continue;
 			}
 
@@ -216,6 +217,11 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	if (fused[0] && fused[1] && fused[2]) {
 		aid_src_mag.fused = true;
 		aid_src_mag.time_last_fuse = _time_delayed_us;
+
+		if (update_all_states) {
+			_time_last_heading_fuse = _time_delayed_us;
+		}
+
 		return true;
 	}
 

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -69,6 +69,12 @@ class VState(sf.Matrix):
 class MState(sf.Matrix):
     SHAPE = (State.n_states, State.n_states)
 
+def state_to_quat(state: VState) -> sf.Quaternion:
+    return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw])
+
+def state_to_rot3(state: VState) -> sf.Rot3:
+    return sf.Rot3(state_to_quat(state))
+
 def predict_covariance(
         state: VState,
         P: MState,
@@ -86,17 +92,17 @@ def predict_covariance(
     d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz])
     d_ang_true = d_ang - d_ang_b
 
-    q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot_simplified(q)
+    q = state_to_quat(state)
+    R_to_earth = sf.Rot3(q)
     v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
     p = sf.V3(state[State.px], state[State.py], state[State.pz])
 
-    q_new = quat_mult(q, sf.V4(1, 0.5 * d_ang_true[0],  0.5 * d_ang_true[1],  0.5 * d_ang_true[2]))
+    q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0],  0.5 * d_ang_true[1],  0.5 * d_ang_true[2]), 1)
     v_new = v + R_to_earth * d_vel_true + sf.V3(0 ,0 ,g) * dt
     p_new = p + v * dt
 
     # Predicted state vector at time t + dt
-    state_new = VState.block_matrix([[q_new], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]])
+    state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]])
 
     # State propagation jacobian
     A = state_new.jacobian(state)
@@ -155,8 +161,7 @@ def predict_sideslip(
 ) -> (sf.Scalar):
 
     vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    relative_wind_body = quat_to_rot(q_att).T * vel_rel
+    relative_wind_body = state_to_rot3(state).inverse() * vel_rel
 
     # Small angle approximation of side slip model
     # Protect division by zero using epsilon
@@ -196,11 +201,10 @@ def compute_sideslip_h_and_k(
     return (H.T, K)
 
 def predict_mag_body(state) -> sf.V3:
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
     mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz])
     mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz])
 
-    mag_body = quat_to_rot(q_att).T * mag_field_earth + mag_bias_body
+    mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body
     return mag_body
 
 def compute_mag_innov_innov_var_and_hx(
@@ -260,8 +264,7 @@ def compute_yaw_321_innov_var_and_h(
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VState):
 
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
+    R_to_earth = state_to_rot3(state).to_rotation_matrix()
     # Fix the singularity at pi/2 by inserting epsilon
     meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon)
 
@@ -277,8 +280,7 @@ def compute_yaw_321_innov_var_and_h_alternate(
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VState):
 
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
+    R_to_earth = state_to_rot3(state).to_rotation_matrix()
     # Alternate form that has a singularity at yaw 0 instead of pi/2
     meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon)
 
@@ -294,8 +296,7 @@ def compute_yaw_312_innov_var_and_h(
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VState):
 
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
+    R_to_earth = state_to_rot3(state).to_rotation_matrix()
     # Alternate form to be used when close to pitch +-pi/2
     meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon)
 
@@ -311,8 +312,7 @@ def compute_yaw_312_innov_var_and_h_alternate(
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VState):
 
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
+    R_to_earth = state_to_rot3(state).to_rotation_matrix()
     # Alternate form to be used when close to pitch +-pi/2
     meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon)
 
@@ -336,9 +336,7 @@ def compute_mag_declination_pred_innov_var_and_h(
     return (meas_pred, innov_var, H.T)
 
 def predict_opt_flow(state, distance, epsilon):
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
-    R_to_body = R_to_earth.T
+    R_to_body = state_to_rot3(state).inverse()
 
     # Calculate earth relative velocity in a non-rotating sensor frame
     v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
@@ -393,8 +391,7 @@ def compute_gnss_yaw_pred_innov_var_and_h(
         epsilon: sf.Scalar
 ) -> (sf.Scalar, sf.Scalar, VState):
 
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
+    R_to_earth = state_to_rot3(state)
 
     # define antenna vector in body frame
     ant_vec_bf = sf.V3(sf.cos(antenna_yaw_offset), sf.sin(antenna_yaw_offset), 0)
@@ -417,9 +414,7 @@ def predict_drag(
         cm: sf.Scalar,
         epsilon: sf.Scalar
         ):
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_earth = quat_to_rot(q_att)
-    R_to_body = R_to_earth.T
+    R_to_body = state_to_rot3(state).inverse()
 
     vel_rel = sf.V3(state[State.vx] - state[State.wx],
                     state[State.vy] - state[State.wy],
@@ -481,8 +476,7 @@ def compute_gravity_innov_var_and_k_and_h(
 ) -> (sf.V3, sf.V3, VState, VState, VState):
 
     # get transform from earth to body frame
-    q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
-    R_to_body = quat_to_rot(q_att).T
+    R_to_body = state_to_rot3(state).inverse()
 
     # the innovation is the error between measured acceleration
     #  and predicted (body frame), assuming no body acceleration

src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py

@@ -38,44 +38,6 @@ import symforce.symbolic as sf
 
 import re
 
-# q: quaternion describing rotation from frame 1 to frame 2
-# returns a rotation matrix derived form q which describes the same
-# rotation
-def quat_to_rot(q):
-    q0 = q[0]
-    q1 = q[1]
-    q2 = q[2]
-    q3 = q[3]
-
-    Rot = sf.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
-                  [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
-                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]])
-
-    return Rot
-
-def quat_to_rot_simplified(q):
-    q0 = q[0]
-    q1 = q[1]
-    q2 = q[2]
-    q3 = q[3]
-
-    # Use the simplified formula for unit quaternion to rotation matrix
-    # as it produces a simpler and more stable EKF derivation given
-    # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1
-    Rot = sf.Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
-                  [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)],
-                   [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]])
-
-    return Rot
-
-def quat_mult(p,q):
-    r = sf.Matrix([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3],
-                p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2],
-                p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1],
-                p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0]])
-
-    return r
-
 def sign_no_zero(x) -> sf.Scalar:
     """
     Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h

@@ -34,136 +34,133 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                               const Scalar cd, const Scalar cm, const Scalar R,
                               const Scalar epsilon, Scalar* const innov_var = nullptr,
                               matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 407
+  // Total ops: 398
 
   // Input arrays
 
-  // Intermediate terms (76)
-  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp1 = 2 * _tmp0;
-  const Scalar _tmp2 = _tmp1 * state(1, 0);
-  const Scalar _tmp3 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp4 = 2 * _tmp3;
-  const Scalar _tmp5 = _tmp4 * state(2, 0);
-  const Scalar _tmp6 = 2 * state(6, 0);
-  const Scalar _tmp7 = _tmp6 * state(0, 0);
-  const Scalar _tmp8 = _tmp2 - _tmp5 - _tmp7;
-  const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp10 = -_tmp9;
-  const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp12 = std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp13 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp14 = -_tmp12 + _tmp13;
-  const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp14;
-  const Scalar _tmp16 = state(0, 0) * state(3, 0);
-  const Scalar _tmp17 = state(1, 0) * state(2, 0);
-  const Scalar _tmp18 = _tmp16 + _tmp17;
-  const Scalar _tmp19 = state(0, 0) * state(2, 0);
-  const Scalar _tmp20 = state(1, 0) * state(3, 0);
-  const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20);
-  const Scalar _tmp22 = state(2, 0) * state(3, 0);
-  const Scalar _tmp23 = state(0, 0) * state(1, 0);
-  const Scalar _tmp24 = _tmp22 - _tmp23;
-  const Scalar _tmp25 = _tmp19 + _tmp20;
-  const Scalar _tmp26 = -_tmp11;
-  const Scalar _tmp27 = _tmp10 + _tmp12 + _tmp13 + _tmp26;
-  const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0);
-  const Scalar _tmp29 = _tmp14 + _tmp26 + _tmp9;
-  const Scalar _tmp30 = -_tmp16 + _tmp17;
-  const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23);
-  const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
-                                         std::pow(_tmp31, Scalar(2)) + epsilon));
-  const Scalar _tmp33 = cd * rho;
-  const Scalar _tmp34 = _tmp32 * _tmp33;
-  const Scalar _tmp35 = Scalar(0.5) * _tmp34;
-  const Scalar _tmp36 = _tmp4 * state(0, 0);
-  const Scalar _tmp37 = _tmp1 * state(3, 0);
-  const Scalar _tmp38 = _tmp6 * state(2, 0);
-  const Scalar _tmp39 = _tmp36 + _tmp37 - _tmp38;
-  const Scalar _tmp40 = 2 * _tmp28;
-  const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0);
-  const Scalar _tmp42 = 2 * _tmp31;
-  const Scalar _tmp43 = 2 * _tmp21;
-  const Scalar _tmp44 = Scalar(0.25) * _tmp21 * _tmp33 / _tmp32;
-  const Scalar _tmp45 =
-      -_tmp35 * _tmp8 - _tmp44 * (_tmp39 * _tmp40 + _tmp41 * _tmp42 + _tmp43 * _tmp8) - _tmp8 * cm;
-  const Scalar _tmp46 = 2 * _tmp19;
-  const Scalar _tmp47 = 2 * _tmp20;
-  const Scalar _tmp48 = -_tmp46 + _tmp47;
-  const Scalar _tmp49 = 2 * _tmp22;
-  const Scalar _tmp50 = 2 * _tmp23;
-  const Scalar _tmp51 = -_tmp35 * _tmp48 -
-                        _tmp44 * (_tmp27 * _tmp40 + _tmp42 * (_tmp49 + _tmp50) + _tmp43 * _tmp48) -
-                        _tmp48 * cm;
-  const Scalar _tmp52 = -_tmp2 + _tmp5 + _tmp7;
-  const Scalar _tmp53 = _tmp4 * state(3, 0);
-  const Scalar _tmp54 = _tmp1 * state(0, 0);
-  const Scalar _tmp55 = _tmp6 * state(1, 0);
-  const Scalar _tmp56 = -_tmp53 + _tmp54 + _tmp55;
-  const Scalar _tmp57 = -_tmp35 * _tmp39 - _tmp39 * cm -
-                        _tmp44 * (_tmp39 * _tmp43 + _tmp40 * _tmp52 + _tmp42 * _tmp56);
-  const Scalar _tmp58 = _tmp15 * cm;
-  const Scalar _tmp59 = _tmp15 * _tmp35;
-  const Scalar _tmp60 = _tmp15 * _tmp43;
-  const Scalar _tmp61 = 2 * _tmp16;
-  const Scalar _tmp62 = 2 * _tmp17;
-  const Scalar _tmp63 =
-      -_tmp44 * (_tmp40 * (_tmp46 + _tmp47) + _tmp42 * (-_tmp61 + _tmp62) + _tmp60) - _tmp58 -
-      _tmp59;
-  const Scalar _tmp64 = 4 * _tmp28;
-  const Scalar _tmp65 = _tmp29 * _tmp42;
-  const Scalar _tmp66 = Scalar(1.0) * _tmp18 * _tmp34 + 2 * _tmp18 * cm -
-                        _tmp44 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp64 - _tmp65);
-  const Scalar _tmp67 =
-      -_tmp35 * _tmp41 - _tmp41 * cm -
-      _tmp44 * (_tmp40 * (_tmp53 - _tmp54 - _tmp55) + _tmp41 * _tmp43 + _tmp42 * _tmp52);
-  const Scalar _tmp68 = _tmp61 + _tmp62;
-  const Scalar _tmp69 = -_tmp35 * _tmp68 -
-                        _tmp44 * (_tmp40 * (_tmp49 - _tmp50) + _tmp43 * _tmp68 + _tmp65) -
-                        _tmp68 * cm;
+  // Intermediate terms (77)
+  const Scalar _tmp0 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp1 = 4 * _tmp0;
+  const Scalar _tmp2 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp3 = 2 * state(0, 0);
+  const Scalar _tmp4 = _tmp2 * _tmp3;
+  const Scalar _tmp5 = 2 * state(6, 0);
+  const Scalar _tmp6 = _tmp5 * state(1, 0);
+  const Scalar _tmp7 = -_tmp1 * state(3, 0) + _tmp4 + _tmp6;
+  const Scalar _tmp8 = 2 * state(2, 0);
+  const Scalar _tmp9 = _tmp2 * _tmp8;
+  const Scalar _tmp10 = 2 * state(1, 0);
+  const Scalar _tmp11 = _tmp0 * _tmp10;
+  const Scalar _tmp12 = _tmp8 * state(3, 0);
+  const Scalar _tmp13 = _tmp3 * state(1, 0);
+  const Scalar _tmp14 = _tmp12 - _tmp13;
+  const Scalar _tmp15 = _tmp8 * state(0, 0);
+  const Scalar _tmp16 = _tmp10 * state(3, 0);
+  const Scalar _tmp17 = _tmp15 + _tmp16;
+  const Scalar _tmp18 = -2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp19 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp20 = _tmp18 + _tmp19;
+  const Scalar _tmp21 = _tmp0 * _tmp17 + _tmp14 * _tmp2 + _tmp20 * state(6, 0);
+  const Scalar _tmp22 = 2 * _tmp21;
+  const Scalar _tmp23 = _tmp0 * _tmp3;
+  const Scalar _tmp24 = 4 * _tmp2;
+  const Scalar _tmp25 = _tmp8 * state(6, 0);
+  const Scalar _tmp26 = -2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp27 = _tmp19 + _tmp26;
+  const Scalar _tmp28 = _tmp3 * state(3, 0);
+  const Scalar _tmp29 = _tmp8 * state(1, 0);
+  const Scalar _tmp30 = -_tmp28 + _tmp29;
+  const Scalar _tmp31 = _tmp12 + _tmp13;
+  const Scalar _tmp32 = _tmp0 * _tmp30 + _tmp2 * _tmp27 + _tmp31 * state(6, 0);
+  const Scalar _tmp33 = 2 * _tmp32;
+  const Scalar _tmp34 = _tmp18 + _tmp26 + 1;
+  const Scalar _tmp35 = _tmp28 + _tmp29;
+  const Scalar _tmp36 = -_tmp15 + _tmp16;
+  const Scalar _tmp37 = _tmp0 * _tmp34 + _tmp2 * _tmp35 + _tmp36 * state(6, 0);
+  const Scalar _tmp38 = 2 * _tmp37;
+  const Scalar _tmp39 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp32, Scalar(2)) +
+                                         std::pow(_tmp37, Scalar(2)) + epsilon));
+  const Scalar _tmp40 = cd * rho;
+  const Scalar _tmp41 = Scalar(0.25) * _tmp37 * _tmp40 / _tmp39;
+  const Scalar _tmp42 = Scalar(0.5) * _tmp39 * _tmp40;
+  const Scalar _tmp43 =
+      -_tmp41 * (_tmp22 * (_tmp11 + _tmp9) + _tmp33 * (-_tmp23 - _tmp24 * state(3, 0) + _tmp25) +
+                 _tmp38 * _tmp7) -
+      _tmp42 * _tmp7 - _tmp7 * cm;
+  const Scalar _tmp44 = _tmp35 * cm;
+  const Scalar _tmp45 = _tmp35 * _tmp38;
+  const Scalar _tmp46 = _tmp14 * _tmp22;
+  const Scalar _tmp47 = _tmp27 * _tmp33;
+  const Scalar _tmp48 = _tmp35 * _tmp42;
+  const Scalar _tmp49 = -_tmp41 * (-_tmp45 - _tmp46 - _tmp47) + _tmp44 + _tmp48;
+  const Scalar _tmp50 = -_tmp41 * (_tmp45 + _tmp46 + _tmp47) - _tmp44 - _tmp48;
+  const Scalar _tmp51 = _tmp5 * state(3, 0);
+  const Scalar _tmp52 = _tmp51 + _tmp9;
+  const Scalar _tmp53 = 2 * state(3, 0);
+  const Scalar _tmp54 = _tmp0 * _tmp53;
+  const Scalar _tmp55 = 4 * state(6, 0);
+  const Scalar _tmp56 = _tmp0 * _tmp8;
+  const Scalar _tmp57 = _tmp3 * state(6, 0);
+  const Scalar _tmp58 =
+      -_tmp41 * (_tmp22 * (-_tmp4 + _tmp54 - _tmp55 * state(1, 0)) +
+                 _tmp33 * (-_tmp24 * state(1, 0) + _tmp56 + _tmp57) + _tmp38 * _tmp52) -
+      _tmp42 * _tmp52 - _tmp52 * cm;
+  const Scalar _tmp59 = _tmp10 * _tmp2;
+  const Scalar _tmp60 = -_tmp1 * state(2, 0) - _tmp57 + _tmp59;
+  const Scalar _tmp61 = _tmp2 * _tmp53;
+  const Scalar _tmp62 = -_tmp41 * (_tmp22 * (_tmp23 - _tmp55 * state(2, 0) + _tmp61) +
+                                   _tmp33 * (_tmp11 + _tmp51) + _tmp38 * _tmp60) -
+                        _tmp42 * _tmp60 - _tmp60 * cm;
+  const Scalar _tmp63 = _tmp34 * cm;
+  const Scalar _tmp64 = _tmp34 * _tmp38;
+  const Scalar _tmp65 = _tmp17 * _tmp22;
+  const Scalar _tmp66 = _tmp30 * _tmp33;
+  const Scalar _tmp67 = _tmp34 * _tmp42;
+  const Scalar _tmp68 = -_tmp41 * (-_tmp64 - _tmp65 - _tmp66) + _tmp63 + _tmp67;
+  const Scalar _tmp69 = -_tmp25 + _tmp61;
   const Scalar _tmp70 =
-      -_tmp35 * _tmp56 -
-      _tmp44 * (_tmp40 * _tmp41 + _tmp42 * (-_tmp36 - _tmp37 + _tmp38) + _tmp43 * _tmp56) -
-      _tmp56 * cm;
-  const Scalar _tmp71 =
-      -_tmp44 * (-_tmp25 * _tmp64 - 4 * _tmp30 * _tmp31 - _tmp60) + _tmp58 + _tmp59;
-  const Scalar _tmp72 = P(23, 23) * _tmp66;
-  const Scalar _tmp73 = P(22, 22) * _tmp71;
-  const Scalar _tmp74 = R +
-                        _tmp45 * (P(0, 2) * _tmp57 + P(1, 2) * _tmp67 + P(2, 2) * _tmp45 +
-                                  P(22, 2) * _tmp71 + P(23, 2) * _tmp66 + P(3, 2) * _tmp70 +
-                                  P(4, 2) * _tmp63 + P(5, 2) * _tmp69 + P(6, 2) * _tmp51) +
-                        _tmp51 * (P(0, 6) * _tmp57 + P(1, 6) * _tmp67 + P(2, 6) * _tmp45 +
-                                  P(22, 6) * _tmp71 + P(23, 6) * _tmp66 + P(3, 6) * _tmp70 +
-                                  P(4, 6) * _tmp63 + P(5, 6) * _tmp69 + P(6, 6) * _tmp51) +
-                        _tmp57 * (P(0, 0) * _tmp57 + P(1, 0) * _tmp67 + P(2, 0) * _tmp45 +
-                                  P(22, 0) * _tmp71 + P(23, 0) * _tmp66 + P(3, 0) * _tmp70 +
-                                  P(4, 0) * _tmp63 + P(5, 0) * _tmp69 + P(6, 0) * _tmp51) +
-                        _tmp63 * (P(0, 4) * _tmp57 + P(1, 4) * _tmp67 + P(2, 4) * _tmp45 +
-                                  P(22, 4) * _tmp71 + P(23, 4) * _tmp66 + P(3, 4) * _tmp70 +
-                                  P(4, 4) * _tmp63 + P(5, 4) * _tmp69 + P(6, 4) * _tmp51) +
-                        _tmp66 * (P(0, 23) * _tmp57 + P(1, 23) * _tmp67 + P(2, 23) * _tmp45 +
-                                  P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp63 +
-                                  P(5, 23) * _tmp69 + P(6, 23) * _tmp51 + _tmp72) +
-                        _tmp67 * (P(0, 1) * _tmp57 + P(1, 1) * _tmp67 + P(2, 1) * _tmp45 +
-                                  P(22, 1) * _tmp71 + P(23, 1) * _tmp66 + P(3, 1) * _tmp70 +
-                                  P(4, 1) * _tmp63 + P(5, 1) * _tmp69 + P(6, 1) * _tmp51) +
-                        _tmp69 * (P(0, 5) * _tmp57 + P(1, 5) * _tmp67 + P(2, 5) * _tmp45 +
-                                  P(22, 5) * _tmp71 + P(23, 5) * _tmp66 + P(3, 5) * _tmp70 +
-                                  P(4, 5) * _tmp63 + P(5, 5) * _tmp69 + P(6, 5) * _tmp51) +
-                        _tmp70 * (P(0, 3) * _tmp57 + P(1, 3) * _tmp67 + P(2, 3) * _tmp45 +
-                                  P(22, 3) * _tmp71 + P(23, 3) * _tmp66 + P(3, 3) * _tmp70 +
-                                  P(4, 3) * _tmp63 + P(5, 3) * _tmp69 + P(6, 3) * _tmp51) +
-                        _tmp71 * (P(0, 22) * _tmp57 + P(1, 22) * _tmp67 + P(2, 22) * _tmp45 +
-                                  P(23, 22) * _tmp66 + P(3, 22) * _tmp70 + P(4, 22) * _tmp63 +
-                                  P(5, 22) * _tmp69 + P(6, 22) * _tmp51 + _tmp73);
-  const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
+      -_tmp41 * (_tmp22 * (_tmp56 - _tmp59) + _tmp33 * (-_tmp54 + _tmp6) + _tmp38 * _tmp69) -
+      _tmp42 * _tmp69 - _tmp69 * cm;
+  const Scalar _tmp71 = -_tmp41 * (_tmp64 + _tmp65 + _tmp66) - _tmp63 - _tmp67;
+  const Scalar _tmp72 = -_tmp36 * _tmp42 - _tmp36 * cm -
+                        _tmp41 * (_tmp20 * _tmp22 + _tmp31 * _tmp33 + _tmp36 * _tmp38);
+  const Scalar _tmp73 = P(23, 23) * _tmp49;
+  const Scalar _tmp74 = P(22, 22) * _tmp68;
+  const Scalar _tmp75 = R +
+                        _tmp43 * (P(0, 3) * _tmp70 + P(1, 3) * _tmp58 + P(2, 3) * _tmp62 +
+                                  P(22, 3) * _tmp68 + P(23, 3) * _tmp49 + P(3, 3) * _tmp43 +
+                                  P(4, 3) * _tmp71 + P(5, 3) * _tmp50 + P(6, 3) * _tmp72) +
+                        _tmp49 * (P(0, 23) * _tmp70 + P(1, 23) * _tmp58 + P(2, 23) * _tmp62 +
+                                  P(22, 23) * _tmp68 + P(3, 23) * _tmp43 + P(4, 23) * _tmp71 +
+                                  P(5, 23) * _tmp50 + P(6, 23) * _tmp72 + _tmp73) +
+                        _tmp50 * (P(0, 5) * _tmp70 + P(1, 5) * _tmp58 + P(2, 5) * _tmp62 +
+                                  P(22, 5) * _tmp68 + P(23, 5) * _tmp49 + P(3, 5) * _tmp43 +
+                                  P(4, 5) * _tmp71 + P(5, 5) * _tmp50 + P(6, 5) * _tmp72) +
+                        _tmp58 * (P(0, 1) * _tmp70 + P(1, 1) * _tmp58 + P(2, 1) * _tmp62 +
+                                  P(22, 1) * _tmp68 + P(23, 1) * _tmp49 + P(3, 1) * _tmp43 +
+                                  P(4, 1) * _tmp71 + P(5, 1) * _tmp50 + P(6, 1) * _tmp72) +
+                        _tmp62 * (P(0, 2) * _tmp70 + P(1, 2) * _tmp58 + P(2, 2) * _tmp62 +
+                                  P(22, 2) * _tmp68 + P(23, 2) * _tmp49 + P(3, 2) * _tmp43 +
+                                  P(4, 2) * _tmp71 + P(5, 2) * _tmp50 + P(6, 2) * _tmp72) +
+                        _tmp68 * (P(0, 22) * _tmp70 + P(1, 22) * _tmp58 + P(2, 22) * _tmp62 +
+                                  P(23, 22) * _tmp49 + P(3, 22) * _tmp43 + P(4, 22) * _tmp71 +
+                                  P(5, 22) * _tmp50 + P(6, 22) * _tmp72 + _tmp74) +
+                        _tmp70 * (P(0, 0) * _tmp70 + P(1, 0) * _tmp58 + P(2, 0) * _tmp62 +
+                                  P(22, 0) * _tmp68 + P(23, 0) * _tmp49 + P(3, 0) * _tmp43 +
+                                  P(4, 0) * _tmp71 + P(5, 0) * _tmp50 + P(6, 0) * _tmp72) +
+                        _tmp71 * (P(0, 4) * _tmp70 + P(1, 4) * _tmp58 + P(2, 4) * _tmp62 +
+                                  P(22, 4) * _tmp68 + P(23, 4) * _tmp49 + P(3, 4) * _tmp43 +
+                                  P(4, 4) * _tmp71 + P(5, 4) * _tmp50 + P(6, 4) * _tmp72) +
+                        _tmp72 * (P(0, 6) * _tmp70 + P(1, 6) * _tmp58 + P(2, 6) * _tmp62 +
+                                  P(22, 6) * _tmp68 + P(23, 6) * _tmp49 + P(3, 6) * _tmp43 +
+                                  P(4, 6) * _tmp71 + P(5, 6) * _tmp50 + P(6, 6) * _tmp72);
+  const Scalar _tmp76 = Scalar(1.0) / (math::max<Scalar>(_tmp75, epsilon));
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
-    _innov_var = _tmp74;
+    _innov_var = _tmp75;
   }
 
   if (K != nullptr) {
@@ -171,12 +168,12 @@ void ComputeDragXInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _k.setZero();
 
-    _k(22, 0) = _tmp75 * (P(22, 0) * _tmp57 + P(22, 1) * _tmp67 + P(22, 2) * _tmp45 +
-                          P(22, 23) * _tmp66 + P(22, 3) * _tmp70 + P(22, 4) * _tmp63 +
-                          P(22, 5) * _tmp69 + P(22, 6) * _tmp51 + _tmp73);
-    _k(23, 0) = _tmp75 * (P(23, 0) * _tmp57 + P(23, 1) * _tmp67 + P(23, 2) * _tmp45 +
-                          P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp63 +
-                          P(23, 5) * _tmp69 + P(23, 6) * _tmp51 + _tmp72);
+    _k(22, 0) = _tmp76 * (P(22, 0) * _tmp70 + P(22, 1) * _tmp58 + P(22, 2) * _tmp62 +
+                          P(22, 23) * _tmp49 + P(22, 3) * _tmp43 + P(22, 4) * _tmp71 +
+                          P(22, 5) * _tmp50 + P(22, 6) * _tmp72 + _tmp74);
+    _k(23, 0) = _tmp76 * (P(23, 0) * _tmp70 + P(23, 1) * _tmp58 + P(23, 2) * _tmp62 +
+                          P(23, 22) * _tmp68 + P(23, 3) * _tmp43 + P(23, 4) * _tmp71 +
+                          P(23, 5) * _tmp50 + P(23, 6) * _tmp72 + _tmp73);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h

@@ -34,130 +34,129 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                               const Scalar cd, const Scalar cm, const Scalar R,
                               const Scalar epsilon, Scalar* const innov_var = nullptr,
                               matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 407
+  // Total ops: 397
 
   // Input arrays
 
   // Intermediate terms (76)
-  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp1 = 2 * _tmp0;
-  const Scalar _tmp2 = _tmp1 * state(1, 0);
-  const Scalar _tmp3 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp4 = 2 * _tmp3;
-  const Scalar _tmp5 = _tmp4 * state(2, 0);
-  const Scalar _tmp6 = 2 * state(6, 0);
-  const Scalar _tmp7 = _tmp6 * state(0, 0);
-  const Scalar _tmp8 = -_tmp2 + _tmp5 + _tmp7;
-  const Scalar _tmp9 = std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp10 = -_tmp9;
-  const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp13 = -_tmp12;
-  const Scalar _tmp14 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp15 = _tmp10 + _tmp11 + _tmp13 + _tmp14;
-  const Scalar _tmp16 = state(0, 0) * state(3, 0);
-  const Scalar _tmp17 = state(1, 0) * state(2, 0);
-  const Scalar _tmp18 = _tmp16 + _tmp17;
-  const Scalar _tmp19 = state(0, 0) * state(2, 0);
-  const Scalar _tmp20 = state(1, 0) * state(3, 0);
-  const Scalar _tmp21 = _tmp1 * _tmp18 + _tmp15 * _tmp3 + _tmp6 * (-_tmp19 + _tmp20);
-  const Scalar _tmp22 = state(2, 0) * state(3, 0);
-  const Scalar _tmp23 = state(0, 0) * state(1, 0);
-  const Scalar _tmp24 = _tmp22 - _tmp23;
-  const Scalar _tmp25 = _tmp19 + _tmp20;
-  const Scalar _tmp26 = _tmp11 - _tmp14;
-  const Scalar _tmp27 = _tmp13 + _tmp26 + _tmp9;
-  const Scalar _tmp28 = _tmp1 * _tmp24 + _tmp25 * _tmp4 + _tmp27 * state(6, 0);
-  const Scalar _tmp29 = _tmp10 + _tmp12 + _tmp26;
-  const Scalar _tmp30 = -_tmp16 + _tmp17;
-  const Scalar _tmp31 = _tmp0 * _tmp29 + _tmp30 * _tmp4 + _tmp6 * (_tmp22 + _tmp23);
-  const Scalar _tmp32 = std::sqrt(Scalar(std::pow(_tmp21, Scalar(2)) + std::pow(_tmp28, Scalar(2)) +
-                                         std::pow(_tmp31, Scalar(2)) + epsilon));
-  const Scalar _tmp33 = cd * rho;
-  const Scalar _tmp34 = _tmp32 * _tmp33;
-  const Scalar _tmp35 = Scalar(0.5) * _tmp34;
-  const Scalar _tmp36 = _tmp4 * state(3, 0);
-  const Scalar _tmp37 = _tmp1 * state(0, 0);
-  const Scalar _tmp38 = _tmp6 * state(1, 0);
-  const Scalar _tmp39 = 2 * _tmp28;
-  const Scalar _tmp40 = 2 * _tmp31;
-  const Scalar _tmp41 = _tmp1 * state(2, 0) + _tmp4 * state(1, 0) + _tmp6 * state(3, 0);
-  const Scalar _tmp42 = 2 * _tmp21;
-  const Scalar _tmp43 = Scalar(0.25) * _tmp31 * _tmp33 / _tmp32;
-  const Scalar _tmp44 =
-      -_tmp35 * _tmp8 -
-      _tmp43 * (_tmp39 * (_tmp36 - _tmp37 - _tmp38) + _tmp40 * _tmp8 + _tmp41 * _tmp42) -
-      _tmp8 * cm;
-  const Scalar _tmp45 = -_tmp36 + _tmp37 + _tmp38;
-  const Scalar _tmp46 = _tmp4 * state(0, 0);
-  const Scalar _tmp47 = _tmp1 * state(3, 0);
-  const Scalar _tmp48 = _tmp6 * state(2, 0);
-  const Scalar _tmp49 = _tmp46 + _tmp47 - _tmp48;
-  const Scalar _tmp50 = -_tmp35 * _tmp45 -
-                        _tmp43 * (_tmp39 * _tmp8 + _tmp40 * _tmp45 + _tmp42 * _tmp49) - _tmp45 * cm;
-  const Scalar _tmp51 =
-      -_tmp35 * _tmp41 - _tmp41 * cm -
-      _tmp43 * (_tmp39 * _tmp49 + _tmp40 * _tmp41 + _tmp42 * (_tmp2 - _tmp5 - _tmp7));
-  const Scalar _tmp52 = -_tmp46 - _tmp47 + _tmp48;
-  const Scalar _tmp53 = -_tmp35 * _tmp52 -
-                        _tmp43 * (_tmp39 * _tmp41 + _tmp40 * _tmp52 + _tmp42 * _tmp45) -
-                        _tmp52 * cm;
-  const Scalar _tmp54 = _tmp29 * cm;
-  const Scalar _tmp55 = _tmp29 * _tmp35;
-  const Scalar _tmp56 = 4 * _tmp28;
-  const Scalar _tmp57 = _tmp29 * _tmp40;
-  const Scalar _tmp58 =
-      -_tmp43 * (-4 * _tmp18 * _tmp21 - _tmp24 * _tmp56 - _tmp57) + _tmp54 + _tmp55;
-  const Scalar _tmp59 = 2 * _tmp16;
-  const Scalar _tmp60 = 2 * _tmp17;
-  const Scalar _tmp61 = -_tmp59 + _tmp60;
-  const Scalar _tmp62 = _tmp15 * _tmp42;
-  const Scalar _tmp63 = 2 * _tmp19;
-  const Scalar _tmp64 = 2 * _tmp20;
-  const Scalar _tmp65 = -_tmp35 * _tmp61 -
-                        _tmp43 * (_tmp39 * (_tmp63 + _tmp64) + _tmp40 * _tmp61 + _tmp62) -
-                        _tmp61 * cm;
-  const Scalar _tmp66 = Scalar(1.0) * _tmp30 * _tmp34 + 2 * _tmp30 * cm -
-                        _tmp43 * (-_tmp25 * _tmp56 - 4 * _tmp30 * _tmp31 - _tmp62);
-  const Scalar _tmp67 = 2 * _tmp22;
-  const Scalar _tmp68 = 2 * _tmp23;
-  const Scalar _tmp69 = _tmp67 + _tmp68;
-  const Scalar _tmp70 = -_tmp35 * _tmp69 -
-                        _tmp43 * (_tmp27 * _tmp39 + _tmp40 * _tmp69 + _tmp42 * (-_tmp63 + _tmp64)) -
+  const Scalar _tmp0 = 2 * state(0, 0);
+  const Scalar _tmp1 = _tmp0 * state(3, 0);
+  const Scalar _tmp2 = 2 * state(1, 0);
+  const Scalar _tmp3 = _tmp2 * state(2, 0);
+  const Scalar _tmp4 = -_tmp1 + _tmp3;
+  const Scalar _tmp5 = _tmp4 * cm;
+  const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp8 = _tmp6 + _tmp7 + 1;
+  const Scalar _tmp9 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp10 = _tmp1 + _tmp3;
+  const Scalar _tmp11 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp12 = _tmp0 * state(2, 0);
+  const Scalar _tmp13 = _tmp2 * state(3, 0);
+  const Scalar _tmp14 = -_tmp12 + _tmp13;
+  const Scalar _tmp15 = _tmp10 * _tmp11 + _tmp14 * state(6, 0) + _tmp8 * _tmp9;
+  const Scalar _tmp16 = 2 * state(2, 0);
+  const Scalar _tmp17 = _tmp16 * state(3, 0);
+  const Scalar _tmp18 = _tmp2 * state(0, 0);
+  const Scalar _tmp19 = _tmp17 - _tmp18;
+  const Scalar _tmp20 = _tmp12 + _tmp13;
+  const Scalar _tmp21 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp22 = _tmp21 + _tmp7;
+  const Scalar _tmp23 = _tmp11 * _tmp19 + _tmp20 * _tmp9 + _tmp22 * state(6, 0);
+  const Scalar _tmp24 = _tmp21 + _tmp6;
+  const Scalar _tmp25 = _tmp17 + _tmp18;
+  const Scalar _tmp26 = _tmp11 * _tmp24 + _tmp25 * state(6, 0) + _tmp4 * _tmp9;
+  const Scalar _tmp27 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp23, Scalar(2)) +
+                                         std::pow(_tmp26, Scalar(2)) + epsilon));
+  const Scalar _tmp28 = cd * rho;
+  const Scalar _tmp29 = Scalar(0.5) * _tmp27 * _tmp28;
+  const Scalar _tmp30 = _tmp29 * _tmp4;
+  const Scalar _tmp31 = 2 * _tmp15;
+  const Scalar _tmp32 = _tmp31 * _tmp8;
+  const Scalar _tmp33 = 2 * _tmp23;
+  const Scalar _tmp34 = _tmp20 * _tmp33;
+  const Scalar _tmp35 = 2 * _tmp26;
+  const Scalar _tmp36 = _tmp35 * _tmp4;
+  const Scalar _tmp37 = Scalar(0.25) * _tmp26 * _tmp28 / _tmp27;
+  const Scalar _tmp38 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5;
+  const Scalar _tmp39 = -_tmp25 * _tmp29 - _tmp25 * cm -
+                        _tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35);
+  const Scalar _tmp40 = 2 * state(3, 0);
+  const Scalar _tmp41 = _tmp40 * _tmp9;
+  const Scalar _tmp42 = _tmp2 * state(6, 0);
+  const Scalar _tmp43 = -_tmp41 + _tmp42;
+  const Scalar _tmp44 = _tmp11 * _tmp2;
+  const Scalar _tmp45 = _tmp16 * _tmp9;
+  const Scalar _tmp46 = _tmp11 * _tmp40;
+  const Scalar _tmp47 = _tmp16 * state(6, 0);
+  const Scalar _tmp48 =
+      -_tmp29 * _tmp43 -
+      _tmp37 * (_tmp31 * (_tmp46 - _tmp47) + _tmp33 * (-_tmp44 + _tmp45) + _tmp35 * _tmp43) -
+      _tmp43 * cm;
+  const Scalar _tmp49 = _tmp2 * _tmp9;
+  const Scalar _tmp50 = _tmp40 * state(6, 0);
+  const Scalar _tmp51 = _tmp49 + _tmp50;
+  const Scalar _tmp52 = _tmp0 * _tmp9;
+  const Scalar _tmp53 = 4 * state(6, 0);
+  const Scalar _tmp54 = 4 * _tmp9;
+  const Scalar _tmp55 = _tmp0 * state(6, 0);
+  const Scalar _tmp56 =
+      -_tmp29 * _tmp51 -
+      _tmp37 * (_tmp31 * (_tmp44 - _tmp54 * state(2, 0) - _tmp55) +
+                _tmp33 * (_tmp46 + _tmp52 - _tmp53 * state(2, 0)) + _tmp35 * _tmp51) -
+      _tmp51 * cm;
+  const Scalar _tmp57 = _tmp24 * cm;
+  const Scalar _tmp58 = _tmp10 * _tmp31;
+  const Scalar _tmp59 = _tmp19 * _tmp33;
+  const Scalar _tmp60 = _tmp24 * _tmp35;
+  const Scalar _tmp61 = _tmp24 * _tmp29;
+  const Scalar _tmp62 = -_tmp37 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61;
+  const Scalar _tmp63 = _tmp0 * _tmp11;
+  const Scalar _tmp64 = _tmp11 * _tmp16;
+  const Scalar _tmp65 = 4 * _tmp11;
+  const Scalar _tmp66 = _tmp45 + _tmp55 - _tmp65 * state(1, 0);
+  const Scalar _tmp67 =
+      -_tmp29 * _tmp66 -
+      _tmp37 * (_tmp31 * (_tmp50 + _tmp64) + _tmp33 * (_tmp41 - _tmp53 * state(1, 0) - _tmp63) +
+                _tmp35 * _tmp66) -
+      _tmp66 * cm;
+  const Scalar _tmp68 = -_tmp37 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61;
+  const Scalar _tmp69 = _tmp47 - _tmp52 - _tmp65 * state(3, 0);
+  const Scalar _tmp70 = -_tmp29 * _tmp69 -
+                        _tmp37 * (_tmp31 * (_tmp42 - _tmp54 * state(3, 0) + _tmp63) +
+                                  _tmp33 * (_tmp49 + _tmp64) + _tmp35 * _tmp69) -
                         _tmp69 * cm;
-  const Scalar _tmp71 =
-      -_tmp43 * (_tmp39 * (_tmp67 - _tmp68) + _tmp42 * (_tmp59 + _tmp60) + _tmp57) - _tmp54 -
-      _tmp55;
-  const Scalar _tmp72 = P(22, 22) * _tmp66;
-  const Scalar _tmp73 = P(23, 23) * _tmp58;
+  const Scalar _tmp71 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5;
+  const Scalar _tmp72 = P(22, 22) * _tmp71;
+  const Scalar _tmp73 = P(23, 23) * _tmp62;
   const Scalar _tmp74 = R +
-                        _tmp44 * (P(0, 1) * _tmp50 + P(1, 1) * _tmp44 + P(2, 1) * _tmp51 +
-                                  P(22, 1) * _tmp66 + P(23, 1) * _tmp58 + P(3, 1) * _tmp53 +
-                                  P(4, 1) * _tmp65 + P(5, 1) * _tmp71 + P(6, 1) * _tmp70) +
-                        _tmp50 * (P(0, 0) * _tmp50 + P(1, 0) * _tmp44 + P(2, 0) * _tmp51 +
-                                  P(22, 0) * _tmp66 + P(23, 0) * _tmp58 + P(3, 0) * _tmp53 +
-                                  P(4, 0) * _tmp65 + P(5, 0) * _tmp71 + P(6, 0) * _tmp70) +
-                        _tmp51 * (P(0, 2) * _tmp50 + P(1, 2) * _tmp44 + P(2, 2) * _tmp51 +
-                                  P(22, 2) * _tmp66 + P(23, 2) * _tmp58 + P(3, 2) * _tmp53 +
-                                  P(4, 2) * _tmp65 + P(5, 2) * _tmp71 + P(6, 2) * _tmp70) +
-                        _tmp53 * (P(0, 3) * _tmp50 + P(1, 3) * _tmp44 + P(2, 3) * _tmp51 +
-                                  P(22, 3) * _tmp66 + P(23, 3) * _tmp58 + P(3, 3) * _tmp53 +
-                                  P(4, 3) * _tmp65 + P(5, 3) * _tmp71 + P(6, 3) * _tmp70) +
-                        _tmp58 * (P(0, 23) * _tmp50 + P(1, 23) * _tmp44 + P(2, 23) * _tmp51 +
-                                  P(22, 23) * _tmp66 + P(3, 23) * _tmp53 + P(4, 23) * _tmp65 +
-                                  P(5, 23) * _tmp71 + P(6, 23) * _tmp70 + _tmp73) +
-                        _tmp65 * (P(0, 4) * _tmp50 + P(1, 4) * _tmp44 + P(2, 4) * _tmp51 +
-                                  P(22, 4) * _tmp66 + P(23, 4) * _tmp58 + P(3, 4) * _tmp53 +
-                                  P(4, 4) * _tmp65 + P(5, 4) * _tmp71 + P(6, 4) * _tmp70) +
-                        _tmp66 * (P(0, 22) * _tmp50 + P(1, 22) * _tmp44 + P(2, 22) * _tmp51 +
-                                  P(23, 22) * _tmp58 + P(3, 22) * _tmp53 + P(4, 22) * _tmp65 +
-                                  P(5, 22) * _tmp71 + P(6, 22) * _tmp70 + _tmp72) +
-                        _tmp70 * (P(0, 6) * _tmp50 + P(1, 6) * _tmp44 + P(2, 6) * _tmp51 +
-                                  P(22, 6) * _tmp66 + P(23, 6) * _tmp58 + P(3, 6) * _tmp53 +
-                                  P(4, 6) * _tmp65 + P(5, 6) * _tmp71 + P(6, 6) * _tmp70) +
-                        _tmp71 * (P(0, 5) * _tmp50 + P(1, 5) * _tmp44 + P(2, 5) * _tmp51 +
-                                  P(22, 5) * _tmp66 + P(23, 5) * _tmp58 + P(3, 5) * _tmp53 +
-                                  P(4, 5) * _tmp65 + P(5, 5) * _tmp71 + P(6, 5) * _tmp70);
+                        _tmp38 * (P(0, 4) * _tmp48 + P(1, 4) * _tmp67 + P(2, 4) * _tmp56 +
+                                  P(22, 4) * _tmp71 + P(23, 4) * _tmp62 + P(3, 4) * _tmp70 +
+                                  P(4, 4) * _tmp38 + P(5, 4) * _tmp68 + P(6, 4) * _tmp39) +
+                        _tmp39 * (P(0, 6) * _tmp48 + P(1, 6) * _tmp67 + P(2, 6) * _tmp56 +
+                                  P(22, 6) * _tmp71 + P(23, 6) * _tmp62 + P(3, 6) * _tmp70 +
+                                  P(4, 6) * _tmp38 + P(5, 6) * _tmp68 + P(6, 6) * _tmp39) +
+                        _tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp67 + P(2, 0) * _tmp56 +
+                                  P(22, 0) * _tmp71 + P(23, 0) * _tmp62 + P(3, 0) * _tmp70 +
+                                  P(4, 0) * _tmp38 + P(5, 0) * _tmp68 + P(6, 0) * _tmp39) +
+                        _tmp56 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp67 + P(2, 2) * _tmp56 +
+                                  P(22, 2) * _tmp71 + P(23, 2) * _tmp62 + P(3, 2) * _tmp70 +
+                                  P(4, 2) * _tmp38 + P(5, 2) * _tmp68 + P(6, 2) * _tmp39) +
+                        _tmp62 * (P(0, 23) * _tmp48 + P(1, 23) * _tmp67 + P(2, 23) * _tmp56 +
+                                  P(22, 23) * _tmp71 + P(3, 23) * _tmp70 + P(4, 23) * _tmp38 +
+                                  P(5, 23) * _tmp68 + P(6, 23) * _tmp39 + _tmp73) +
+                        _tmp67 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp67 + P(2, 1) * _tmp56 +
+                                  P(22, 1) * _tmp71 + P(23, 1) * _tmp62 + P(3, 1) * _tmp70 +
+                                  P(4, 1) * _tmp38 + P(5, 1) * _tmp68 + P(6, 1) * _tmp39) +
+                        _tmp68 * (P(0, 5) * _tmp48 + P(1, 5) * _tmp67 + P(2, 5) * _tmp56 +
+                                  P(22, 5) * _tmp71 + P(23, 5) * _tmp62 + P(3, 5) * _tmp70 +
+                                  P(4, 5) * _tmp38 + P(5, 5) * _tmp68 + P(6, 5) * _tmp39) +
+                        _tmp70 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp67 + P(2, 3) * _tmp56 +
+                                  P(22, 3) * _tmp71 + P(23, 3) * _tmp62 + P(3, 3) * _tmp70 +
+                                  P(4, 3) * _tmp38 + P(5, 3) * _tmp68 + P(6, 3) * _tmp39) +
+                        _tmp71 * (P(0, 22) * _tmp48 + P(1, 22) * _tmp67 + P(2, 22) * _tmp56 +
+                                  P(23, 22) * _tmp62 + P(3, 22) * _tmp70 + P(4, 22) * _tmp38 +
+                                  P(5, 22) * _tmp68 + P(6, 22) * _tmp39 + _tmp72);
   const Scalar _tmp75 = Scalar(1.0) / (math::max<Scalar>(_tmp74, epsilon));
 
   // Output terms (2)
@@ -172,12 +171,12 @@ void ComputeDragYInnovVarAndK(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _k.setZero();
 
-    _k(22, 0) = _tmp75 * (P(22, 0) * _tmp50 + P(22, 1) * _tmp44 + P(22, 2) * _tmp51 +
-                          P(22, 23) * _tmp58 + P(22, 3) * _tmp53 + P(22, 4) * _tmp65 +
-                          P(22, 5) * _tmp71 + P(22, 6) * _tmp70 + _tmp72);
-    _k(23, 0) = _tmp75 * (P(23, 0) * _tmp50 + P(23, 1) * _tmp44 + P(23, 2) * _tmp51 +
-                          P(23, 22) * _tmp66 + P(23, 3) * _tmp53 + P(23, 4) * _tmp65 +
-                          P(23, 5) * _tmp71 + P(23, 6) * _tmp70 + _tmp73);
+    _k(22, 0) = _tmp75 * (P(22, 0) * _tmp48 + P(22, 1) * _tmp67 + P(22, 2) * _tmp56 +
+                          P(22, 23) * _tmp62 + P(22, 3) * _tmp70 + P(22, 4) * _tmp38 +
+                          P(22, 5) * _tmp68 + P(22, 6) * _tmp39 + _tmp72);
+    _k(23, 0) = _tmp75 * (P(23, 0) * _tmp48 + P(23, 1) * _tmp67 + P(23, 2) * _tmp56 +
+                          P(23, 22) * _tmp71 + P(23, 3) * _tmp70 + P(23, 4) * _tmp38 +
+                          P(23, 5) * _tmp68 + P(23, 6) * _tmp39 + _tmp73);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h

@@ -32,43 +32,41 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
                                 const Scalar R, const Scalar epsilon,
                                 matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
                                 matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 285
+  // Total ops: 291
 
   // Input arrays
 
-  // Intermediate terms (29)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 =
+  // Intermediate terms (28)
+  const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp1 =
       Scalar(1.0) /
       (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp4 = _tmp3 * (_tmp0 - _tmp1 + _tmp2);
-  const Scalar _tmp5 = 2 * state(3, 0);
-  const Scalar _tmp6 = _tmp5 * state(0, 0);
-  const Scalar _tmp7 = 2 * state(2, 0);
-  const Scalar _tmp8 = _tmp7 * state(1, 0);
-  const Scalar _tmp9 = _tmp3 * (-_tmp6 + _tmp8);
-  const Scalar _tmp10 = 2 * state(4, 0);
-  const Scalar _tmp11 = _tmp10 * state(0, 0);
-  const Scalar _tmp12 = 2 * state(5, 0);
-  const Scalar _tmp13 = _tmp12 * state(3, 0);
-  const Scalar _tmp14 = _tmp7 * state(6, 0);
-  const Scalar _tmp15 = _tmp3 * (-_tmp11 - _tmp13 + _tmp14);
-  const Scalar _tmp16 = 2 * state(1, 0);
-  const Scalar _tmp17 =
-      _tmp3 * (-_tmp10 * state(3, 0) + _tmp12 * state(0, 0) + _tmp16 * state(6, 0));
-  const Scalar _tmp18 = _tmp7 * state(4, 0);
-  const Scalar _tmp19 = _tmp12 * state(1, 0);
-  const Scalar _tmp20 = 2 * state(0, 0) * state(6, 0);
-  const Scalar _tmp21 = _tmp3 * (_tmp18 - _tmp19 + _tmp20);
-  const Scalar _tmp22 = _tmp3 * (_tmp10 * state(1, 0) + _tmp5 * state(6, 0) + _tmp7 * state(5, 0));
-  const Scalar _tmp23 = _tmp3 * (_tmp16 * state(0, 0) + _tmp7 * state(3, 0));
-  const Scalar _tmp24 = _tmp3 * (-_tmp0 + _tmp1 + _tmp2);
-  const Scalar _tmp25 = _tmp3 * (_tmp6 + _tmp8);
-  const Scalar _tmp26 = _tmp3 * (_tmp16 * state(3, 0) - _tmp7 * state(0, 0));
-  const Scalar _tmp27 = _tmp3 * (_tmp11 + _tmp13 - _tmp14);
-  const Scalar _tmp28 = _tmp3 * (-_tmp18 + _tmp19 - _tmp20);
+  const Scalar _tmp2 = _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2)));
+  const Scalar _tmp3 = 2 * state(1, 0);
+  const Scalar _tmp4 = 2 * state(3, 0);
+  const Scalar _tmp5 = _tmp4 * state(6, 0);
+  const Scalar _tmp6 = _tmp1 * (_tmp3 * state(4, 0) + _tmp5);
+  const Scalar _tmp7 = _tmp3 * state(6, 0);
+  const Scalar _tmp8 = _tmp1 * (-_tmp4 * state(4, 0) + _tmp7);
+  const Scalar _tmp9 = _tmp4 * state(0, 0);
+  const Scalar _tmp10 = _tmp3 * state(2, 0);
+  const Scalar _tmp11 = _tmp1 * (_tmp10 - _tmp9);
+  const Scalar _tmp12 = 2 * state(0, 0);
+  const Scalar _tmp13 = 4 * state(5, 0);
+  const Scalar _tmp14 = 2 * state(2, 0);
+  const Scalar _tmp15 = _tmp14 * state(6, 0);
+  const Scalar _tmp16 = _tmp1 * (-_tmp12 * state(4, 0) - _tmp13 * state(3, 0) + _tmp15);
+  const Scalar _tmp17 = _tmp12 * state(6, 0);
+  const Scalar _tmp18 = _tmp1 * (-_tmp13 * state(1, 0) + _tmp14 * state(4, 0) + _tmp17);
+  const Scalar _tmp19 = _tmp1 * (_tmp12 * state(1, 0) + _tmp4 * state(2, 0));
+  const Scalar _tmp20 = _tmp1 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
+  const Scalar _tmp21 = _tmp1 * (_tmp10 + _tmp9);
+  const Scalar _tmp22 = _tmp1 * (-_tmp12 * state(2, 0) + _tmp4 * state(1, 0));
+  const Scalar _tmp23 = 4 * state(4, 0);
+  const Scalar _tmp24 = _tmp1 * (_tmp12 * state(5, 0) - _tmp23 * state(3, 0) + _tmp7);
+  const Scalar _tmp25 = _tmp1 * (-_tmp17 - _tmp23 * state(2, 0) + _tmp3 * state(5, 0));
+  const Scalar _tmp26 = _tmp1 * (-_tmp15 + _tmp4 * state(5, 0));
+  const Scalar _tmp27 = _tmp1 * (_tmp14 * state(5, 0) + _tmp5);
 
   // Output terms (2)
   if (innov_var != nullptr) {
@@ -76,36 +74,36 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _innov_var(0, 0) =
         R +
-        _tmp15 * (P(0, 3) * _tmp17 + P(1, 3) * _tmp21 + P(2, 3) * _tmp22 + P(3, 3) * _tmp15 +
-                  P(4, 3) * _tmp9 + P(5, 3) * _tmp4 + P(6, 3) * _tmp23) +
-        _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp21 + P(2, 0) * _tmp22 + P(3, 0) * _tmp15 +
-                  P(4, 0) * _tmp9 + P(5, 0) * _tmp4 + P(6, 0) * _tmp23) +
-        _tmp21 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp21 + P(2, 1) * _tmp22 + P(3, 1) * _tmp15 +
-                  P(4, 1) * _tmp9 + P(5, 1) * _tmp4 + P(6, 1) * _tmp23) +
-        _tmp22 * (P(0, 2) * _tmp17 + P(1, 2) * _tmp21 + P(2, 2) * _tmp22 + P(3, 2) * _tmp15 +
-                  P(4, 2) * _tmp9 + P(5, 2) * _tmp4 + P(6, 2) * _tmp23) +
-        _tmp23 * (P(0, 6) * _tmp17 + P(1, 6) * _tmp21 + P(2, 6) * _tmp22 + P(3, 6) * _tmp15 +
-                  P(4, 6) * _tmp9 + P(5, 6) * _tmp4 + P(6, 6) * _tmp23) +
-        _tmp4 * (P(0, 5) * _tmp17 + P(1, 5) * _tmp21 + P(2, 5) * _tmp22 + P(3, 5) * _tmp15 +
-                 P(4, 5) * _tmp9 + P(5, 5) * _tmp4 + P(6, 5) * _tmp23) +
-        _tmp9 * (P(0, 4) * _tmp17 + P(1, 4) * _tmp21 + P(2, 4) * _tmp22 + P(3, 4) * _tmp15 +
-                 P(4, 4) * _tmp9 + P(5, 4) * _tmp4 + P(6, 4) * _tmp23);
+        _tmp11 * (P(0, 4) * _tmp8 + P(1, 4) * _tmp18 + P(2, 4) * _tmp6 + P(3, 4) * _tmp16 +
+                  P(4, 4) * _tmp11 + P(5, 4) * _tmp2 + P(6, 4) * _tmp19) +
+        _tmp16 * (P(0, 3) * _tmp8 + P(1, 3) * _tmp18 + P(2, 3) * _tmp6 + P(3, 3) * _tmp16 +
+                  P(4, 3) * _tmp11 + P(5, 3) * _tmp2 + P(6, 3) * _tmp19) +
+        _tmp18 * (P(0, 1) * _tmp8 + P(1, 1) * _tmp18 + P(2, 1) * _tmp6 + P(3, 1) * _tmp16 +
+                  P(4, 1) * _tmp11 + P(5, 1) * _tmp2 + P(6, 1) * _tmp19) +
+        _tmp19 * (P(0, 6) * _tmp8 + P(1, 6) * _tmp18 + P(2, 6) * _tmp6 + P(3, 6) * _tmp16 +
+                  P(4, 6) * _tmp11 + P(5, 6) * _tmp2 + P(6, 6) * _tmp19) +
+        _tmp2 * (P(0, 5) * _tmp8 + P(1, 5) * _tmp18 + P(2, 5) * _tmp6 + P(3, 5) * _tmp16 +
+                 P(4, 5) * _tmp11 + P(5, 5) * _tmp2 + P(6, 5) * _tmp19) +
+        _tmp6 * (P(0, 2) * _tmp8 + P(1, 2) * _tmp18 + P(2, 2) * _tmp6 + P(3, 2) * _tmp16 +
+                 P(4, 2) * _tmp11 + P(5, 2) * _tmp2 + P(6, 2) * _tmp19) +
+        _tmp8 * (P(0, 0) * _tmp8 + P(1, 0) * _tmp18 + P(2, 0) * _tmp6 + P(3, 0) * _tmp16 +
+                 P(4, 0) * _tmp11 + P(5, 0) * _tmp2 + P(6, 0) * _tmp19);
     _innov_var(1, 0) =
         R -
-        _tmp17 * (-P(0, 3) * _tmp27 - P(1, 3) * _tmp22 - P(2, 3) * _tmp28 - P(3, 3) * _tmp17 -
-                  P(4, 3) * _tmp24 - P(5, 3) * _tmp25 - P(6, 3) * _tmp26) -
-        _tmp22 * (-P(0, 1) * _tmp27 - P(1, 1) * _tmp22 - P(2, 1) * _tmp28 - P(3, 1) * _tmp17 -
-                  P(4, 1) * _tmp24 - P(5, 1) * _tmp25 - P(6, 1) * _tmp26) -
-        _tmp24 * (-P(0, 4) * _tmp27 - P(1, 4) * _tmp22 - P(2, 4) * _tmp28 - P(3, 4) * _tmp17 -
-                  P(4, 4) * _tmp24 - P(5, 4) * _tmp25 - P(6, 4) * _tmp26) -
-        _tmp25 * (-P(0, 5) * _tmp27 - P(1, 5) * _tmp22 - P(2, 5) * _tmp28 - P(3, 5) * _tmp17 -
-                  P(4, 5) * _tmp24 - P(5, 5) * _tmp25 - P(6, 5) * _tmp26) -
-        _tmp26 * (-P(0, 6) * _tmp27 - P(1, 6) * _tmp22 - P(2, 6) * _tmp28 - P(3, 6) * _tmp17 -
-                  P(4, 6) * _tmp24 - P(5, 6) * _tmp25 - P(6, 6) * _tmp26) -
-        _tmp27 * (-P(0, 0) * _tmp27 - P(1, 0) * _tmp22 - P(2, 0) * _tmp28 - P(3, 0) * _tmp17 -
-                  P(4, 0) * _tmp24 - P(5, 0) * _tmp25 - P(6, 0) * _tmp26) -
-        _tmp28 * (-P(0, 2) * _tmp27 - P(1, 2) * _tmp22 - P(2, 2) * _tmp28 - P(3, 2) * _tmp17 -
-                  P(4, 2) * _tmp24 - P(5, 2) * _tmp25 - P(6, 2) * _tmp26);
+        _tmp20 * (-P(0, 4) * _tmp26 - P(1, 4) * _tmp27 - P(2, 4) * _tmp25 - P(3, 4) * _tmp24 -
+                  P(4, 4) * _tmp20 - P(5, 4) * _tmp21 - P(6, 4) * _tmp22) -
+        _tmp21 * (-P(0, 5) * _tmp26 - P(1, 5) * _tmp27 - P(2, 5) * _tmp25 - P(3, 5) * _tmp24 -
+                  P(4, 5) * _tmp20 - P(5, 5) * _tmp21 - P(6, 5) * _tmp22) -
+        _tmp22 * (-P(0, 6) * _tmp26 - P(1, 6) * _tmp27 - P(2, 6) * _tmp25 - P(3, 6) * _tmp24 -
+                  P(4, 6) * _tmp20 - P(5, 6) * _tmp21 - P(6, 6) * _tmp22) -
+        _tmp24 * (-P(0, 3) * _tmp26 - P(1, 3) * _tmp27 - P(2, 3) * _tmp25 - P(3, 3) * _tmp24 -
+                  P(4, 3) * _tmp20 - P(5, 3) * _tmp21 - P(6, 3) * _tmp22) -
+        _tmp25 * (-P(0, 2) * _tmp26 - P(1, 2) * _tmp27 - P(2, 2) * _tmp25 - P(3, 2) * _tmp24 -
+                  P(4, 2) * _tmp20 - P(5, 2) * _tmp21 - P(6, 2) * _tmp22) -
+        _tmp26 * (-P(0, 0) * _tmp26 - P(1, 0) * _tmp27 - P(2, 0) * _tmp25 - P(3, 0) * _tmp24 -
+                  P(4, 0) * _tmp20 - P(5, 0) * _tmp21 - P(6, 0) * _tmp22) -
+        _tmp27 * (-P(0, 1) * _tmp26 - P(1, 1) * _tmp27 - P(2, 1) * _tmp25 - P(3, 1) * _tmp24 -
+                  P(4, 1) * _tmp20 - P(5, 1) * _tmp21 - P(6, 1) * _tmp22);
   }
 
   if (H != nullptr) {
@@ -113,13 +111,13 @@ void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp17;
-    _h(1, 0) = _tmp21;
-    _h(2, 0) = _tmp22;
-    _h(3, 0) = _tmp15;
-    _h(4, 0) = _tmp9;
-    _h(5, 0) = _tmp4;
-    _h(6, 0) = _tmp23;
+    _h(0, 0) = _tmp8;
+    _h(1, 0) = _tmp18;
+    _h(2, 0) = _tmp6;
+    _h(3, 0) = _tmp16;
+    _h(4, 0) = _tmp11;
+    _h(5, 0) = _tmp2;
+    _h(6, 0) = _tmp19;
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h

@@ -32,7 +32,7 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
                               const Scalar R, const Scalar epsilon,
                               Scalar* const innov_var = nullptr,
                               matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 171
+  // Total ops: 166
 
   // Input arrays
 
@@ -41,39 +41,38 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
       Scalar(1.0) /
       (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
   const Scalar _tmp1 =
-      _tmp0 * (std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) -
-               std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)));
-  const Scalar _tmp2 = 2 * state(0, 0);
-  const Scalar _tmp3 = 2 * state(1, 0);
-  const Scalar _tmp4 = _tmp0 * (_tmp2 * state(3, 0) + _tmp3 * state(2, 0));
-  const Scalar _tmp5 = _tmp0 * (-_tmp2 * state(2, 0) + _tmp3 * state(3, 0));
-  const Scalar _tmp6 = 2 * state(4, 0);
-  const Scalar _tmp7 = 2 * state(6, 0);
-  const Scalar _tmp8 = _tmp0 * (_tmp2 * state(5, 0) - _tmp6 * state(3, 0) + _tmp7 * state(1, 0));
-  const Scalar _tmp9 = 2 * state(5, 0);
-  const Scalar _tmp10 = _tmp0 * (_tmp2 * state(4, 0) - _tmp7 * state(2, 0) + _tmp9 * state(3, 0));
-  const Scalar _tmp11 = _tmp0 * (_tmp3 * state(4, 0) + _tmp7 * state(3, 0) + _tmp9 * state(2, 0));
-  const Scalar _tmp12 = _tmp0 * (_tmp3 * state(5, 0) - _tmp6 * state(2, 0) - _tmp7 * state(0, 0));
+      _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
+  const Scalar _tmp2 = 2 * state(3, 0);
+  const Scalar _tmp3 = 2 * state(2, 0);
+  const Scalar _tmp4 = _tmp0 * (_tmp2 * state(0, 0) + _tmp3 * state(1, 0));
+  const Scalar _tmp5 = _tmp0 * (_tmp2 * state(1, 0) - _tmp3 * state(0, 0));
+  const Scalar _tmp6 = 4 * state(4, 0);
+  const Scalar _tmp7 = 2 * state(5, 0);
+  const Scalar _tmp8 = 2 * state(6, 0);
+  const Scalar _tmp9 = _tmp0 * (-_tmp6 * state(3, 0) + _tmp7 * state(0, 0) + _tmp8 * state(1, 0));
+  const Scalar _tmp10 = _tmp0 * (-_tmp6 * state(2, 0) + _tmp7 * state(1, 0) - _tmp8 * state(0, 0));
+  const Scalar _tmp11 = _tmp0 * (-_tmp3 * state(6, 0) + _tmp7 * state(3, 0));
+  const Scalar _tmp12 = _tmp0 * (_tmp2 * state(6, 0) + _tmp3 * state(5, 0));
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
     _innov_var = R -
-                 _tmp1 * (-P(0, 4) * _tmp10 - P(1, 4) * _tmp11 - P(2, 4) * _tmp12 -
-                          P(3, 4) * _tmp8 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
-                 _tmp10 * (-P(0, 0) * _tmp10 - P(1, 0) * _tmp11 - P(2, 0) * _tmp12 -
-                           P(3, 0) * _tmp8 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
-                 _tmp11 * (-P(0, 1) * _tmp10 - P(1, 1) * _tmp11 - P(2, 1) * _tmp12 -
-                           P(3, 1) * _tmp8 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
-                 _tmp12 * (-P(0, 2) * _tmp10 - P(1, 2) * _tmp11 - P(2, 2) * _tmp12 -
-                           P(3, 2) * _tmp8 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
-                 _tmp4 * (-P(0, 5) * _tmp10 - P(1, 5) * _tmp11 - P(2, 5) * _tmp12 -
-                          P(3, 5) * _tmp8 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
-                 _tmp5 * (-P(0, 6) * _tmp10 - P(1, 6) * _tmp11 - P(2, 6) * _tmp12 -
-                          P(3, 6) * _tmp8 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
-                 _tmp8 * (-P(0, 3) * _tmp10 - P(1, 3) * _tmp11 - P(2, 3) * _tmp12 -
-                          P(3, 3) * _tmp8 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
+                 _tmp1 * (-P(0, 4) * _tmp11 - P(1, 4) * _tmp12 - P(2, 4) * _tmp10 -
+                          P(3, 4) * _tmp9 - P(4, 4) * _tmp1 - P(5, 4) * _tmp4 - P(6, 4) * _tmp5) -
+                 _tmp10 * (-P(0, 2) * _tmp11 - P(1, 2) * _tmp12 - P(2, 2) * _tmp10 -
+                           P(3, 2) * _tmp9 - P(4, 2) * _tmp1 - P(5, 2) * _tmp4 - P(6, 2) * _tmp5) -
+                 _tmp11 * (-P(0, 0) * _tmp11 - P(1, 0) * _tmp12 - P(2, 0) * _tmp10 -
+                           P(3, 0) * _tmp9 - P(4, 0) * _tmp1 - P(5, 0) * _tmp4 - P(6, 0) * _tmp5) -
+                 _tmp12 * (-P(0, 1) * _tmp11 - P(1, 1) * _tmp12 - P(2, 1) * _tmp10 -
+                           P(3, 1) * _tmp9 - P(4, 1) * _tmp1 - P(5, 1) * _tmp4 - P(6, 1) * _tmp5) -
+                 _tmp4 * (-P(0, 5) * _tmp11 - P(1, 5) * _tmp12 - P(2, 5) * _tmp10 -
+                          P(3, 5) * _tmp9 - P(4, 5) * _tmp1 - P(5, 5) * _tmp4 - P(6, 5) * _tmp5) -
+                 _tmp5 * (-P(0, 6) * _tmp11 - P(1, 6) * _tmp12 - P(2, 6) * _tmp10 -
+                          P(3, 6) * _tmp9 - P(4, 6) * _tmp1 - P(5, 6) * _tmp4 - P(6, 6) * _tmp5) -
+                 _tmp9 * (-P(0, 3) * _tmp11 - P(1, 3) * _tmp12 - P(2, 3) * _tmp10 -
+                          P(3, 3) * _tmp9 - P(4, 3) * _tmp1 - P(5, 3) * _tmp4 - P(6, 3) * _tmp5);
   }
 
   if (H != nullptr) {
@@ -81,10 +80,10 @@ void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = -_tmp10;
-    _h(1, 0) = -_tmp11;
-    _h(2, 0) = -_tmp12;
-    _h(3, 0) = -_tmp8;
+    _h(0, 0) = -_tmp11;
+    _h(1, 0) = -_tmp12;
+    _h(2, 0) = -_tmp10;
+    _h(3, 0) = -_tmp9;
     _h(4, 0) = -_tmp1;
     _h(5, 0) = -_tmp4;
     _h(6, 0) = -_tmp5;

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h

@@ -34,53 +34,54 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
                                     const Scalar epsilon, Scalar* const meas_pred = nullptr,
                                     Scalar* const innov_var = nullptr,
                                     matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 101
+  // Total ops: 105
 
   // Input arrays
 
-  // Intermediate terms (26)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 = std::sin(antenna_yaw_offset);
-  const Scalar _tmp4 = state(0, 0) * state(3, 0);
-  const Scalar _tmp5 = state(1, 0) * state(2, 0);
-  const Scalar _tmp6 = std::cos(antenna_yaw_offset);
-  const Scalar _tmp7 = 2 * _tmp6;
-  const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5);
-  const Scalar _tmp9 = 2 * _tmp3;
-  const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5);
-  const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5));
-  const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0);
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp11);
-  const Scalar _tmp14 = _tmp7 * state(3, 0);
-  const Scalar _tmp15 = _tmp9 * state(0, 0);
-  const Scalar _tmp16 = std::pow(_tmp11, Scalar(2));
-  const Scalar _tmp17 = _tmp8 / _tmp16;
-  const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2)));
-  const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
-  const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0);
-  const Scalar _tmp21 = _tmp9 * state(1, 0);
-  const Scalar _tmp22 = _tmp7 * state(2, 0);
-  const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20);
-  const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
-  const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22));
+  // Intermediate terms (22)
+  const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp1 = std::sin(antenna_yaw_offset);
+  const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0);
+  const Scalar _tmp3 = 2 * state(1, 0) * state(2, 0);
+  const Scalar _tmp4 = std::cos(antenna_yaw_offset);
+  const Scalar _tmp5 =
+      _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))) + _tmp4 * (_tmp2 + _tmp3);
+  const Scalar _tmp6 =
+      _tmp1 * (-_tmp2 + _tmp3) + _tmp4 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2)));
+  const Scalar _tmp7 = _tmp6 + epsilon * ((((_tmp6) > 0) - ((_tmp6) < 0)) + Scalar(0.5));
+  const Scalar _tmp8 = 4 * _tmp1;
+  const Scalar _tmp9 = 2 * _tmp4;
+  const Scalar _tmp10 = Scalar(1.0) / (_tmp7);
+  const Scalar _tmp11 = 4 * _tmp4;
+  const Scalar _tmp12 = 2 * _tmp1;
+  const Scalar _tmp13 = std::pow(_tmp7, Scalar(2));
+  const Scalar _tmp14 = _tmp5 / _tmp13;
+  const Scalar _tmp15 = _tmp13 / (_tmp13 + std::pow(_tmp5, Scalar(2)));
+  const Scalar _tmp16 = _tmp15 * (_tmp10 * (-_tmp8 * state(3, 0) + _tmp9 * state(0, 0)) -
+                                  _tmp14 * (-_tmp11 * state(3, 0) - _tmp12 * state(0, 0)));
+  const Scalar _tmp17 = _tmp12 * _tmp14;
+  const Scalar _tmp18 = _tmp10 * _tmp9;
+  const Scalar _tmp19 = _tmp15 * (_tmp17 * state(3, 0) + _tmp18 * state(3, 0));
+  const Scalar _tmp20 =
+      _tmp15 * (-_tmp14 * (-_tmp11 * state(2, 0) + _tmp12 * state(1, 0)) + _tmp18 * state(1, 0));
+  const Scalar _tmp21 =
+      _tmp15 * (_tmp10 * (-_tmp8 * state(1, 0) + _tmp9 * state(2, 0)) - _tmp17 * state(2, 0));
 
   // Output terms (3)
   if (meas_pred != nullptr) {
     Scalar& _meas_pred = (*meas_pred);
 
-    _meas_pred = std::atan2(_tmp8, _tmp11);
+    _meas_pred = std::atan2(_tmp5, _tmp7);
   }
 
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
     _innov_var =
-        R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) +
-        _tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) +
-        _tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) +
-        _tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19);
+        R + _tmp16 * (P(0, 3) * _tmp19 + P(1, 3) * _tmp21 + P(2, 3) * _tmp20 + P(3, 3) * _tmp16) +
+        _tmp19 * (P(0, 0) * _tmp19 + P(1, 0) * _tmp21 + P(2, 0) * _tmp20 + P(3, 0) * _tmp16) +
+        _tmp20 * (P(0, 2) * _tmp19 + P(1, 2) * _tmp21 + P(2, 2) * _tmp20 + P(3, 2) * _tmp16) +
+        _tmp21 * (P(0, 1) * _tmp19 + P(1, 1) * _tmp21 + P(2, 1) * _tmp20 + P(3, 1) * _tmp16);
   }
 
   if (H != nullptr) {
@@ -88,10 +89,10 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp24;
-    _h(1, 0) = _tmp23;
-    _h(2, 0) = _tmp25;
-    _h(3, 0) = _tmp19;
+    _h(0, 0) = _tmp19;
+    _h(1, 0) = _tmp21;
+    _h(2, 0) = _tmp20;
+    _h(3, 0) = _tmp16;
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h

@@ -39,236 +39,192 @@ void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
                                     matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
                                     matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
                                     matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
-  // Total ops: 736
+  // Total ops: 617
 
   // Input arrays
 
-  // Intermediate terms (54)
-  const Scalar _tmp0 =
+  // Intermediate terms (34)
+  const Scalar _tmp0 = 2 * state(2, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 =
       Scalar(9.8066499999999994) /
       std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
                        std::pow(meas(2, 0), Scalar(2))));
-  const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0);
-  const Scalar _tmp2 = -P(3, 0) * _tmp1;
-  const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0);
-  const Scalar _tmp4 = P(0, 0) * _tmp3;
-  const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0);
-  const Scalar _tmp6 = P(2, 0) * _tmp5;
-  const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0);
-  const Scalar _tmp8 = P(3, 1) * _tmp1;
-  const Scalar _tmp9 = P(2, 1) * _tmp5;
-  const Scalar _tmp10 = -P(1, 1) * _tmp7;
-  const Scalar _tmp11 = P(0, 2) * _tmp3;
-  const Scalar _tmp12 = P(2, 2) * _tmp5;
-  const Scalar _tmp13 = -P(1, 2) * _tmp7;
-  const Scalar _tmp14 = -P(3, 3) * _tmp1;
-  const Scalar _tmp15 = P(0, 3) * _tmp3;
-  const Scalar _tmp16 = -P(1, 3) * _tmp7;
-  const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) +
-                        _tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) +
-                        _tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) -
-                        _tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9);
-  const Scalar _tmp18 = P(3, 0) * _tmp3;
-  const Scalar _tmp19 = -P(0, 0) * _tmp1;
-  const Scalar _tmp20 = -P(1, 0) * _tmp5;
-  const Scalar _tmp21 = P(3, 2) * _tmp3;
-  const Scalar _tmp22 = -P(2, 2) * _tmp7;
-  const Scalar _tmp23 = P(1, 2) * _tmp5;
-  const Scalar _tmp24 = P(0, 1) * _tmp1;
-  const Scalar _tmp25 = -P(2, 1) * _tmp7;
-  const Scalar _tmp26 = -P(1, 1) * _tmp5;
-  const Scalar _tmp27 = -P(3, 3) * _tmp3;
-  const Scalar _tmp28 = -P(0, 3) * _tmp1;
-  const Scalar _tmp29 = -P(2, 3) * _tmp7;
-  const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) -
-                        _tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) -
-                        _tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) -
-                        _tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23);
-  const Scalar _tmp31 = -P(0, 0) * _tmp5;
-  const Scalar _tmp32 = P(2, 0) * _tmp3;
-  const Scalar _tmp33 = P(1, 0) * _tmp1;
-  const Scalar _tmp34 = -P(3, 2) * _tmp7;
-  const Scalar _tmp35 = P(0, 2) * _tmp5;
-  const Scalar _tmp36 = P(2, 2) * _tmp3;
-  const Scalar _tmp37 = -P(3, 1) * _tmp7;
-  const Scalar _tmp38 = -P(0, 1) * _tmp5;
-  const Scalar _tmp39 = P(1, 1) * _tmp1;
-  const Scalar _tmp40 = -P(3, 3) * _tmp7;
-  const Scalar _tmp41 = P(2, 3) * _tmp3;
-  const Scalar _tmp42 = P(1, 3) * _tmp1;
-  const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) +
-                        _tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) -
-                        _tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) -
-                        _tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42);
-  const Scalar _tmp44 = Scalar(1.0) / (_tmp17);
-  const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3);
-  const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0);
-  const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2);
-  const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3);
-  const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2);
-  const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0);
-  const Scalar _tmp51 = Scalar(1.0) / (_tmp30);
-  const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0);
-  const Scalar _tmp53 = Scalar(1.0) / (_tmp43);
+  const Scalar _tmp3 = Scalar(19.613299999999999) * state(1, 0);
+  const Scalar _tmp4 = -P(3, 0) * _tmp3;
+  const Scalar _tmp5 = Scalar(19.613299999999999) * state(2, 0);
+  const Scalar _tmp6 = P(0, 0) * _tmp5;
+  const Scalar _tmp7 = Scalar(19.613299999999999) * state(0, 0);
+  const Scalar _tmp8 = Scalar(19.613299999999999) * state(3, 0);
+  const Scalar _tmp9 = P(2, 1) * _tmp7;
+  const Scalar _tmp10 = -P(1, 1) * _tmp8;
+  const Scalar _tmp11 = P(2, 2) * _tmp7;
+  const Scalar _tmp12 = -P(1, 2) * _tmp8;
+  const Scalar _tmp13 = -P(3, 3) * _tmp3;
+  const Scalar _tmp14 = P(0, 3) * _tmp5;
+  const Scalar _tmp15 = R - _tmp3 * (-P(1, 3) * _tmp8 + P(2, 3) * _tmp7 + _tmp13 + _tmp14) +
+                        _tmp5 * (-P(1, 0) * _tmp8 + P(2, 0) * _tmp7 + _tmp4 + _tmp6) +
+                        _tmp7 * (P(0, 2) * _tmp5 - P(3, 2) * _tmp3 + _tmp11 + _tmp12) -
+                        _tmp8 * (P(0, 1) * _tmp5 - P(3, 1) * _tmp3 + _tmp10 + _tmp9);
+  const Scalar _tmp16 = P(3, 0) * _tmp5;
+  const Scalar _tmp17 = -P(0, 0) * _tmp3;
+  const Scalar _tmp18 = -P(2, 2) * _tmp8;
+  const Scalar _tmp19 = P(1, 2) * _tmp7;
+  const Scalar _tmp20 = -P(2, 1) * _tmp8;
+  const Scalar _tmp21 = -P(1, 1) * _tmp7;
+  const Scalar _tmp22 = -P(3, 3) * _tmp5;
+  const Scalar _tmp23 = -P(0, 3) * _tmp3;
+  const Scalar _tmp24 = R - _tmp3 * (-P(1, 0) * _tmp7 - P(2, 0) * _tmp8 - _tmp16 + _tmp17) -
+                        _tmp5 * (-P(1, 3) * _tmp7 - P(2, 3) * _tmp8 + _tmp22 + _tmp23) -
+                        _tmp7 * (-P(0, 1) * _tmp3 - P(3, 1) * _tmp5 + _tmp20 + _tmp21) -
+                        _tmp8 * (-P(0, 2) * _tmp3 - P(3, 2) * _tmp5 + _tmp18 - _tmp19);
+  const Scalar _tmp25 = Scalar(39.226599999999998) * state(2, 0);
+  const Scalar _tmp26 = P(2, 2) * _tmp25;
+  const Scalar _tmp27 = Scalar(39.226599999999998) * state(1, 0);
+  const Scalar _tmp28 = P(1, 1) * _tmp27;
+  const Scalar _tmp29 =
+      R + _tmp25 * (P(1, 2) * _tmp27 + _tmp26) + _tmp27 * (P(2, 1) * _tmp25 + _tmp28);
+  const Scalar _tmp30 = Scalar(1.0) / (_tmp15);
+  const Scalar _tmp31 = Scalar(19.613299999999999) * P(6, 3);
+  const Scalar _tmp32 = Scalar(1.0) / (_tmp24);
+  const Scalar _tmp33 = Scalar(1.0) / (_tmp29);
 
   // Output terms (5)
   if (innov != nullptr) {
     matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
 
-    _innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) -
-                   Scalar(19.613299999999999) * state(1, 0) * state(3, 0);
-    _innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) -
-                   Scalar(19.613299999999999) * state(2, 0) * state(3, 0);
-    _innov(2, 0) = -_tmp0 * meas(2, 0) -
-                   Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) +
-                   Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) +
-                   Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) -
-                   Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2));
+    _innov(0, 0) = Scalar(9.8066499999999994) * _tmp0 * state(0, 0) -
+                   Scalar(9.8066499999999994) * _tmp1 * state(3, 0) - _tmp2 * meas(0, 0);
+    _innov(1, 0) = -Scalar(9.8066499999999994) * _tmp0 * state(3, 0) -
+                   Scalar(9.8066499999999994) * _tmp1 * state(0, 0) - _tmp2 * meas(1, 0);
+    _innov(2, 0) =
+        -_tmp2 * meas(2, 0) + Scalar(19.613299999999999) * std::pow(state(1, 0), Scalar(2)) +
+        Scalar(19.613299999999999) * std::pow(state(2, 0), Scalar(2)) + Scalar(-9.8066499999999994);
   }
 
   if (innov_var != nullptr) {
     matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
 
-    _innov_var(0, 0) = _tmp17;
-    _innov_var(1, 0) = _tmp30;
-    _innov_var(2, 0) = _tmp43;
+    _innov_var(0, 0) = _tmp15;
+    _innov_var(1, 0) = _tmp24;
+    _innov_var(2, 0) = _tmp29;
   }
 
   if (Kx != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
 
-    _kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4);
-    _kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42);
-    _kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32);
-    _kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37);
-    _kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1);
-    _kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1);
-    _kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1);
-    _kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1);
-    _kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) +
-                          _tmp47 * state(0, 0));
-    _kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) +
-                          _tmp50 * state(2, 0));
+    _kx(0, 0) = _tmp30 * (-P(0, 1) * _tmp8 + P(0, 2) * _tmp7 + _tmp23 + _tmp6);
+    _kx(1, 0) = _tmp30 * (P(1, 0) * _tmp5 - P(1, 3) * _tmp3 + _tmp10 + _tmp19);
+    _kx(2, 0) = _tmp30 * (P(2, 0) * _tmp5 - P(2, 3) * _tmp3 + _tmp11 + _tmp20);
+    _kx(3, 0) = _tmp30 * (-P(3, 1) * _tmp8 + P(3, 2) * _tmp7 + _tmp13 + _tmp16);
+    _kx(4, 0) = _tmp30 * (P(4, 0) * _tmp5 - P(4, 1) * _tmp8 + P(4, 2) * _tmp7 - P(4, 3) * _tmp3);
+    _kx(5, 0) = _tmp30 * (P(5, 0) * _tmp5 - P(5, 1) * _tmp8 + P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
+    _kx(6, 0) =
+        _tmp30 * (P(6, 0) * _tmp5 - P(6, 1) * _tmp8 + P(6, 2) * _tmp7 - _tmp31 * state(1, 0));
+    _kx(7, 0) = _tmp30 * (P(7, 0) * _tmp5 - P(7, 1) * _tmp8 + P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
+    _kx(8, 0) = _tmp30 * (P(8, 0) * _tmp5 - P(8, 1) * _tmp8 + P(8, 2) * _tmp7 - P(8, 3) * _tmp3);
+    _kx(9, 0) = _tmp30 * (P(9, 0) * _tmp5 - P(9, 1) * _tmp8 + P(9, 2) * _tmp7 - P(9, 3) * _tmp3);
     _kx(10, 0) =
-        _tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1);
+        _tmp30 * (P(10, 0) * _tmp5 - P(10, 1) * _tmp8 + P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
     _kx(11, 0) =
-        _tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1);
+        _tmp30 * (P(11, 0) * _tmp5 - P(11, 1) * _tmp8 + P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
     _kx(12, 0) =
-        _tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1);
+        _tmp30 * (P(12, 0) * _tmp5 - P(12, 1) * _tmp8 + P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
     _kx(13, 0) =
-        _tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1);
+        _tmp30 * (P(13, 0) * _tmp5 - P(13, 1) * _tmp8 + P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
     _kx(14, 0) =
-        _tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1);
+        _tmp30 * (P(14, 0) * _tmp5 - P(14, 1) * _tmp8 + P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
     _kx(15, 0) =
-        _tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1);
+        _tmp30 * (P(15, 0) * _tmp5 - P(15, 1) * _tmp8 + P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
     _kx(16, 0) =
-        _tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1);
+        _tmp30 * (P(16, 0) * _tmp5 - P(16, 1) * _tmp8 + P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
     _kx(17, 0) =
-        _tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1);
+        _tmp30 * (P(17, 0) * _tmp5 - P(17, 1) * _tmp8 + P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
     _kx(18, 0) =
-        _tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1);
+        _tmp30 * (P(18, 0) * _tmp5 - P(18, 1) * _tmp8 + P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
     _kx(19, 0) =
-        _tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1);
+        _tmp30 * (P(19, 0) * _tmp5 - P(19, 1) * _tmp8 + P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
     _kx(20, 0) =
-        _tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1);
+        _tmp30 * (P(20, 0) * _tmp5 - P(20, 1) * _tmp8 + P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
     _kx(21, 0) =
-        _tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1);
+        _tmp30 * (P(21, 0) * _tmp5 - P(21, 1) * _tmp8 + P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
     _kx(22, 0) =
-        _tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1);
+        _tmp30 * (P(22, 0) * _tmp5 - P(22, 1) * _tmp8 + P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
     _kx(23, 0) =
-        _tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1);
+        _tmp30 * (P(23, 0) * _tmp5 - P(23, 1) * _tmp8 + P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
   }
 
   if (Ky != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
 
-    _ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38);
-    _ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33);
-    _ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9);
-    _ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34);
-    _ky(4, 0) =
-        _tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0));
-    _ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
-    _ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3);
-    _ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
-    _ky(8, 0) =
-        _tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0));
-    _ky(9, 0) =
-        _tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0));
+    _ky(0, 0) = _tmp32 * (-P(0, 1) * _tmp7 - P(0, 2) * _tmp8 - _tmp14 + _tmp17);
+    _ky(1, 0) = _tmp32 * (-P(1, 0) * _tmp3 - P(1, 3) * _tmp5 + _tmp12 + _tmp21);
+    _ky(2, 0) = _tmp32 * (-P(2, 0) * _tmp3 - P(2, 3) * _tmp5 + _tmp18 - _tmp9);
+    _ky(3, 0) = _tmp32 * (-P(3, 1) * _tmp7 - P(3, 2) * _tmp8 + _tmp22 + _tmp4);
+    _ky(4, 0) = _tmp32 * (-P(4, 0) * _tmp3 - P(4, 1) * _tmp7 - P(4, 2) * _tmp8 - P(4, 3) * _tmp5);
+    _ky(5, 0) = _tmp32 * (-P(5, 0) * _tmp3 - P(5, 1) * _tmp7 - P(5, 2) * _tmp8 - P(5, 3) * _tmp5);
+    _ky(6, 0) =
+        _tmp32 * (-P(6, 0) * _tmp3 - P(6, 1) * _tmp7 - P(6, 2) * _tmp8 - _tmp31 * state(2, 0));
+    _ky(7, 0) = _tmp32 * (-P(7, 0) * _tmp3 - P(7, 1) * _tmp7 - P(7, 2) * _tmp8 - P(7, 3) * _tmp5);
+    _ky(8, 0) = _tmp32 * (-P(8, 0) * _tmp3 - P(8, 1) * _tmp7 - P(8, 2) * _tmp8 - P(8, 3) * _tmp5);
+    _ky(9, 0) = _tmp32 * (-P(9, 0) * _tmp3 - P(9, 1) * _tmp7 - P(9, 2) * _tmp8 - P(9, 3) * _tmp5);
     _ky(10, 0) =
-        _tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
+        _tmp32 * (-P(10, 0) * _tmp3 - P(10, 1) * _tmp7 - P(10, 2) * _tmp8 - P(10, 3) * _tmp5);
     _ky(11, 0) =
-        _tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
+        _tmp32 * (-P(11, 0) * _tmp3 - P(11, 1) * _tmp7 - P(11, 2) * _tmp8 - P(11, 3) * _tmp5);
     _ky(12, 0) =
-        _tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
+        _tmp32 * (-P(12, 0) * _tmp3 - P(12, 1) * _tmp7 - P(12, 2) * _tmp8 - P(12, 3) * _tmp5);
     _ky(13, 0) =
-        _tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
+        _tmp32 * (-P(13, 0) * _tmp3 - P(13, 1) * _tmp7 - P(13, 2) * _tmp8 - P(13, 3) * _tmp5);
     _ky(14, 0) =
-        _tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
+        _tmp32 * (-P(14, 0) * _tmp3 - P(14, 1) * _tmp7 - P(14, 2) * _tmp8 - P(14, 3) * _tmp5);
     _ky(15, 0) =
-        _tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
+        _tmp32 * (-P(15, 0) * _tmp3 - P(15, 1) * _tmp7 - P(15, 2) * _tmp8 - P(15, 3) * _tmp5);
     _ky(16, 0) =
-        _tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
+        _tmp32 * (-P(16, 0) * _tmp3 - P(16, 1) * _tmp7 - P(16, 2) * _tmp8 - P(16, 3) * _tmp5);
     _ky(17, 0) =
-        _tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
+        _tmp32 * (-P(17, 0) * _tmp3 - P(17, 1) * _tmp7 - P(17, 2) * _tmp8 - P(17, 3) * _tmp5);
     _ky(18, 0) =
-        _tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
+        _tmp32 * (-P(18, 0) * _tmp3 - P(18, 1) * _tmp7 - P(18, 2) * _tmp8 - P(18, 3) * _tmp5);
     _ky(19, 0) =
-        _tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
+        _tmp32 * (-P(19, 0) * _tmp3 - P(19, 1) * _tmp7 - P(19, 2) * _tmp8 - P(19, 3) * _tmp5);
     _ky(20, 0) =
-        _tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
+        _tmp32 * (-P(20, 0) * _tmp3 - P(20, 1) * _tmp7 - P(20, 2) * _tmp8 - P(20, 3) * _tmp5);
     _ky(21, 0) =
-        _tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
+        _tmp32 * (-P(21, 0) * _tmp3 - P(21, 1) * _tmp7 - P(21, 2) * _tmp8 - P(21, 3) * _tmp5);
     _ky(22, 0) =
-        _tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
+        _tmp32 * (-P(22, 0) * _tmp3 - P(22, 1) * _tmp7 - P(22, 2) * _tmp8 - P(22, 3) * _tmp5);
     _ky(23, 0) =
-        _tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
+        _tmp32 * (-P(23, 0) * _tmp3 - P(23, 1) * _tmp7 - P(23, 2) * _tmp8 - P(23, 3) * _tmp5);
   }
 
   if (Kz != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
 
-    _kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31);
-    _kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39);
-    _kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6);
-    _kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8);
-    _kz(4, 0) =
-        _tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0));
-    _kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7);
-    _kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7);
-    _kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7);
-    _kz(8, 0) =
-        _tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0));
-    _kz(9, 0) =
-        _tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0));
-    _kz(10, 0) =
-        _tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7);
-    _kz(11, 0) =
-        _tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7);
-    _kz(12, 0) =
-        _tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7);
-    _kz(13, 0) =
-        _tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7);
-    _kz(14, 0) =
-        _tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7);
-    _kz(15, 0) =
-        _tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7);
-    _kz(16, 0) =
-        _tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7);
-    _kz(17, 0) =
-        _tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7);
-    _kz(18, 0) =
-        _tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7);
-    _kz(19, 0) =
-        _tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7);
-    _kz(20, 0) =
-        _tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7);
-    _kz(21, 0) =
-        _tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7);
-    _kz(22, 0) =
-        _tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7);
-    _kz(23, 0) =
-        _tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7);
+    _kz(0, 0) = _tmp33 * (P(0, 1) * _tmp27 + P(0, 2) * _tmp25);
+    _kz(1, 0) = _tmp33 * (P(1, 2) * _tmp25 + _tmp28);
+    _kz(2, 0) = _tmp33 * (P(2, 1) * _tmp27 + _tmp26);
+    _kz(3, 0) = _tmp33 * (P(3, 1) * _tmp27 + P(3, 2) * _tmp25);
+    _kz(4, 0) = _tmp33 * (P(4, 1) * _tmp27 + P(4, 2) * _tmp25);
+    _kz(5, 0) = _tmp33 * (P(5, 1) * _tmp27 + P(5, 2) * _tmp25);
+    _kz(6, 0) = _tmp33 * (P(6, 1) * _tmp27 + P(6, 2) * _tmp25);
+    _kz(7, 0) = _tmp33 * (P(7, 1) * _tmp27 + P(7, 2) * _tmp25);
+    _kz(8, 0) = _tmp33 * (P(8, 1) * _tmp27 + P(8, 2) * _tmp25);
+    _kz(9, 0) = _tmp33 * (P(9, 1) * _tmp27 + P(9, 2) * _tmp25);
+    _kz(10, 0) = _tmp33 * (P(10, 1) * _tmp27 + P(10, 2) * _tmp25);
+    _kz(11, 0) = _tmp33 * (P(11, 1) * _tmp27 + P(11, 2) * _tmp25);
+    _kz(12, 0) = _tmp33 * (P(12, 1) * _tmp27 + P(12, 2) * _tmp25);
+    _kz(13, 0) = _tmp33 * (P(13, 1) * _tmp27 + P(13, 2) * _tmp25);
+    _kz(14, 0) = _tmp33 * (P(14, 1) * _tmp27 + P(14, 2) * _tmp25);
+    _kz(15, 0) = _tmp33 * (P(15, 1) * _tmp27 + P(15, 2) * _tmp25);
+    _kz(16, 0) = _tmp33 * (P(16, 1) * _tmp27 + P(16, 2) * _tmp25);
+    _kz(17, 0) = _tmp33 * (P(17, 1) * _tmp27 + P(17, 2) * _tmp25);
+    _kz(18, 0) = _tmp33 * (P(18, 1) * _tmp27 + P(18, 2) * _tmp25);
+    _kz(19, 0) = _tmp33 * (P(19, 1) * _tmp27 + P(19, 2) * _tmp25);
+    _kz(20, 0) = _tmp33 * (P(20, 1) * _tmp27 + P(20, 2) * _tmp25);
+    _kz(21, 0) = _tmp33 * (P(21, 1) * _tmp27 + P(21, 2) * _tmp25);
+    _kz(22, 0) = _tmp33 * (P(22, 1) * _tmp27 + P(22, 2) * _tmp25);
+    _kz(23, 0) = _tmp33 * (P(23, 1) * _tmp27 + P(23, 2) * _tmp25);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h

@@ -35,69 +35,70 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
                                   matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
                                   matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
                                   matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
-  // Total ops: 470
+  // Total ops: 471
 
   // Input arrays
 
-  // Intermediate terms (48)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = -_tmp0;
-  const Scalar _tmp2 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp3 = std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2));
-  const Scalar _tmp5 = -_tmp3 + _tmp4;
-  const Scalar _tmp6 = _tmp1 + _tmp2 + _tmp5;
-  const Scalar _tmp7 = state(0, 0) * state(3, 0);
-  const Scalar _tmp8 = state(1, 0) * state(2, 0);
-  const Scalar _tmp9 = 2 * state(17, 0);
-  const Scalar _tmp10 = state(0, 0) * state(2, 0);
-  const Scalar _tmp11 = state(1, 0) * state(3, 0);
-  const Scalar _tmp12 = 2 * state(18, 0);
-  const Scalar _tmp13 = -_tmp2;
-  const Scalar _tmp14 = _tmp0 + _tmp13 + _tmp5;
-  const Scalar _tmp15 = state(2, 0) * state(3, 0);
-  const Scalar _tmp16 = state(0, 0) * state(1, 0);
-  const Scalar _tmp17 = 2 * state(16, 0);
-  const Scalar _tmp18 = _tmp1 + _tmp13 + _tmp3 + _tmp4;
-  const Scalar _tmp19 = _tmp9 * state(3, 0);
-  const Scalar _tmp20 = _tmp12 * state(2, 0);
-  const Scalar _tmp21 = 2 * state(0, 0);
-  const Scalar _tmp22 = _tmp21 * state(16, 0);
-  const Scalar _tmp23 = _tmp19 - _tmp20 + _tmp22;
-  const Scalar _tmp24 = _tmp12 * state(3, 0) + _tmp17 * state(1, 0) + _tmp9 * state(2, 0);
-  const Scalar _tmp25 = _tmp17 * state(3, 0);
-  const Scalar _tmp26 = _tmp12 * state(1, 0);
-  const Scalar _tmp27 = _tmp21 * state(17, 0);
-  const Scalar _tmp28 = -_tmp25 + _tmp26 + _tmp27;
-  const Scalar _tmp29 = _tmp17 * state(2, 0);
-  const Scalar _tmp30 = _tmp9 * state(1, 0);
-  const Scalar _tmp31 = _tmp12 * state(0, 0);
-  const Scalar _tmp32 = -_tmp29 + _tmp30 - _tmp31;
-  const Scalar _tmp33 = 2 * _tmp7;
-  const Scalar _tmp34 = 2 * _tmp8;
+  // Intermediate terms (49)
+  const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp1 = -2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp2 = _tmp0 + _tmp1 + 1;
+  const Scalar _tmp3 = 2 * state(0, 0);
+  const Scalar _tmp4 = _tmp3 * state(3, 0);
+  const Scalar _tmp5 = 2 * state(2, 0);
+  const Scalar _tmp6 = _tmp5 * state(1, 0);
+  const Scalar _tmp7 = _tmp4 + _tmp6;
+  const Scalar _tmp8 = _tmp5 * state(0, 0);
+  const Scalar _tmp9 = 2 * state(1, 0);
+  const Scalar _tmp10 = _tmp9 * state(3, 0);
+  const Scalar _tmp11 = _tmp10 - _tmp8;
+  const Scalar _tmp12 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp0 + _tmp12;
+  const Scalar _tmp14 = _tmp5 * state(3, 0);
+  const Scalar _tmp15 = _tmp3 * state(1, 0);
+  const Scalar _tmp16 = _tmp14 + _tmp15;
+  const Scalar _tmp17 = -_tmp4 + _tmp6;
+  const Scalar _tmp18 = _tmp1 + _tmp12;
+  const Scalar _tmp19 = _tmp14 - _tmp15;
+  const Scalar _tmp20 = _tmp10 + _tmp8;
+  const Scalar _tmp21 = _tmp9 * state(18, 0);
+  const Scalar _tmp22 = _tmp3 * state(17, 0);
+  const Scalar _tmp23 = _tmp21 + _tmp22 - 4 * state(16, 0) * state(3, 0);
+  const Scalar _tmp24 = 4 * state(2, 0);
+  const Scalar _tmp25 = _tmp9 * state(17, 0);
+  const Scalar _tmp26 = _tmp3 * state(18, 0);
+  const Scalar _tmp27 = -_tmp24 * state(16, 0) + _tmp25 - _tmp26;
+  const Scalar _tmp28 = state(17, 0) * state(3, 0);
+  const Scalar _tmp29 = 2 * _tmp28;
+  const Scalar _tmp30 = _tmp5 * state(18, 0);
+  const Scalar _tmp31 = _tmp29 - _tmp30;
+  const Scalar _tmp32 = 2 * state(3, 0);
+  const Scalar _tmp33 = _tmp32 * state(18, 0);
+  const Scalar _tmp34 = _tmp5 * state(17, 0);
   const Scalar _tmp35 = _tmp33 + _tmp34;
-  const Scalar _tmp36 = 2 * _tmp10;
-  const Scalar _tmp37 = 2 * _tmp11;
-  const Scalar _tmp38 = -_tmp36 + _tmp37;
-  const Scalar _tmp39 = _tmp29 - _tmp30 + _tmp31;
-  const Scalar _tmp40 = -_tmp19 + _tmp20 - _tmp22;
-  const Scalar _tmp41 = -_tmp33 + _tmp34;
-  const Scalar _tmp42 = 2 * _tmp15;
-  const Scalar _tmp43 = 2 * _tmp16;
-  const Scalar _tmp44 = _tmp42 + _tmp43;
-  const Scalar _tmp45 = _tmp25 - _tmp26 - _tmp27;
-  const Scalar _tmp46 = _tmp36 + _tmp37;
-  const Scalar _tmp47 = _tmp42 - _tmp43;
+  const Scalar _tmp36 = _tmp5 * state(16, 0);
+  const Scalar _tmp37 = 4 * state(1, 0);
+  const Scalar _tmp38 = _tmp26 + _tmp36 - _tmp37 * state(17, 0);
+  const Scalar _tmp39 = _tmp3 * state(16, 0);
+  const Scalar _tmp40 = -4 * _tmp28 + _tmp30 - _tmp39;
+  const Scalar _tmp41 = _tmp32 * state(16, 0);
+  const Scalar _tmp42 = _tmp21 - _tmp41;
+  const Scalar _tmp43 = _tmp9 * state(16, 0);
+  const Scalar _tmp44 = _tmp33 + _tmp43;
+  const Scalar _tmp45 = -_tmp22 - _tmp37 * state(18, 0) + _tmp41;
+  const Scalar _tmp46 = -_tmp24 * state(18, 0) + _tmp29 + _tmp39;
+  const Scalar _tmp47 = _tmp34 + _tmp43;
+  const Scalar _tmp48 = -_tmp25 + _tmp36;
 
   // Output terms (3)
   if (innov != nullptr) {
     matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
 
-    _innov(0, 0) = _tmp12 * (-_tmp10 + _tmp11) + _tmp6 * state(16, 0) + _tmp9 * (_tmp7 + _tmp8) -
+    _innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) -
                    meas(0, 0) + state(19, 0);
-    _innov(1, 0) = _tmp12 * (_tmp15 + _tmp16) + _tmp14 * state(17, 0) + _tmp17 * (-_tmp7 + _tmp8) -
+    _innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) -
                    meas(1, 0) + state(20, 0);
-    _innov(2, 0) = _tmp17 * (_tmp10 + _tmp11) + _tmp18 * state(18, 0) + _tmp9 * (_tmp15 - _tmp16) -
+    _innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) -
                    meas(2, 0) + state(21, 0);
   }
 
@@ -105,56 +106,56 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
     matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
 
     _innov_var(0, 0) =
-        P(0, 19) * _tmp23 + P(1, 19) * _tmp24 + P(16, 19) * _tmp6 + P(17, 19) * _tmp35 +
-        P(18, 19) * _tmp38 + P(19, 19) + P(2, 19) * _tmp32 + P(3, 19) * _tmp28 + R +
-        _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp24 + P(16, 0) * _tmp6 + P(17, 0) * _tmp35 +
-                  P(18, 0) * _tmp38 + P(19, 0) + P(2, 0) * _tmp32 + P(3, 0) * _tmp28) +
-        _tmp24 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp24 + P(16, 1) * _tmp6 + P(17, 1) * _tmp35 +
-                  P(18, 1) * _tmp38 + P(19, 1) + P(2, 1) * _tmp32 + P(3, 1) * _tmp28) +
-        _tmp28 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp24 + P(16, 3) * _tmp6 + P(17, 3) * _tmp35 +
-                  P(18, 3) * _tmp38 + P(19, 3) + P(2, 3) * _tmp32 + P(3, 3) * _tmp28) +
-        _tmp32 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp24 + P(16, 2) * _tmp6 + P(17, 2) * _tmp35 +
-                  P(18, 2) * _tmp38 + P(19, 2) + P(2, 2) * _tmp32 + P(3, 2) * _tmp28) +
-        _tmp35 * (P(0, 17) * _tmp23 + P(1, 17) * _tmp24 + P(16, 17) * _tmp6 + P(17, 17) * _tmp35 +
-                  P(18, 17) * _tmp38 + P(19, 17) + P(2, 17) * _tmp32 + P(3, 17) * _tmp28) +
-        _tmp38 * (P(0, 18) * _tmp23 + P(1, 18) * _tmp24 + P(16, 18) * _tmp6 + P(17, 18) * _tmp35 +
-                  P(18, 18) * _tmp38 + P(19, 18) + P(2, 18) * _tmp32 + P(3, 18) * _tmp28) +
-        _tmp6 * (P(0, 16) * _tmp23 + P(1, 16) * _tmp24 + P(16, 16) * _tmp6 + P(17, 16) * _tmp35 +
-                 P(18, 16) * _tmp38 + P(19, 16) + P(2, 16) * _tmp32 + P(3, 16) * _tmp28);
+        P(0, 19) * _tmp31 + P(1, 19) * _tmp35 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 +
+        P(18, 19) * _tmp11 + P(19, 19) + P(2, 19) * _tmp27 + P(3, 19) * _tmp23 + R +
+        _tmp11 * (P(0, 18) * _tmp31 + P(1, 18) * _tmp35 + P(16, 18) * _tmp2 + P(17, 18) * _tmp7 +
+                  P(18, 18) * _tmp11 + P(19, 18) + P(2, 18) * _tmp27 + P(3, 18) * _tmp23) +
+        _tmp2 * (P(0, 16) * _tmp31 + P(1, 16) * _tmp35 + P(16, 16) * _tmp2 + P(17, 16) * _tmp7 +
+                 P(18, 16) * _tmp11 + P(19, 16) + P(2, 16) * _tmp27 + P(3, 16) * _tmp23) +
+        _tmp23 * (P(0, 3) * _tmp31 + P(1, 3) * _tmp35 + P(16, 3) * _tmp2 + P(17, 3) * _tmp7 +
+                  P(18, 3) * _tmp11 + P(19, 3) + P(2, 3) * _tmp27 + P(3, 3) * _tmp23) +
+        _tmp27 * (P(0, 2) * _tmp31 + P(1, 2) * _tmp35 + P(16, 2) * _tmp2 + P(17, 2) * _tmp7 +
+                  P(18, 2) * _tmp11 + P(19, 2) + P(2, 2) * _tmp27 + P(3, 2) * _tmp23) +
+        _tmp31 * (P(0, 0) * _tmp31 + P(1, 0) * _tmp35 + P(16, 0) * _tmp2 + P(17, 0) * _tmp7 +
+                  P(18, 0) * _tmp11 + P(19, 0) + P(2, 0) * _tmp27 + P(3, 0) * _tmp23) +
+        _tmp35 * (P(0, 1) * _tmp31 + P(1, 1) * _tmp35 + P(16, 1) * _tmp2 + P(17, 1) * _tmp7 +
+                  P(18, 1) * _tmp11 + P(19, 1) + P(2, 1) * _tmp27 + P(3, 1) * _tmp23) +
+        _tmp7 * (P(0, 17) * _tmp31 + P(1, 17) * _tmp35 + P(16, 17) * _tmp2 + P(17, 17) * _tmp7 +
+                 P(18, 17) * _tmp11 + P(19, 17) + P(2, 17) * _tmp27 + P(3, 17) * _tmp23);
     _innov_var(1, 0) =
-        P(0, 20) * _tmp28 + P(1, 20) * _tmp39 + P(16, 20) * _tmp41 + P(17, 20) * _tmp14 +
-        P(18, 20) * _tmp44 + P(2, 20) * _tmp24 + P(20, 20) + P(3, 20) * _tmp40 + R +
-        _tmp14 * (P(0, 17) * _tmp28 + P(1, 17) * _tmp39 + P(16, 17) * _tmp41 + P(17, 17) * _tmp14 +
-                  P(18, 17) * _tmp44 + P(2, 17) * _tmp24 + P(20, 17) + P(3, 17) * _tmp40) +
-        _tmp24 * (P(0, 2) * _tmp28 + P(1, 2) * _tmp39 + P(16, 2) * _tmp41 + P(17, 2) * _tmp14 +
-                  P(18, 2) * _tmp44 + P(2, 2) * _tmp24 + P(20, 2) + P(3, 2) * _tmp40) +
-        _tmp28 * (P(0, 0) * _tmp28 + P(1, 0) * _tmp39 + P(16, 0) * _tmp41 + P(17, 0) * _tmp14 +
-                  P(18, 0) * _tmp44 + P(2, 0) * _tmp24 + P(20, 0) + P(3, 0) * _tmp40) +
-        _tmp39 * (P(0, 1) * _tmp28 + P(1, 1) * _tmp39 + P(16, 1) * _tmp41 + P(17, 1) * _tmp14 +
-                  P(18, 1) * _tmp44 + P(2, 1) * _tmp24 + P(20, 1) + P(3, 1) * _tmp40) +
-        _tmp40 * (P(0, 3) * _tmp28 + P(1, 3) * _tmp39 + P(16, 3) * _tmp41 + P(17, 3) * _tmp14 +
-                  P(18, 3) * _tmp44 + P(2, 3) * _tmp24 + P(20, 3) + P(3, 3) * _tmp40) +
-        _tmp41 * (P(0, 16) * _tmp28 + P(1, 16) * _tmp39 + P(16, 16) * _tmp41 + P(17, 16) * _tmp14 +
-                  P(18, 16) * _tmp44 + P(2, 16) * _tmp24 + P(20, 16) + P(3, 16) * _tmp40) +
-        _tmp44 * (P(0, 18) * _tmp28 + P(1, 18) * _tmp39 + P(16, 18) * _tmp41 + P(17, 18) * _tmp14 +
-                  P(18, 18) * _tmp44 + P(2, 18) * _tmp24 + P(20, 18) + P(3, 18) * _tmp40);
+        P(0, 20) * _tmp42 + P(1, 20) * _tmp38 + P(16, 20) * _tmp17 + P(17, 20) * _tmp13 +
+        P(18, 20) * _tmp16 + P(2, 20) * _tmp44 + P(20, 20) + P(3, 20) * _tmp40 + R +
+        _tmp13 * (P(0, 17) * _tmp42 + P(1, 17) * _tmp38 + P(16, 17) * _tmp17 + P(17, 17) * _tmp13 +
+                  P(18, 17) * _tmp16 + P(2, 17) * _tmp44 + P(20, 17) + P(3, 17) * _tmp40) +
+        _tmp16 * (P(0, 18) * _tmp42 + P(1, 18) * _tmp38 + P(16, 18) * _tmp17 + P(17, 18) * _tmp13 +
+                  P(18, 18) * _tmp16 + P(2, 18) * _tmp44 + P(20, 18) + P(3, 18) * _tmp40) +
+        _tmp17 * (P(0, 16) * _tmp42 + P(1, 16) * _tmp38 + P(16, 16) * _tmp17 + P(17, 16) * _tmp13 +
+                  P(18, 16) * _tmp16 + P(2, 16) * _tmp44 + P(20, 16) + P(3, 16) * _tmp40) +
+        _tmp38 * (P(0, 1) * _tmp42 + P(1, 1) * _tmp38 + P(16, 1) * _tmp17 + P(17, 1) * _tmp13 +
+                  P(18, 1) * _tmp16 + P(2, 1) * _tmp44 + P(20, 1) + P(3, 1) * _tmp40) +
+        _tmp40 * (P(0, 3) * _tmp42 + P(1, 3) * _tmp38 + P(16, 3) * _tmp17 + P(17, 3) * _tmp13 +
+                  P(18, 3) * _tmp16 + P(2, 3) * _tmp44 + P(20, 3) + P(3, 3) * _tmp40) +
+        _tmp42 * (P(0, 0) * _tmp42 + P(1, 0) * _tmp38 + P(16, 0) * _tmp17 + P(17, 0) * _tmp13 +
+                  P(18, 0) * _tmp16 + P(2, 0) * _tmp44 + P(20, 0) + P(3, 0) * _tmp40) +
+        _tmp44 * (P(0, 2) * _tmp42 + P(1, 2) * _tmp38 + P(16, 2) * _tmp17 + P(17, 2) * _tmp13 +
+                  P(18, 2) * _tmp16 + P(2, 2) * _tmp44 + P(20, 2) + P(3, 2) * _tmp40);
     _innov_var(2, 0) =
-        P(0, 21) * _tmp39 + P(1, 21) * _tmp45 + P(16, 21) * _tmp46 + P(17, 21) * _tmp47 +
-        P(18, 21) * _tmp18 + P(2, 21) * _tmp23 + P(21, 21) + P(3, 21) * _tmp24 + R +
-        _tmp18 * (P(0, 18) * _tmp39 + P(1, 18) * _tmp45 + P(16, 18) * _tmp46 + P(17, 18) * _tmp47 +
-                  P(18, 18) * _tmp18 + P(2, 18) * _tmp23 + P(21, 18) + P(3, 18) * _tmp24) +
-        _tmp23 * (P(0, 2) * _tmp39 + P(1, 2) * _tmp45 + P(16, 2) * _tmp46 + P(17, 2) * _tmp47 +
-                  P(18, 2) * _tmp18 + P(2, 2) * _tmp23 + P(21, 2) + P(3, 2) * _tmp24) +
-        _tmp24 * (P(0, 3) * _tmp39 + P(1, 3) * _tmp45 + P(16, 3) * _tmp46 + P(17, 3) * _tmp47 +
-                  P(18, 3) * _tmp18 + P(2, 3) * _tmp23 + P(21, 3) + P(3, 3) * _tmp24) +
-        _tmp39 * (P(0, 0) * _tmp39 + P(1, 0) * _tmp45 + P(16, 0) * _tmp46 + P(17, 0) * _tmp47 +
-                  P(18, 0) * _tmp18 + P(2, 0) * _tmp23 + P(21, 0) + P(3, 0) * _tmp24) +
-        _tmp45 * (P(0, 1) * _tmp39 + P(1, 1) * _tmp45 + P(16, 1) * _tmp46 + P(17, 1) * _tmp47 +
-                  P(18, 1) * _tmp18 + P(2, 1) * _tmp23 + P(21, 1) + P(3, 1) * _tmp24) +
-        _tmp46 * (P(0, 16) * _tmp39 + P(1, 16) * _tmp45 + P(16, 16) * _tmp46 + P(17, 16) * _tmp47 +
-                  P(18, 16) * _tmp18 + P(2, 16) * _tmp23 + P(21, 16) + P(3, 16) * _tmp24) +
-        _tmp47 * (P(0, 17) * _tmp39 + P(1, 17) * _tmp45 + P(16, 17) * _tmp46 + P(17, 17) * _tmp47 +
-                  P(18, 17) * _tmp18 + P(2, 17) * _tmp23 + P(21, 17) + P(3, 17) * _tmp24);
+        P(0, 21) * _tmp48 + P(1, 21) * _tmp45 + P(16, 21) * _tmp20 + P(17, 21) * _tmp19 +
+        P(18, 21) * _tmp18 + P(2, 21) * _tmp46 + P(21, 21) + P(3, 21) * _tmp47 + R +
+        _tmp18 * (P(0, 18) * _tmp48 + P(1, 18) * _tmp45 + P(16, 18) * _tmp20 + P(17, 18) * _tmp19 +
+                  P(18, 18) * _tmp18 + P(2, 18) * _tmp46 + P(21, 18) + P(3, 18) * _tmp47) +
+        _tmp19 * (P(0, 17) * _tmp48 + P(1, 17) * _tmp45 + P(16, 17) * _tmp20 + P(17, 17) * _tmp19 +
+                  P(18, 17) * _tmp18 + P(2, 17) * _tmp46 + P(21, 17) + P(3, 17) * _tmp47) +
+        _tmp20 * (P(0, 16) * _tmp48 + P(1, 16) * _tmp45 + P(16, 16) * _tmp20 + P(17, 16) * _tmp19 +
+                  P(18, 16) * _tmp18 + P(2, 16) * _tmp46 + P(21, 16) + P(3, 16) * _tmp47) +
+        _tmp45 * (P(0, 1) * _tmp48 + P(1, 1) * _tmp45 + P(16, 1) * _tmp20 + P(17, 1) * _tmp19 +
+                  P(18, 1) * _tmp18 + P(2, 1) * _tmp46 + P(21, 1) + P(3, 1) * _tmp47) +
+        _tmp46 * (P(0, 2) * _tmp48 + P(1, 2) * _tmp45 + P(16, 2) * _tmp20 + P(17, 2) * _tmp19 +
+                  P(18, 2) * _tmp18 + P(2, 2) * _tmp46 + P(21, 2) + P(3, 2) * _tmp47) +
+        _tmp47 * (P(0, 3) * _tmp48 + P(1, 3) * _tmp45 + P(16, 3) * _tmp20 + P(17, 3) * _tmp19 +
+                  P(18, 3) * _tmp18 + P(2, 3) * _tmp46 + P(21, 3) + P(3, 3) * _tmp47) +
+        _tmp48 * (P(0, 0) * _tmp48 + P(1, 0) * _tmp45 + P(16, 0) * _tmp20 + P(17, 0) * _tmp19 +
+                  P(18, 0) * _tmp18 + P(2, 0) * _tmp46 + P(21, 0) + P(3, 0) * _tmp47);
   }
 
   if (Hx != nullptr) {
@@ -162,13 +163,13 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _hx.setZero();
 
-    _hx(0, 0) = _tmp23;
-    _hx(1, 0) = _tmp24;
-    _hx(2, 0) = _tmp32;
-    _hx(3, 0) = _tmp28;
-    _hx(16, 0) = _tmp6;
-    _hx(17, 0) = _tmp35;
-    _hx(18, 0) = _tmp38;
+    _hx(0, 0) = _tmp31;
+    _hx(1, 0) = _tmp35;
+    _hx(2, 0) = _tmp27;
+    _hx(3, 0) = _tmp23;
+    _hx(16, 0) = _tmp2;
+    _hx(17, 0) = _tmp7;
+    _hx(18, 0) = _tmp11;
     _hx(19, 0) = 1;
   }
 }  // NOLINT(readability/fn_size)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h

@@ -30,45 +30,46 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 164
+  // Total ops: 160
 
   // Input arrays
 
-  // Intermediate terms (11)
-  const Scalar _tmp0 = 2 * state(3, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = 2 * state(0, 0);
-  const Scalar _tmp3 = -_tmp0 * state(16, 0) + _tmp1 * state(18, 0) + _tmp2 * state(17, 0);
-  const Scalar _tmp4 = 2 * state(2, 0);
-  const Scalar _tmp5 = -_tmp1 * state(17, 0) + _tmp2 * state(18, 0) + _tmp4 * state(16, 0);
-  const Scalar _tmp6 = -_tmp0 * state(17, 0) - _tmp2 * state(16, 0) + _tmp4 * state(18, 0);
-  const Scalar _tmp7 = _tmp0 * state(18, 0) + _tmp1 * state(16, 0) + _tmp4 * state(17, 0);
-  const Scalar _tmp8 = -_tmp0 * state(0, 0) + _tmp1 * state(2, 0);
-  const Scalar _tmp9 = _tmp0 * state(2, 0) + _tmp1 * state(0, 0);
-  const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) +
-                        std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
+  // Intermediate terms (12)
+  const Scalar _tmp0 = 2 * state(16, 0);
+  const Scalar _tmp1 = 4 * state(17, 0);
+  const Scalar _tmp2 = 2 * state(18, 0);
+  const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0) + _tmp2 * state(0, 0);
+  const Scalar _tmp4 = -_tmp0 * state(0, 0) - _tmp1 * state(3, 0) + _tmp2 * state(2, 0);
+  const Scalar _tmp5 = -_tmp0 * state(3, 0) + _tmp2 * state(1, 0);
+  const Scalar _tmp6 = _tmp0 * state(1, 0) + _tmp2 * state(3, 0);
+  const Scalar _tmp7 =
+      -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
+  const Scalar _tmp8 = 2 * state(3, 0);
+  const Scalar _tmp9 = 2 * state(1, 0);
+  const Scalar _tmp10 = _tmp8 * state(2, 0) + _tmp9 * state(0, 0);
+  const Scalar _tmp11 = -_tmp8 * state(0, 0) + _tmp9 * state(2, 0);
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
     _innov_var =
-        P(0, 20) * _tmp3 + P(1, 20) * _tmp5 + P(16, 20) * _tmp8 + P(17, 20) * _tmp10 +
-        P(18, 20) * _tmp9 + P(2, 20) * _tmp7 + P(20, 20) + P(3, 20) * _tmp6 + R +
-        _tmp10 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp10 +
-                  P(18, 17) * _tmp9 + P(2, 17) * _tmp7 + P(20, 17) + P(3, 17) * _tmp6) +
-        _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp10 +
-                 P(18, 0) * _tmp9 + P(2, 0) * _tmp7 + P(20, 0) + P(3, 0) * _tmp6) +
-        _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp10 +
-                 P(18, 1) * _tmp9 + P(2, 1) * _tmp7 + P(20, 1) + P(3, 1) * _tmp6) +
-        _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp10 +
-                 P(18, 3) * _tmp9 + P(2, 3) * _tmp7 + P(20, 3) + P(3, 3) * _tmp6) +
-        _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp10 +
-                 P(18, 2) * _tmp9 + P(2, 2) * _tmp7 + P(20, 2) + P(3, 2) * _tmp6) +
-        _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp10 +
-                 P(18, 16) * _tmp9 + P(2, 16) * _tmp7 + P(20, 16) + P(3, 16) * _tmp6) +
-        _tmp9 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp10 +
-                 P(18, 18) * _tmp9 + P(2, 18) * _tmp7 + P(20, 18) + P(3, 18) * _tmp6);
+        P(0, 20) * _tmp5 + P(1, 20) * _tmp3 + P(16, 20) * _tmp11 + P(17, 20) * _tmp7 +
+        P(18, 20) * _tmp10 + P(2, 20) * _tmp6 + P(20, 20) + P(3, 20) * _tmp4 + R +
+        _tmp10 * (P(0, 18) * _tmp5 + P(1, 18) * _tmp3 + P(16, 18) * _tmp11 + P(17, 18) * _tmp7 +
+                  P(18, 18) * _tmp10 + P(2, 18) * _tmp6 + P(20, 18) + P(3, 18) * _tmp4) +
+        _tmp11 * (P(0, 16) * _tmp5 + P(1, 16) * _tmp3 + P(16, 16) * _tmp11 + P(17, 16) * _tmp7 +
+                  P(18, 16) * _tmp10 + P(2, 16) * _tmp6 + P(20, 16) + P(3, 16) * _tmp4) +
+        _tmp3 * (P(0, 1) * _tmp5 + P(1, 1) * _tmp3 + P(16, 1) * _tmp11 + P(17, 1) * _tmp7 +
+                 P(18, 1) * _tmp10 + P(2, 1) * _tmp6 + P(20, 1) + P(3, 1) * _tmp4) +
+        _tmp4 * (P(0, 3) * _tmp5 + P(1, 3) * _tmp3 + P(16, 3) * _tmp11 + P(17, 3) * _tmp7 +
+                 P(18, 3) * _tmp10 + P(2, 3) * _tmp6 + P(20, 3) + P(3, 3) * _tmp4) +
+        _tmp5 * (P(0, 0) * _tmp5 + P(1, 0) * _tmp3 + P(16, 0) * _tmp11 + P(17, 0) * _tmp7 +
+                 P(18, 0) * _tmp10 + P(2, 0) * _tmp6 + P(20, 0) + P(3, 0) * _tmp4) +
+        _tmp6 * (P(0, 2) * _tmp5 + P(1, 2) * _tmp3 + P(16, 2) * _tmp11 + P(17, 2) * _tmp7 +
+                 P(18, 2) * _tmp10 + P(2, 2) * _tmp6 + P(20, 2) + P(3, 2) * _tmp4) +
+        _tmp7 * (P(0, 17) * _tmp5 + P(1, 17) * _tmp3 + P(16, 17) * _tmp11 + P(17, 17) * _tmp7 +
+                 P(18, 17) * _tmp10 + P(2, 17) * _tmp6 + P(20, 17) + P(3, 17) * _tmp4);
   }
 
   if (H != nullptr) {
@@ -76,13 +77,13 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp3;
-    _h(1, 0) = _tmp5;
-    _h(2, 0) = _tmp7;
-    _h(3, 0) = _tmp6;
-    _h(16, 0) = _tmp8;
-    _h(17, 0) = _tmp10;
-    _h(18, 0) = _tmp9;
+    _h(0, 0) = _tmp5;
+    _h(1, 0) = _tmp3;
+    _h(2, 0) = _tmp6;
+    _h(3, 0) = _tmp4;
+    _h(16, 0) = _tmp11;
+    _h(17, 0) = _tmp7;
+    _h(18, 0) = _tmp10;
     _h(20, 0) = 1;
   }
 }  // NOLINT(readability/fn_size)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h

@@ -30,45 +30,46 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 164
+  // Total ops: 160
 
   // Input arrays
 
-  // Intermediate terms (11)
-  const Scalar _tmp0 = 2 * state(2, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = 2 * state(0, 0);
-  const Scalar _tmp3 = _tmp0 * state(16, 0) - _tmp1 * state(17, 0) + _tmp2 * state(18, 0);
-  const Scalar _tmp4 = 2 * state(3, 0);
-  const Scalar _tmp5 = -_tmp1 * state(18, 0) - _tmp2 * state(17, 0) + _tmp4 * state(16, 0);
-  const Scalar _tmp6 = _tmp0 * state(17, 0) + _tmp1 * state(16, 0) + _tmp4 * state(18, 0);
-  const Scalar _tmp7 = -_tmp0 * state(18, 0) + _tmp2 * state(16, 0) + _tmp4 * state(17, 0);
-  const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0);
-  const Scalar _tmp9 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0);
-  const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) -
-                        std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2));
+  // Intermediate terms (12)
+  const Scalar _tmp0 = 2 * state(16, 0);
+  const Scalar _tmp1 = 4 * state(18, 0);
+  const Scalar _tmp2 = 2 * state(17, 0);
+  const Scalar _tmp3 = _tmp0 * state(3, 0) - _tmp1 * state(1, 0) - _tmp2 * state(0, 0);
+  const Scalar _tmp4 = _tmp0 * state(0, 0) - _tmp1 * state(2, 0) + _tmp2 * state(3, 0);
+  const Scalar _tmp5 = _tmp0 * state(1, 0) + _tmp2 * state(2, 0);
+  const Scalar _tmp6 = _tmp0 * state(2, 0) - _tmp2 * state(1, 0);
+  const Scalar _tmp7 = 2 * state(2, 0);
+  const Scalar _tmp8 = 2 * state(1, 0);
+  const Scalar _tmp9 = _tmp7 * state(0, 0) + _tmp8 * state(3, 0);
+  const Scalar _tmp10 = _tmp7 * state(3, 0) - _tmp8 * state(0, 0);
+  const Scalar _tmp11 =
+      -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
     _innov_var =
-        P(0, 21) * _tmp3 + P(1, 21) * _tmp5 + P(16, 21) * _tmp8 + P(17, 21) * _tmp9 +
-        P(18, 21) * _tmp10 + P(2, 21) * _tmp7 + P(21, 21) + P(3, 21) * _tmp6 + R +
-        _tmp10 * (P(0, 18) * _tmp3 + P(1, 18) * _tmp5 + P(16, 18) * _tmp8 + P(17, 18) * _tmp9 +
-                  P(18, 18) * _tmp10 + P(2, 18) * _tmp7 + P(21, 18) + P(3, 18) * _tmp6) +
-        _tmp3 * (P(0, 0) * _tmp3 + P(1, 0) * _tmp5 + P(16, 0) * _tmp8 + P(17, 0) * _tmp9 +
-                 P(18, 0) * _tmp10 + P(2, 0) * _tmp7 + P(21, 0) + P(3, 0) * _tmp6) +
-        _tmp5 * (P(0, 1) * _tmp3 + P(1, 1) * _tmp5 + P(16, 1) * _tmp8 + P(17, 1) * _tmp9 +
-                 P(18, 1) * _tmp10 + P(2, 1) * _tmp7 + P(21, 1) + P(3, 1) * _tmp6) +
-        _tmp6 * (P(0, 3) * _tmp3 + P(1, 3) * _tmp5 + P(16, 3) * _tmp8 + P(17, 3) * _tmp9 +
-                 P(18, 3) * _tmp10 + P(2, 3) * _tmp7 + P(21, 3) + P(3, 3) * _tmp6) +
-        _tmp7 * (P(0, 2) * _tmp3 + P(1, 2) * _tmp5 + P(16, 2) * _tmp8 + P(17, 2) * _tmp9 +
-                 P(18, 2) * _tmp10 + P(2, 2) * _tmp7 + P(21, 2) + P(3, 2) * _tmp6) +
-        _tmp8 * (P(0, 16) * _tmp3 + P(1, 16) * _tmp5 + P(16, 16) * _tmp8 + P(17, 16) * _tmp9 +
-                 P(18, 16) * _tmp10 + P(2, 16) * _tmp7 + P(21, 16) + P(3, 16) * _tmp6) +
-        _tmp9 * (P(0, 17) * _tmp3 + P(1, 17) * _tmp5 + P(16, 17) * _tmp8 + P(17, 17) * _tmp9 +
-                 P(18, 17) * _tmp10 + P(2, 17) * _tmp7 + P(21, 17) + P(3, 17) * _tmp6);
+        P(0, 21) * _tmp6 + P(1, 21) * _tmp3 + P(16, 21) * _tmp9 + P(17, 21) * _tmp10 +
+        P(18, 21) * _tmp11 + P(2, 21) * _tmp4 + P(21, 21) + P(3, 21) * _tmp5 + R +
+        _tmp10 * (P(0, 17) * _tmp6 + P(1, 17) * _tmp3 + P(16, 17) * _tmp9 + P(17, 17) * _tmp10 +
+                  P(18, 17) * _tmp11 + P(2, 17) * _tmp4 + P(21, 17) + P(3, 17) * _tmp5) +
+        _tmp11 * (P(0, 18) * _tmp6 + P(1, 18) * _tmp3 + P(16, 18) * _tmp9 + P(17, 18) * _tmp10 +
+                  P(18, 18) * _tmp11 + P(2, 18) * _tmp4 + P(21, 18) + P(3, 18) * _tmp5) +
+        _tmp3 * (P(0, 1) * _tmp6 + P(1, 1) * _tmp3 + P(16, 1) * _tmp9 + P(17, 1) * _tmp10 +
+                 P(18, 1) * _tmp11 + P(2, 1) * _tmp4 + P(21, 1) + P(3, 1) * _tmp5) +
+        _tmp4 * (P(0, 2) * _tmp6 + P(1, 2) * _tmp3 + P(16, 2) * _tmp9 + P(17, 2) * _tmp10 +
+                 P(18, 2) * _tmp11 + P(2, 2) * _tmp4 + P(21, 2) + P(3, 2) * _tmp5) +
+        _tmp5 * (P(0, 3) * _tmp6 + P(1, 3) * _tmp3 + P(16, 3) * _tmp9 + P(17, 3) * _tmp10 +
+                 P(18, 3) * _tmp11 + P(2, 3) * _tmp4 + P(21, 3) + P(3, 3) * _tmp5) +
+        _tmp6 * (P(0, 0) * _tmp6 + P(1, 0) * _tmp3 + P(16, 0) * _tmp9 + P(17, 0) * _tmp10 +
+                 P(18, 0) * _tmp11 + P(2, 0) * _tmp4 + P(21, 0) + P(3, 0) * _tmp5) +
+        _tmp9 * (P(0, 16) * _tmp6 + P(1, 16) * _tmp3 + P(16, 16) * _tmp9 + P(17, 16) * _tmp10 +
+                 P(18, 16) * _tmp11 + P(2, 16) * _tmp4 + P(21, 16) + P(3, 16) * _tmp5);
   }
 
   if (H != nullptr) {
@@ -76,13 +77,13 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp3;
-    _h(1, 0) = _tmp5;
-    _h(2, 0) = _tmp7;
-    _h(3, 0) = _tmp6;
-    _h(16, 0) = _tmp8;
-    _h(17, 0) = _tmp9;
-    _h(18, 0) = _tmp10;
+    _h(0, 0) = _tmp6;
+    _h(1, 0) = _tmp3;
+    _h(2, 0) = _tmp4;
+    _h(3, 0) = _tmp5;
+    _h(16, 0) = _tmp9;
+    _h(17, 0) = _tmp10;
+    _h(18, 0) = _tmp11;
     _h(21, 0) = 1;
   }
 }  // NOLINT(readability/fn_size)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h

@@ -30,57 +30,56 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
                           const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
                           matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
-  // Total ops: 539
+  // Total ops: 533
 
   // Input arrays
 
-  // Intermediate terms (44)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
-  const Scalar _tmp4 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp5 = state(0, 0) * state(3, 0);
-  const Scalar _tmp6 = state(1, 0) * state(2, 0);
-  const Scalar _tmp7 = _tmp5 + _tmp6;
-  const Scalar _tmp8 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp9 = 2 * _tmp8;
-  const Scalar _tmp10 = state(0, 0) * state(2, 0);
-  const Scalar _tmp11 = state(1, 0) * state(3, 0);
-  const Scalar _tmp12 = 2 * state(6, 0);
-  const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
-  const Scalar _tmp14 =
-      _tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
-  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
-  const Scalar _tmp16 = 2 * _tmp4;
-  const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0);
-  const Scalar _tmp18 = _tmp16 * state(0, 0);
-  const Scalar _tmp19 = _tmp9 * state(3, 0);
-  const Scalar _tmp20 = _tmp12 * state(2, 0);
-  const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2;
-  const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6;
-  const Scalar _tmp23 = state(2, 0) * state(3, 0);
-  const Scalar _tmp24 = state(0, 0) * state(1, 0);
-  const Scalar _tmp25 =
-      (_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2));
-  const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20);
-  const Scalar _tmp27 = _tmp9 * state(1, 0);
-  const Scalar _tmp28 = _tmp16 * state(2, 0);
-  const Scalar _tmp29 = _tmp12 * state(0, 0);
-  const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0);
-  const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30;
-  const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29);
-  const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25;
-  const Scalar _tmp34 = _tmp25 * _tmp3;
-  const Scalar _tmp35 = 2 * _tmp5;
-  const Scalar _tmp36 = 2 * _tmp6;
-  const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34;
-  const Scalar _tmp38 = _tmp15 * _tmp21;
-  const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38;
-  const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11);
-  const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34;
-  const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38;
-  const Scalar _tmp43 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+  // Intermediate terms (40)
+  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
+  const Scalar _tmp1 = 2 * state(3, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
+  const Scalar _tmp3 = _tmp2 * state(2, 0);
+  const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp6 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp7 = _tmp1 * state(0, 0);
+  const Scalar _tmp8 = 2 * state(1, 0) * state(2, 0);
+  const Scalar _tmp9 = _tmp7 + _tmp8;
+  const Scalar _tmp10 = 2 * state(0, 0);
+  const Scalar _tmp11 = _tmp1 * state(1, 0) - _tmp10 * state(2, 0);
+  const Scalar _tmp12 = _tmp0 * _tmp9 + _tmp11 * state(6, 0) + _tmp5 * _tmp6;
+  const Scalar _tmp13 =
+      _tmp12 + epsilon * (2 * math::min<Scalar>(0, (((_tmp12) > 0) - ((_tmp12) < 0))) + 1);
+  const Scalar _tmp14 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp15 = -_tmp7 + _tmp8;
+  const Scalar _tmp16 = _tmp1 * state(2, 0) + _tmp10 * state(1, 0);
+  const Scalar _tmp17 =
+      (_tmp0 * _tmp14 + _tmp15 * _tmp6 + _tmp16 * state(6, 0)) / std::pow(_tmp13, Scalar(2));
+  const Scalar _tmp18 = _tmp2 * state(1, 0);
+  const Scalar _tmp19 = Scalar(1.0) / (_tmp13);
+  const Scalar _tmp20 = -_tmp17 * (_tmp0 * _tmp1 - _tmp3) + _tmp19 * (-_tmp1 * _tmp6 + _tmp18);
+  const Scalar _tmp21 = 2 * _tmp0;
+  const Scalar _tmp22 = _tmp1 * state(6, 0);
+  const Scalar _tmp23 = 4 * _tmp0;
+  const Scalar _tmp24 = 2 * _tmp6;
+  const Scalar _tmp25 = _tmp2 * state(0, 0);
+  const Scalar _tmp26 = -_tmp17 * (_tmp21 * state(2, 0) + _tmp22) +
+                        _tmp19 * (-_tmp23 * state(1, 0) + _tmp24 * state(2, 0) + _tmp25);
+  const Scalar _tmp27 = 4 * _tmp6;
+  const Scalar _tmp28 = -_tmp17 * (_tmp21 * state(1, 0) - _tmp25 - _tmp27 * state(2, 0)) +
+                        _tmp19 * (_tmp22 + _tmp24 * state(1, 0));
+  const Scalar _tmp29 = -_tmp17 * (_tmp18 + _tmp21 * state(0, 0) - _tmp27 * state(3, 0)) +
+                        _tmp19 * (-_tmp23 * state(3, 0) - _tmp24 * state(0, 0) + _tmp3);
+  const Scalar _tmp30 = _tmp17 * _tmp5;
+  const Scalar _tmp31 = _tmp15 * _tmp19;
+  const Scalar _tmp32 = -_tmp30 + _tmp31;
+  const Scalar _tmp33 = _tmp17 * _tmp9;
+  const Scalar _tmp34 = _tmp14 * _tmp19;
+  const Scalar _tmp35 = -_tmp33 + _tmp34;
+  const Scalar _tmp36 = -_tmp11 * _tmp17 + _tmp16 * _tmp19;
+  const Scalar _tmp37 = _tmp30 - _tmp31;
+  const Scalar _tmp38 = _tmp33 - _tmp34;
+  const Scalar _tmp39 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
 
   // Output terms (2)
   if (H != nullptr) {
@@ -88,92 +87,92 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
 
     _h.setZero();
 
-    _h(0, 0) = _tmp26;
-    _h(1, 0) = _tmp31;
-    _h(2, 0) = _tmp32;
-    _h(3, 0) = _tmp33;
-    _h(4, 0) = _tmp37;
-    _h(5, 0) = _tmp39;
-    _h(6, 0) = _tmp40;
-    _h(22, 0) = _tmp41;
-    _h(23, 0) = _tmp42;
+    _h(0, 0) = _tmp20;
+    _h(1, 0) = _tmp26;
+    _h(2, 0) = _tmp28;
+    _h(3, 0) = _tmp29;
+    _h(4, 0) = _tmp32;
+    _h(5, 0) = _tmp35;
+    _h(6, 0) = _tmp36;
+    _h(22, 0) = _tmp37;
+    _h(23, 0) = _tmp38;
   }
 
   if (K != nullptr) {
     matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
-    _k(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
-                         P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
-                         P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
-    _k(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
-                         P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
-                         P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
-    _k(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
-                         P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
-                         P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
-    _k(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
-                         P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
-                         P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
-    _k(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
-                         P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
-                         P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
-    _k(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
-                         P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
-                         P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
-    _k(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
-                         P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
-                         P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
-    _k(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
-                         P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
-                         P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
-    _k(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
-                         P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
-                         P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
-    _k(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
-                         P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
-                         P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
-    _k(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
-                          P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
-                          P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
-    _k(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
-                          P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
-                          P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
-    _k(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
-                          P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
-                          P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
-    _k(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
-                          P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
-                          P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
-    _k(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
-                          P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
-                          P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
-    _k(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
-                          P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
-                          P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
-    _k(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
-                          P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
-                          P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
-    _k(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
-                          P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
-                          P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
-    _k(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
-                          P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
-                          P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
-    _k(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
-                          P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
-                          P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
-    _k(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
-                          P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
-                          P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
-    _k(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
-                          P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
-                          P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
-    _k(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
-                          P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
-                          P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
-    _k(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
-                          P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
-                          P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
+    _k(0, 0) = _tmp39 * (P(0, 0) * _tmp20 + P(0, 1) * _tmp26 + P(0, 2) * _tmp28 +
+                         P(0, 22) * _tmp37 + P(0, 23) * _tmp38 + P(0, 3) * _tmp29 +
+                         P(0, 4) * _tmp32 + P(0, 5) * _tmp35 + P(0, 6) * _tmp36);
+    _k(1, 0) = _tmp39 * (P(1, 0) * _tmp20 + P(1, 1) * _tmp26 + P(1, 2) * _tmp28 +
+                         P(1, 22) * _tmp37 + P(1, 23) * _tmp38 + P(1, 3) * _tmp29 +
+                         P(1, 4) * _tmp32 + P(1, 5) * _tmp35 + P(1, 6) * _tmp36);
+    _k(2, 0) = _tmp39 * (P(2, 0) * _tmp20 + P(2, 1) * _tmp26 + P(2, 2) * _tmp28 +
+                         P(2, 22) * _tmp37 + P(2, 23) * _tmp38 + P(2, 3) * _tmp29 +
+                         P(2, 4) * _tmp32 + P(2, 5) * _tmp35 + P(2, 6) * _tmp36);
+    _k(3, 0) = _tmp39 * (P(3, 0) * _tmp20 + P(3, 1) * _tmp26 + P(3, 2) * _tmp28 +
+                         P(3, 22) * _tmp37 + P(3, 23) * _tmp38 + P(3, 3) * _tmp29 +
+                         P(3, 4) * _tmp32 + P(3, 5) * _tmp35 + P(3, 6) * _tmp36);
+    _k(4, 0) = _tmp39 * (P(4, 0) * _tmp20 + P(4, 1) * _tmp26 + P(4, 2) * _tmp28 +
+                         P(4, 22) * _tmp37 + P(4, 23) * _tmp38 + P(4, 3) * _tmp29 +
+                         P(4, 4) * _tmp32 + P(4, 5) * _tmp35 + P(4, 6) * _tmp36);
+    _k(5, 0) = _tmp39 * (P(5, 0) * _tmp20 + P(5, 1) * _tmp26 + P(5, 2) * _tmp28 +
+                         P(5, 22) * _tmp37 + P(5, 23) * _tmp38 + P(5, 3) * _tmp29 +
+                         P(5, 4) * _tmp32 + P(5, 5) * _tmp35 + P(5, 6) * _tmp36);
+    _k(6, 0) = _tmp39 * (P(6, 0) * _tmp20 + P(6, 1) * _tmp26 + P(6, 2) * _tmp28 +
+                         P(6, 22) * _tmp37 + P(6, 23) * _tmp38 + P(6, 3) * _tmp29 +
+                         P(6, 4) * _tmp32 + P(6, 5) * _tmp35 + P(6, 6) * _tmp36);
+    _k(7, 0) = _tmp39 * (P(7, 0) * _tmp20 + P(7, 1) * _tmp26 + P(7, 2) * _tmp28 +
+                         P(7, 22) * _tmp37 + P(7, 23) * _tmp38 + P(7, 3) * _tmp29 +
+                         P(7, 4) * _tmp32 + P(7, 5) * _tmp35 + P(7, 6) * _tmp36);
+    _k(8, 0) = _tmp39 * (P(8, 0) * _tmp20 + P(8, 1) * _tmp26 + P(8, 2) * _tmp28 +
+                         P(8, 22) * _tmp37 + P(8, 23) * _tmp38 + P(8, 3) * _tmp29 +
+                         P(8, 4) * _tmp32 + P(8, 5) * _tmp35 + P(8, 6) * _tmp36);
+    _k(9, 0) = _tmp39 * (P(9, 0) * _tmp20 + P(9, 1) * _tmp26 + P(9, 2) * _tmp28 +
+                         P(9, 22) * _tmp37 + P(9, 23) * _tmp38 + P(9, 3) * _tmp29 +
+                         P(9, 4) * _tmp32 + P(9, 5) * _tmp35 + P(9, 6) * _tmp36);
+    _k(10, 0) = _tmp39 * (P(10, 0) * _tmp20 + P(10, 1) * _tmp26 + P(10, 2) * _tmp28 +
+                          P(10, 22) * _tmp37 + P(10, 23) * _tmp38 + P(10, 3) * _tmp29 +
+                          P(10, 4) * _tmp32 + P(10, 5) * _tmp35 + P(10, 6) * _tmp36);
+    _k(11, 0) = _tmp39 * (P(11, 0) * _tmp20 + P(11, 1) * _tmp26 + P(11, 2) * _tmp28 +
+                          P(11, 22) * _tmp37 + P(11, 23) * _tmp38 + P(11, 3) * _tmp29 +
+                          P(11, 4) * _tmp32 + P(11, 5) * _tmp35 + P(11, 6) * _tmp36);
+    _k(12, 0) = _tmp39 * (P(12, 0) * _tmp20 + P(12, 1) * _tmp26 + P(12, 2) * _tmp28 +
+                          P(12, 22) * _tmp37 + P(12, 23) * _tmp38 + P(12, 3) * _tmp29 +
+                          P(12, 4) * _tmp32 + P(12, 5) * _tmp35 + P(12, 6) * _tmp36);
+    _k(13, 0) = _tmp39 * (P(13, 0) * _tmp20 + P(13, 1) * _tmp26 + P(13, 2) * _tmp28 +
+                          P(13, 22) * _tmp37 + P(13, 23) * _tmp38 + P(13, 3) * _tmp29 +
+                          P(13, 4) * _tmp32 + P(13, 5) * _tmp35 + P(13, 6) * _tmp36);
+    _k(14, 0) = _tmp39 * (P(14, 0) * _tmp20 + P(14, 1) * _tmp26 + P(14, 2) * _tmp28 +
+                          P(14, 22) * _tmp37 + P(14, 23) * _tmp38 + P(14, 3) * _tmp29 +
+                          P(14, 4) * _tmp32 + P(14, 5) * _tmp35 + P(14, 6) * _tmp36);
+    _k(15, 0) = _tmp39 * (P(15, 0) * _tmp20 + P(15, 1) * _tmp26 + P(15, 2) * _tmp28 +
+                          P(15, 22) * _tmp37 + P(15, 23) * _tmp38 + P(15, 3) * _tmp29 +
+                          P(15, 4) * _tmp32 + P(15, 5) * _tmp35 + P(15, 6) * _tmp36);
+    _k(16, 0) = _tmp39 * (P(16, 0) * _tmp20 + P(16, 1) * _tmp26 + P(16, 2) * _tmp28 +
+                          P(16, 22) * _tmp37 + P(16, 23) * _tmp38 + P(16, 3) * _tmp29 +
+                          P(16, 4) * _tmp32 + P(16, 5) * _tmp35 + P(16, 6) * _tmp36);
+    _k(17, 0) = _tmp39 * (P(17, 0) * _tmp20 + P(17, 1) * _tmp26 + P(17, 2) * _tmp28 +
+                          P(17, 22) * _tmp37 + P(17, 23) * _tmp38 + P(17, 3) * _tmp29 +
+                          P(17, 4) * _tmp32 + P(17, 5) * _tmp35 + P(17, 6) * _tmp36);
+    _k(18, 0) = _tmp39 * (P(18, 0) * _tmp20 + P(18, 1) * _tmp26 + P(18, 2) * _tmp28 +
+                          P(18, 22) * _tmp37 + P(18, 23) * _tmp38 + P(18, 3) * _tmp29 +
+                          P(18, 4) * _tmp32 + P(18, 5) * _tmp35 + P(18, 6) * _tmp36);
+    _k(19, 0) = _tmp39 * (P(19, 0) * _tmp20 + P(19, 1) * _tmp26 + P(19, 2) * _tmp28 +
+                          P(19, 22) * _tmp37 + P(19, 23) * _tmp38 + P(19, 3) * _tmp29 +
+                          P(19, 4) * _tmp32 + P(19, 5) * _tmp35 + P(19, 6) * _tmp36);
+    _k(20, 0) = _tmp39 * (P(20, 0) * _tmp20 + P(20, 1) * _tmp26 + P(20, 2) * _tmp28 +
+                          P(20, 22) * _tmp37 + P(20, 23) * _tmp38 + P(20, 3) * _tmp29 +
+                          P(20, 4) * _tmp32 + P(20, 5) * _tmp35 + P(20, 6) * _tmp36);
+    _k(21, 0) = _tmp39 * (P(21, 0) * _tmp20 + P(21, 1) * _tmp26 + P(21, 2) * _tmp28 +
+                          P(21, 22) * _tmp37 + P(21, 23) * _tmp38 + P(21, 3) * _tmp29 +
+                          P(21, 4) * _tmp32 + P(21, 5) * _tmp35 + P(21, 6) * _tmp36);
+    _k(22, 0) = _tmp39 * (P(22, 0) * _tmp20 + P(22, 1) * _tmp26 + P(22, 2) * _tmp28 +
+                          P(22, 22) * _tmp37 + P(22, 23) * _tmp38 + P(22, 3) * _tmp29 +
+                          P(22, 4) * _tmp32 + P(22, 5) * _tmp35 + P(22, 6) * _tmp36);
+    _k(23, 0) = _tmp39 * (P(23, 0) * _tmp20 + P(23, 1) * _tmp26 + P(23, 2) * _tmp28 +
+                          P(23, 22) * _tmp37 + P(23, 23) * _tmp38 + P(23, 3) * _tmp29 +
+                          P(23, 4) * _tmp32 + P(23, 5) * _tmp35 + P(23, 6) * _tmp36);
   }
 }  // NOLINT(readability/fn_size)
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h

@@ -30,95 +30,93 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
                                      const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                      const Scalar epsilon, Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {
-  // Total ops: 276
+  // Total ops: 269
 
   // Input arrays
 
-  // Intermediate terms (44)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
-  const Scalar _tmp4 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp5 = state(0, 0) * state(3, 0);
-  const Scalar _tmp6 = state(1, 0) * state(2, 0);
-  const Scalar _tmp7 = _tmp5 + _tmp6;
+  // Intermediate terms (39)
+  const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
+  const Scalar _tmp3 = 2 * state(3, 0);
+  const Scalar _tmp4 = _tmp3 * state(0, 0);
+  const Scalar _tmp5 = 2 * state(1, 0);
+  const Scalar _tmp6 = _tmp5 * state(2, 0);
+  const Scalar _tmp7 = _tmp4 + _tmp6;
   const Scalar _tmp8 = -state(23, 0) + state(5, 0);
-  const Scalar _tmp9 = 2 * _tmp8;
-  const Scalar _tmp10 = state(0, 0) * state(2, 0);
-  const Scalar _tmp11 = state(1, 0) * state(3, 0);
-  const Scalar _tmp12 = 2 * state(6, 0);
-  const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
-  const Scalar _tmp14 =
-      _tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
-  const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
-  const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2;
-  const Scalar _tmp17 = -_tmp5 + _tmp6;
-  const Scalar _tmp18 = 2 * _tmp4;
-  const Scalar _tmp19 = state(2, 0) * state(3, 0);
-  const Scalar _tmp20 = state(0, 0) * state(1, 0);
-  const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18;
-  const Scalar _tmp22 = _tmp9 * state(1, 0);
-  const Scalar _tmp23 = _tmp18 * state(2, 0);
-  const Scalar _tmp24 = _tmp12 * state(0, 0);
-  const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0);
-  const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2));
-  const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26;
-  const Scalar _tmp28 = _tmp15 * _tmp16;
-  const Scalar _tmp29 = 2 * _tmp5;
-  const Scalar _tmp30 = 2 * _tmp6;
-  const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28;
-  const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0);
-  const Scalar _tmp33 = _tmp18 * state(0, 0);
-  const Scalar _tmp34 = _tmp9 * state(3, 0);
-  const Scalar _tmp35 = _tmp12 * state(2, 0);
-  const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35);
-  const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28;
-  const Scalar _tmp38 = _tmp26 * _tmp3;
-  const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38;
-  const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24);
-  const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32;
-  const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11);
-  const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38;
+  const Scalar _tmp9 = 2 * state(2, 0);
+  const Scalar _tmp10 = _tmp3 * state(1, 0) - _tmp9 * state(0, 0);
+  const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
+  const Scalar _tmp12 =
+      _tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
+  const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
+  const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp15 = -_tmp4 + _tmp6;
+  const Scalar _tmp16 = _tmp3 * state(2, 0) + _tmp5 * state(0, 0);
+  const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
+  const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
+  const Scalar _tmp19 = _tmp1 * _tmp18;
+  const Scalar _tmp20 = _tmp13 * _tmp15;
+  const Scalar _tmp21 = -_tmp19 + _tmp20;
+  const Scalar _tmp22 = _tmp3 * state(6, 0);
+  const Scalar _tmp23 = 4 * _tmp8;
+  const Scalar _tmp24 = 2 * state(0, 0);
+  const Scalar _tmp25 = _tmp24 * state(6, 0);
+  const Scalar _tmp26 =
+      _tmp13 * (_tmp2 * _tmp9 - _tmp23 * state(1, 0) + _tmp25) - _tmp18 * (_tmp22 + _tmp8 * _tmp9);
+  const Scalar _tmp27 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
+  const Scalar _tmp28 = 4 * _tmp2;
+  const Scalar _tmp29 =
+      _tmp13 * (_tmp2 * _tmp5 + _tmp22) - _tmp18 * (-_tmp25 - _tmp28 * state(2, 0) + _tmp5 * _tmp8);
+  const Scalar _tmp30 = _tmp9 * state(6, 0);
+  const Scalar _tmp31 = _tmp5 * state(6, 0);
+  const Scalar _tmp32 = _tmp13 * (-_tmp2 * _tmp3 + _tmp31) - _tmp18 * (_tmp3 * _tmp8 - _tmp30);
+  const Scalar _tmp33 = _tmp19 - _tmp20;
+  const Scalar _tmp34 = _tmp18 * _tmp7;
+  const Scalar _tmp35 = _tmp13 * _tmp14;
+  const Scalar _tmp36 = -_tmp34 + _tmp35;
+  const Scalar _tmp37 = _tmp34 - _tmp35;
+  const Scalar _tmp38 = _tmp13 * (-_tmp2 * _tmp24 - _tmp23 * state(3, 0) + _tmp30) -
+                        _tmp18 * (_tmp24 * _tmp8 - _tmp28 * state(3, 0) + _tmp31);
 
   // Output terms (2)
   if (innov != nullptr) {
     Scalar& _innov = (*innov);
 
-    _innov = _tmp15 * _tmp21;
+    _innov = _tmp13 * _tmp17;
   }
 
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
     _innov_var = R +
-                 _tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 +
-                           P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 +
-                           P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) +
-                 _tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 +
-                           P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 +
-                           P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) +
-                 _tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 +
-                           P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 +
-                           P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) +
-                 _tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 +
-                           P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 +
-                           P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) +
-                 _tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 +
-                           P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 +
-                           P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) +
-                 _tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 +
-                           P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 +
-                           P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) +
-                 _tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 +
-                           P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 +
-                           P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) +
-                 _tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 +
-                           P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 +
-                           P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) +
-                 _tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 +
-                           P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 +
-                           P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42);
+                 _tmp21 * (P(0, 4) * _tmp32 + P(1, 4) * _tmp26 + P(2, 4) * _tmp29 +
+                           P(22, 4) * _tmp33 + P(23, 4) * _tmp37 + P(3, 4) * _tmp38 +
+                           P(4, 4) * _tmp21 + P(5, 4) * _tmp36 + P(6, 4) * _tmp27) +
+                 _tmp26 * (P(0, 1) * _tmp32 + P(1, 1) * _tmp26 + P(2, 1) * _tmp29 +
+                           P(22, 1) * _tmp33 + P(23, 1) * _tmp37 + P(3, 1) * _tmp38 +
+                           P(4, 1) * _tmp21 + P(5, 1) * _tmp36 + P(6, 1) * _tmp27) +
+                 _tmp27 * (P(0, 6) * _tmp32 + P(1, 6) * _tmp26 + P(2, 6) * _tmp29 +
+                           P(22, 6) * _tmp33 + P(23, 6) * _tmp37 + P(3, 6) * _tmp38 +
+                           P(4, 6) * _tmp21 + P(5, 6) * _tmp36 + P(6, 6) * _tmp27) +
+                 _tmp29 * (P(0, 2) * _tmp32 + P(1, 2) * _tmp26 + P(2, 2) * _tmp29 +
+                           P(22, 2) * _tmp33 + P(23, 2) * _tmp37 + P(3, 2) * _tmp38 +
+                           P(4, 2) * _tmp21 + P(5, 2) * _tmp36 + P(6, 2) * _tmp27) +
+                 _tmp32 * (P(0, 0) * _tmp32 + P(1, 0) * _tmp26 + P(2, 0) * _tmp29 +
+                           P(22, 0) * _tmp33 + P(23, 0) * _tmp37 + P(3, 0) * _tmp38 +
+                           P(4, 0) * _tmp21 + P(5, 0) * _tmp36 + P(6, 0) * _tmp27) +
+                 _tmp33 * (P(0, 22) * _tmp32 + P(1, 22) * _tmp26 + P(2, 22) * _tmp29 +
+                           P(22, 22) * _tmp33 + P(23, 22) * _tmp37 + P(3, 22) * _tmp38 +
+                           P(4, 22) * _tmp21 + P(5, 22) * _tmp36 + P(6, 22) * _tmp27) +
+                 _tmp36 * (P(0, 5) * _tmp32 + P(1, 5) * _tmp26 + P(2, 5) * _tmp29 +
+                           P(22, 5) * _tmp33 + P(23, 5) * _tmp37 + P(3, 5) * _tmp38 +
+                           P(4, 5) * _tmp21 + P(5, 5) * _tmp36 + P(6, 5) * _tmp27) +
+                 _tmp37 * (P(0, 23) * _tmp32 + P(1, 23) * _tmp26 + P(2, 23) * _tmp29 +
+                           P(22, 23) * _tmp33 + P(23, 23) * _tmp37 + P(3, 23) * _tmp38 +
+                           P(4, 23) * _tmp21 + P(5, 23) * _tmp36 + P(6, 23) * _tmp27) +
+                 _tmp38 * (P(0, 3) * _tmp32 + P(1, 3) * _tmp26 + P(2, 3) * _tmp29 +
+                           P(22, 3) * _tmp33 + P(23, 3) * _tmp37 + P(3, 3) * _tmp38 +
+                           P(4, 3) * _tmp21 + P(5, 3) * _tmp36 + P(6, 3) * _tmp27);
   }
 }  // NOLINT(readability/fn_size)