Merge remote-tracking branch 'upstream/master'

3 files changed, 134 insertions, 66 deletions

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)