Fixes and style, deamonized filter

5 files changed, 166 insertions, 204 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 58d415d86..2e294226a 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -87,9 +87,6 @@ static float P_aposteriori_k[81] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
 				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,
 				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,
 				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
 				  };
 static float P_aposteriori[81] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
 				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,
@@ -100,9 +97,6 @@ static float P_aposteriori[81] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
 				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,
 				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,
 				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,   0,
 				  };	/**< init: diagonal matrix with big values */
 static float knownConst[20] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
 static float Rot_matrix[9] = {1.f,  0,  0,
@@ -110,6 +104,73 @@ static float Rot_matrix[9] = {1.f,  0,  0,
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int attitude_estimator_ekf_task;				/**< Handle of deamon task / thread */
+
+/**
+ * Mainloop of attitude_estimator_ekf.
+ */
+int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The attitude_estimator_ekf app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int attitude_estimator_ekf_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("attitude_estimator_ekf already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		attitude_estimator_ekf_task = task_create("attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 4096, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tattitude_estimator_ekf app is running\n");
+		} else {
+			printf("\tattitude_estimator_ekf app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
 /*
  * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
  */
@@ -122,7 +183,7 @@ static float Rot_matrix[9] = {1.f,  0,  0,
  * @param argc number of commandline arguments (plus command name)
  * @param argv strings containing the arguments
  */
-int attitude_estimator_ekf_main(int argc, char *argv[])
+int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 {
 	// print text
 	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
@@ -136,8 +197,8 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 	/* store start time to guard against too slow update rates */
 	uint64_t last_run = hrt_absolute_time();
 
-	struct sensor_combined_s raw = {0};
-	struct vehicle_attitude_s att = {};
+	struct sensor_combined_s raw;
+	struct vehicle_attitude_s att;
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -151,8 +212,10 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 	int loopcounter = 0;
 	int printcounter = 0;
 
+	thread_running = true;
+
 	/* Main loop*/
-	while (true) {
+	while (!thread_should_exit) {
 
 		struct pollfd fds[1] = {
 			{ .fd = sub_raw,   .events = POLLIN },
@@ -216,7 +279,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 			/* print rotation matrix every 200th time */
 			if (printcounter % 200 == 0) {
 				printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
-				printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", euler[0], euler[1], euler[2]);
+				printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]);
 				printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
 				       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
 				       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
@@ -230,9 +293,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 
 			/* send out */
 			att.timestamp = raw.timestamp;
-			att.roll = euler.x;
-			att.pitch = euler.y;
-			att.yaw = euler.z;
+			att.roll = euler[0];
+			att.pitch = euler[1];
+			att.yaw = euler[2];
 
 			// att.rollspeed = rates.x;
 			// att.pitchspeed = rates.y;
@@ -245,11 +308,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		loopcounter++;
 	}
 
-	/* Should never reach here */
+	thread_running = false;
+
 	return 0;
 }
-
-
-
-
-
diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c
index 9d1b40acf..053163789 100644
--- a/apps/examples/px4_deamon_app/px4_deamon_app.c
+++ b/apps/examples/px4_deamon_app/px4_deamon_app.c
@@ -60,20 +60,6 @@ int px4_deamon_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-int px4_deamon_thread_main(int argc, char *argv[]) {
-
-	printf("[deamon] starting\n");
-
-	while (!thread_should_exit) {
-		printf("Hello Deamon!\n");
-		sleep(10);
-	}
-
-	printf("[deamon] exiting.\n");
-
-	return 0;
-}
-
 static void
 usage(const char *reason)
 {
@@ -127,3 +113,17 @@ int px4_deamon_app_main(int argc, char *argv[])
 	usage("unrecognized command");
 	exit(1);
 }
+
+int px4_deamon_thread_main(int argc, char *argv[]) {
+
+	printf("[deamon] starting\n");
+
+	while (!thread_should_exit) {
+		printf("Hello Deamon!\n");
+		sleep(10);
+	}
+
+	printf("[deamon] exiting.\n");
+
+	return 0;
+}
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index efddb8886..67e5d1b28 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -63,8 +63,79 @@
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/param/param.h>
 
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
 __EXPORT int fixedwing_control_main(int argc, char *argv[]);
 
+/**
+ * Mainloop of deamon.
+ */
+int fixedwing_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int fixedwing_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("fixedwing_control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tfixedwing_control is running\n");
+		} else {
+			printf("\tfixedwing_control not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
 #define PID_DT 0.01f
 #define PID_SCALER 0.1f
 #define PID_DERIVMODE_CALC 0
@@ -614,14 +685,12 @@ static float calc_pitch_setpoint()
 }
 
 /**
- * calc_throttle_setpoint
  *
  * Calculates the throttle setpoint for different flight modes
  *
  * @return throttle output setpoint
  *
  */
-
 static float calc_throttle_setpoint()
 {
 	float setpoint = 0;
@@ -644,8 +713,7 @@ static float calc_throttle_setpoint()
 	return setpoint;
 }
 
-/*
- * fixedwing_control_main
+/**
  *
  * @param argc number of arguments
  * @param argv argument array
@@ -653,8 +721,7 @@ static float calc_throttle_setpoint()
  * @return 0
  *
  */
-
-int fixedwing_control_main(int argc, char *argv[])
+int fixedwing_control_thread_main(int argc, char *argv[])
 {
 	/* default values for arguments */
 	char *fixedwing_uart_name = "/dev/ttyS1";
diff --git a/apps/fixedwing_control/pid.c b/apps/fixedwing_control/pid.c
deleted file mode 100644
index 652e4143f..000000000
--- a/apps/fixedwing_control/pid.c
+++ /dev/null
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file pid.c
- * Implementation of a fixed wing attitude and position controller.
- */
-
-#include "pid.h"
-#include "fixedwing_control.h"
-
-/*******************************************************************************
- * pid()
- *
- * Calculates the PID control output given an error
- *
- * Input: float error, uint16_t dt, float scaler, float K_p, float K_i, float K_d
- *
- * Output: PID control value
- *
- ******************************************************************************/
-
-static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax)
-{
-	// PID parameters
-
-	float Kp = K_p;
-	float Ki = K_i;
-	float Kd = K_d;
-	float delta_time = dt;	// delta time
-	float lerror;		// last error value
-	float imax = intmax;			// max integral value
-	float integrator;
-	float derivative;
-	float lderiv;
-	int fCut = 20;		// anything above 20 Hz is considered noise - low pass filter for the derivative
-	float output = 0;		// the output of the PID controller
-
-	output += error * Kp;
-
-	if ((fabs(Kd) > 0) && (dt > 0)) {
-
-		if (PID_DERIVMODE_CALC) {
-			derivative = (error - lerror) / delta_time;
-
-			// discrete low pass filter, cuts out the
-			// high frequency noise that can drive the controller crazy
-			float RC = 1 / (2 * M_PI * fCut);
-			derivative = lderiv +
-				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
-
-			// update state
-			lerror 	= error;
-			lderiv  = derivative;
-
-		} else {
-			derivative = error_deriv;
-		}
-
-		// add in derivative component
-		output 	+= Kd * derivative;
-	}
-
-	printf("PID derivative %i\n", (int)(1000 * derivative));
-
-	// scale the P and D components
-	output *= scaler;
-
-	// Compute integral component if time has elapsed
-	if ((fabs(Ki) > 0) && (dt > 0)) {
-		integrator 		+= (error * Ki) * scaler * delta_time;
-
-		if (integrator < -imax) {
-			integrator = -imax;
-
-		} else if (integrator > imax) {
-			integrator = imax;
-		}
-
-		output += integrator;
-	}
-
-	printf("PID Integrator %i\n", (int)(1000 * integrator));
-
-	return output;
-}
-
diff --git a/apps/fixedwing_control/pid.h b/apps/fixedwing_control/pid.h
deleted file mode 100644
index 2f85c6c30..000000000
--- a/apps/fixedwing_control/pid.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/****************************************************************************
- * pid.h
- *
- *   Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
- *   Authors: Ivan Ovinnikov <oivan@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-
-#ifndef PID_H_
-#define PID_H_
-
-static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
-
-#endif /* PID_H_ */