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
@@ -135,6 +136,18 @@ do {                                              \
       (d) = __u.f;                                \
 } 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
 
diff --git a/stmhal/servo.c b/stmhal/servo.c
index 9c757c5659..893fb11bdb 100644
--- a/stmhal/servo.c
+++ b/stmhal/servo.c
@@ -27,8 +27,6 @@ typedef struct _pyb_servo_obj_t {
     uint16_t pulse_dest;
 } pyb_servo_obj_t;
 
-STATIC const mp_obj_type_t servo_obj_type;
-
 STATIC pyb_servo_obj_t pyb_servo_obj[PYB_SERVO_NUM];
 
 void servo_init(void) {
@@ -36,7 +34,7 @@ void servo_init(void) {
 
     // reset servo objects
     for (int i = 0; i < PYB_SERVO_NUM; i++) {
-        pyb_servo_obj[i].base.type = &servo_obj_type;
+        pyb_servo_obj[i].base.type = &pyb_servo_type;
         pyb_servo_obj[i].servo_id = i + 1;
         pyb_servo_obj[i].time_left = 0;
         pyb_servo_obj[i].pulse_cur = 150; // units of 10us
@@ -149,7 +147,7 @@ STATIC mp_obj_t pyb_servo_make_new(mp_obj_t type_in, uint n_args, uint n_kw, con
 
     // check servo number
     if (!(0 <= servo_id && servo_id < PYB_SERVO_NUM)) {
-        nlr_jump(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "Servo %d does not exist", servo_id));
+        nlr_jump(mp_obj_new_exception_msg_varg(&mp_type_ValueError, "Servo %d does not exist", servo_id + 1));
     }
 
     // get and init servo object