mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-07 05:50:34 +08:00
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:
@@ -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"), ¶m_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,
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user