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

ekf2: move controlAuxVelFusion control.cpp -> auxvel_fusion.cpp

Browse Source
This commit is contained in:
Daniel Agar 2023-02-21 10:04:47 -05:00
parent b4e845d7c0
commit ed8cef6cf0
4 changed files with 55 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/CMakeLists.txt
View File

@ -82,6 +82,7 @@ px4_add_module(
3600
SRCS
EKF/airspeed_fusion.cpp
EKF/auxvel_fusion.cpp
EKF/baro_height_control.cpp
EKF/bias_estimator.cpp
EKF/control.cpp

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -33,6 +33,7 @@
add_library(ecl_EKF
airspeed_fusion.cpp
auxvel_fusion.cpp
baro_height_control.cpp
bias_estimator.cpp
control.cpp

53
src/modules/ekf2/EKF/auxvel_fusion.cpp Normal file
View File

@ -0,0 +1,53 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ekf.h"
void Ekf::controlAuxVelFusion()
{
if (_auxvel_buffer) {
auxVelSample auxvel_sample_delayed;
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
resetEstimatorAidStatus(_aid_src_aux_vel);
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
if (isHorizontalAidingActive()) {
_aid_src_aux_vel.fusion_enabled = true;
fuseVelocity(_aid_src_aux_vel);
}
}
}
}

19
src/modules/ekf2/EKF/control.cpp
View File

@ -217,22 +217,3 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
// check if we are no longer fusing measurements that directly constrain velocity drift
updateDeadReckoningStatus();
}
void Ekf::controlAuxVelFusion()
{
if (_auxvel_buffer) {
auxVelSample auxvel_sample_delayed;
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
resetEstimatorAidStatus(_aid_src_aux_vel);
updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
if (isHorizontalAidingActive()) {
_aid_src_aux_vel.fusion_enabled = true;
fuseVelocity(_aid_src_aux_vel);
}
}
}
}