printf log: colorize output under POSIX & tty output according to the log level

src/platforms/posix/main.cpp

@@ -377,6 +377,7 @@ int main(int argc, char **argv)
 					cout << c;
 					mystr += c;
 				}
+
 				break;
 			}
 		}

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()
 {

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");
 	}
 }