Merge remote-tracking branch 'upstream/master'

6 files changed, 335 insertions, 210 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index f2e7ca67d..ad219cd25 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate)
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
-			1);
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
@@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate)
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
 		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
-			0);
+			UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
 		// /* insist of receiving the ACK for this packet */
 		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
 		// 	continue;
@@ -224,35 +224,18 @@ UBX::receive(unsigned timeout)
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
-	uint8_t buf[32];
+	uint8_t buf[128];
 
 	/* timeout additional to poll */
 	uint64_t time_started = hrt_absolute_time();
 
-	int j = 0;
 	ssize_t count = 0;
 
-	while (true) {
-
-		/* pass received bytes to the packet decoder */
-		while (j < count) {
-			if (parse_char(buf[j]) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
-				 * if a packet has arrived */
-				 if (handle_message() > 0)
-					return 1;
-			}
-			/* in case we keep trying but only get crap from GPS */
-			if (time_started + timeout*1000 < hrt_absolute_time() ) {
-				return -1;
-			}
-			j++;
-		}
+	bool position_updated = false;
 
-		/* everything is read */
-		j = count = 0;
+	while (true) {
 
-		/* then poll for new data */
+		/* poll for new data */
 		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
 
 		if (ret < 0) {
@@ -272,8 +255,26 @@ UBX::receive(unsigned timeout)
 				 * available, we'll go back to poll() again...
 				 */
 				count = ::read(_fd, buf, sizeof(buf));
+						/* pass received bytes to the packet decoder */
+				for (int i = 0; i < count; i++) {
+					if (parse_char(buf[i])) {
+						/* return to configure during configuration or to the gps driver during normal work
+						 * if a packet has arrived */
+						if (handle_message())
+							position_updated = true;
+					}
+				}
 			}
 		}
+
+		/* return success after receiving a packet */
+		if (position_updated)
+			return 1;
+
+		/* abort after timeout if no packet parsed successfully */
+		if (time_started + timeout*1000 < hrt_absolute_time() ) {
+			return -1;
+		}
 	}
 }
 
@@ -327,6 +328,7 @@ UBX::parse_char(uint8_t b)
 			}
 			break;
 		case UBX_DECODE_GOT_CLASS:
+			{
 			add_byte_to_checksum(b);
 			switch (_message_class) {
 				case NAV:
@@ -413,6 +415,14 @@ UBX::parse_char(uint8_t b)
 //					config_needed = true;
 					break;
 				}
+				// Evaluate state machine - if the state changed,
+				// the state machine was reset via decode_init()
+				// and we want to tell the module to stop sending this message
+
+				// disable unknown message
+				//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
+				//configure_message_rate(_message_class, b, 0);
+			}
 				break;
 			case UBX_DECODE_GOT_MESSAGEID:
 				add_byte_to_checksum(b);
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index df1958186..220842536 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -272,6 +272,11 @@ private:
 	 */
 	void _set_dlpf_filter(uint16_t frequency_hz);
 
