From 15daded6b012d8fa147b9af01ec25cfa477a8dcb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Tue, 14 Sep 2021 13:00:22 +0200
Subject: [PATCH] refactor output_limit: split output_limit_calc_single into
 separate function

---
 .../include/px4_platform_common/defines.h     |  6 +++--
 src/lib/output_limit/output_limit.cpp         | 26 ++-----------------
 src/lib/output_limit/output_limit.h           | 26 +++++++++++++++++++
 3 files changed, 32 insertions(+), 26 deletions(-)

diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h
index def1786a3a..6c325fa803 100644
--- a/platforms/common/include/px4_platform_common/defines.h
+++ b/platforms/common/include/px4_platform_common/defines.h
@@ -50,8 +50,10 @@
 
 /* Define PX4_ISFINITE */
 #ifdef __cplusplus
-constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
-constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
+static inline constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
+static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
+#else
+#define PX4_ISFINITE(x) __builtin_isfinite(x)
 #endif /* __cplusplus */
 
 #if defined(__PX4_NUTTX)
diff --git a/src/lib/output_limit/output_limit.cpp b/src/lib/output_limit/output_limit.cpp
index c997d18822..cbff58bbb8 100644
--- a/src/lib/output_limit/output_limit.cpp
+++ b/src/lib/output_limit/output_limit.cpp
@@ -33,7 +33,6 @@
 
 #include "output_limit.h"
 
-#include <px4_platform_common/defines.h>
 #include <math.h>
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
@@ -192,29 +191,8 @@ void output_limit_calc(const bool armed, const bool pre_armed, const unsigned nu
 	case OUTPUT_LIMIT_STATE_ON:
 
 		for (unsigned i = 0; i < num_channels; i++) {
-
-			float control_value = output[i];
-
-			/* check for invalid / disabled channels */
-			if (!PX4_ISFINITE(control_value)) {
-				effective_output[i] = disarmed_output[i];
-				continue;
-			}
-
-			if (reverse_mask & (1 << i)) {
-				control_value = -1.0f * control_value;
-			}
-
-			effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2;
-
-			/* last line of defense against invalid inputs */
-			if (effective_output[i] < min_output[i]) {
-				effective_output[i] = min_output[i];
-
-			} else if (effective_output[i] > max_output[i]) {
-				effective_output[i] = max_output[i];
-			}
-
+			effective_output[i] = output_limit_calc_single(reverse_mask & (1 << i), disarmed_output[i],
+					      min_output[i], max_output[i], output[i]);
 		}
 
 		break;
diff --git a/src/lib/output_limit/output_limit.h b/src/lib/output_limit/output_limit.h
index e240ae6a19..0984f79db0 100644
--- a/src/lib/output_limit/output_limit.h
+++ b/src/lib/output_limit/output_limit.h
@@ -43,6 +43,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
+#include <px4_platform_common/defines.h>
 
 __BEGIN_DECLS
 
@@ -76,4 +77,29 @@ __EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const un
 				const uint16_t *min_output, const uint16_t *max_output,
 				const float *output, uint16_t *effective_output, output_limit_t *limit);
 
+static inline uint16_t output_limit_calc_single(bool reversed, uint16_t disarmed_output,
+		uint16_t min_output, uint16_t max_output, float output)
+{
+	/* check for invalid / disabled channels */
+	if (!PX4_ISFINITE(output)) {
+		return disarmed_output;
+	}
+
+	if (reversed) {
+		output = -1.0f * output;
+	}
+
+	uint16_t effective_output = output * (max_output - min_output) / 2 + (max_output + min_output) / 2;
+
+	/* last line of defense against invalid inputs */
+	if (effective_output < min_output) {
+		effective_output = min_output;
+
+	} else if (effective_output > max_output) {
+		effective_output = max_output;
+	}
+
+	return effective_output;
+}
+
 __END_DECLS