dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

delete SYS_MC_EST_GROUP 

- 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 8da106df6a
commit 9be379b581
23 changed files with 131 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -187,10 +187,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
@ -330,7 +326,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 EKF2_EN 1
then
set LOGGER_ARGS "-p ekf2_timestamps"
else

1
ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21
View File

@ -18,7 +18,6 @@
. ${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

43
ROMFS/px4fmu_common/init.d/rc.airship_apps
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
#

36
ROMFS/px4fmu_common/init.d/rc.balloon_apps
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

4
ROMFS/px4fmu_common/init.d/rc.balloon_defaults
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

43
ROMFS/px4fmu_common/init.d/rc.mc_apps
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
#

17
ROMFS/px4fmu_common/init.d/rcS
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.

5
boards/diatone/mamba-f405-mk2/init/rc.board_defaults
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

2
boards/flywoo/gn-f405/init/rc.board_defaults
View File

@ -12,8 +12,6 @@ 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

5
boards/omnibus/f4sd/init/rc.board_defaults
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

5
boards/spracing/h7extreme/init/rc.board_defaults
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

4
src/drivers/ins/vectornav/VectorNav.cpp
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);

64
src/lib/parameters/param_translation.cpp
View File

@ -43,6 +43,70 @@
bool param_modify_on_import(bson_node_t node)
{
// 2023-12-18 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);
}
}
// 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);
}
// 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 true;
}
return false;
}

14
src/lib/systemlib/system_params.c
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.
*

8
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
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;

10
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
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
*

5
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
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,

1
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
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,

1
src/modules/ekf2/EKF2.cpp
View File

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

8
src/modules/ekf2/ekf2_params.c
View File

@ -39,6 +39,14 @@
*
*/
/**
* EKF2 enable
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_EN, 1);
/**
* EKF prediction period
*

8
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
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;

8
src/modules/local_position_estimator/params.c
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

5
test/mavsdk_tests/configs/sitl.json
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",