+	/*
+	  set sample rate (approximate) - 1kHz to 5Hz
+	*/
+	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+
 };
 
 /**
@@ -378,7 +383,8 @@ MPU6000::init()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	_set_sample_rate(200); // default sample rate = 200Hz
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -494,6 +500,18 @@ MPU6000::probe()
 }
 
 /*
+  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
+*/
+void
+MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+{
+  uint8_t div = 1000 / desired_sample_rate_hz;
+  if(div>200) div=200;
+  if(div<1) div=1;
+  write_reg(MPUREG_SMPLRT_DIV, div-1);
+}
+
+/*
   set the DLPF filter frequency. This affects both accel and gyro.
  */
 void
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSAMPLERATE:
 	case ACCELIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSAMPLERATE:
 	case GYROIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 7d3812abd..52dac652b 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,42 +44,42 @@
 /* Extended Kalman Filter covariances */
 
 /* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
 /* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f);
 /* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f);
 
 /* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
-	h->q0 	=	param_find("EKF_ATT_V2_Q0");
-	h->q1 	=	param_find("EKF_ATT_V2_Q1");
-	h->q2 	=	param_find("EKF_ATT_V2_Q2");
-	h->q3 	=	param_find("EKF_ATT_V2_Q3");
-	h->q4 	=	param_find("EKF_ATT_V2_Q4");
+	h->q0 	=	param_find("EKF_ATT_V3_Q0");
+	h->q1 	=	param_find("EKF_ATT_V3_Q1");
+	h->q2 	=	param_find("EKF_ATT_V3_Q2");
+	h->q3 	=	param_find("EKF_ATT_V3_Q3");
+	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
-	h->r0 	=	param_find("EKF_ATT_V2_R0");
-	h->r1 	=	param_find("EKF_ATT_V2_R1");
-	h->r2 	=	param_find("EKF_ATT_V2_R2");
-	h->r3 	=	param_find("EKF_ATT_V2_R3");
+	h->r0 	=	param_find("EKF_ATT_V3_R0");
+	h->r1 	=	param_find("EKF_ATT_V3_R1");
+	h->r2 	=	param_find("EKF_ATT_V3_R2");
+	h->r3 	=	param_find("EKF_ATT_V3_R3");
 
-	h->roll_off  =	param_find("ATT_ROLL_OFFS");
-	h->pitch_off =	param_find("ATT_PITCH_OFFS");
-	h->yaw_off   =	param_find("ATT_YAW_OFFS");
+	h->roll_off  =	param_find("ATT_ROLL_OFF3");
+	h->pitch_off =	param_find("ATT_PITCH_OFF3");
+	h->yaw_off   =	param_find("ATT_YAW_OFF3");
 
 	return OK;
 }
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 43cbe74c7..8b4c0cb30 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -54,9 +54,15 @@
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 
+#define PX4IO_RELAY1	(1<<0)
+#define PX4IO_RELAY2	(1<<1)
+#define PX4IO_ACC1		(1<<2)
+#define PX4IO_ACC2		(1<<3)
+
 struct gpio_led_s {
 	struct work_s work;
 	int gpio_fd;
+	bool use_io;
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
@@ -75,51 +81,97 @@ void gpio_led_cycle(FAR void *arg);
 
 int gpio_led_main(int argc, char *argv[])
 {
-	int pin = GPIO_EXT_1;
-
 	if (argc < 2) {
-		errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
+		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
+		     "\t-p\tUse pin:\n"
+		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
+		     "\t\t2\tPX4FMU GPIO_EXT2\n"
+		     "\t\ta1\tPX4IO ACC1\n"
+		     "\t\ta2\tPX4IO ACC2\n"
+		     "\t\tr1\tPX4IO RELAY1\n"
+		     "\t\tr2\tPX4IO RELAY2");
 
 	} else {
 
-		/* START COMMAND HANDLING */
 		if (!strcmp(argv[1], "start")) {
+			if (gpio_led_started) {
+				errx(1, "already running");
+			}
+
+			bool use_io = false;
+			int pin = GPIO_EXT_1;
 
 			if (argc > 2) {
-				if (!strcmp(argv[1], "-p")) {
-					if (!strcmp(argv[2], "1")) {
+				if (!strcmp(argv[2], "-p")) {
+					if (!strcmp(argv[3], "1")) {
+						use_io = false;
 						pin = GPIO_EXT_1;
 
-					} else if (!strcmp(argv[2], "2")) {
+					} else if (!strcmp(argv[3], "2")) {
+						use_io = false;
 						pin = GPIO_EXT_2;
 
+					} else if (!strcmp(argv[3], "a1")) {
+						use_io = true;
+						pin = PX4IO_ACC1;
+
+					} else if (!strcmp(argv[3], "a2")) {
+						use_io = true;
+						pin = PX4IO_ACC2;
+
+					} else if (!strcmp(argv[3], "r1")) {
+						use_io = true;
+						pin = PX4IO_RELAY1;
+
+					} else if (!strcmp(argv[3], "r2")) {
+						use_io = true;
+						pin = PX4IO_RELAY2;
+
 					} else {
-						warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
-						exit(1);
+						errx(1, "unsupported pin: %s", argv[3]);
 					}
 				}
 			}
 
 			memset(&gpio_led_data, 0, sizeof(gpio_led_data));
+			gpio_led_data.use_io = use_io;
 			gpio_led_data.pin = pin;
 			int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
 
 			if (ret != 0) {
-				warnx("[gpio_led] Failed to queue work: %d\n", ret);
-				exit(1);
+				errx(1, "failed to queue work: %d", ret);
 
 			} else {
 				gpio_led_started = true;
+				char pin_name[24];
+
+				if (use_io) {
+					if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
+
+					} else {
+						sprintf(pin_name, "PX4IO RELAY%i", pin);
+					}
+
+				} else {
+					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
+
+				}
+
+				warnx("start, using pin: %s", pin_name);
 			}
 
 			exit(0);
 
-			/* STOP COMMAND HANDLING */
 
 		} else if (!strcmp(argv[1], "stop")) {
-			gpio_led_started = false;
+			if (gpio_led_started) {
+				gpio_led_started = false;
+				warnx("stop");
 
-			/* INVALID COMMAND */
+			} else {
+				errx(1, "not running");
+			}
 
 		} else {
 			errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@@ -131,11 +183,22 @@ void gpio_led_start(FAR void *arg)
 {
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
+	char *gpio_dev;
+
+	if (priv->use_io) {
+		gpio_dev = "/dev/px4io";
+
+	} else {
+		gpio_dev = "/dev/px4fmu";
+	}
+
 	/* open GPIO device */
-	priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
+	priv->gpio_fd = open(gpio_dev, 0);
 
 	if (priv->gpio_fd < 0) {
-		warnx("[gpio_led] GPIO: open fail\n");
+		// TODO find way to print errors
+		//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
+		gpio_led_started = false;
 		return;
 	}
 
@@ -150,11 +213,11 @@ void gpio_led_start(FAR void *arg)
 	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
 
 	if (ret != 0) {
-		warnx("[gpio_led] Failed to queue work: %d\n", ret);
+		// TODO find way to print errors
+		//printf("gpio_led: failed to queue work: %d\n", ret);
+		gpio_led_started = false;
 		return;
 	}
-
-	warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
 }
 
 void gpio_led_cycle(FAR void *arg)
@@ -211,7 +274,12 @@ void gpio_led_cycle(FAR void *arg)
 	if (priv->counter > 5)
 		priv->counter = 0;
 
-	/* repeat cycle at 5 Hz*/
-	if (gpio_led_started)
+	/* repeat cycle at 5 Hz */
+	if (gpio_led_started) {
 		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+
+	} else {
+		/* switch off LED on stop */
+		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
+	}
 }
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 8fdff8ac0..afc5b072c 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
-//	if (!system_load.initialized)
-//	{
 	system_load.start_time = hrt_absolute_time();
 	int i;
 
@@ -80,27 +78,29 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	system_load.total_count = 0;
-
 	uint64_t now = hrt_absolute_time();
 
-	/* initialize idle thread statically */
-	system_load.tasks[0].start_time = now;
-	system_load.tasks[0].total_runtime = 0;
-	system_load.tasks[0].curr_start_time = 0;
-	system_load.tasks[0].tcb = sched_gettcb(0);
-	system_load.tasks[0].valid = true;
-	system_load.total_count++;
-
-	/* initialize init thread statically */
-	system_load.tasks[1].start_time = now;
-	system_load.tasks[1].total_runtime = 0;
-	system_load.tasks[1].curr_start_time = 0;
-	system_load.tasks[1].tcb = sched_gettcb(1);
-	system_load.tasks[1].valid = true;
-	/* count init thread */
-	system_load.total_count++;
-	//	}
+	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
+
+#ifdef CONFIG_PAGING
+	static_tasks_count++;	// include paging thread in initialization
+#endif /* CONFIG_PAGING */
+#if CONFIG_SCHED_WORKQUEUE
+	static_tasks_count++;	// include high priority work0 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+#if CONFIG_SCHED_LPWORK
+	static_tasks_count++;	// include low priority work1 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+
+	// perform static initialization of "system" threads
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
+	{
+		system_load.tasks[system_load.total_count].start_time = now;
+		system_load.tasks[system_load.total_count].total_runtime = 0;
+		system_load.tasks[system_load.total_count].curr_start_time = 0;
+		system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
+		system_load.tasks[system_load.total_count].valid = true;
+	}
 }
 
 void sched_note_start(FAR struct tcb_s *tcb)
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index efe62685c..0d064a05e 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -51,19 +51,46 @@
 #include <systemlib/cpuload.h>
 #include <drivers/drv_hrt.h>
 
