UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-25 12:09:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor output_limit: split output_limit_calc_single into separate function

Browse Source
This commit is contained in:
Beat Küng
2021-09-14 13:00:22 +02:00
committed by Daniel Agar
parent c55626d2d6
commit 15daded6b0
3 changed files with 32 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
platforms/common/include/px4_platform_common/defines.h
View File

@@ -50,8 +50,10 @@
/* Define PX4_ISFINITE */ /* Define PX4_ISFINITE */
#ifdef __cplusplus #ifdef __cplusplus
constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); } static inline 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(double x) { return __builtin_isfinite(x); }
#else
#define PX4_ISFINITE(x) __builtin_isfinite(x)
#endif /* __cplusplus */ #endif /* __cplusplus */
#if defined(__PX4_NUTTX) #if defined(__PX4_NUTTX)

26
src/lib/output_limit/output_limit.cpp
View File

@@ -33,7 +33,6 @@
#include "output_limit.h" #include "output_limit.h"
#include <px4_platform_common/defines.h>
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.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: case OUTPUT_LIMIT_STATE_ON:
for (unsigned i = 0; i < num_channels; i++) { for (unsigned i = 0; i < num_channels; i++) {
effective_output[i] = output_limit_calc_single(reverse_mask & (1 << i), disarmed_output[i],
float control_value = output[i]; min_output[i], max_output[i], 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];
}
} }
break; break;

26
src/lib/output_limit/output_limit.h
View File

@@ -43,6 +43,7 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include <px4_platform_common/defines.h>
__BEGIN_DECLS __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 uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_output, output_limit_t *limit); 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 __END_DECLS