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

Interface: output vector quantities by "return-by-value"

Browse Source
This commit is contained in:
kamilritz
2020-03-30 13:07:00 +02:00
committed by Mathieu Bresciani
parent 72d8f91b4d
commit 2fa43419d2
16 changed files with 126 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+5 -5
View File
@@ -91,25 +91,25 @@ void EstimatorInterface::computeVibrationMetric()
{
// calculate a metric which indicates the amount of coning vibration
Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev;
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
// calculate a metric which indicates the amount of high frequency gyro vibration
temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev;
_delta_ang_prev = _newest_high_rate_imu_sample.delta_ang;
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
// calculate a metric which indicates the amount of high frequency accelerometer vibration
temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev;
_delta_vel_prev = _newest_high_rate_imu_sample.delta_vel;
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
}
bool EstimatorInterface::checkIfVehicleAtRest(float dt)
{
// detect if the vehicle is not moving when on ground
if (!_control_status.flags.in_air) {
if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
|| (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
|| (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)
|| ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us;