+#define CL "\033[K" // clear line
+
 /**
  * Start the top application.
  */
-__EXPORT int top_main(int argc, char *argv[]);
+__EXPORT int top_main(void);
 
 extern struct system_load_s system_load;
 
-bool top_sigusr1_rcvd = false;
-
-int top_main(int argc, char *argv[])
+static const char *
+tstate_name(const tstate_t s)
 {
-	int t;
+	switch (s) {
+	case TSTATE_TASK_INVALID:    return "init";
+
+	case TSTATE_TASK_PENDING:    return "PEND";
+	case TSTATE_TASK_READYTORUN: return "READY";
+	case TSTATE_TASK_RUNNING:    return "RUN";
+
+	case TSTATE_TASK_INACTIVE:   return "inact";
+	case TSTATE_WAIT_SEM:        return "w:sem";
+#ifndef CONFIG_DISABLE_SIGNALS
+	case TSTATE_WAIT_SIG:        return "w:sig";
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+	case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
+	case TSTATE_WAIT_MQNOTFULL:  return "w:mqf";
+#endif
+#ifdef CONFIG_PAGING
+	case TSTATE_WAIT_PAGEFILL:   return "w:pgf";
+#endif
+
+	default:
+		return "ERROR";
+	}
+}
 
+int
+top_main(void)
+{
 	uint64_t total_user_time = 0;
 
 	int running_count = 0;
@@ -75,7 +102,7 @@ int top_main(int argc, char *argv[])
 	uint64_t last_times[CONFIG_MAX_TASKS];
 	float curr_loads[CONFIG_MAX_TASKS];
 
-	for (t = 0; t < CONFIG_MAX_TASKS; t++)
+	for (int t = 0; t < CONFIG_MAX_TASKS; t++)
 		last_times[t] = 0;
 
 	float interval_time_ms_inv = 0.f;
@@ -83,16 +110,16 @@ int top_main(int argc, char *argv[])
 	/* Open console directly to grab CTRL-C signal */
 	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
 
-	while (true)
-//	for (t = 0; t < 10; t++)
-	{
-		int i;
+	/* clear screen */
+	printf("\033[2J");
 
-		uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
-		unsigned int curr_time_s = curr_time_ms / 1000LLU;
+	for (;;) {
+		int   i;
+		uint64_t curr_time_us;
+		uint64_t idle_time_us;
 
-		uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
-		unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
+		curr_time_us = hrt_absolute_time();
+		idle_time_us = system_load.tasks[0].total_runtime;
 
 		if (new_time > interval_start_time)
 			interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
@@ -102,7 +129,38 @@ int top_main(int argc, char *argv[])
 		total_user_time = 0;
 
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
-			uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
+			uint64_t interval_runtime;
+
+			if (system_load.tasks[i].valid) {
+				switch (system_load.tasks[i].tcb->task_state) {
+				case TSTATE_TASK_PENDING:
+				case TSTATE_TASK_READYTORUN:
+				case TSTATE_TASK_RUNNING:
+					running_count++;
+					break;
+
+				case TSTATE_TASK_INVALID:
+				case TSTATE_TASK_INACTIVE:
+				case TSTATE_WAIT_SEM:
+#ifndef CONFIG_DISABLE_SIGNALS
+				case TSTATE_WAIT_SIG:
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+				case TSTATE_WAIT_MQNOTEMPTY:
+				case TSTATE_WAIT_MQNOTFULL:
+#endif
+#ifdef CONFIG_PAGING
+				case TSTATE_WAIT_PAGEFILL:
+#endif
+					blocked_count++;
+					break;
+				}
+			}
+
+			interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
+								system_load.tasks[i].total_runtime > last_times[i])
+				? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
+				: 0;
 
 			last_times[i] = system_load.tasks[i].total_runtime;
 
@@ -111,7 +169,6 @@ int top_main(int argc, char *argv[])
 
 				if (i > 0)
 					total_user_time += interval_runtime;
-
 			} else
 				curr_loads[i] = 0;
 		}
