diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 58d415d86b..2e294226a6 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 ]\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 9d1b40acfa..0531637897 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 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_ */