diff options
Diffstat (limited to 'src/modules/att_pos_estimator_ekf/kalman_main.cpp')
-rw-r--r-- | src/modules/att_pos_estimator_ekf/kalman_main.cpp | 157 |
1 files changed, 0 insertions, 157 deletions
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 372b2d162..000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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 kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <systemlib/systemlib.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> -#include <drivers/drv_hrt.h> -#include <math.h> -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_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); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]"); - 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 att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 4096, - kalman_demo_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) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} |