fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 04:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

it builds

Browse Source
This commit is contained in:
Jacob Dahl
2025-07-13 15:33:35 -08:00
parent 878c049215
commit 5fb3846b50
4 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500
+6
View File
@@ -49,3 +49,9 @@ param set-default SIM_GZ_EC_MAX4 1000
param set-default MPC_THR_HOVER 0.60
param set-default NAV_DLL_ACT 2
# DEBUGGING: ekf range fusion
# default, ekf replay, cv+avoid, high rate sensors
param set-default SDLOG_PROFILE 2179
# sim distance on ground
param set-default EKF2_MIN_RNG 0.177
src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
+7 -9
View File
@@ -69,7 +69,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// Set all of the parameters
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
// Update sensor to earth rotation
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
@@ -110,9 +110,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// TODO: refactor into apply_body_offset()
// Correct the range data for position offset relative to the IMU
const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
const Vector3f imu_pos_body = _params.imu_pos_body;
const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
@@ -146,7 +144,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// Fuse Range data as Primary height source
// NOTE: terrain is not estimated in this mode
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
fuseRangeAsHeightSource();
} else {
// Fuse Range data as aiding height source (Primary GPS or Baro)
@@ -193,8 +191,8 @@ void Ekf::fuseRangeAsHeightAiding()
//// ALTITUDE FUSION ////
bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL;
bool range_aid_enabled = _params.rng_ctrl == RngCtrl::ENABLED;
bool range_aid_conditional = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::CONDITIONAL;
bool range_aid_enabled = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::ENABLED;
bool range_aid_conditions_passed = rangeAidConditionsPassed();
@@ -366,7 +364,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
float innovation_variance;
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
@@ -387,7 +385,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
float Ekf::getRngVar() const
{
const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
const float dist_var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
return dist_var;
}
src/modules/ekf2/EKF2.cpp
-1
View File
@@ -162,7 +162,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
_param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax),
_param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax),
_param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t),
_param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate),
_param_ekf2_rng_fog(_params->ekf2_rng_fog),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
src/modules/ekf2/EKF2.hpp
-1
View File
@@ -621,7 +621,6 @@ private:
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,