[WIP]: initial Multi-EKF proof of concept

PX4 · May 5, 2020 · 2c0b564 · 2c0b564
1 parent 682aa70
commit 2c0b564
Show file tree

Hide file tree

Showing 23 changed files with 853 additions and 101 deletions.
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -40,7 +40,9 @@ else
 		# EKF2
 		#
 		param set SYS_MC_EST_GROUP 2
+
 		ekf2 start
+		ekf2_selector start
 	fi
 fi
 

diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
@@ -7,3 +7,4 @@ float32[4] delta_q_reset 	# Amount by which quaternion has changed during last r
 uint8 quat_reset_counter	# Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
+# TOPICS estimator_attitude
diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg
@@ -27,3 +27,4 @@ bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth
+# TOPICS estimator_global_position
diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
@@ -63,3 +63,4 @@ float32 hagl_min			# minimum height above ground level - set to 0 when limiting
 float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth
+# TOPICS estimator_local_position
diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg
@@ -59,3 +59,4 @@ float32 yawspeed		# Angular velocity about Z body axis
 float32[21] velocity_covariance
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned
+# TOPICS estimator_odometry estimator_visual_odometry_aligned
diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
@@ -64,24 +64,28 @@ static constexpr wq_config_t I2C2{"wq:I2C2", 1472, -10};
 static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11};
 static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12};
 
+static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -13};
+
 // PX4 att/pos controllers, highest priority after sensors.
-static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1536, -13};
-static constexpr wq_config_t navigation_and_controllers{"wq:navigation_and_controllers", 7200, -14};
-
-static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -15};
-
-static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -16};
-
-static constexpr wq_config_t UART0{"wq:UART0", 1400, -17};
-static constexpr wq_config_t UART1{"wq:UART1", 1400, -18};
-static constexpr wq_config_t UART2{"wq:UART2", 1400, -19};
-static constexpr wq_config_t UART3{"wq:UART3", 1400, -20};
-static constexpr wq_config_t UART4{"wq:UART4", 1400, -21};
-static constexpr wq_config_t UART5{"wq:UART5", 1400, -22};
-static constexpr wq_config_t UART6{"wq:UART6", 1400, -23};
-static constexpr wq_config_t UART7{"wq:UART7", 1400, -24};
-static constexpr wq_config_t UART8{"wq:UART8", 1400, -25};
-static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26};
+static constexpr wq_config_t attitude_ctrl{"wq:attitude_ctrl", 1536, -14};
+static constexpr wq_config_t navigation_and_controllers{"wq:navigation_and_controllers", 7200, -15};
+
+static constexpr wq_config_t INS0{"wq:INS0", 7200, -16};
+static constexpr wq_config_t INS1{"wq:INS1", 7200, -17};
+static constexpr wq_config_t INS2{"wq:INS2", 7200, -18};
+
+static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -19};
+
+static constexpr wq_config_t UART0{"wq:UART0", 1400, -20};
+static constexpr wq_config_t UART1{"wq:UART1", 1400, -21};
+static constexpr wq_config_t UART2{"wq:UART2", 1400, -22};
+static constexpr wq_config_t UART3{"wq:UART3", 1400, -23};
+static constexpr wq_config_t UART4{"wq:UART4", 1400, -24};
+static constexpr wq_config_t UART5{"wq:UART5", 1400, -25};
+static constexpr wq_config_t UART6{"wq:UART6", 1400, -26};
+static constexpr wq_config_t UART7{"wq:UART7", 1400, -27};
+static constexpr wq_config_t UART8{"wq:UART8", 1400, -28};
+static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -29};
 
 static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
 

diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt
@@ -46,4 +46,15 @@ px4_add_module(
 		ecl_geo
 		perf
 		Ekf2Utility
+		px4_work_queue
+	)
+
+px4_add_module(
+	MODULE modules__ekf2_selector
+	MAIN ekf2_selector
+	SRCS
+		EKF2Selector.cpp
+		EKF2Selector.hpp
+	DEPENDS
+		px4_work_queue
 	)