@@ -119,127 +176,99 @@ int top_main(int argc, char *argv[])
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
 			if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
 				if (system_load.tasks[i].tcb->pid == 0) {
-					float idle = curr_loads[0];
-					float task_load = (float)(total_user_time) * interval_time_ms_inv;
+					float idle;
+					float task_load;
+					float sched_load;
 
-					if (task_load > (1.f - idle)) task_load = (1.f - idle);	/* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
+					idle = curr_loads[0];
+					task_load = (float)(total_user_time) * interval_time_ms_inv;
 
-					float sched_load = 1.f - idle - task_load;
+					/* this can happen if one tasks total runtime was not computed
+					   correctly by the scheduler instrumentation TODO */
+					if (task_load > (1.f - idle))
+						task_load = (1.f - idle);
+
+					sched_load = 1.f - idle - task_load;
 
 					/* print system information */
-					printf("\033[H"); /* cursor home */
-					printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
-					printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
-					printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
-
-					/* 34 chars command name length (32 chars plus two spaces) */
-					char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
-					memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
-					header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
+					printf("\033[H"); /* move cursor home and clear screen */
+					printf(CL "Processes: %d total, %d running, %d sleeping\n",
+						   system_load.total_count,
+						   running_count,
+						   blocked_count);
+					printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
+						   (double)(task_load * 100.f),
+						   (double)(sched_load * 100.f),
+						   (double)(idle * 100.f));
+					printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
+						   (double)curr_time_us / 1000000.d,
+						   (double)idle_time_us / 1000000.d);
+
+					/* header for task list */
+					printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
+						   "PID",
+						   CONFIG_TASK_NAME_SIZE, "COMMAND",
+						   "CPU(ms)",
+						   "CPU(%)",
+						   "USED/STACK",
+						   "PRIO(BASE)",
 #if CONFIG_RR_INTERVAL > 0
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+						   "TSLICE"
 #else
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
-#endif
-
-				} else {
-					enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
-
-					if (task_state == TSTATE_TASK_PENDING ||
-					    task_state == TSTATE_TASK_READYTORUN ||
-					    task_state == TSTATE_TASK_RUNNING) {
-						running_count++;
-					}
-
-					if (task_state == TSTATE_TASK_INACTIVE ||      /* BLOCKED      - Initialized but not yet activated */
-					    task_state == TSTATE_WAIT_SEM           /* BLOCKED      - Waiting for a semaphore */
-#ifndef CONFIG_DISABLE_SIGNALS
-					    || task_state == TSTATE_WAIT_SIG           /* BLOCKED      - Waiting for a signal */
+						   "STATE"
 #endif
-#ifndef CONFIG_DISABLE_MQUEUE
-					    || task_state == TSTATE_WAIT_MQNOTEMPTY     /* BLOCKED      - Waiting for a MQ to become not empty. */
-					    || task_state == TSTATE_WAIT_MQNOTFULL      /* BLOCKED      - Waiting for a MQ to become not full. */
-#endif
-#ifdef CONFIG_PAGING
-					    || task_state == TSTATE_WAIT_PAGEFILL       /* BLOCKED      - Waiting for page fill */
-#endif
-					   ) {
-						blocked_count++;
-					}
-
-					char spaces[CONFIG_TASK_NAME_SIZE + 2];
-
-					/* count name len */
-					int namelen = 0;
-
-					while (namelen < CONFIG_TASK_NAME_SIZE) {
-						if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
-
-						namelen++;
-					}
-
-					int s = 0;
-
-					for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
-						spaces[s] = ' ';
-					}
-
-					spaces[s] = '\0';
-
-					char *runtime_spaces = "  ";
+						   );
+				}
 
