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

ekf2: move range finder files to aid_sources/range_finder

Browse Source
This commit is contained in:
Daniel Agar
2024-04-04 14:21:56 -04:00
parent 3dbd3f8a1a
commit 24fdd696cb
14 changed files with 59 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+3 -3
View File
@@ -203,9 +203,9 @@ endif()
if(CONFIG_EKF2_RANGE_FINDER)
list(APPEND EKF_SRCS
EKF/range_finder_consistency_check.cpp
EKF/range_height_control.cpp
EKF/sensor_range_finder.cpp
EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
EKF/aid_sources/range_finder/range_height_control.cpp
EKF/aid_sources/range_finder/sensor_range_finder.cpp
)
endif()
src/modules/ekf2/EKF/CMakeLists.txt
+3 -3
View File
@@ -122,9 +122,9 @@ endif()
if(CONFIG_EKF2_RANGE_FINDER)
list(APPEND EKF_SRCS
range_finder_consistency_check.cpp
range_height_control.cpp
sensor_range_finder.cpp
aid_sources/range_finder/range_finder_consistency_check.cpp
aid_sources/range_finder/range_height_control.cpp
aid_sources/range_finder/sensor_range_finder.cpp
)
endif()
src/modules/ekf2/EKF/Sensor.hpp → src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp
+1 -1
View File
@@ -42,7 +42,7 @@
#ifndef EKF_SENSOR_HPP
#define EKF_SENSOR_HPP
#include "common.h"
#include <cstdint>
namespace estimator
{
src/modules/ekf2/EKF/range_finder_consistency_check.cpp → src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+8 -5
View File
@@ -35,9 +35,10 @@
* @file range_finder_consistency_check.cpp
*/
#include "range_finder_consistency_check.hpp"
#include <aid_sources/range_finder/range_finder_consistency_check.hpp>
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
bool horizontal_motion, uint64_t time_us)
{
if (horizontal_motion) {
_time_last_horizontal_motion = time_us;
@@ -55,7 +56,8 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
const float var = 2.f * dist_bottom_var / (dt * dt);
_innov_var = var + vz_var;
const float normalized_innov_sq = (_innov * _innov) / _innov_var;
@@ -84,8 +86,9 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
}
} else {
if (_test_ratio < 1.f
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
if ((_test_ratio < 1.f)
&& ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
) {
_is_kinematically_consistent = true;
}
}
src/modules/ekf2/EKF/range_finder_consistency_check.hpp → src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View File
src/modules/ekf2/EKF/range_height_control.cpp → src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+16 -7
View File
@@ -64,13 +64,14 @@ void Ekf::controlRangeHeightFusion()
if (_control_status.flags.in_air) {
const bool horizontal_motion = _control_status.flags.fixed_wing
|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
}
} else {
@@ -121,13 +122,15 @@ void Ekf::controlRangeHeightFusion()
}
// determine if we should use height aiding
const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable();
const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& isConditionalRangeAidSuitable();
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
&& measurement_valid
&& _range_sensor.isDataHealthy();
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
&& _range_sensor.isRegularlySendingData();
if (_control_status.flags.rng_hgt) {
@@ -165,13 +168,17 @@ void Ekf::controlRangeHeightFusion()
} else {
if (starting_conditions_passing) {
if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
) {
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::RANGE;
bias_est.setBias(_state.pos(2) + measurement);
} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
&& (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
) {
// Range finder is the primary height source, the ground is now the datum used
// to compute the local vertical position
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
@@ -193,7 +200,7 @@ void Ekf::controlRangeHeightFusion()
}
} else if (_control_status.flags.rng_hgt
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) {
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopRngHgtFusion();
@@ -203,6 +210,7 @@ void Ekf::controlRangeHeightFusion()
bool Ekf::isConditionalRangeAidSuitable()
{
#if defined(CONFIG_EKF2_TERRAIN)
if (_control_status.flags.in_air
&& _range_sensor.isHealthy()
&& isTerrainEstimateValid()) {
@@ -236,6 +244,7 @@ bool Ekf::isConditionalRangeAidSuitable()
return is_in_range && is_hagl_stable && is_below_max_speed;
}
#endif // CONFIG_EKF2_TERRAIN
return false;
src/modules/ekf2/EKF/sensor_range_finder.cpp → src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
+6 -4
View File
@@ -38,20 +38,22 @@
*
*/
#include "sensor_range_finder.hpp"
#include <aid_sources/range_finder/sensor_range_finder.hpp>
#include <lib/matrix/matrix/math.hpp>
namespace estimator
{
namespace sensor
{
void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth)
void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
{
updateSensorToEarthRotation(R_to_earth);
updateValidity(current_time_us);
}
void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth)
void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
{
// calculate 2,2 element of rotation matrix from sensor frame to earth frame
// this is required for use of range finder and flow data
@@ -114,7 +116,7 @@ inline bool SensorRangeFinder::isDataInRange() const
void SensorRangeFinder::updateStuckCheck()
{
if(!isStuckDetectorEnabled()){
if (!isStuckDetectorEnabled()) {
// Stuck detector disabled
_is_stuck = false;
return;
src/modules/ekf2/EKF/sensor_range_finder.hpp → src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp
+9
View File
@@ -42,6 +42,7 @@
#define EKF_SENSOR_RANGE_FINDER_HPP
#include "Sensor.hpp"
#include <matrix/math.hpp>
namespace estimator
@@ -49,6 +50,14 @@ namespace estimator
namespace sensor
{
struct rangeSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float rng{}; ///< range (distance to ground) measurement (m)
int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
};
static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
class SensorRangeFinder : public Sensor
{
public:
src/modules/ekf2/EKF/common.h
-7
View File
@@ -70,7 +70,6 @@ static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable
static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec)
static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec)
static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec)
static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec)
// bad accelerometer detection and mitigation
@@ -197,12 +196,6 @@ struct baroSample {
bool reset{false};
};
struct rangeSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float rng{}; ///< range (distance to ground) measurement (m)
int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
};
struct airspeedSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
float true_airspeed{}; ///< true airspeed measurement (m/sec)
src/modules/ekf2/EKF/estimator_interface.cpp
+3 -3
View File
@@ -266,7 +266,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_RANGE_FINDER)
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
{
if (!_initialised) {
return;
@@ -274,7 +274,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
// Allocate the required buffer size if not previously done
if (_range_buffer == nullptr) {
_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
if (_range_buffer == nullptr || !_range_buffer->valid()) {
delete _range_buffer;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
// limit data rate to prevent data being lost
if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
rangeSample range_sample_new{range_sample};
sensor::rangeSample range_sample_new{range_sample};
range_sample_new.time_us = time_us;
_range_buffer->push(range_sample_new);
src/modules/ekf2/EKF/estimator_interface.h
+5 -5
View File
@@ -67,8 +67,8 @@
#include "output_predictor.h"
#if defined(CONFIG_EKF2_RANGE_FINDER)
# include "range_finder_consistency_check.hpp"
# include "sensor_range_finder.hpp"
# include "aid_sources/range_finder/range_finder_consistency_check.hpp"
# include "aid_sources/range_finder/sensor_range_finder.hpp"
#endif // CONFIG_EKF2_RANGE_FINDER
#include <lib/atmosphere/atmosphere.h>
@@ -107,7 +107,7 @@ public:
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_RANGE_FINDER)
void setRangeData(const rangeSample &range_sample);
void setRangeData(const estimator::sensor::rangeSample &range_sample);
// set sensor limitations reported by the rangefinder
void set_rangefinder_limits(float min_distance, float max_distance)
@@ -115,7 +115,7 @@ public:
_range_sensor.setLimits(min_distance, max_distance);
}
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -356,7 +356,7 @@ protected:
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_RANGE_FINDER)
RingBuffer<rangeSample> *_range_buffer{nullptr};
RingBuffer<sensor::rangeSample> *_range_buffer{nullptr};
uint64_t _time_last_range_buffer_push{0};
sensor::SensorRangeFinder _range_sensor{};
src/modules/ekf2/EKF2.cpp
+2 -2
View File
@@ -2368,7 +2368,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
rangeSample range_sample {
estimator::sensor::rangeSample range_sample {
.time_us = optical_flow.timestamp_sample,
.rng = optical_flow.distance_m,
.quality = quality,
@@ -2507,7 +2507,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
// EKF range sample
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
rangeSample range_sample {
estimator::sensor::rangeSample range_sample {
.time_us = distance_sensor.timestamp,
.rng = distance_sensor.current_distance,
.quality = distance_sensor.signal_quality,
src/modules/ekf2/test/sensor_simulator/range_finder.h
+1 -1
View File
@@ -55,7 +55,7 @@ public:
void setLimits(float min_distance_m, float max_distance_m);
private:
rangeSample _range_sample{};
estimator::sensor::rangeSample _range_sample{};
float _min_distance{0.2f};
float _max_distance{20.0f};
src/modules/ekf2/test/test_SensorRangeFinder.cpp
+2 -2
View File
@@ -34,10 +34,10 @@
#include <gtest/gtest.h>
#include <math.h>
#include "EKF/common.h"
#include "EKF/sensor_range_finder.hpp"
#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
#include <matrix/math.hpp>
using estimator::rangeSample;
using estimator::sensor::rangeSample;
using matrix::Dcmf;
using matrix::Eulerf;
using namespace estimator::sensor;