From 060738cae1129fbf14197c9b7cb343b25ef7258d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 12 Aug 2022 11:23:22 +0200 Subject: [PATCH] src: avoid use of double precision pow & exp saves around 5KB flash on px4-fmu-v5 --- src/drivers/uwb/uwb_sr150/uwb_sr150.cpp | 14 +++++++------- src/drivers/uwb/uwb_sr150/uwb_sr150.h | 2 ++ src/lib/pid_design/pid_design.hpp | 2 +- .../follow_target_estimator/TargetEstimator.cpp | 2 +- 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp index 6fcce6ea92..7f917adbc0 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp @@ -587,18 +587,18 @@ UWB_POS_ERROR_CODES UWB_SR150::localization() /* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */ for (int i = 1; i < no_valid_distances; i++) { /* Matrix (needed to be multiplied with 2, afterwards) */ - M_11 += (int64_t)pow((int64_t)(anchor_pos[i].x - anchor_pos[0].x), 2); + M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x)); M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y)); M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - M_22 += (int64_t)pow((int64_t)(anchor_pos[i].y - anchor_pos[0].y), 2); + M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y)); M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - M_33 += (int64_t)pow((int64_t)(anchor_pos[i].z - anchor_pos[0].z), 2); + M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z)); /* Vector */ - temp = (int64_t)((int64_t)pow(distances_cm[0], 2) - (int64_t)pow(distances_cm[i], 2) - + (int64_t)pow(anchor_pos[i].x, 2) + (int64_t)pow(anchor_pos[i].y, 2) - + (int64_t)pow(anchor_pos[i].z, 2) - (int64_t)pow(anchor_pos[0].x, 2) - - (int64_t)pow(anchor_pos[0].y, 2) - (int64_t)pow(anchor_pos[0].z, 2)); + temp = sq(distances_cm[0]) - sq(distances_cm[i]) + + sq(anchor_pos[i].x) + sq(anchor_pos[i].y) + + sq(anchor_pos[i].z) - sq(anchor_pos[0].x) + - sq(anchor_pos[0].y) - sq(anchor_pos[0].z); b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp); b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp); diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.h b/src/drivers/uwb/uwb_sr150/uwb_sr150.h index 54ef50b7af..5b42b50e16 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.h +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.h @@ -163,6 +163,8 @@ public: void run() override; private: + static constexpr int64_t sq(int64_t x) { return x * x; } + void parameters_update(); void grid_info_read(position_t *grid); diff --git a/src/lib/pid_design/pid_design.hpp b/src/lib/pid_design/pid_design.hpp index b1c7e9d042..9f5b1396d2 100644 --- a/src/lib/pid_design/pid_design.hpp +++ b/src/lib/pid_design/pid_design.hpp @@ -82,7 +82,7 @@ inline matrix::Vector3f computePidGmvc(const matrix::Vector3f &num, const matrix const float rho = dt / sigma; const float mu = 0.25f * (1.f - delta) + 0.51f * delta; // mu is in the interval [0.25 0.51] const float p1 = -2.f * expf(-rho / (2.f * mu)) * cosf(sqrtf(4.f * mu - 1.f) * rho / (2.f * mu)); - const float p2 = exp(-rho / mu); + const float p2 = expf(-rho / mu); const float e1 = -a1 + p1 + 1.f; const float f0 = -a1 * e1 + a1 - a2 + e1 + p2; const float f1 = a1 * e1 - a2 * e1 + a2; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index 46b670d4f8..880be632d1 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -193,7 +193,7 @@ void TargetEstimator::update_filter_gains(filter_gains_s &filter_gains) const // The "G" gain is equivalent to "(1-responsiveness)", but beta is required for H and K gains // From alpha-beta-gamma filter equations: G = 1-beta^3 // Therefore: beta = (1-Gp)^(1/3) = (1-(1-responsiveness))^(1/3) = (r)^(1/3) - const float beta_p = pow((filter_gains.responsiveness), 1.0f / 3.0f); + const float beta_p = powf((filter_gains.responsiveness), 1.0f / 3.0f); const float beta_v = 0.9f * beta_p; // velocity fusion gain is slightly lower. TODO: individual parameter? // Estimator gains for horizontal position update