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

Fix max-hagl restriction to position/altitude control (#23667)

Browse Source 
* fix max-hagl restriction to position/altitude control

* max hagl vel restriction in ManAcc position mode

* use interpolate func, change naming

* simplyfied vertical vel limitation

* move velocity-constraint adjustment to StickAccelXY
This commit is contained in:
Marco Hauswirth
2025-01-20 15:50:21 +01:00
committed by GitHub
parent 0d22905558
commit 4a5aa1e947
15 changed files with 90 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+3 -2
View File
@@ -208,8 +208,9 @@ public:
// vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
// vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
// hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
// hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed.
// hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed.
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const;
void resetGyroBias();
void resetGyroBiasCov();
src/modules/ekf2/EKF/ekf_helper.cpp
+8 -5
View File
@@ -327,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
}
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z,
float *hagl_max_xy) const
{
// Do not require limiting by default
*vxy_max = NAN;
*vz_max = NAN;
*hagl_min = NAN;
*hagl_max = NAN;
*hagl_max_z = NAN;
*hagl_max_xy = NAN;
#if defined(CONFIG_EKF2_RANGE_FINDER)
// Calculate range finder limits
@@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
if (relying_on_rangefinder) {
*hagl_min = rangefinder_hagl_min;
*hagl_max = rangefinder_hagl_max;
*hagl_max_z = rangefinder_hagl_max;
}
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -370,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f);
*vxy_max = flow_vxy_max;
*hagl_min = flow_hagl_min;
*hagl_max = flow_hagl_max;
*hagl_max_xy = flow_hagl_max;
}
# endif // CONFIG_EKF2_OPTICAL_FLOW
src/modules/ekf2/EKF2.cpp
+7 -3
View File
@@ -1628,7 +1628,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
|| _ekf.control_status_flags().wind_dead_reckoning;
// get control limit information
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy);
// convert NaN to INFINITY
if (!PX4_ISFINITE(lpos.vxy_max)) {
@@ -1643,8 +1643,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.hagl_min = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_max)) {
lpos.hagl_max = INFINITY;
if (!PX4_ISFINITE(lpos.hagl_max_z)) {
lpos.hagl_max_z = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_max_xy)) {
lpos.hagl_max_xy = INFINITY;
}
// publish vehicle local position data