diff options
Diffstat (limited to 'include/math_util.h')
-rw-r--r-- | include/math_util.h | 24 |
1 files changed, 24 insertions, 0 deletions
diff --git a/include/math_util.h b/include/math_util.h index 9e6b73aac7..942aaf8908 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -56,6 +56,12 @@ static inline fp_t fp_div(fp_t a, fp_t b) { return a / b; } + +/* Don't handle divided-by-zero with FPU, since this should be rare. */ +static inline fp_t fp_div_dbz(fp_t a, fp_t b) +{ + return fp_div(a, b); +} #else /** * Multiplication - return (a * b) @@ -72,6 +78,24 @@ static inline fp_t fp_div(fp_t a, fp_t b) { return (fp_t)(((fp_inter_t)a << FP_BITS) / b); } + +/** + * Division which handles division-by-zero - returns (a / b) if b != 0, + * INT32_MAX if b == 0. + */ +static inline fp_t fp_div_dbz(fp_t a, fp_t b) +{ + /* + * Fixed-point numbers has limited value range. It is very easy to + * be trapped in a divided-by-zero error especially when doing + * magnetometer calculation. We only use fixed-point operations for + * motion sensors now, so the precision and correctness for these + * operations is not the most important point to consider. Here + * we just let divided-by-zero result becomes INT32_MAX, to prevent + * the system failure. + */ + return b == FLOAT_TO_FP(0) ? INT32_MAX : fp_div(a, b); +} #endif /** |