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 1b1479a92b
commit 78bbb66568
29 changed files with 154 additions and 209 deletions
@@ -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;
@@ -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
*
@@ -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,
@@ -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
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();
+8
View File
@@ -39,6 +39,14 @@
*
*/
/**
* EKF2 enable
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_EN, 1);
/**
* EKF prediction period
*
@@ -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;
@@ -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