Skip to content

Commit

Permalink
[WIP]: initial Multi-EKF proof of concept
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Apr 13, 2020
1 parent 66eacd2 commit a305628
Show file tree
Hide file tree
Showing 29 changed files with 845 additions and 101 deletions.
4 changes: 4 additions & 0 deletions .ci/Jenkinsfile-hardware
View file
Original file line number Diff line number Diff line change
Expand Up @@ -826,6 +826,8 @@ void statusFTDI() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2_selector status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"'
Expand Down Expand Up @@ -876,6 +878,8 @@ void statusSEGGER() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2_selector status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "fmu status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"'
Expand Down
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/rc.fw_apps
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
# Start the attitude and position estimator.
#
ekf2 start
ekf2_selector start

#
# Start attitude controller.
Expand Down
2 changes: 2 additions & 0 deletions ROMFS/px4fmu_common/init.d/rc.mc_apps
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,9 @@ else
# EKF2
#
param set SYS_MC_EST_GROUP 2

ekf2 start
ekf2_selector start
fi
fi

Expand Down
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/rc.rover_apps
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
# Start the attitude and position estimator.
#
ekf2 start
ekf2_selector start
#attitude_estimator_q start
#local_position_estimator start

Expand Down
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/rc.uuv_apps
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
###############################################################################

ekf2 start
ekf2_selector start

###############################################################################
# End Estimator Group Selection #
Expand Down
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,4 +174,5 @@ if [ $VEHICLE_TYPE = none ]
then
echo "No autostart ID found"
ekf2 start
ekf2_selector start
fi
1 change: 1 addition & 0 deletions ROMFS/px4fmu_common/init.d/rc.vtol_apps
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
###############################################################################

ekf2 start
ekf2_selector start

###############################################################################
# End Estimator group selection #
Expand Down
1 change: 1 addition & 0 deletions msg/vehicle_attitude.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
1 change: 1 addition & 0 deletions msg/vehicle_global_position.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
1 change: 1 addition & 0 deletions msg/vehicle_local_position.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
1 change: 1 addition & 0 deletions msg/vehicle_odometry.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,3 +57,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
36 changes: 20 additions & 16 deletions platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,23 +64,27 @@ 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 att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13};

static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -14};

static constexpr wq_config_t uavcan{"wq:uavcan", 3000, -15};

static constexpr wq_config_t UART0{"wq:UART0", 1400, -16};
static constexpr wq_config_t UART1{"wq:UART1", 1400, -17};
static constexpr wq_config_t UART2{"wq:UART2", 1400, -18};
static constexpr wq_config_t UART3{"wq:UART3", 1400, -19};
static constexpr wq_config_t UART4{"wq:UART4", 1400, -20};
static constexpr wq_config_t UART5{"wq:UART5", 1400, -21};
static constexpr wq_config_t UART6{"wq:UART6", 1400, -22};
static constexpr wq_config_t UART7{"wq:UART7", 1400, -23};
static constexpr wq_config_t UART8{"wq:UART8", 1400, -24};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -25};
static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -14};

static constexpr wq_config_t INS0{"wq:INS0", 7200, -15};
static constexpr wq_config_t INS1{"wq:INS1", 7200, -16};
static constexpr wq_config_t INS2{"wq:INS2", 7200, -17};

static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};

static constexpr wq_config_t UART0{"wq:UART0", 1400, -19};
static constexpr wq_config_t UART1{"wq:UART1", 1400, -20};
static constexpr wq_config_t UART2{"wq:UART2", 1400, -21};
static constexpr wq_config_t UART3{"wq:UART3", 1400, -22};
static constexpr wq_config_t UART4{"wq:UART4", 1400, -23};
static constexpr wq_config_t UART5{"wq:UART5", 1400, -24};
static constexpr wq_config_t UART6{"wq:UART6", 1400, -25};
static constexpr wq_config_t UART7{"wq:UART7", 1400, -26};
static constexpr wq_config_t UART8{"wq:UART8", 1400, -27};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -28};

static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};

Expand Down
11 changes: 11 additions & 0 deletions src/modules/ekf2/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Loading

0 comments on commit a305628

Please sign in to comment.