-					if ((system_load.tasks[i].total_runtime / 1000) < 99) {
-						runtime_spaces = "";
-					}
+				unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+					(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+				unsigned stack_free = 0;
+				uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
 
-					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
-							      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
-					unsigned stack_free = 0;
-					uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+				while (stack_free < stack_size) {
+					if (*stack_sweeper++ != 0xff)
+						break;
 
-					while (stack_free < stack_size) {
-						if (*stack_sweeper++ != 0xff)
-							break;
+					stack_free++;
+				}
 
-						stack_free++;
-					}
+				printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
+					   system_load.tasks[i].tcb->pid,
+					   CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
+					   (system_load.tasks[i].total_runtime / 1000),
+					   (int)(curr_loads[i] * 100),
+					   (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					   stack_size - stack_free,
+					   stack_size,
+					   system_load.tasks[i].tcb->sched_priority,
+					   system_load.tasks[i].tcb->base_priority);
 
-					printf("\033[K % 2d\t%s%s % 8lld ms%s  \t % 2d.%03d \t % 4u / % 4u",
-					       (int)system_load.tasks[i].tcb->pid,
-					       system_load.tasks[i].tcb->name,
-					       spaces,
-					       (system_load.tasks[i].total_runtime / 1000),
-					       runtime_spaces,
-					       (int)(curr_loads[i] * 100),
-					       (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
-					       stack_size - stack_free,
-					       stack_size);
-					/* Print scheduling info with RR time slice */
 #if CONFIG_RR_INTERVAL > 0
-					printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
+				/* print scheduling info with RR time slice */
+				printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
 #else
-					/* Print scheduling info without time slice*/
-					printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
+				// print task state instead
+				printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
 #endif
-				}
 			}
 		}
 
-		printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
-		fflush(stdout);
-
 		interval_start_time = new_time;
 
-		char c;
-
-		/* Sleep 200 ms waiting for user input four times */
+		/* Sleep 200 ms waiting for user input five times ~ 1s */
 		/* XXX use poll ... */
-		for (int k = 0; k < 4; k++) {
+		for (int k = 0; k < 5; k++) {
+			char c;
+
 			if (read(console, &c, 1) == 1) {
-				if (c == 0x03 || c == 0x63) {
-					printf("Abort\n");
+				switch (c) {
+				case 0x03: // ctrl-c
+				case 0x1b: // esc
+				case 'c':
+				case 'q':
 					close(console);
 					return OK;
+					/* not reached */
 				}
 			}