stmhal: Fix servo object; add fpclassify to math functions.

1 files changed, 14 insertions, 1 deletions

diff --git a/stmhal/math.c b/stmhal/math.c
index 8afdc82a64..534389df51 100644
--- a/stmhal/math.c
+++ b/stmhal/math.c
@@ -1,4 +1,6 @@
 #include <stdint.h>
+#include <math.h>
+
 typedef float float_t;
 typedef union {
     float f;
@@ -86,7 +88,6 @@ float erfcf(float x) { return 0.0; }
 float modff(float x, float *y) { return 0.0; }
 float frexpf(float x, int *exp) { return 0.0; }
 float ldexpf(float x, int exp) { return 0.0; }
-int __fpclassifyf(float x) { return 0; }
 
 /*****************************************************************************/
 // from musl-0.9.15 libm.h
@@ -136,6 +137,18 @@ do {                                              \
 } while (0)
 
 /*****************************************************************************/
+// __fpclassifyf from musl-0.9.15
+
+int __fpclassifyf(float x)
+{
+	union {float f; uint32_t i;} u = {x};
+	int e = u.i>>23 & 0xff;
+	if (!e) return u.i<<1 ? FP_SUBNORMAL : FP_ZERO;
+	if (e==0xff) return u.i<<9 ? FP_NAN : FP_INFINITE;
+	return FP_NORMAL;
+}
+
+/*****************************************************************************/
 // scalbnf from musl-0.9.15
 
 float scalbnf(float x, int n)