From 68e2940f584651fced4ca79991dce02a2c3dc9a5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 6 Dec 2021 14:31:46 +0100 Subject: [PATCH] math: support max of three values additional to min and use it everywhere --- src/drivers/imu/invensense/icm20602/ICM20602.hpp | 2 +- src/drivers/imu/invensense/icm20649/ICM20649.hpp | 2 +- src/drivers/imu/invensense/icm20948/ICM20948.hpp | 2 +- src/drivers/imu/invensense/icm42605/ICM42605.hpp | 2 +- src/drivers/imu/invensense/icm42688p/ICM42688P.hpp | 2 +- src/drivers/imu/invensense/mpu6500/MPU6500.hpp | 2 +- src/drivers/imu/invensense/mpu9250/MPU9250.hpp | 2 +- src/drivers/uavcan/sensors/gnss.cpp | 2 +- src/lib/mathlib/math/Limits.hpp | 6 ++++++ src/lib/motion_planning/TrajectoryConstraints.hpp | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 4 ++-- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 2 +- 12 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index eb36e56d80..3855ce12ea 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -76,7 +76,7 @@ private: static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index cd1da7992b..531f0be988 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -76,7 +76,7 @@ private: static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 59ce391510..103a03fe57 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -78,7 +78,7 @@ private: static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index 2059b7a1b2..98ad916d90 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -75,7 +75,7 @@ private: static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index 8894532338..47e4562d81 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -75,7 +75,7 @@ private: static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 6d0c6222ed..7686db5114 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -76,7 +76,7 @@ private: static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index b5efdcdb26..369d2ec6e7 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -78,7 +78,7 @@ private: static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index b6eb0509c3..de9c62005d 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -335,7 +335,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure } if (valid_vel_cov) { - report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + report.s_variance_m_s = math::max(vel_cov[0], vel_cov[4], vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index 900b426665..0c055149c7 100644 --- a/src/lib/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp @@ -68,6 +68,12 @@ constexpr _Tp max(_Tp a, _Tp b) return (a > b) ? a : b; } +template +constexpr _Tp max(_Tp a, _Tp b, _Tp c) +{ + return max(max(a, b), c); +} + template constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val) { diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index 51530681ef..6d52e1861e 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -89,7 +89,7 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co const float safe_alpha = constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); float accel_tmp = config.max_acc_xy_radius_scale * config.max_acc_xy; float max_speed_in_turn = computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, config.xy_accept_rad); - speed_at_target = min(min(max_speed_in_turn, exit_speed), config.max_speed_xy); + speed_at_target = min(max_speed_in_turn, exit_speed, config.max_speed_xy); } float start_to_target = (start_position - target).xy().norm(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 4c5086c346..59b440a122 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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)) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 33adbd3656..857ac1368e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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