fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 18:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 : update to use new navigation limits architechture

Browse Source
This commit is contained in:
Mohammed Kabir
2018-06-09 13:38:04 +02:00
committed by Lorenz Meier
parent 8299f571c8
commit 6a9377846f
2 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+16 -3
View File
@@ -322,7 +322,6 @@ private:
(ParamExtInt<px4::params::EKF2_OF_QMIN>) _flow_qual_min, ///< minimum acceptable quality integer from the flow sensor
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
_flow_innov_gate, ///< optical flow fusion innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_OF_RMAX>) _flow_rate_max, ///< maximum valid optical flow rate (rad/sec)
// sensor positions in body frame
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _imu_pos_x, ///< X position of IMU in body frame (m)
@@ -470,7 +469,6 @@ Ekf2::Ekf2():
_flow_noise_qual_min(_params->flow_noise_qual_min),
_flow_qual_min(_params->flow_qual_min),
_flow_innov_gate(_params->flow_innov_gate),
_flow_rate_max(_params->flow_rate_max),
_imu_pos_x(_params->imu_pos_body(0)),
_imu_pos_y(_params->imu_pos_body(1)),
_imu_pos_z(_params->imu_pos_body(2)),
@@ -903,6 +901,9 @@ void Ekf2::run()
_ekf.setOpticalFlowData(optical_flow.timestamp, &flow);
}
// Save sensor limits reported by the optical flow sensor
_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, optical_flow.max_ground_distance);
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
@@ -931,6 +932,9 @@ void Ekf2::run()
_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
// Save sensor limits reported by the rangefinder
_ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance);
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
@@ -1127,12 +1131,21 @@ void Ekf2::run()
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
// get control limit information
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.limit_hagl);
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
// convert NaN to zero
if (!PX4_ISFINITE(lpos.vxy_max)) {
lpos.vxy_max = 0.0f;
}
if (!PX4_ISFINITE(lpos.vz_max)) {
lpos.vz_max = 0.0f;
}
if (!PX4_ISFINITE(lpos.hagl_min)) {
lpos.hagl_min = 0.0f;
}
if (!PX4_ISFINITE(lpos.hagl_max)) {
lpos.hagl_max = 0.0f;
}
// publish vehicle local position data
_vehicle_local_position_pub.update();
src/modules/ekf2/ekf2_params.c
-11
View File
@@ -738,17 +738,6 @@ PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
*/
PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f);
/**
* Optical Flow data will not fused if the magnitude of the flow rate > EKF2_OF_RMAX.
* Control loops will be instructed to limit ground speed such that the flow rate produced by movement over ground is less than 50% of EKF2_OF_RMAX.
*
* @group EKF2
* @min 1.0
* @unit rad/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_OF_RMAX, 2.5f);
/**
* Terrain altitude process noise - accounts for instability in vehicle height estimate
*