From 88a1e9443e23ce43f8e2d33f7bb535ab1d16ffaa Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Sun, 11 Mar 2012 17:02:06 +0000
Subject: Merged enhanced chprintf().

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4034 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/various/chprintf.c | 54 ++++++++++++++++++++++++++++++++++++++++++++++++---
 os/various/chprintf.h |  7 +++++++
 2 files changed, 58 insertions(+), 3 deletions(-)

(limited to 'os/various')

diff --git a/os/various/chprintf.c b/os/various/chprintf.c
index 3936226c1..d7472b3a4 100644
--- a/os/various/chprintf.c
+++ b/os/various/chprintf.c
@@ -17,25 +17,42 @@
     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
+/*
+   Concepts and parts of this file have been contributed by Fabio Utzig.
+ */
 
 #include <stdarg.h>
 
 #include "ch.h"
+#include "chprintf.h"
 
 #define MAX_FILLER 11
+#define FLOAT_PRECISION 100000
 
-static char *ltoa(char *p, long num, unsigned radix) {
+static char *long_to_string_with_divisor(char *p,
+                                         long num,
+                                         unsigned radix,
+                                         long divisor) {
   int i;
   char *q;
+  long l, ll;
+
+  l = num;
+  if (divisor == 0) {
+    ll = num;
+  } else {
+    ll = divisor;
+  }
 
   q = p + MAX_FILLER;
   do {
-    i = (int)(num % radix);
+    i = (int)(l % radix);
     i += '0';
     if (i > '9')
       i += 'A' - '0' - 10;
     *--q = i;
-  } while ((num /= radix) != 0);
+    l /= radix;
+  } while ((ll /= radix) != 0);
 
   i = (int)(p + MAX_FILLER - q);
   do
@@ -45,6 +62,24 @@ static char *ltoa(char *p, long num, unsigned radix) {
   return p;
 }
 
+static char *ltoa(char *p, long num, unsigned radix) {
+
+  return long_to_string_with_divisor(p, num, radix, 0);
+}
+
+#if CHPRINTF_USE_FLOAT
+static char *ftoa(char *p, double num) {
+  long l;
+  unsigned long precision = FLOAT_PRECISION;
+
+  l = num;
+  p = long_to_string_with_divisor(p, l, 10, 0);
+  *p++ = '.';
+  l = (num - l) * precision;
+  return long_to_string_with_divisor(p, l, 10, precision / 10);
+}
+#endif
+
 /**
  * @brief   System formatted output function.
  * @details This function implements a minimal @p printf() like functionality
@@ -75,6 +110,9 @@ void chprintf(BaseChannel *chp, const char *fmt, ...) {
   int i, precision, width;
   bool_t is_long, left_align;
   long l;
+#if CHPRINTF_USE_FLOAT
+  float f;
+#endif
 
   va_start(ap, fmt);
   while (TRUE) {
@@ -160,6 +198,16 @@ void chprintf(BaseChannel *chp, const char *fmt, ...) {
       }
       p = ltoa(p, l, 10);
       break;
+#if CHPRINTF_USE_FLOAT
+    case 'f':
+      f = (float) va_arg(ap, double);
+      if (f < 0) {
+        *p++ = '-';
+        f = -f;
+      }
+      p = ftoa(p, f);
+      break;
+#endif
     case 'X':
     case 'x':
       c = 16;
diff --git a/os/various/chprintf.h b/os/various/chprintf.h
index 929da639e..866dfa067 100644
--- a/os/various/chprintf.h
+++ b/os/various/chprintf.h
@@ -29,6 +29,13 @@
 #ifndef _CHPRINTF_H_
 #define _CHPRINTF_H_
 
+/**
+ * @brief   Float type support.
+ */
+#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
+#define CHPRINTF_USE_FLOAT          FALSE
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif
-- 
cgit v1.2.3