AP_HAL: fix up ftoa_engine to avoid wrong results and hangs

libraries/AP_HAL/tests/test_vsnprintf.cpp

@@ -62,4 +62,55 @@ TEST(vsnprintf_Test, Basic)
     }
 }
 
+static const char* do_subnormal_format(uint32_t val_hex) {
+    // format float represented as a hex number long enough to see all digits
+
+    // note that something else is wrong here and all the strings should be the
+    // same width of 99 chars (or whatever the format string means)! when that
+    // is fixed, update the test and add an assert here.
+
+    static char buf[256];
+
+    float val;
+    static_assert(sizeof(uint32_t) == sizeof(float));
+    memcpy(&val, &val_hex, sizeof(float));
+
+    hal.util->snprintf(buf, ARRAY_SIZE(buf), "%.99f", val);
+
+    return buf;
+}
+
+TEST(vsnprintf_Test, SubnormalFormat)
+{
+    // arbitrary small number (1e-31)
+    // EXPECT_STREQ("0.000000000000000000000000000000099999998000000000000000000000000000000000000000000000000000000000000",
+    EXPECT_STREQ("0.0000000000000000000000000000001000000",
+        do_subnormal_format(0x0C01CEB3));
+
+    // smallest normal (1.1754944e-38)
+    // EXPECT_STREQ("0.000000000000000000000000000000000000011754944000000000000000000000000000000000000000000000000000000",
+    EXPECT_STREQ("0.00000000000000000000000000000000000001175494",
+        do_subnormal_format(0x00800000));
+
+    // largest subnormal (1.1754942e-38)
+    // EXPECT_STREQ("0.000000000000000000000000000000000000011754942000000000000000000000000000000000000000000000000000000",
+    EXPECT_STREQ("0.00000000000000000000000000000000000001175494", // !! same as above
+        do_subnormal_format(0x007FFFFF));
+
+    // moderate subnormal (1.1478e-41)
+    // EXPECT_STREQ("0.000000000000000000000000000000000000000011478036000000000000000000000000000000000000000000000000000",
+    EXPECT_STREQ("0.00000000000000000000000000000000000000001147804",
+        do_subnormal_format(0x00001FFF));
+
+    // smallest subnormal (1e-45)
+    // EXPECT_STREQ("0.000000000000000000000000000000000000000000001401290000000000000000000000000000000000000000000000000",
+    EXPECT_STREQ("0.00000000000000000000000000000000000000000000140130",
+        do_subnormal_format(0x00000001));
+
+    // zero
+    EXPECT_STREQ("0.000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000",
+        do_subnormal_format(0x00000000));
+}
+
+
 AP_GTEST_MAIN()

libraries/AP_HAL/utility/ftoa_engine.cpp

@@ -103,25 +103,26 @@ int16_t ftoa_engine(float val, char *buf, uint8_t precision, uint8_t maxDecimals
     // Read the sign, shift the exponent in place and delete it from frac.
     if (valbits[3] & (1<<7)) flags = FTOA_MINUS; else flags = 0;
     uint8_t exp = valbits[3]<<1;
-    if(valbits[2] & (1<<7)) exp++;    // TODO possible but in case of subnormal
-
-    // Test for easy cases, zero and NaN
-    if(exp==0 && frac==0) {
-        buf[0] = flags | FTOA_ZERO;
-        uint8_t i;
-        for(i=0; i<=precision; i++) {
-            buf[i+1] = '0';
+    if(valbits[2] & (1<<7)) exp++;
+
+    // Test for easy cases
+    if(exp==0) { // Zero or subnormal
+        if(frac == 0) { // Zero
+            buf[0] = flags | FTOA_ZERO;
+            uint8_t i;
+            for(i=0; i<=precision; i++) {
+                buf[i+1] = '0';
+            }
+            return 0;
+        } else { // Subnormal
+            exp = 1; // Subnormal mantissa has same significance as exp=1
         }
-        return 0;
-    }
-
-    if(exp == 0xff) {
-        if(frac == 0) flags |= FTOA_INF; else flags |= FTOA_NAN;
+    } else if(exp==0xff) { // Infinity or NaN
+        if(frac==0) flags |= FTOA_INF; else flags |= FTOA_NAN;
+    } else { // Normal number
+        frac |= (1UL<<23); // The implicit leading 1 is made explicit
     }
 
-    // The implicit leading 1 is made explicit, except if value subnormal.
-    if (exp != 0) frac |= (1UL<<23);
-
     uint8_t idx = exp>>3;
     int8_t exp10 = exponentTable[idx];
 
@@ -145,6 +146,12 @@ int16_t ftoa_engine(float val, char *buf, uint8_t precision, uint8_t maxDecimals
 
     do {
         char digit = '0';
+        if(decimal == 0) {
+            // Abnormal case, can't ever add enough digits to exit the loop,
+            // bail out. This will happen for subnormals with frac <= 7 and
+            // precision == 7.
+            break;
+        }
         while(1) {// find the first nonzero digit or any of the next digits.
             while ((prod -= decimal) >= 0)
                 digit++;