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

EKF: Add parameter to adjust on-ground movement detector sensitivity

This commit is contained in:
Paul Riseborough 2018-05-16 16:51:10 +10:00 committed by Lorenz Meier
parent ea9e8246ed
commit 1562a82dc2
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/common.h
View File

@ -340,6 +340,9 @@ struct parameters {
// auxilliary velocity fusion
float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s)
float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
// control of on-ground movement check
float is_moving_scaler{1.0f}; ///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive.
};
struct stateSample {

6
EKF/estimator_interface.cpp
View File

@ -89,9 +89,9 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// detect if the vehicle is not moving when on ground
if (!_control_status.flags.in_air) {
if ((_vibe_metrics[1] * 4.0E4f > 1.0f)
|| (_vibe_metrics[2] * 2.1E2f > 1.0f)
|| ((imu_sample_new.delta_ang.norm() / dt) > 0.05f)) {
if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
|| (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
|| ((imu_sample_new.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
_time_last_move_detect_us = _imu_sample_new.time_us;
}
_vehicle_at_rest = ((_imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6);