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

math: support max of three values additional to min and use it everywhere

Browse Source
This commit is contained in:
Matthias Grob
2021-12-06 14:31:46 +01:00
parent e8676fe87a
commit 68e2940f58
12 changed files with 18 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -953,12 +953,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
if (_control_status.flags.ev_vel) {
float ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)));
vel = math::max(math::max(vel, ev_vel), FLT_MIN);
vel = math::max(vel, ev_vel, FLT_MIN);
}
if (_control_status.flags.ev_pos) {
float ev_pos = sqrtf(_ev_pos_test_ratio(0));
pos = math::max(math::max(pos, ev_pos), FLT_MIN);
pos = math::max(pos, ev_pos, FLT_MIN);
}
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+1 -1
View File
@@ -1059,7 +1059,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
// Save distance to waypoint if it is the smallest ever achieved, however make sure that
// _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp
_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
_min_current_sp_distance_xy = math::min(d_curr, _min_current_sp_distance_xy, d_curr_prev);
// if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
// navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude