From 66770812bf1f895708be5fb9b23078e5a373277a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 20 Jun 2025 11:29:14 -0800 Subject: [PATCH] squashed --- msg/CMakeLists.txt | 1 + msg/EstimatorStatus.msg | 2 - msg/EstimatorStatusFlags.msg | 3 - msg/PositionSetpoint.msg | 2 + msg/TaskLocalPositionSetpoint.msg | 19 + msg/VehicleConstraints.msg | 2 + range_alt_notes.txt | 20 + src/modules/ekf2/CMakeLists.txt | 2 +- src/modules/ekf2/EKF/CMakeLists.txt | 2 +- .../EKF/aid_sources/range_finder/Sensor.hpp | 84 ---- .../jake_range_height_control.cpp | 425 ++++++++++++++++++ .../range_finder_consistency_check.cpp | 31 +- .../range_finder_consistency_check.hpp | 1 - .../range_finder/range_height_control.cpp | 125 +++--- .../range_finder/sensor_range_finder.cpp | 140 ++---- .../range_finder/sensor_range_finder.hpp | 117 ++--- src/modules/ekf2/EKF/common.h | 17 +- src/modules/ekf2/EKF/ekf.cpp | 19 +- src/modules/ekf2/EKF/ekf.h | 13 +- src/modules/ekf2/EKF/estimator_interface.h | 2 - src/modules/ekf2/EKF/terrain_control.cpp | 1 - src/modules/ekf2/EKF2.cpp | 22 +- src/modules/ekf2/EKF2.hpp | 1 - src/modules/ekf2/params_range_finder.yaml | 14 +- src/modules/ekf2/test/test_EKF_basics.cpp | 2 - .../ekf2/test/test_SensorRangeFinder.cpp | 10 +- .../FlightTaskManualAltitude.cpp | 197 ++++---- .../FlightTaskManualAltitude.hpp | 35 +- .../FlightTaskManualAltitudeSmoothVel.cpp | 17 +- .../FlightTaskManualAltitudeSmoothVel.hpp | 10 +- src/modules/logger/logged_topics.cpp | 6 + src/modules/simulation/gz_bridge/GZBridge.cpp | 2 +- 32 files changed, 819 insertions(+), 525 deletions(-) create mode 100644 msg/TaskLocalPositionSetpoint.msg create mode 100644 range_alt_notes.txt delete mode 100644 src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp create mode 100644 src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fdc23a889f..80990ecb74 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -227,6 +227,7 @@ set(msg_files VehicleImu.msg VehicleImuStatus.msg VehicleLocalPositionSetpoint.msg + TaskLocalPositionSetpoint.msg VehicleMagnetometer.msg VehicleOpticalFlow.msg VehicleOpticalFlowVel.msg diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 3de2bfd722..921063db13 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -39,14 +39,12 @@ uint8 CS_FIXED_WING = 17 # 17 - true when thought to be operating as a fixed win uint8 CS_MAG_FAULT = 18 # 18 - true when the magnetometer has been declared faulty and is no longer being used uint8 CS_ASPD = 19 # 19 - true when airspeed measurements are being fused uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect induced static pressure rise is active -uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from external vision measurements is intended uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measurement for the magnetometer Z component uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used -uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used uint8 CS_GNSS_VEL = 44 # 44 - true if GNSS velocity measurements are being fused uint32 filter_fault_flags # Bitmask to indicate EKF internal faults diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 7fed72cc28..7fae5312a8 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -25,14 +25,12 @@ bool cs_fixed_wing # 17 - true when the vehicle is operating as a fix bool cs_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active -bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough bool cs_gnss_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest bool cs_gnss_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used -bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing @@ -49,7 +47,6 @@ bool cs_valid_fake_pos # 41 - true if a valid constant position is bein bool cs_constant_pos # 42 - true if the vehicle is at a constant position bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used bool cs_gnss_vel # 44 - true if GNSS velocity measurement fusion is intended -bool cs_rng_kin_unknown # 45 - true when the range finder kinematic consistency check is not running # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 51bfaa3da0..e7cb29d17f 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -36,3 +36,5 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed float32 cruising_speed # the generally desired cruising speed (not a hard constraint) bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover + +# TOPICS position_setpoint task_position_setpoint diff --git a/msg/TaskLocalPositionSetpoint.msg b/msg/TaskLocalPositionSetpoint.msg new file mode 100644 index 0000000000..0093d52d77 --- /dev/null +++ b/msg/TaskLocalPositionSetpoint.msg @@ -0,0 +1,19 @@ +# Local position setpoint in NED frame +# Telemetry of PID position controller to monitor tracking. +# NaN means the state was not controlled + +uint64 timestamp # time since system start (microseconds) + +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED + +float32 vx # in meters/sec +float32 vy # in meters/sec +float32 vz # in meters/sec + +float32[3] acceleration # in meters/sec^2 +float32[3] thrust # normalized thrust vector in NED + +float32 yaw # in radians NED -PI..+PI +float32 yawspeed # in radians/sec diff --git a/msg/VehicleConstraints.msg b/msg/VehicleConstraints.msg index aa3a491b12..3070ba4ab6 100644 --- a/msg/VehicleConstraints.msg +++ b/msg/VehicleConstraints.msg @@ -7,3 +7,5 @@ float32 speed_up # in meters/sec float32 speed_down # in meters/sec bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) + +bool lock_dist_bottom # altitude is locked to the current distance to ground diff --git a/range_alt_notes.txt b/range_alt_notes.txt new file mode 100644 index 0000000000..cae965ac8c --- /dev/null +++ b/range_alt_notes.txt @@ -0,0 +1,20 @@ +## FlightTaskManualAltitude +- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected +- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove? +- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity +- _respectMinAltitude ... weird, remove? No need for a function + + +## FlightTask +- _dist_to_bottom and _dist_to_ground ... confusing, unify +- + +## FlightTaskAuto +- reuse logic for range alt lock +- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable) + +## EKF +- Vz does not reflect true Vz due to noisy baro +- Vz errors cause rangefinder kinematic consistency check to fail +- Position setpoint error remains over long periods (might be related to Vz issues above) + diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a7b8e18f8a..36eca7ec50 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -211,7 +211,7 @@ endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS EKF/aid_sources/range_finder/range_finder_consistency_check.cpp - EKF/aid_sources/range_finder/range_height_control.cpp + EKF/aid_sources/range_finder/jake_range_height_control.cpp EKF/aid_sources/range_finder/range_height_fusion.cpp EKF/aid_sources/range_finder/sensor_range_finder.cpp ) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 381e265380..c210dcc868 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -124,7 +124,7 @@ endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS aid_sources/range_finder/range_finder_consistency_check.cpp - aid_sources/range_finder/range_height_control.cpp + aid_sources/range_finder/jake_range_height_control.cpp aid_sources/range_finder/range_height_fusion.cpp aid_sources/range_finder/sensor_range_finder.cpp ) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp deleted file mode 100644 index 8134418fab..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-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. - * - ****************************************************************************/ - -/** - * @file Sensor.hpp - * Abstract class for sensors - * - * @author Mathieu Bresciani - * - */ - -#ifndef EKF_SENSOR_HPP -#define EKF_SENSOR_HPP - -#include - -namespace estimator -{ -namespace sensor -{ - -class Sensor -{ -public: - virtual ~Sensor() {}; - - /* - * run sanity checks on the current data - * this has to be called immediately after - * setting new data - */ - virtual void runChecks() {}; - - /* - * return true if the sensor is healthy - */ - virtual bool isHealthy() const = 0; - - /* - * return true if the delayed sample is healthy - * and can be fused in the estimator - */ - virtual bool isDataHealthy() const = 0; - - /* - * return true if the sensor data rate is - * stable and high enough - */ - virtual bool isRegularlySendingData() const = 0; -}; - -} // namespace sensor -} // namespace estimator -#endif // !EKF_SENSOR_HPP diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp new file mode 100644 index 0000000000..852ecfe572 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp @@ -0,0 +1,425 @@ +/**************************************************************************** + * + * 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 range_height_control.cpp + * Control functions for ekf range finder height fusion + */ + +#include "ekf.h" +#include "ekf_derivation/generated/compute_hagl_h.h" +#include "ekf_derivation/generated/compute_hagl_innov_var.h" + +void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) +{ + // Check if rangefinder is available/enabled + if (!_range_buffer) { + // PX4_INFO("no buff"); + return; + } + + // Pop rangefinder measurement from buffer of samples into active sample + sensor::rangeSample sample = {}; + if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { + if (_range_sensor.timedOut(imu_sample.time_us)) { + // Disable fusion if it's currently enabled + if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { + PX4_INFO("stopping RNG fusion, sensor timed out"); + stopRangeAltitudeFusion(); + stopRangeTerrainFusion(); + } + // PX4_INFO("timed out1"); + } + return; + } + + // PX4_INFO("got one!"); + // PX4_INFO("rng: %f", (double)sample.rng); + // PX4_INFO("quality: %d", sample.quality); + + // Set the raw sample + _range_sensor.setSample(sample); + + // TODO: move setting params to init function + // Set all of the parameters + _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + + // Update sensor to earth rotation + _range_sensor.updateSensorToEarthRotation(_R_to_earth); + + + // TODO: refactor into validity_checks() + // Gate sample consumption on these checks + bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1) + bool tilt_ok = _range_sensor.isTiltOk(); + bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal(); + // - Not stuck value + // - Not fog detected + + // If quality, tilt, or value are outside of bounds -- throw away measurement + if (!quality_ok || !tilt_ok || !range_ok) { + if (_range_sensor.timedOut(imu_sample.time_us)) { + // Disable fusion if it's currently enabled + if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { + PX4_INFO("stopping RNG fusion, sensor data invalid"); + stopRangeAltitudeFusion(); + stopRangeTerrainFusion(); + } + PX4_INFO("timed out2"); + } + + if (!quality_ok) { + PX4_INFO("!quality_ok"); + } + if (!tilt_ok) { + PX4_INFO("!tilt_ok"); + } + if (!range_ok) { + PX4_INFO("!range_ok"); + } // commander takeoff + return; + } + + + // TODO: refactor into apply_body_offset() + // Correct the range data for position offset relative to the IMU + const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z }; + const Vector3f imu_pos_body = _params.imu_pos_body; + const Vector3f pos_offset_body = rng_pos_body - imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt(); + + // Provide sample from buffer to object + _range_sensor.setSample(sample); + + + // TODO: refactor into consintency_filter_update() that runs consistency status + // Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate + _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + + const float z = _gpos.altitude(); + const float vz = _state.vel(2); + const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame + const float dist_var = 0.05; + const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); + const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + + // Run the kinematic consistency check + _rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us); + + // Track kinematic consistency + // _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + + // Publish EstimatorAidSource1d (observation, variance, rejected, fused) + updateRangeHagl(_aid_src_rng_hgt); + + if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) { + PX4_INFO("INFINIFY OBSERVATION INVALID"); + } + + // Fuse Range data as Primary height source + // NOTE: terrain is not estimated in this mode + if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { + fuseRangeAsHeightSource(); + } else { + // Fuse Range data as aiding height source (Primary GPS or Baro) + fuseRangeAsHeightAiding(); + } +} + +void Ekf::fuseRangeAsHeightSource() +{ + // When primary height source is RANGE, we always fuse it + // I don't think conditional range aid mode makes sense in the context of RANGE = primary + + // Fusion init logic + if (_height_sensor_ref != HeightSensor::RANGE) { + + _height_sensor_ref = HeightSensor::RANGE; + _information_events.flags.reset_hgt_to_rng = true; + + // Reset aid source innovation + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); + + // Reset altitude to RANGE + resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance); + _control_status.flags.rng_hgt = true; + + // Cannot have terrain estimate fusion while RANGE is primary + stopRangeTerrainFusion(); + _state.terrain = 0.f; + + // TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation() + // _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us; + } + + +} + +void Ekf::fuseRangeAsHeightAiding() +{ + bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL; + bool range_aid_enabled = _params.rng_ctrl == RngCtrl::ENABLED; + + bool range_aid_conditions_passed = rangeAidConditionsPassed(); + bool kinematically_consistent = _control_status.flags.rng_kin_consistent; + + bool do_range_aid = kinematically_consistent && + (range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed)); + + bool fuse_measurement = false; + + + // Variables to use below + bool innovation_rejected = _aid_src_rng_hgt.innovation_rejected; + bool optical_flow_for_terrain = _control_status.flags.opt_flow_terrain; + + // Fuse Range into Altitude if: + // - passes range_aid_conditionalchecks + // - kinematically consistent + if (do_range_aid) { + // Start fusion + if (!_control_status.flags.rng_hgt) { + // Fusion init logic + PX4_INFO("starting RNG Altitude fusion"); + _control_status.flags.rng_hgt = true; + _height_sensor_ref = HeightSensor::RANGE; + + // TODO: do we really need to stop terrain fusion here? + stopRangeTerrainFusion(); + + // TODO: review for correctness + if (innovation_rejected && kinematically_consistent) { + // Reset aid source + PX4_INFO("range alt fusion, resetting aid source"); + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); + } + } + + // Fuse + fuse_measurement = true; + + } else { + // Stop fusion + stopRangeAltitudeFusion(); + + // if (!range_aid_conditions_passed) { + // PX4_INFO("range aid conditions failed"); + // } + // if (!kinematically_consistent) { + // PX4_INFO("kinematically inconsistent"); + // } + } + + // Fuse Range into Terrain if: + // - kinematically consistent + if (kinematically_consistent) { + + // Start fusion + if (!_control_status.flags.rng_terrain) { + // Fusion init logic + PX4_INFO("starting RNG Terrain fusion"); + _control_status.flags.rng_terrain = true; + + static bool first_init = true; + // Reset terrain when we first init + if (!optical_flow_for_terrain && first_init) { + first_init = false; + // Reset aid source and then reset terrain estimate + PX4_INFO("FIRST range terrain fusion, resetting terrain to range"); + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); + resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain); + resetTerrainToRng(_aid_src_rng_hgt); + } + } + + // Reset terrain to range if innovation is rejected + if (!optical_flow_for_terrain && innovation_rejected + && kinematically_consistent) { + PX4_INFO("range terrain fusion, resetting terrain to range"); + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); + resetTerrainToRng(_aid_src_rng_hgt); + } + + // Fuse + fuse_measurement = true; + + } else { + // Stop fusion + stopRangeTerrainFusion(); + } + + if (fuse_measurement) { + // PX4_INFO("fusing: %f", (double)_aid_src_rng_hgt.observation); + // PX4_INFO("_control_status.flags.rng_hgt: %d", _control_status.flags.rng_hgt); + // PX4_INFO("_control_status.flags.rng_terrain: %d", _control_status.flags.rng_terrain); + fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); + + // commander takeoff + + // TODO: _height_sensor_ref == HeightSensor::RANGE is redundant + if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { + PX4_INFO("RNG height fusion reset required, all height sources failing"); + + uint64_t timestamp = _aid_src_rng_hgt.timestamp; + _information_events.flags.reset_hgt_to_rng = true; + resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain); + resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt); + + // reset vertical velocity if no valid sources available + if (!isVerticalVelocityAidingActive()) { + PX4_INFO("resetting vertical velocity"); + resetVerticalVelocityToZero(); + } + + _aid_src_rng_hgt.time_last_fuse = timestamp; + } + } +} + +bool Ekf::rangeAidConditionsPassed() +{ + // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching + // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice + float range_hagl_max = _params.max_hagl_for_range_aid; + float max_vel_xy = _params.max_vel_for_range_aid; + + const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio; + + bool is_hagl_stable = (hagl_test_ratio < 1.f); + + if (!_control_status.flags.rng_hgt) { + range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; + max_vel_xy = 0.7f * _params.max_vel_for_range_aid; + is_hagl_stable = (hagl_test_ratio < 0.01f); + } + + const bool is_in_range = (getHagl() < range_hagl_max); + + bool is_below_max_speed = true; + + if (isHorizontalAidingActive()) { + is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy); + } + + return is_in_range && is_hagl_stable && is_below_max_speed; +} + +void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) +{ + // Since the distance is not a direct observation of the terrain state but is based + // on the height state, a reset should consider the height uncertainty. This can be + // done by manipulating the Kalman gain to inject all the innovation in the terrain state + // and create the correct correlation with the terrain state with a covariance update. + P.uncorrelateCovarianceSetVariance(State::terrain.idx, 0.f); + + const float old_terrain = _state.terrain; + + VectorState H; + sym::ComputeHaglH(&H); + + VectorState K; + K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset" + + measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); + + // record the state change + const float delta_terrain = _state.terrain - old_terrain; + + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_terrain; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_terrain; + } + + _state_reset_status.reset_count.hagl++; + + aid_src.time_last_fuse = _time_delayed_us; +} + +void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) +{ + const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); + const float measurement_variance = getRngVar(); + + float innovation_variance; + sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance); + + const float innov_gate = math::max(_params.range_innov_gate, 1.f); + updateAidSourceStatus(aid_src, + _range_sensor.sample()->time_us, // sample timestamp + measurement, // observation + measurement_variance, // observation variance + getHagl() - measurement, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate + + // z special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { + const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); + aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } +} + +float Ekf::getRngVar() const +{ + const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom()); + const float dist_var = sq(_params.range_noise) + dist_dependant_var; + return dist_var; +} + +void Ekf::stopRangeAltitudeFusion() +{ + if (_control_status.flags.rng_hgt) { + PX4_INFO("stopping RNG Altitude fusion"); + _control_status.flags.rng_hgt = false; + if (_height_sensor_ref == HeightSensor::RANGE) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + } +} + +void Ekf::stopRangeTerrainFusion() +{ + if (_control_status.flags.rng_terrain) { + PX4_INFO("stopping RNG Terrain fusion"); + _control_status.flags.rng_terrain = false; + } +} diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index d6fa31dcb8..65236e36d0 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -44,6 +44,8 @@ using namespace matrix; void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom, const float dist_bottom_var) { + printf("RangeFinderConsistencyCheck::init\n"); + _P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var); _Ht = sym::RangeValidationFilterH(); @@ -89,7 +91,6 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const H(RangeFilter::z.idx) = 1.f; H(RangeFilter::terrain.idx) = 0.f; R = z_var; - } // residual @@ -130,25 +131,19 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var) { // start the consistency check after 1s - if (_t_since_first_sample > _t_to_init) { - if (fabsf(_test_ratio_lpf.getState()) < 1.f) { - const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); - - if (!horizontal_motion && vertical_motion) { - _state = KinematicState::CONSISTENT; - - } else if (_state == KinematicState::CONSISTENT || vertical_motion) { - _state = KinematicState::UNKNOWN; - } - - } else { - _t_since_first_sample = 0.f; - _state = KinematicState::INCONSISTENT; - } - - } else { + if (_t_since_first_sample < _t_to_init) { _t_since_first_sample += dt; + return; } + + if (fabsf(_test_ratio_lpf.getState()) > 1.f) { + printf("_test_ratio_lpf failed (>1)\n"); + _t_since_first_sample = 0.f; + _state = KinematicState::INCONSISTENT; + return; + } + + _state = KinematicState::CONSISTENT; } void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var, diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 0fb9e11a5e..6ecc56c29a 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -73,7 +73,6 @@ public: } uint8_t current_posD_reset_count{0}; - bool horizontal_motion{false}; private: diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 422c594e29..92629a130b 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -44,64 +44,61 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "RNG"; - bool rng_data_ready = false; - - if (_range_buffer) { - // Get range data from buffer and check validity - rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); - _range_sensor.setDataReadiness(rng_data_ready); - - if (_range_sensor.isDataReady()) { - - _range_sensor.setPitchOffset(_params.rng_sens_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - - _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); - - if (_range_sensor.isDataHealthy()) { - // correct the range data for position offset relative to the IMU - const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - - const float dist_var = getRngVar(); - _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - - const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - - if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { - _rng_consistency_check.reset(); - } - - _rng_consistency_check.horizontal_motion = updated_horizontal_motion; - const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); - const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); - _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), - dist_var, imu_sample.time_us); - - } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); - - } else { - _rng_consistency_check.reset(); - } - } - - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); - _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() - && _rng_consistency_check.isNotKinematicallyInconsistent(); - - } else { + if (!_range_buffer) { return; } + // Get range data from buffer and check validity + bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample()); + + _range_sensor.setSample(); + + _range_sensor.setDataReadiness(rng_data_ready); + + if (_range_sensor.isDataReady()) { + + _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setMaxFogDistance(_params.rng_fog); + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); + + if (_range_sensor.isDataHealthy()) { + // correct the range data for position offset relative to the IMU + const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + + // TODO: this is a constant + const float dist_var = getRngVar(); + _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + + const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); + const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + + // TODO: review -- variance + _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), + dist_var, imu_sample.time_us); + + } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setValidity(true); + + } else { + _rng_consistency_check.reset(); + } + } + + _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + + _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() + && _rng_consistency_check.isNotKinematicallyInconsistent(); + + auto &aid_src = _aid_src_rng_hgt; - if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) { + if (_range_sensor.isDataReady() && _range_sensor.sample()) { updateRangeHagl(aid_src); const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); @@ -113,6 +110,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _range_sensor.isDataHealthy() && _rng_consistency_check.isNotKinematicallyInconsistent(); + // SUS: _rng_consistency_check.isNotKinematicallyInconsistent() + const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData(); @@ -121,12 +120,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); + // SUS: isConditionalRangeAidSuitable() + const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); if (_control_status.flags.rng_hgt) { if (!(do_conditional_range_aid || do_range_aid)) { - ECL_INFO("stopping %s fusion", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion", HGT_SRC_NAME); stopRngHgtFusion(); } @@ -134,7 +135,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { if (do_conditional_range_aid) { // 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); + PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; _control_status.flags.rng_hgt = true; @@ -149,7 +150,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if (do_range_aid) { // 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); + PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; @@ -164,7 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (do_conditional_range_aid || do_range_aid) { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + PX4_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected @@ -185,7 +186,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation - _state.terrain); @@ -201,7 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if (is_fusion_failing) { // Some other height source is still working if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) { - ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); @@ -212,7 +213,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } @@ -239,7 +240,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } @@ -255,7 +256,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) const float innov_gate = math::max(_params.range_innov_gate, 1.f); updateAidSourceStatus(aid_src, - _range_sensor.getSampleAddress()->time_us, // sample timestamp + _range_sensor.sample()->time_us, // sample timestamp measurement, // observation measurement_variance, // observation variance getHagl() - measurement, // innovation diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 891447a4cd..8dbd0f444b 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -47,10 +47,34 @@ namespace estimator namespace sensor { -void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth) +void SensorRangeFinder::setSample(const rangeSample &sample) { - updateSensorToEarthRotation(R_to_earth); - updateValidity(current_time_us); + _sample = sample; +} + +bool SensorRangeFinder::timedOut(uint64_t time_now) const +{ + if (_sample.time_us > time_now) { + return false; + } + + // TODO: 200ms? + return time_now > _sample.time_us + 200'000; +} + +void SensorRangeFinder::setPitchOffset(float new_pitch_offset) +{ + if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { + _sin_pitch_offset = sinf(new_pitch_offset); + _cos_pitch_offset = cosf(new_pitch_offset); + _pitch_offset_rad = new_pitch_offset; + } +} + +void SensorRangeFinder::setLimits(float min_distance, float max_distance) +{ + _rng_valid_min_val = min_distance; + _rng_valid_max_val = max_distance; } void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth) @@ -60,115 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear _cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset; } -void SensorRangeFinder::updateValidity(uint64_t current_time_us) -{ - updateDtDataLpf(current_time_us); - - if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) { - _is_sample_valid = false; - _is_regularly_sending_data = false; - return; - } - - _is_regularly_sending_data = true; - - // Don't run the checks unless we have retrieved new data from the buffer - if (_is_sample_ready) { - _is_sample_valid = false; - - _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; - - if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) { - return; - } - - updateStuckCheck(); - updateFogCheck(getDistBottom(), _sample.time_us); - - if (!_is_stuck && !_is_blocked) { - _is_sample_valid = true; - _time_last_valid_us = _sample.time_us; - } - } -} - -bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const -{ - return current_time_us - _time_bad_quality_us > _quality_hyst_us; -} - -void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) -{ - // Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant. - float alpha = 0.5f * _dt_update; - _dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us); - - // Apply spike protection to the filter state. - _dt_data_lpf = fminf(_dt_data_lpf, 4e6f); -} - -inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const -{ - return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL; -} - -inline bool SensorRangeFinder::isDataInRange() const -{ - return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val); -} - -void SensorRangeFinder::updateStuckCheck() -{ - if (!isStuckDetectorEnabled()) { - // Stuck detector disabled - _is_stuck = false; - return; - } - - // Check for "stuck" range finder measurements when range was not valid for certain period - // This handles a failure mode observed with some lidar sensors - if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) { - - // require a variance of rangefinder values to check for "stuck" measurements - if (_stuck_max_val - _stuck_min_val > _stuck_threshold) { - _stuck_min_val = 0.0f; - _stuck_max_val = 0.0f; - _is_stuck = false; - - } else { - if (_sample.rng > _stuck_max_val) { - _stuck_max_val = _sample.rng; - } - - if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) { - _stuck_min_val = _sample.rng; - } - - _is_stuck = true; - } - } -} - -void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us) -{ - if (_max_fog_dist > 0.f) { - - const float median_dist = _median_dist.apply(dist_bottom); - const float factor = 2.f; // magic hardcoded factor - - if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) { - _is_blocked = true; - - } else if (_is_blocked && median_dist > factor * _max_fog_dist) { - _is_blocked = false; - } - - _prev_median_dist = median_dist; - - } else { - _prev_median_dist = dist_bottom; - } -} - } // namespace sensor } // namespace estimator diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index e3e4f6e6cd..0e92012f1c 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -41,8 +41,6 @@ #ifndef EKF_SENSOR_RANGE_FINDER_HPP #define EKF_SENSOR_RANGE_FINDER_HPP -#include "Sensor.hpp" - #include #include @@ -57,105 +55,62 @@ struct rangeSample { 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 +class SensorRangeFinder { public: SensorRangeFinder() = default; - ~SensorRangeFinder() override = default; + ~SensorRangeFinder() = default; - void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth); - bool isHealthy() const override { return _is_sample_valid; } - bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; } - bool isDataReady() const { return _is_sample_ready; } - bool isRegularlySendingData() const override { return _is_regularly_sending_data; } - bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; } + struct Parameters { + float ekf2_imu_pos_x{}; + float ekf2_imu_pos_y{}; + float ekf2_imu_pos_z{}; + + float ekf2_rng_pos_x{}; + float ekf2_rng_pos_y{}; + float ekf2_rng_pos_z{}; + + float ekf2_rng_pitch{}; + + float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt + }; + + void updateParameters(Parameters& parameters) { _parameters = parameters; }; + + bool timedOut(uint64_t time_now) const; + void setSample(const rangeSample &sample); - void setSample(const rangeSample &sample) - { - _sample = sample; - _is_sample_ready = true; - } // This is required because of the ring buffer // TODO: move the ring buffer here - rangeSample *getSampleAddress() { return &_sample; } + rangeSample *sample() { return &_sample; } - void setPitchOffset(float new_pitch_offset) - { - if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { - _sin_pitch_offset = sinf(new_pitch_offset); - _cos_pitch_offset = cosf(new_pitch_offset); - _pitch_offset_rad = new_pitch_offset; - } - } + void setPitchOffset(float new_pitch_offset); void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; } - void setLimits(float min_distance, float max_distance) - { - _rng_valid_min_val = min_distance; - _rng_valid_max_val = max_distance; - } - - void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; } - - void setQualityHysteresis(float valid_quality_threshold_s) - { - _quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f); - } - float getCosTilt() const { return _cos_tilt_rng_to_earth; } - void setRange(float rng) { _sample.rng = rng; } - float getRange() const { return _sample.rng; } + void setLimits(float min_distance, float max_distance); + + // void setRange(float rng) { _sample.rng = rng; } + // float getRange() const { return _sample.rng; } float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; } - void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; } - void setValidity(bool is_valid) { _is_sample_valid = is_valid; } - float getValidMinVal() const { return _rng_valid_min_val; } float getValidMaxVal() const { return _rng_valid_max_val; } - void setFaulty(bool faulty = true) { _is_faulty = faulty; } - -private: void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); - void updateValidity(uint64_t current_time_us); - void updateDtDataLpf(uint64_t current_time_us); - bool isSampleOutOfDate(uint64_t current_time_us) const; - bool isDataContinuous() const { return _dt_data_lpf < 2e6f; } bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } - bool isDataInRange() const; - bool isQualityOk(uint64_t current_time_us) const; - void updateStuckCheck(); - void updateFogCheck(const float dist_bottom, const uint64_t time_us); + +private: rangeSample _sample{}; - bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused - bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid - bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval - uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec) - bool _is_faulty{false}; ///< the sensor should not be used anymore - - /* - * Stuck check - */ - bool _is_stuck{}; - float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable - float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck - float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck - - /* - * Data regularity check - */ - static constexpr float _dt_update{0.01f}; ///< delta time since last ekf update TODO: this should be a parameter - float _dt_data_lpf{}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) + Parameters _parameters{}; /* * Tilt check @@ -172,20 +127,6 @@ private: float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m) float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m) - /* - * Quality check - */ - uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis) - uint64_t _quality_hyst_us{}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us) - - /* - * Fog check - */ - bool _is_blocked{false}; - float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m) - math::MedianFilter _median_dist{}; - float _prev_median_dist{0.f}; - }; } // namespace sensor diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ae62583e4b..10b6751a46 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -148,7 +148,7 @@ enum class GnssCtrl : uint8_t { YAW = (1 << 3) }; -enum class RngCtrl : uint8_t { +enum RngCtrl : uint8_t { DISABLED = 0, CONDITIONAL = 1, ENABLED = 2 @@ -415,17 +415,19 @@ struct parameters { float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. - float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) + float ekf2_rng_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. + float ekf2_rng_sfe{0.0f}; ///< scaling from range measurement to noise (m/m) float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion - float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] - Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) + float ekf2_rng_pos_x{0.f}; + float ekf2_rng_pos_y{0.f}; + float ekf2_rng_pos_z{0.f}; + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -570,8 +572,6 @@ uint64_t mag_fault : uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active -uint64_t rng_stuck : - 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough uint64_t gnss_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed @@ -582,8 +582,6 @@ uint64_t synthetic_mag_z : uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest uint64_t gnss_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used -uint64_t rng_fault : - 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements @@ -604,7 +602,6 @@ uint64_t mag_heading_consistent : uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used uint64_t gnss_vel : 1; ///< 44 - true if GNSS velocity measurement fusion is intended - uint64_t rng_kin_unknown : 1; ///< 45 - true when the range finder kinematic consistency check is not running } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b5c8b044fb..b7422c183a 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -77,13 +77,6 @@ void Ekf::reset() _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; #endif // CONFIG_EKF2_TERRAIN -#if defined(CONFIG_EKF2_RANGE_FINDER) - _range_sensor.setPitchOffset(_params.rng_sens_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); -#endif // CONFIG_EKF2_RANGE_FINDER - _control_status.value = 0; _control_status_prev.value = 0; @@ -390,6 +383,18 @@ void Ekf::updateParameters() #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +#if defined (CONFIG_EKF2_RANGE_FINDER) + + sensor::SensorRangeFinder::Parameters params = {}; + // params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get(); + // params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get(); + // params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get(); + // params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get(); + // params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get(); + // params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get(); + params.ekf2_rng_pitch = _params.ekf2_rng_pitch; + _range_sensor.updateParameters(params); +#endif } template diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fb5b482204..2a2f6e672c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -762,9 +762,18 @@ private: #if defined(CONFIG_EKF2_RANGE_FINDER) // range height void controlRangeHaglFusion(const imuSample &imu_delayed); + + + void fuseRangeAsHeightSource(); + void fuseRangeAsHeightAiding(); + bool isConditionalRangeAidSuitable(); - void stopRngHgtFusion(); - void stopRngTerrFusion(); + bool rangeAidConditionsPassed(); + + + void stopRangeAltitudeFusion(); + void stopRangeTerrainFusion(); + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 56e8334fba..2cc29fd824 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -120,8 +120,6 @@ public: { _range_sensor.setLimits(min_distance, max_distance); } - - const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index d005022289..6a8ae625af 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -54,7 +54,6 @@ void Ekf::controlTerrainFakeFusion() // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { _last_on_ground_posD = -_gpos.altitude(); - _control_status.flags.rng_fault = false; } else if (!_control_status_prev.flags.in_air) { // Let the estimator run freely before arming for bench testing purposes, but reset on takeoff diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 29089b2b32..e8c6afd447 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -155,18 +155,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), - _param_ekf2_rng_sfe(_params->range_noise_scaler), + _param_ekf2_rng_sfe(_params->ekf2_rng_sfe), _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_rng_pitch(_params->rng_sens_pitch), + _param_ekf2_rng_pitch(_params->ekf2_rng_pitch), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), - _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), _param_ekf2_rng_fog(_params->rng_fog), - _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), - _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), - _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), + _param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x), + _param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y), + _param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z), #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ekf2_ev_delay), @@ -1601,8 +1600,16 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); + + // TODO: this makes dist_bottom a function of the Terrain State estimate, which is not + // exactly correct. Terrain State can diverge from the distance_sensor.current_distance + // when fusion is disabled. This causes Terrain Hold to drift due to dist_bottom diverging. + // Ultimately the logic for rangefinder fusion needs to be improved, and when distance_sensor + // is not fused into the Terrain State estimate we should not use dist_bottom for Altitude Lock. lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f); + // TODO: review, we should use sensor variance not terrain lpos.dist_bottom_var = _ekf.getTerrainVariance(); + _ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter); lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE; @@ -1910,14 +1917,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_mag_fault = _ekf.control_status_flags().mag_fault; status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd; status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect; - status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck; status_flags.cs_gnss_yaw = _ekf.control_status_flags().gnss_yaw; status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; status_flags.cs_gnss_yaw_fault = _ekf.control_status_flags().gnss_yaw_fault; - status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault; status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning; status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning; status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent; @@ -1934,7 +1939,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.cs_gnss_vel = _ekf.control_status_flags().gnss_vel; - status_flags.cs_rng_kin_unknown = _ekf.control_status_flags().rng_kin_unknown; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index bf60078e8f..0d65359dff 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -619,7 +619,6 @@ private: (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_a_igate, - (ParamExtFloat) _param_ekf2_rng_qlty_t, (ParamExtFloat) _param_ekf2_rng_k_gate, (ParamExtFloat) _param_ekf2_rng_fog, (ParamExtFloat) _param_ekf2_rng_pos_x, diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 90525178af..fbd1f6db3f 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -48,7 +48,7 @@ parameters: description: short: Measurement noise for range finder fusion type: float - default: 0.1 + default: 0.05 min: 0.01 unit: m decimal: 2 @@ -93,16 +93,6 @@ parameters: unit: SD min: 0.1 max: 5.0 - EKF2_RNG_QLTY_T: - description: - short: Minumum range validity period - long: Minimum duration during which the reported range finder signal quality - needs to be non-zero in order to be declared valid (s) - type: float - default: 1.0 - unit: s - min: 0.1 - max: 5 EKF2_RNG_K_GATE: description: short: Gate size used for range finder kinematic consistency check @@ -122,7 +112,7 @@ parameters: short: Range finder range dependent noise scaler long: Specifies the increase in range finder noise with range. type: float - default: 0.05 + default: 0.01 min: 0.0 max: 0.2 unit: m/m diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index 396d50ff76..6f24b80fc6 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -123,7 +123,6 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); - EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); @@ -179,7 +178,6 @@ TEST_F(EkfBasicsTest, gpsFusion) EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); - EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index ecf69d83b5..74e35565e9 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -51,7 +51,6 @@ public: _range_finder.setPitchOffset(0.f); _range_finder.setCosMaxTilt(0.707f); _range_finder.setLimits(_min_range, _max_range); - _range_finder.setMaxFogDistance(2.f); } // Use this method to clean up any memory, network etc. after each test @@ -118,9 +117,8 @@ TEST_F(SensorRangeFinderTest, setRange) sample.time_us = 1e6; sample.quality = 9; - _range_finder.setRange(sample.rng); - _range_finder.setDataReadiness(true); - _range_finder.setValidity(true); + _range_finder.setSample(sample) + EXPECT_TRUE(_range_finder.isDataHealthy()); EXPECT_TRUE(_range_finder.isHealthy()); } @@ -363,7 +361,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: sensor is then blocked by fog // range jumps to value below 2m - uint64_t t_now_us = _range_finder.getSampleAddress()->time_us; + uint64_t t_now_us = _range_finder.sample()->time_us; rangeSample sample{t_now_us, 1.f, 100}; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); @@ -373,7 +371,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: the sensor is not blocked by fog anymore sample.rng = 5.f; - sample.time_us = _range_finder.getSampleAddress()->time_us; + sample.time_us = _range_finder.sample()->time_us; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy again diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 2f27fd1aed..b01432e836 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -97,94 +97,132 @@ void FlightTaskManualAltitude::_scaleSticks() void FlightTaskManualAltitude::_updateAltitudeLock() { - // Depending on stick inputs and velocity, position is locked. - // If not locked, altitude setpoint is set to NAN. + // - check if sticks are released (braking) + // - check if no vertical motion (altitude lock) (noisy baro, increase default threshold) + // - check if horizontal motion is within limit (altitude lock) - // Check if user wants to break - const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON; - - // Check if vehicle has stopped - const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get()); - - // Manage transition between use of distance to ground and distance to local origin - // when terrain hold behaviour has been selected. - if (_param_mpc_alt_mode.get() == 2) { - // Use horizontal speed as a transition criteria - float spd_xy = Vector2f(_velocity).length(); - - // Use presence of horizontal stick inputs as a transition criteria - float stick_xy = Vector2f(_sticks.getPitchRollExpo()).length(); - bool stick_input = stick_xy > 0.001f; - - if (_terrain_hold) { - bool too_fast = spd_xy > _param_mpc_hold_max_xy.get(); - - if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) { - // Stop using distance to ground - _terrain_hold = false; - - // Adjust the setpoint to maintain the same height error to reduce control transients - if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { - _position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom); - - } else { - _position_setpoint(2) = _position(2); - _dist_to_ground_lock = NAN; - } - } - - } else { - bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get() && stopped; - - if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) { - // Start using distance to ground - _terrain_hold = true; - - // Adjust the setpoint to maintain the same height error to reduce control transients - if (PX4_ISFINITE(_position_setpoint(2))) { - _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); - } - } + // First check if user is controlling Z velocity with sticks + if (fabsf(_sticks.getPositionExpo()(2)) > 0.001f) { + if (PX4_ISFINITE(_position_setpoint(2)) || PX4_ISFINITE(_dist_to_bottom_lock)) { + // PX4_INFO("Setting position sp to NAN"); + _current_mode = AltitudeMode::None; + _position_setpoint(2) = NAN; + _dist_to_bottom_lock = NAN; + _constraints.lock_dist_bottom = false; } + return; } - if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { - // terrain following - _terrainFollowing(apply_brake, stopped); + switch ((AltitudeMode)_param_mpc_alt_mode.get()) { + case AltitudeMode::AltitudeFollow: { + // Altitude following - relative earth frame origin which may drift due to sensors + // - No user throttle + _altitude_follow_mode(); + break; + } - } else { - // normal mode where height is dependent on local frame + case AltitudeMode::TerrainFollow: { + // Terrain following - relative to ground which changes with terrain variation + // - distance sensor valid + // - No user throttle - if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) { - // lock position - _position_setpoint(2) = _position(2); - - // Ensure that minimum altitude is respected if - // there is a distance sensor and distance to bottom is below minimum. - if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _min_distance_to_ground) { - _terrainFollowing(apply_brake, stopped); + // Cannot perform terrain follow without distance sensor + if (PX4_ISFINITE(_dist_to_bottom)) { + _terrain_follow_mode(); } else { - _dist_to_ground_lock = NAN; + _altitude_follow_mode(); + break; } + break; + } - } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) { - // Position is locked but check if a reset event has happened. - // We will shift the setpoints. - if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) { - _position_setpoint(2) = _position(2); - _reset_counter = _sub_vehicle_local_position.get().z_reset_counter; + case AltitudeMode::TerrainHold: { + // Terrain hold - relative to ground when within thresholds + // - distance sensor valid + // - No user throttle + // - XY vel low + // - Z vel stopped + + // Cannot perform terrain hold without distance sensor + if (PX4_ISFINITE(_dist_to_bottom)) { + _terrain_hold_mode(); + + } else { + _altitude_follow_mode(); + break; } + } - } else { - // user demands velocity change - _position_setpoint(2) = NAN; - // ensure that maximum altitude is respected + case AltitudeMode::None: { + // Nothing to do + break; } } +} + +void FlightTaskManualAltitude::_terrain_hold_mode() +{ + // Check if XY velocity is within limit to activate Terrain Hold + bool xy_vel_okay = Vector2f(_velocity).length() < _param_mpc_hold_max_xy.get(); + + if (!xy_vel_okay) { + // XY_vel is above threshold, just follow altitude setpoint + // Lock the position setpoint to the current Z position estimate + if (_current_mode != AltitudeMode::AltitudeFollow) { + _current_mode = AltitudeMode::AltitudeFollow; + + PX4_INFO("Locking to Z estimate"); + _position_setpoint(2) = _position(2); + _dist_to_bottom_lock = NAN; + _constraints.lock_dist_bottom = false; + } + + return; + } + + if (_current_mode != AltitudeMode::TerrainHold) { + // Set velocity setpoint to 0, switch into TerrainHold + _current_mode = AltitudeMode::TerrainHold; + _velocity_setpoint(2) = 0.f; + PX4_INFO("setting terrain hold"); + return; + } + + if (!PX4_ISFINITE(_dist_to_bottom_lock)) { + // Wait for Z to come to a stop before locking in the distance + if (fabsf(_velocity(2)) < 0.1f) { + _dist_to_bottom_lock = _dist_to_bottom; + _position_setpoint(2) = _position(2); + _constraints.lock_dist_bottom = true; + PX4_INFO("Locking distance to %f", (double)_dist_to_bottom); + } + + return; + } + + // TODO: + // - use dist_bottom_var as heuristic for distance lock + + // All criteria met + // - distance sensor valid + // - No user throttle + // - XY vel low + // - Z vel stopped + // Update position setpoint to keep fixed distance from terrain + float delta_distance = _dist_to_bottom - _dist_to_bottom_lock; + _position_setpoint(2) = _position(2) + delta_distance; +} + +void FlightTaskManualAltitude::_terrain_follow_mode() +{ + +} + +void FlightTaskManualAltitude::_altitude_follow_mode() +{ - _respectMaxAltitude(); } void FlightTaskManualAltitude::_respectMinAltitude() @@ -198,7 +236,7 @@ void FlightTaskManualAltitude::_respectMinAltitude() void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped) { - if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) { + if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_bottom_lock)) { // User wants to break and vehicle reached zero velocity. Lock height to ground. // lock position @@ -206,19 +244,19 @@ void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped) // ensure that minimum altitude is respected _respectMinAltitude(); // lock distance to ground but adjust first for minimum altitude - _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); + _dist_to_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); - } else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) { + } else if (apply_brake && PX4_ISFINITE(_dist_to_bottom_lock)) { // vehicle needs to follow terrain // difference between the current distance to ground and the desired distance to ground - const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom; + const float delta_distance_to_ground = _dist_to_bottom_lock - _dist_to_bottom; // adjust position setpoint for the delta (note: NED frame) _position_setpoint(2) = _position(2) - delta_distance_to_ground; } else { // user demands velocity change in D-direction - _dist_to_ground_lock = _position_setpoint(2) = NAN; + _dist_to_bottom_lock = _position_setpoint(2) = NAN; } } @@ -269,7 +307,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl) { - _dist_to_ground_lock = NAN; + PX4_INFO("_ekfResetHandlerHagl"); + _dist_to_bottom_lock = NAN; } void FlightTaskManualAltitude::_updateSetpoints() @@ -278,7 +317,7 @@ void FlightTaskManualAltitude::_updateSetpoints() _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); _updateAltitudeLock(); - _respectGroundSlowdown(); + // _respectGroundSlowdown(); } bool FlightTaskManualAltitude::_checkTakeoff() diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 8386dfedad..4dfcad5be9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -45,6 +45,13 @@ #include "StickTiltXY.hpp" #include +enum class AltitudeMode : int { + AltitudeFollow, + TerrainFollow, + TerrainHold, + None, +}; + class FlightTaskManualAltitude : public FlightTask { public: @@ -76,7 +83,8 @@ protected: StickYaw _stick_yaw{this}; bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + // bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ + AltitudeMode _current_mode = AltitudeMode::AltitudeFollow; float _velocity_constraint_up{INFINITY}; float _velocity_constraint_down{INFINITY}; @@ -85,15 +93,20 @@ protected: (ParamFloat) _param_mpc_hold_max_z, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_hold_max_xy, - (ParamFloat) _param_mpc_z_p, /**< position controller altitude propotional gain */ - (ParamFloat) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */ - (ParamFloat) _param_mpc_land_alt2, /**< altitude below which to land with land speed */ - (ParamFloat) - _param_mpc_land_speed, /**< desired downwards speed when approaching the ground */ - (ParamFloat) - _param_mpc_tko_speed /**< desired upwards speed when still close to the ground */ + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_land_alt1, + (ParamFloat) _param_mpc_land_alt2, + (ParamFloat) _param_mpc_land_speed, + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max ) private: + + void _terrain_hold_mode(); + void _terrain_follow_mode(); + void _altitude_follow_mode(); /** * Terrain following. * During terrain following, the position setpoint is adjusted @@ -119,8 +132,6 @@ private: */ void _respectGroundSlowdown(); - void setGearAccordingToSwitch(); - bool _updateYawCorrection(); uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ @@ -132,7 +143,7 @@ private: * Distance to ground during terrain following. * If user does not demand velocity change in D-direction and the vehcile * is in terrain-following mode, then height to ground will be locked to - * _dist_to_ground_lock. + * _dist_to_bottom_lock. */ - float _dist_to_ground_lock = NAN; + float _dist_to_bottom_lock = NAN; }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 351daa86bf..afdcde1b79 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -107,7 +107,7 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState() _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); - if (!_terrain_hold) { + if (_current_mode != AltitudeMode::TerrainHold) { if (_terrain_hold_previous) { // Reset position setpoint to current position when switching from terrain hold to non-terrain hold _smoothing.setCurrentPosition(_position(2)); @@ -116,5 +116,18 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState() _position_setpoint(2) = _smoothing.getCurrentPosition(); } - _terrain_hold_previous = _terrain_hold; + _terrain_hold_previous = _current_mode == AltitudeMode::TerrainHold; + + task_local_position_setpoint_s msg = {}; + + msg.timestamp = hrt_absolute_time(); + + msg.x = _position_setpoint(0); + msg.y = _position_setpoint(1); + msg.z = _position_setpoint(2); + msg.vx = _velocity_setpoint(0); + msg.vy = _velocity_setpoint(1); + msg.vz = _velocity_setpoint(2); + + _setpoint_pub.publish(msg); } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index ce12b40f03..6fafe628a0 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -42,6 +42,9 @@ #include "FlightTaskManualAltitude.hpp" #include +#include +#include + class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude { public: @@ -62,12 +65,7 @@ protected: ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - private: + uORB::Publication _setpoint_pub{ORB_ID(task_local_position_setpoint)}; bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 597b4abf4e..2b705e6f01 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -143,6 +143,8 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); + // DEBUG + add_topic("task_local_position_setpoint"); add_topic("vehicle_magnetometer", 200); add_topic("vehicle_rates_setpoint", 20); add_topic("vehicle_roi", 1000); @@ -316,6 +318,10 @@ void LoggedTopics::add_estimator_replay_topics() add_topic("vehicle_visual_odometry"); add_topic("aux_global_position"); add_topic_multi("distance_sensor"); + add_topic("vehicle_local_position"); + add_topic("estimator_states"); + add_topic("estimator_status"); + add_topic("estimator_aid_src_rng_hgt"); } void LoggedTopics::add_thermal_calibration_topics() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index ddf54a36cc..227a8b1faa 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -800,7 +800,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) report.max_distance = static_cast(msg.range_max()); report.current_distance = static_cast(msg.ranges()[0]); report.variance = 0.0f; - report.signal_quality = -1; + report.signal_quality = 100; // Always perfect in sim report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation();