Minor cleanups in fixed wing control

3 files changed, 73 insertions, 170 deletions

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_ */