Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

delete SYS_MC_EST_GROUP

Browse Source 
- introduce per module parameters (EKF2_EN, LPE_EN, ATT_EN)
 - add basic checks to prevent EKF2 + LPE running simultaneously
This commit is contained in:
Daniel Agar
2023-12-18 13:03:52 -05:00
parent 1b1479a92b
commit 78bbb66568
29 changed files with 154 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/rcS
+18 -5
View File
@@ -182,10 +182,6 @@ param set-default SYS_FAILURE_EN 1
# does not go below 50% by default, but failure injection can trigger failsafes.
param set-default COM_LOW_BAT_ACT 2
# Allow to override SYS_MC_EST_GROUP via env
if [ -n "$SYS_MC_EST_GROUP" ]; then
param set SYS_MC_EST_GROUP $SYS_MC_EST_GROUP
fi
# Adapt timeout parameters if simulation runs faster or slower than realtime.
if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
@@ -256,6 +252,23 @@ manual_control start
sensors start
commander start
#
# state estimator selection
if param compare -s EKF2_EN 1
then
ekf2 start &
fi
if param compare -s LPE_EN 1
then
local_position_estimator start
fi
if param compare -s ATT_EN 1
then
attitude_estimator_q start
fi
if ! pwm_out_sim start -m sim
then
tune_control play error
@@ -330,7 +343,7 @@ fi
[ -e "$autostart_file".post ] && . "$autostart_file".post
# Run script to start logging
if param compare SYS_MC_EST_GROUP 2
if param compare -s EKF2_EN 1
then
set LOGGER_ARGS "-p ekf2_timestamps"
else
ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21
+1 -1
View File
@@ -17,7 +17,7 @@
#
. ${R}etc/init.d/rc.mc_defaults
param set-default SYS_MC_EST_GROUP 2
param set-default SYS_HAS_MAG 0
param set-default EKF2_OF_CTRL 1
param set-default EKF2_GPS_CTRL 0
ROMFS/px4fmu_common/init.d/rc.airship_apps
-43
View File
@@ -5,49 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator Group Selection #
###############################################################################
#
# LPE
#
if param compare SYS_MC_EST_GROUP 1
then
#
# Try to start LPE. If it fails, start EKF2 as a default.
# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
#
if attitude_estimator_q start
then
echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
local_position_estimator start
else
echo "ERROR [init] Estimator LPE not available. Using EKF2"
param set SYS_MC_EST_GROUP 2
param save
reboot
fi
else
#
# Q estimator (attitude estimation only)
#
if param compare SYS_MC_EST_GROUP 3
then
attitude_estimator_q start
else
#
# EKF2
#
param set SYS_MC_EST_GROUP 2
ekf2 start &
fi
fi
###############################################################################
# End Estimator Group Selection #
###############################################################################
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rc.balloon_apps
-36
View File
@@ -4,39 +4,3 @@
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
if param compare SYS_MC_EST_GROUP 1
then
#
# Try to start LPE. If it fails, start EKF2 as a default.
# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
#
if attitude_estimator_q start
then
echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
local_position_estimator start
else
echo "ERROR [init] Estimator LPE not available. Using EKF2"
param set SYS_MC_EST_GROUP 2
param save
reboot
fi
else
#
# Q estimator (attitude estimation only)
#
if param compare SYS_MC_EST_GROUP 3
then
attitude_estimator_q start
else
#
# EKF2
#
param set SYS_MC_EST_GROUP 2
ekf2 start &
fi
fi
ROMFS/px4fmu_common/init.d/rc.balloon_defaults
+3 -1
View File
@@ -13,4 +13,6 @@ param set-default MAV_TYPE 8
#
# Default parameters for balloon UAVs.
#
param set-default SYS_MC_EST_GROUP 1
param set-default LPE_EN 1
param set-default ATT_EN 1
param set-default EKF2_EN 0
ROMFS/px4fmu_common/init.d/rc.fw_apps
-5
View File
@@ -5,11 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
ekf2 start &
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rc.mc_apps
-43
View File
@@ -5,49 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator Group Selection #
###############################################################################
#
# LPE
#
if param compare SYS_MC_EST_GROUP 1
then
#
# Try to start LPE. If it fails, start EKF2 as a default.
# Unfortunately we do not build it on px4_fmu-v2 due to a limited flash.
#
if attitude_estimator_q start
then
echo "WARN [init] Estimator LPE unsupported, EKF2 recommended."
local_position_estimator start
else
echo "ERROR [init] Estimator LPE not available. Using EKF2"
param set SYS_MC_EST_GROUP 2
param save
reboot
fi
else
#
# Q estimator (attitude estimation only)
#
if param compare SYS_MC_EST_GROUP 3
then
attitude_estimator_q start
else
#
# EKF2
#
param set SYS_MC_EST_GROUP 2
ekf2 start &
fi
fi
###############################################################################
# End Estimator Group Selection #
###############################################################################
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rc.rover_apps
-5
View File
@@ -5,11 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
#
# Start the attitude and position estimator.
#
ekf2 start &
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rc.rover_differential_apps
-3
View File
@@ -1,9 +1,6 @@
#!/bin/sh
# Standard apps for a differential drive rover.
# Start the attitude and position estimator.
ekf2 start &
# Start rover differential drive controller.
differential_drive start
ROMFS/px4fmu_common/init.d/rc.uuv_apps
-10
View File
@@ -5,16 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator Group Selection #
###############################################################################
ekf2 start &
###############################################################################
# End Estimator Group Selection #
###############################################################################
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
-1
View File
@@ -76,5 +76,4 @@ fi
if [ $VEHICLE_TYPE = none ]
then
echo "No autostart ID found"
ekf2 start &
fi
ROMFS/px4fmu_common/init.d/rc.vtol_apps
-10
View File
@@ -5,16 +5,6 @@
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
###############################################################################
# Begin Estimator group selection #
###############################################################################
ekf2 start &
###############################################################################
# End Estimator group selection #
###############################################################################
#
# Start Control Allocator
#
ROMFS/px4fmu_common/init.d/rcS
+17
View File
@@ -391,6 +391,23 @@ else
pwm_out start
fi
#
# state estimator selection
if param compare -s EKF2_EN 1
then
ekf2 start &
fi
if param compare -s LPE_EN 1
then
local_position_estimator start
fi
if param compare -s ATT_EN 1
then
attitude_estimator_q start
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
boards/diatone/mamba-f405-mk2/init/rc.board_defaults
-5
View File
@@ -9,9 +9,4 @@ param set-default CBRK_SUPPLY_CHK 894281
# Disable safety switch by default
param set-default CBRK_IO_SAFETY 22027
# use the Q attitude estimator, it works w/o mag or GPS.
param set-default SYS_MC_EST_GROUP 3
param set-default ATT_W_ACC 0.4
param set-default ATT_W_GYRO_BIAS 0
param set-default SYS_HAS_MAG 0
boards/flywoo/gn-f405/init/rc.board_defaults
-5
View File
@@ -12,14 +12,9 @@ param set-default CBRK_SUPPLY_CHK 894281
# Disable safety switch by default
param set-default CBRK_IO_SAFETY 22027
# EKF2 can be enabled when baro is avaialble and EKF2_MAG_TYPE is set to 5
param set-default SYS_MC_EST_GROUP 2
param set-default EKF2_MAG_TYPE 5
param set-default SENS_BOARD_ROT 6
param set-default ATT_W_ACC 0.4
param set-default ATT_W_GYRO_BIAS 0
param set-default SYS_HAS_MAG 0
# GPS is on Uart6
boards/omnibus/f4sd/init/rc.board_defaults
-5
View File
@@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281
# Disable safety switch by default
param set-default CBRK_IO_SAFETY 22027
# use the Q attitude estimator, it works w/o mag or GPS.
param set-default SYS_MC_EST_GROUP 3
param set-default ATT_W_ACC 0.4
param set-default ATT_W_GYRO_BIAS 0
param set-default SYS_HAS_MAG 0
boards/spracing/h7extreme/init/rc.board_defaults
-5
View File
@@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281
# Select the Generic 250 Racer by default
param set-default SYS_AUTOSTART 4050
# use the Q attitude estimator, it works w/o mag or GPS.
param set-default SYS_MC_EST_GROUP 3
param set-default ATT_W_ACC 0.4
param set-default ATT_W_GYRO_BIAS 0
param set-default SYS_HAS_MAG 0
src/drivers/ins/vectornav/VectorNav.cpp
-4
View File
@@ -62,10 +62,6 @@ VectorNav::VectorNav(const char *port) :
v = 0;
param_set(param_find("EKF2_EN"), &v);
// SYS_MC_EST_GROUP 0 (disabled)
v = 0;
param_set(param_find("SYS_MC_EST_GROUP"), &v);
// SENS_IMU_MODE (VN handles sensor selection)
v = 0;
param_set(param_find("SENS_IMU_MODE"), &v);
src/lib/parameters/param_translation.cpp
+69
View File
@@ -73,5 +73,74 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node)
}
}
// 2024-04-15 SYS_MC_EST_GROUP removed
if ((node->type == bson_type_t::BSON_INT32) && (strcmp("SYS_MC_EST_GROUP", node->name) == 0)) {
int32_t value = node->i32;
// value 1 local_position_estimator, attitude_estimator_q (unsupported)
if (value == 1) {
// enable local_position_estimator
int32_t lpe_en_val = 1;
int lpe_en_ret = param_set(param_find("LPE_EN"), &lpe_en_val);
// enable attitude_estimator_q
int32_t att_en_val = 1;
int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val);
// disable ekf2 (only if enabling lpe and att_w was successful)
if (lpe_en_ret == PX4_OK && att_en_ret == PX4_OK) {
int32_t ekf2_en_val = 0;
param_set(param_find("EKF2_EN"), &ekf2_en_val);
} else {
int32_t ekf2_en_val = 1;
param_set(param_find("EKF2_EN"), &ekf2_en_val);
}
return param_modify_on_import_ret::PARAM_MODIFIED;
}
// value 2 ekf2 (recommended)
if (value == 2) {
// disable local_position_estimator
int32_t lpe_en_val = 0;
param_set(param_find("LPE_EN"), &lpe_en_val);
// disable attitude_estimator_q
int32_t att_en_val = 0;
param_set(param_find("ATT_EN"), &att_en_val);
// enable ekf2
int32_t ekf2_en_val = 1;
param_set(param_find("EKF2_EN"), &ekf2_en_val);
return param_modify_on_import_ret::PARAM_MODIFIED;
}
// value 3 Q attitude estimator (no position)
if (value == 3) {
// disable local_position_estimator
int32_t lpe_en_val = 0;
param_set(param_find("LPE_EN"), &lpe_en_val);
// enable attitude_estimator_q
int32_t att_en_val = 1;
int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val);
// disable ekf2 (only if enabling att_w was successful)
if (att_en_ret == PX4_OK) {
int32_t ekf2_en_val = 0;
param_set(param_find("EKF2_EN"), &ekf2_en_val);
} else {
int32_t ekf2_en_val = 1;
param_set(param_find("EKF2_EN"), &ekf2_en_val);
}
return param_modify_on_import_ret::PARAM_MODIFIED;
}
}
return param_modify_on_import_ret::PARAM_NOT_MODIFIED;
}
src/lib/systemlib/system_params.c
-14
View File
@@ -83,20 +83,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
*/
PARAM_DEFINE_INT32(SYS_HITL, 0);
/**
* Set multicopter estimator group
*
* Set the group of estimators used for multicopters and VTOLs
*
* @value 1 local_position_estimator, attitude_estimator_q (unsupported)
* @value 2 ekf2 (recommended)
* @value 3 Q attitude estimator (no position)
*
* @reboot_required true
* @group System
*/
PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 2);
/**
* Enable auto start of rate gyro thermal calibration at the next power up.
*
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+8
View File
@@ -180,6 +180,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
bool AttitudeEstimatorQ::init()
{
uORB::SubscriptionData<vehicle_attitude_s> vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
vehicle_attitude_sub.update();
if (vehicle_attitude_sub.advertised() && (hrt_elapsed_time(&vehicle_attitude_sub.get().timestamp) < 1_s)) {
PX4_ERR("init failed, vehicle_attitude already advertised");
return false;
}
if (!_sensors_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+10
View File
@@ -39,6 +39,16 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
/**
* standalone attitude estimator enable (unsupported)
*
* Enable standalone quaternion based attitude estimator.
*
* @group Attitude Q estimator
* @boolean
*/
PARAM_DEFINE_INT32(ATT_EN, 0);
/**
* Complimentary filter accelerometer weight
*
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+4 -1
View File
@@ -99,7 +99,10 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
}
}
if (missing_data && _param_sys_mc_est_group.get() == 2) {
int32_t param_ekf2_en = 0;
param_get(param_find_no_notification("EKF2_EN"), &param_ekf2_en);
if (missing_data && (param_ekf2_en == 1)) {
/* EVENT
*/
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
-1
View File
@@ -106,7 +106,6 @@ private:
bool _nav_failure_imminent_warned{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
src/modules/ekf2/EKF2.cpp
-1
View File
@@ -237,7 +237,6 @@ EKF2::~EKF2()
bool EKF2::multi_init(int imu, int mag)
{
// advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
src/modules/ekf2/ekf2_params.c
+8
View File
@@ -39,6 +39,14 @@
*
*/
/**
* EKF2 enable
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_EN, 1);
/**
* EKF prediction period
*
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+8
View File
@@ -137,6 +137,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
bool
BlockLocalPositionEstimator::init()
{
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
vehicle_local_position_sub.update();
if (vehicle_local_position_sub.advertised() && (hrt_elapsed_time(&vehicle_local_position_sub.get().timestamp) < 1_s)) {
PX4_ERR("init failed, vehicle_local_position already advertised");
return false;
}
if (!_sensors_sub.registerCallback()) {
PX4_ERR("callback registration failed");
return false;
src/modules/local_position_estimator/params.c
+7 -1
View File
@@ -1,6 +1,12 @@
#include <parameters/param.h>
// 16 is max name length
/**
* Local position estimator enable (unsupported)
*
* @group Local Position Estimator
* @boolean
*/
PARAM_DEFINE_INT32(LPE_EN, 0);
/**
* Optical flow z offset from center
test/mavsdk_tests/configs/sitl.json
+1 -4
View File
@@ -14,10 +14,7 @@
"model": "iris",
"vehicle": "iris",
"test_filter": "[offboard_attitude]",
"timeout_min": 10,
"env": {
"SYS_MC_EST_GROUP": 3
}
"timeout_min": 10
},
{
"model": "standard_vtol",