diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index efddb88865..67e5d1b289 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -63,8 +63,79 @@ #include #include +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 652e4143fb..0000000000 --- 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 - * - * 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 2f85c6c30a..0000000000 --- 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 - * - * 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_ */