From 513900fb16b41026613725194871392b01ba5b09 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 9 Jun 2016 12:51:04 +0200
Subject: [PATCH] printf log: colorize output under POSIX & tty output
 according to the log level

---
 src/platforms/posix/main.cpp            |  1 +
 src/platforms/posix/px4_layer/px4_log.c |  2 ++
 src/platforms/px4_log.h                 | 39 ++++++++++++++++++++++++-
 3 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 6d9d33e7e2..ccb9aaf494 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -377,6 +377,7 @@ int main(int argc, char **argv)
 					cout << c;
 					mystr += c;
 				}
+
 				break;
 			}
 		}
diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c
index 074ae8b466..88406a8854 100644
--- a/src/platforms/posix/px4_layer/px4_log.c
+++ b/src/platforms/posix/px4_layer/px4_log.c
@@ -7,6 +7,8 @@
 __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME;
 
 __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" };
+__EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] =
+	{ PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED };
 
 void px4_backtrace()
 {
diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h
index f9e47196e2..b912f7dad3 100644
--- a/src/platforms/px4_log.h
+++ b/src/platforms/px4_log.h
@@ -130,7 +130,8 @@ static inline void do_nothing(int level, ...)
 __BEGIN_DECLS
 __EXPORT extern uint64_t hrt_absolute_time(void);
 
-__EXPORT extern const char *__px4_log_level_str[5];
+__EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1];
+__EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1];
 __EXPORT extern int __px4_log_level_current;
 __EXPORT extern void px4_backtrace(void);
 __END_DECLS
@@ -171,6 +172,38 @@ __END_DECLS
 #define __px4__log_file_and_line_arg 	, __FILE__, __LINE__
 #define __px4__log_end_fmt 		"\n"
 
+#define PX4_ANSI_COLOR_RED     "\x1b[31m"
+#define PX4_ANSI_COLOR_GREEN   "\x1b[32m"
+#define PX4_ANSI_COLOR_YELLOW  "\x1b[33m"
+#define PX4_ANSI_COLOR_BLUE    "\x1b[34m"
+#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m"
+#define PX4_ANSI_COLOR_CYAN    "\x1b[36m"
+#define PX4_ANSI_COLOR_GRAY    "\x1B[37m"
+#define PX4_ANSI_COLOR_RESET   "\x1b[0m"
+
+#ifdef __PX4_POSIX
+#define PX4_LOG_COLORIZED_OUTPUT //if defined and output is a tty, colorize the output according to the log level
+#endif /* __PX4_POSIX */
+
+
+#ifdef PX4_LOG_COLORIZED_OUTPUT
+#include <unistd.h>
+#define PX4_LOG_COLOR_START \
+	int use_color = isatty(STDOUT_FILENO); \
+	if (use_color) printf("%s", __px4_log_level_color[level]);
+#define PX4_LOG_COLOR_MODULE \
+	if (use_color) printf(PX4_ANSI_COLOR_GRAY);
+#define PX4_LOG_COLOR_MESSAGE \
+	if (use_color) printf("%s", __px4_log_level_color[level]);
+#define PX4_LOG_COLOR_END \
+	if (use_color) printf(PX4_ANSI_COLOR_RESET);
+#else
+#define PX4_LOG_COLOR_START
+#define PX4_LOG_COLOR_MODULE
+#define PX4_LOG_COLOR_MESSAGE
+#define PX4_LOG_COLOR_END
+#endif /* PX4_LOG_COLORIZED_OUTPUT */
+
 /****************************************************************************
  * Output format macros
  * Use these to implement the code level macros below
@@ -235,12 +268,16 @@ __END_DECLS
 static inline void __px4_log_modulename(int level, const char *fmt, ...)
 {
 	if (level <= __px4_log_level_current) {
+		PX4_LOG_COLOR_START
 		printf(__px4__log_level_fmt __px4__log_level_arg(level));
+		PX4_LOG_COLOR_MODULE
 		printf(__px4__log_modulename_fmt __px4__log_modulename_arg);
+		PX4_LOG_COLOR_MESSAGE
 		va_list argptr;
 		va_start(argptr, fmt);
 		vprintf(fmt, argptr);
 		va_end(argptr);
+		PX4_LOG_COLOR_END
 		printf("\n");
 	}
 }