Diff: util.cpp

Revision:

12:e9ec35413787

Parent:

8:cd489b7c49a0

--- a/util.cpp	Wed Aug 22 21:25:52 2018 +0000
+++ b/util.cpp	Thu Aug 23 14:23:46 2018 +0000
@@ -10,6 +10,44 @@
 #include "mbed.h"
 #include <string>
 
+
+/* Helper function for printing floats & doubles */
+static char *print_double(char* str, double v, int decimalDigits)
+{
+  int i = 1;
+  int intPart, fractPart;
+  int len;
+  char *ptr;
+
+  /* prepare decimal digits multiplicator */
+  for (;decimalDigits!=0; i*=10, decimalDigits--);
+
+  /* calculate integer & fractinal parts */
+  intPart = (int)v;
+  fractPart = (int)((v-(double)(int)v)*i);
+
+  /* fill in integer part */
+  sprintf(str, "%i.", intPart);
+
+  /* prepare fill in of fractional part */
+  len = strlen(str);
+  ptr = &str[len];
+
+  /* fill in leading fractional zeros */
+  for (i/=10;i>1; i/=10, ptr++) {
+    if (fractPart >= i) {
+      break;
+    }
+    *ptr = '0';
+  }
+
+  /* fill in (rest of) fractional part */
+  sprintf(ptr, "%i", fractPart);
+
+  return str;
+}
+
+
 // Convert a byte to string
 string binToString (unsigned char *inputData, int dataLength)
 {
@@ -25,3 +63,29 @@
     for (i = 0; i < len; i++) 
         printf("%02x", buf[i]); printf("\n"); 
 }
+
+
+// LITTLE ENDIAN!
+uint16_t read16(uint8_t *src)
+{
+  uint16_t d;
+  uint8_t b;
+  b = *src;
+  d = *(src+1);
+  d <<= 8;
+  d |= b;
+  return d;
+}
+
+// LITTLE ENDIAN!
+uint32_t read32(uint8_t *src)
+{
+  uint32_t d;
+  uint16_t b;
+
+  b = read16(src);
+  d = read16(src+2);
+  d <<= 16;
+  d |= b;
+  return d;
+}
\ No newline at end of file