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

ekf2: move ev control to file

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent f555281c94
commit b6f76b5eaa
5 changed files with 127 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+1
View File
@@ -57,6 +57,7 @@ px4_add_module(
EKF/ekf_helper.cpp
EKF/EKFGSF_yaw.cpp
EKF/estimator_interface.cpp
EKF/ev_height_control.cpp
EKF/fake_pos_control.cpp
EKF/gps_checks.cpp
EKF/gps_control.cpp
src/modules/ekf2/EKF/CMakeLists.txt
+1
View File
@@ -42,6 +42,7 @@ add_library(ecl_EKF
ekf_helper.cpp
EKFGSF_yaw.cpp
estimator_interface.cpp
ev_height_control.cpp
fake_pos_control.cpp
gps_checks.cpp
gps_control.cpp
src/modules/ekf2/EKF/control.cpp
-40
View File
@@ -793,46 +793,6 @@ void Ekf::controlRangeHeightFusion()
}
}
void Ekf::controlEvHeightFusion()
{
if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter
stopEvHgtFusion();
return;
}
_ev_hgt_b_est.predict(_dt_ekf_avg);
const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL);
if (_ev_data_ready) {
const bool continuing_conditions_passing = !ev_intermittent;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
if (continuing_conditions_passing) {
fuseEvHgt();
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToRng();
resetVerticalVelocityToZero();
}
} else {
stopEvHgtFusion();
}
} else {
if (starting_conditions_passing) {
startEvHgtFusion();
}
}
} else if (_control_status.flags.ev_hgt && ev_intermittent) {
stopEvHgtFusion();
}
}
void Ekf::controlAirDataFusion()
{
// control activation and initialisation/reset of wind states required for airspeed fusion
src/modules/ekf2/EKF/ekf_helper.cpp
-46
View File
@@ -294,21 +294,6 @@ void Ekf::resetHeightToRng()
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
}
void Ekf::resetHeightToEv()
{
ECL_INFO("reset height to EV");
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias());
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
}
void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed)
{
resetVerticalVelocityTo(gps_sample_delayed.vel(2));
@@ -1357,37 +1342,6 @@ void Ekf::stopRngHgtFusion()
}
}
void Ekf::startEvHgtFusion()
{
if (!_control_status.flags.ev_hgt) {
if (_params.height_sensor_ref == HeightSensor::EV) {
_rng_hgt_b_est.reset();
_height_sensor_ref = HeightSensor::EV;
resetHeightToEv();
} else {
_ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2));
}
_control_status.flags.ev_hgt = true;
_ev_hgt_b_est.setFusionActive();
ECL_INFO("starting EV height fusion");
}
}
void Ekf::stopEvHgtFusion()
{
if (_control_status.flags.ev_hgt) {
if (_height_sensor_ref == HeightSensor::EV) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.ev_hgt = false;
_ev_hgt_b_est.setFusionInactive();
ECL_INFO("stopping EV height fusion");
}
}
float Ekf::getGpsHeightVariance()
{
// observation variance - receiver defined and parameter limited
src/modules/ekf2/EKF/ev_height_control.cpp
+125
View File
@@ -0,0 +1,125 @@
/****************************************************************************
*
* Copyright (c) 2022 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.
*
****************************************************************************/
/**
* @file ev_height_control.cpp
* Control functions for ekf external vision height fusion
*/
#include "ekf.h"
void Ekf::controlEvHeightFusion()
{
if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter
stopEvHgtFusion();
return;
}
_ev_hgt_b_est.predict(_dt_ekf_avg);
const bool ev_intermittent = !isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL);
if (_ev_data_ready) {
const bool continuing_conditions_passing = !ev_intermittent;
const bool starting_conditions_passing = continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
if (continuing_conditions_passing) {
fuseEvHgt();
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToEv();
resetVerticalVelocityToZero();
}
} else {
stopEvHgtFusion();
}
} else {
if (starting_conditions_passing) {
startEvHgtFusion();
}
}
} else if (_control_status.flags.ev_hgt && ev_intermittent) {
stopEvHgtFusion();
}
}
void Ekf::startEvHgtFusion()
{
if (!_control_status.flags.ev_hgt) {
if (_params.height_sensor_ref == HeightSensor::EV) {
_rng_hgt_b_est.reset();
_height_sensor_ref = HeightSensor::EV;
resetHeightToEv();
} else {
_ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2));
}
_control_status.flags.ev_hgt = true;
_ev_hgt_b_est.setFusionActive();
ECL_INFO("starting EV height fusion");
}
}
void Ekf::resetHeightToEv()
{
ECL_INFO("reset height to EV");
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias());
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
}
void Ekf::stopEvHgtFusion()
{
if (_control_status.flags.ev_hgt) {
if (_height_sensor_ref == HeightSensor::EV) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.ev_hgt = false;
_ev_hgt_b_est.setFusionInactive();
ECL_INFO("stopping EV height fusion");
}
}