From 13fc6703862862f4263d8d5d085b7a16b87190e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 09:54:11 +0200 Subject: Moved last libs, drivers and headers, cleaned up IO build --- src/examples/math_demo/math_demo.cpp | 106 +++++++++++++++++++++++++++++++++++ src/examples/math_demo/module.mk | 41 ++++++++++++++ 2 files changed, 147 insertions(+) create mode 100644 src/examples/math_demo/math_demo.cpp create mode 100644 src/examples/math_demo/module.mk (limited to 'src/examples') diff --git a/src/examples/math_demo/math_demo.cpp b/src/examples/math_demo/math_demo.cpp new file mode 100644 index 000000000..36d3c2e84 --- /dev/null +++ b/src/examples/math_demo/math_demo.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 math_demo.cpp + * @author James Goppert + * Demonstration of math library + */ + +#include +#include +#include +#include +#include +#include +#include + +/** + * Management function. + */ +extern "C" __EXPORT int math_demo_main(int argc, char *argv[]); + +/** + * Test function + */ +void test(); + +/** + * 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: math_demo {test}\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 math_demo_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "test")) { + test(); + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void test() +{ + printf("beginning math lib test\n"); + using namespace math; + vectorTest(); + matrixTest(); + vector3Test(); + eulerAnglesTest(); + quaternionTest(); + dcmTest(); +} diff --git a/src/examples/math_demo/module.mk b/src/examples/math_demo/module.mk new file mode 100644 index 000000000..deba13fd0 --- /dev/null +++ b/src/examples/math_demo/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Mathlib / operations demo application +# + +MODULE_COMMAND = math_demo +MODULE_STACKSIZE = 12000 + +SRCS = math_demo.cpp -- cgit v1.2.3 From 25612cebc2c4ecfc131e545c926f6ff581e11030 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 10:37:07 +0200 Subject: Cleaned up NuttX appconfig, added examples to config --- apps/examples/px4_deamon_app/Makefile | 42 ---- apps/examples/px4_deamon_app/px4_deamon_app.c | 137 ------------ apps/examples/px4_mavlink_debug/Makefile | 42 ---- .../examples/px4_mavlink_debug/px4_mavlink_debug.c | 74 ------- apps/examples/px4_simple_app/Makefile | 42 ---- apps/examples/px4_simple_app/px4_simple_app.c | 115 ---------- apps/mk/app.mk | 237 --------------------- makefiles/config_px4fmu_default.mk | 11 + nuttx/configs/px4fmu/nsh/appconfig | 87 -------- src/examples/px4_daemon_app/module.mk | 40 ++++ src/examples/px4_daemon_app/px4_daemon_app.c | 137 ++++++++++++ src/examples/px4_mavlink_debug/module.mk | 40 ++++ src/examples/px4_mavlink_debug/px4_mavlink_debug.c | 74 +++++++ src/examples/px4_simple_app/module.mk | 40 ++++ src/examples/px4_simple_app/px4_simple_app.c | 115 ++++++++++ 15 files changed, 457 insertions(+), 776 deletions(-) delete mode 100644 apps/examples/px4_deamon_app/Makefile delete mode 100644 apps/examples/px4_deamon_app/px4_deamon_app.c delete mode 100644 apps/examples/px4_mavlink_debug/Makefile delete mode 100644 apps/examples/px4_mavlink_debug/px4_mavlink_debug.c delete mode 100644 apps/examples/px4_simple_app/Makefile delete mode 100644 apps/examples/px4_simple_app/px4_simple_app.c delete mode 100644 apps/mk/app.mk create mode 100644 src/examples/px4_daemon_app/module.mk create mode 100644 src/examples/px4_daemon_app/px4_daemon_app.c create mode 100644 src/examples/px4_mavlink_debug/module.mk create mode 100644 src/examples/px4_mavlink_debug/px4_mavlink_debug.c create mode 100644 src/examples/px4_simple_app/module.mk create mode 100644 src/examples/px4_simple_app/px4_simple_app.c (limited to 'src/examples') diff --git a/apps/examples/px4_deamon_app/Makefile b/apps/examples/px4_deamon_app/Makefile deleted file mode 100644 index e4c169872..000000000 --- a/apps/examples/px4_deamon_app/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# 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. -# -############################################################################ - -# -# Basic example application -# - -APPNAME = px4_deamon_app -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c deleted file mode 100644 index a5d847777..000000000 --- a/apps/examples/px4_deamon_app/px4_deamon_app.c +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User - * - * 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 px4_deamon_app.c - * Deamon application example for PX4 autopilot - */ - -#include -#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 px4_deamon_app_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int px4_deamon_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: deamon {start|stop|status} [-p ]\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 px4_deamon_app_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("deamon already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn("deamon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - px4_deamon_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("\tdeamon app is running\n"); - } else { - printf("\tdeamon app not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int px4_deamon_thread_main(int argc, char *argv[]) { - - printf("[deamon] starting\n"); - - thread_running = true; - - while (!thread_should_exit) { - printf("Hello Deamon!\n"); - sleep(10); - } - - printf("[deamon] exiting.\n"); - - thread_running = false; - - return 0; -} diff --git a/apps/examples/px4_mavlink_debug/Makefile b/apps/examples/px4_mavlink_debug/Makefile deleted file mode 100644 index f59aef16f..000000000 --- a/apps/examples/px4_mavlink_debug/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# 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. -# -############################################################################ - -# -# Basic example application -# - -APPNAME = px4_mavlink_debug -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/examples/px4_mavlink_debug/px4_mavlink_debug.c b/apps/examples/px4_mavlink_debug/px4_mavlink_debug.c deleted file mode 100644 index aa1eb5ed8..000000000 --- a/apps/examples/px4_mavlink_debug/px4_mavlink_debug.c +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User - * - * 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 px4_mavlink_debug.c - * Debug application example for PX4 autopilot - */ - -#include -#include -#include -#include - -#include - -#include -#include - -__EXPORT int px4_mavlink_debug_main(int argc, char *argv[]); - -int px4_mavlink_debug_main(int argc, char *argv[]) -{ - printf("Hello Debug!\n"); - - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "velx", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - int value_counter = 0; - - while (value_counter < 100) { - /* send one value */ - dbg.value = value_counter; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - - warnx("sent one more value.."); - - value_counter++; - usleep(500000); - } - - return 0; -} diff --git a/apps/examples/px4_simple_app/Makefile b/apps/examples/px4_simple_app/Makefile deleted file mode 100644 index 102f51fd4..000000000 --- a/apps/examples/px4_simple_app/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# 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. -# -############################################################################ - -# -# Basic example application -# - -APPNAME = px4_simple_app -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c deleted file mode 100644 index 7f655964d..000000000 --- a/apps/examples/px4_simple_app/px4_simple_app.c +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User - * - * 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 px4_simple_app.c - * Minimal application example for PX4 autopilot - */ - -#include -#include -#include -#include - -#include -#include -#include - -__EXPORT int px4_simple_app_main(int argc, char *argv[]); - -int px4_simple_app_main(int argc, char *argv[]) -{ - printf("Hello Sky!\n"); - - /* subscribe to sensor_combined topic */ - int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); - orb_set_interval(sensor_sub_fd, 1000); - - /* advertise attitude topic */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - /* one could wait for multiple topics with this technique, just using one here */ - struct pollfd fds[] = { - { .fd = sensor_sub_fd, .events = POLLIN }, - /* there could be more file descriptors here, in the form like: - * { .fd = other_sub_fd, .events = POLLIN }, - */ - }; - - int error_counter = 0; - - while (true) { - /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ - int poll_ret = poll(fds, 1, 1000); - - /* handle the poll result */ - if (poll_ret == 0) { - /* this means none of our providers is giving us data */ - printf("[px4_simple_app] Got no data within a second\n"); - } else if (poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - if (error_counter < 10 || error_counter % 50 == 0) { - /* use a counter to prevent flooding (and slowing us down) */ - printf("[px4_simple_app] ERROR return value from poll(): %d\n" - , poll_ret); - } - error_counter++; - } else { - - if (fds[0].revents & POLLIN) { - /* obtained data for the first file descriptor */ - struct sensor_combined_s raw; - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); - printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); - - /* set att and publish this information for other apps */ - att.roll = raw.accelerometer_m_s2[0]; - att.pitch = raw.accelerometer_m_s2[1]; - att.yaw = raw.accelerometer_m_s2[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); - } - /* there could be more file descriptors here, in the form like: - * if (fds[1..n].revents & POLLIN) {} - */ - } - } - - return 0; -} diff --git a/apps/mk/app.mk b/apps/mk/app.mk deleted file mode 100644 index fa4a12cab..000000000 --- a/apps/mk/app.mk +++ /dev/null @@ -1,237 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012 PX4 Development Team. All rights reserved. -# -# 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. -# -############################################################################ - -# -# Common Makefile for nsh command modules and utility libraries; should be -# included by the module-specific Makefile. -# -# To build an app that appears as an nsh external command, the caller -# must define: -# -# APPNAME - the name of the application, defaults to the name -# of the parent directory. -# -# If APPNAME is not defined, a utility library is built instead. The library -# name is normally automatically determined, but it can be overridden by -# setting: -# -# LIBNAME - the name of the library, defaults to the name of the -# directory -# -# The calling makefile may also set: -# -# ASRCS - list of assembly source files, defaults to all .S -# files in the directory -# -# CSRCS - list of C source files, defaults to all .c files -# in the directory -# -# CXXSRCS - list of C++ source files, defaults to all .cpp -# files in the directory -# -# INCLUDES - list of directories to be added to the include -# search path -# -# PRIORITY - thread priority for the command (defaults to -# SCHED_PRIORITY_DEFAULT) -# -# STACKSIZE - stack size for the command (defaults to -# CONFIG_PTHREAD_STACK_DEFAULT) -# -# Symbols in the module are private to the module unless deliberately exported -# using the __EXPORT tag, or DEFAULT_VISIBILITY is set -# - -############################################################################ -# No user-serviceable parts below -############################################################################ - - -############################################################################ -# Work out who included us so we can report decent errors -# -THIS_MAKEFILE := $(lastword $(MAKEFILE_LIST)) -ifeq ($(APP_MAKEFILE),) -APP_MAKEFILE := $(lastword $(filter-out $(THIS_MAKEFILE),$(MAKEFILE_LIST))) -endif - -############################################################################ -# Get configuration -# --include $(TOPDIR)/.config --include $(TOPDIR)/Make.defs -include $(APPDIR)/Make.defs - -############################################################################ -# Sanity-check the information we've been given and set any defaults -# -SRCDIR ?= $(dir $(APP_MAKEFILE)) -PRIORITY ?= SCHED_PRIORITY_DEFAULT -STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT - -INCLUDES += $(APPDIR) - -ASRCS ?= $(wildcard $(SRCDIR)/*.S) -CSRCS ?= $(wildcard $(SRCDIR)/*.c) -CHDRS ?= $(wildcard $(SRCDIR)/*.h) -CXXSRCS ?= $(wildcard $(SRCDIR)/*.cpp) -CXXHDRS ?= $(wildcard $(SRCDIR)/*.hpp) - -# if APPNAME is not set, this is a library -ifeq ($(APPNAME),) -LIBNAME ?= $(lastword $(subst /, ,$(realpath $(SRCDIR)))) -endif - -# there has to be a source file -ifeq ($(ASRCS)$(CSRCS)$(CXXSRCS),) -$(error $(realpath $(APP_MAKEFILE)): at least one of ASRCS, CSRCS or CXXSRCS must be set) -endif - -# check that C++ is configured if we have C++ source files and we are building -ifneq ($(CXXSRCS),) -ifneq ($(CONFIG_HAVE_CXX),y) -ifeq ($(MAKECMDGOALS),build) -$(error $(realpath $(APP_MAKEFILE)): cannot set CXXSRCS if CONFIG_HAVE_CXX not set in configuration) -endif -endif -endif - -############################################################################ -# Adjust compilation flags to implement EXPORT -# - -ifeq ($(DEFAULT_VISIBILITY),) -DEFAULT_VISIBILITY = hidden -else -DEFAULT_VISIBILITY = default -endif - -CFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(APPDIR)/systemlib/visibility.h -CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(APPDIR)/systemlib/visibility.h - -############################################################################ -# Add extra include directories -# -CFLAGS += $(addprefix -I,$(INCLUDES)) -CXXFLAGS += $(addprefix -I,$(INCLUDES)) - -############################################################################ -# Things we are going to build -# - -SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS) -AOBJS = $(patsubst %.S,%.o,$(ASRCS)) -COBJS = $(patsubst %.c,%.o,$(CSRCS)) -CXXOBJS = $(patsubst %.cpp,%.o,$(CXXSRCS)) -OBJS = $(AOBJS) $(COBJS) $(CXXOBJS) - -# Automatic depdendency generation -DEPS = $(OBJS:$(OBJEXT)=.d) -CFLAGS += -MD -CXXFLAGS += -MD - -# The prelinked object that we are ultimately going to build -ifneq ($(APPNAME),) -PRELINKOBJ = $(APPNAME).pre.o -else -PRELINKOBJ = $(LIBNAME).pre.o -endif - -# The archive the prelinked object will be linked into -# XXX does WINTOOL ever get set? -ifeq ($(WINTOOL),y) - INCDIROPT = -w - BIN = "$(shell cygpath -w $(APPDIR)/libapps$(LIBEXT))" -else - BIN = "$(APPDIR)/libapps$(LIBEXT)" -endif - -############################################################################ -# Rules for building things -# - -all: .built -.PHONY: clean depend distclean - -# -# Top-level build; add prelinked object to the apps archive -# -.built: $(PRELINKOBJ) - @$(call ARCHIVE, $(BIN), $(PRELINKOBJ)) - @touch $@ - -# -# Source dependencies -# -depend: - @exit 0 - -ifneq ($(APPNAME),) -# -# App registration -# -context: .context -.context: $(MAKEFILE_LIST) - $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) - @touch $@ -else -context: - @exit 0 -endif - -# -# Object files -# -$(PRELINKOBJ): $(OBJS) - $(call PRELINK, $@, $(OBJS)) - -$(AOBJS): %.o : %.S $(MAKEFILE_LIST) - $(call ASSEMBLE, $<, $@) - -$(COBJS): %.o : %.c $(MAKEFILE_LIST) - $(call COMPILE, $<, $@) - -$(CXXOBJS): %.o : %.cpp $(MAKEFILE_LIST) - $(call COMPILEXX, $<, $@) - -# -# Tidying up -# -clean: - @rm -f $(OBJS) $(DEPS) $(PRELINKOBJ) .built - $(call CLEAN) - -distclean: clean - @rm -f Make.dep .depend - --include $(DEPS) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index bc3031cd6..52b7e7e48 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -89,6 +89,17 @@ MODULES += modules/uORB # Demo apps # MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +MODULES += examples/px4_mavlink_debug # # Transitional support - add commands from the NuttX export archive. diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 6e91f3d1a..0e18aa8ef 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -41,93 +41,6 @@ CONFIGURED_APPS += examples/nsh CONFIGURED_APPS += nshlib CONFIGURED_APPS += system/readline -# System library - utility functions -CONFIGURED_APPS += systemlib -CONFIGURED_APPS += systemlib/mixer - -# Math library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += mathlib -CONFIGURED_APPS += mathlib/CMSIS -CONFIGURED_APPS += examples/math_demo -endif - -# Control library -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += controllib -CONFIGURED_APPS += examples/control_demo -CONFIGURED_APPS += examples/kalman_demo -endif - -# System utility commands -CONFIGURED_APPS += systemcmds/reboot -CONFIGURED_APPS += systemcmds/perf -CONFIGURED_APPS += systemcmds/top -CONFIGURED_APPS += systemcmds/boardinfo -CONFIGURED_APPS += systemcmds/mixer -CONFIGURED_APPS += systemcmds/param -CONFIGURED_APPS += systemcmds/pwm -CONFIGURED_APPS += systemcmds/bl_update -CONFIGURED_APPS += systemcmds/preflight_check -CONFIGURED_APPS += systemcmds/delay_test - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -# CONFIGURED_APPS += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/deamon -# CONFIGURED_APPS += examples/px4_deamon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -# CONFIGURED_APPS += examples/px4_mavlink_debug - -# Shared object broker; required by many parts of the system. -CONFIGURED_APPS += uORB - -CONFIGURED_APPS += mavlink -CONFIGURED_APPS += mavlink_onboard -CONFIGURED_APPS += commander -CONFIGURED_APPS += sdlog -CONFIGURED_APPS += sensors - -ifneq ($(CONFIG_APM),y) -CONFIGURED_APPS += ardrone_interface -CONFIGURED_APPS += multirotor_att_control -CONFIGURED_APPS += multirotor_pos_control -CONFIGURED_APPS += position_estimator_mc -CONFIGURED_APPS += fixedwing_att_control -CONFIGURED_APPS += fixedwing_pos_control -CONFIGURED_APPS += position_estimator -CONFIGURED_APPS += attitude_estimator_ekf -CONFIGURED_APPS += hott_telemetry -endif - -# Hacking tools -#CONFIGURED_APPS += system/i2c -CONFIGURED_APPS += systemcmds/i2c - -# Communication and Drivers -#CONFIGURED_APPS += drivers/boards/px4fmu -CONFIGURED_APPS += drivers/device -CONFIGURED_APPS += drivers/ms5611 -CONFIGURED_APPS += drivers/hmc5883 -CONFIGURED_APPS += drivers/mpu6000 -CONFIGURED_APPS += drivers/bma180 -CONFIGURED_APPS += drivers/px4io -CONFIGURED_APPS += drivers/stm32 -#CONFIGURED_APPS += drivers/led -CONFIGURED_APPS += drivers/blinkm -CONFIGURED_APPS += drivers/stm32/tone_alarm -CONFIGURED_APPS += drivers/stm32/adc -CONFIGURED_APPS += drivers/hil -CONFIGURED_APPS += drivers/gps -CONFIGURED_APPS += drivers/mb12xx - -# Testing stuff -CONFIGURED_APPS += px4/sensors_bringup - ifeq ($(CONFIG_CAN),y) #CONFIGURED_APPS += examples/can endif diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk new file mode 100644 index 000000000..d23c19b75 --- /dev/null +++ b/src/examples/px4_daemon_app/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = px4_daemon_app + +SRCS = px4_daemon_app.c diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c new file mode 100644 index 000000000..a5d847777 --- /dev/null +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * 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 px4_deamon_app.c + * Deamon application example for PX4 autopilot + */ + +#include +#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 px4_deamon_app_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int px4_deamon_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: deamon {start|stop|status} [-p ]\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 px4_deamon_app_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("deamon already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("deamon", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + px4_deamon_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("\tdeamon app is running\n"); + } else { + printf("\tdeamon app not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int px4_deamon_thread_main(int argc, char *argv[]) { + + printf("[deamon] starting\n"); + + thread_running = true; + + while (!thread_should_exit) { + printf("Hello Deamon!\n"); + sleep(10); + } + + printf("[deamon] exiting.\n"); + + thread_running = false; + + return 0; +} diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk new file mode 100644 index 000000000..3d400fdbf --- /dev/null +++ b/src/examples/px4_mavlink_debug/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Basic example application +# + +MODULE_COMMAND = px4_mavlink_debug + +SRCS = px4_mavlink_debug.c \ No newline at end of file diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c new file mode 100644 index 000000000..aa1eb5ed8 --- /dev/null +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * 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 px4_mavlink_debug.c + * Debug application example for PX4 autopilot + */ + +#include +#include +#include +#include + +#include + +#include +#include + +__EXPORT int px4_mavlink_debug_main(int argc, char *argv[]); + +int px4_mavlink_debug_main(int argc, char *argv[]) +{ + printf("Hello Debug!\n"); + + /* advertise debug value */ + struct debug_key_value_s dbg = { .key = "velx", .value = 0.0f }; + orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + + int value_counter = 0; + + while (value_counter < 100) { + /* send one value */ + dbg.value = value_counter; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + + warnx("sent one more value.."); + + value_counter++; + usleep(500000); + } + + return 0; +} diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk new file mode 100644 index 000000000..2c102fa50 --- /dev/null +++ b/src/examples/px4_simple_app/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Basic example application +# + +MODULE_COMMAND = px4_simple_app + +SRCS = px4_simple_app.c diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c new file mode 100644 index 000000000..7f655964d --- /dev/null +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * 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 px4_simple_app.c + * Minimal application example for PX4 autopilot + */ + +#include +#include +#include +#include + +#include +#include +#include + +__EXPORT int px4_simple_app_main(int argc, char *argv[]); + +int px4_simple_app_main(int argc, char *argv[]) +{ + printf("Hello Sky!\n"); + + /* subscribe to sensor_combined topic */ + int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined)); + orb_set_interval(sensor_sub_fd, 1000); + + /* advertise attitude topic */ + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + + /* one could wait for multiple topics with this technique, just using one here */ + struct pollfd fds[] = { + { .fd = sensor_sub_fd, .events = POLLIN }, + /* there could be more file descriptors here, in the form like: + * { .fd = other_sub_fd, .events = POLLIN }, + */ + }; + + int error_counter = 0; + + while (true) { + /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ + int poll_ret = poll(fds, 1, 1000); + + /* handle the poll result */ + if (poll_ret == 0) { + /* this means none of our providers is giving us data */ + printf("[px4_simple_app] Got no data within a second\n"); + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + printf("[px4_simple_app] ERROR return value from poll(): %d\n" + , poll_ret); + } + error_counter++; + } else { + + if (fds[0].revents & POLLIN) { + /* obtained data for the first file descriptor */ + struct sensor_combined_s raw; + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); + printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); + + /* set att and publish this information for other apps */ + att.roll = raw.accelerometer_m_s2[0]; + att.pitch = raw.accelerometer_m_s2[1]; + att.yaw = raw.accelerometer_m_s2[2]; + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); + } + /* there could be more file descriptors here, in the form like: + * if (fds[1..n].revents & POLLIN) {} + */ + } + } + + return 0; +} -- cgit v1.2.3 From 6ea204c8136a3e4c81e2e542e56a865c57f381e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 May 2013 20:08:09 +0200 Subject: Added fixed wing controller example --- makefiles/config_px4fmu_default.mk | 4 + src/examples/fixedwing_control/main.c | 474 +++++++++++++++++++++++++++++++ src/examples/fixedwing_control/module.mk | 41 +++ src/examples/fixedwing_control/params.c | 77 +++++ src/examples/fixedwing_control/params.h | 65 +++++ 5 files changed, 661 insertions(+) create mode 100644 src/examples/fixedwing_control/main.c create mode 100644 src/examples/fixedwing_control/module.mk create mode 100644 src/examples/fixedwing_control/params.c create mode 100644 src/examples/fixedwing_control/params.h (limited to 'src/examples') diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 00d402c4a..27bf0f973 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -105,6 +105,10 @@ MODULES += modules/uORB # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +MODULES += examples/fixedwing_control + # # Transitional support - add commands from the NuttX export archive. # diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c new file mode 100644 index 000000000..1753070e2 --- /dev/null +++ b/src/examples/fixedwing_control/main.c @@ -0,0 +1,474 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 main.c + * Implementation of a fixed wing attitude controller. This file is a complete + * fixed wing controller flying manual attitude control or auto waypoint control. + * There is no need to touch any other system components to extend / modify the + * complete control architecture. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* process-specific header files */ +#include "params.h" + +/* Prototypes */ +/** + * Daemon management function. + */ +__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int fixedwing_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators); + +void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); + +/* Variables */ +static bool thread_should_exit = false; /**< Daemon exit flag */ +static bool thread_running = false; /**< Daemon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static struct params p; +static struct param_handles ph; + +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators) +{ + + /* + * The PX4 architecture provides a mixer outside of the controller. + * The mixer is fed with a default vector of actuator controls, representing + * moments applied to the vehicle frame. This vector + * is structured as: + * + * Control Group 0 (attitude): + * + * 0 - roll (-1..+1) + * 1 - pitch (-1..+1) + * 2 - yaw (-1..+1) + * 3 - thrust ( 0..+1) + * 4 - flaps (-1..+1) + * ... + * + * Control Group 1 (payloads / special): + * + * ... + */ + + /* + * Calculate roll error and apply P gain + */ + float roll_err = att->roll - att_sp->roll_body; + actuators->control[0] = roll_err * p.roll_p; + + /* + * Calculate pitch error and apply P gain + */ + float pitch_err = att->pitch - att_sp->pitch_body; + actuators->control[1] = pitch_err * p.pitch_p; +} + +void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) +{ + + /* + * Calculate heading error of current position to desired position + */ + + /* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */ + float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + + /* calculate heading error */ + float yaw_err = att->yaw - bearing; + /* apply control gain */ + att_sp->roll_body = yaw_err * p.hdng_p; +} + +/* Main Thread */ +int fixedwing_control_thread_main(int argc, char *argv[]) +{ + /* read arguments */ + bool verbose = false; + + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + verbose = true; + } + } + + /* welcome user (warnx prints a line, including an appended\n, with variable arguments */ + warnx("[example fixedwing control] started"); + + /* initialize parameters, first the handles, then the values */ + parameters_init(&ph); + parameters_update(&ph, &p); + + /* declare and safely initialize all structs to zero */ + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + struct manual_control_setpoint_s manual_sp; + memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + struct vehicle_global_position_setpoint_s global_sp; + memset(&global_sp, 0, sizeof(global_sp)); + + /* output structs */ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + actuators.control[i] = 0.0f; + } + + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + /* subscribe */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + + /* Setup of loop */ + float gyro[3] = {0.0f, 0.0f, 0.0f}; + float speed_body[3] = {0.0f, 0.0f, 0.0f}; + struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN }}; + + while (!thread_should_exit) { + + /* + * Wait for a sensor or param update, check for exit condition every 500 ms. + * This means that the execution will block here without consuming any resources, + * but will continue to execute the very moment a new attitude measurement or + * a param update is published. So no latency in contrast to the polling + * design pattern (do not confuse the poll() system call with polling). + * + * This design pattern makes the controller also agnostic of the attitude + * update speed - it runs as fast as the attitude updates with minimal latency. + */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, this will not really happen in practice */ + warnx("poll error"); + + } else if (ret == 0) { + /* no return value = nothing changed for 500 ms, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag (uORB API requirement) */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); + + /* if a param update occured, re-read our parameters */ + parameters_update(&ph, &p); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + /* Check if there is a new position measurement or position setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_sp_sub, &global_sp_updated); + + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (global_sp_updated) + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp); + + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + + if (att.R_valid) { + speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; + speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; + speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; + + } else { + speed_body[0] = 0; + speed_body[1] = 0; + speed_body[2] = 0; + + warnx("Did not get a valid R\n"); + } + } + + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + /* control */ + + if (vstatus.state_machine == SYSTEM_STATE_AUTO || + vstatus.state_machine == SYSTEM_STATE_STABILIZED) { + + /* simple heading control */ + control_heading(&global_pos, &global_sp, &att, &att_sp); + + /* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */ + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + + /* simple attitude control */ + control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; + + } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { + if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (vstatus.rc_signal_lost) { + + /* put plane into loiter */ + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + + /* limit throttle to 60 % of last value if sane */ + if (isfinite(manual_sp.throttle) && + (manual_sp.throttle >= 0.0f) && + (manual_sp.throttle <= 1.0f)) { + att_sp.thrust = 0.6f * manual_sp.throttle; + + } else { + att_sp.thrust = 0.0f; + } + + att_sp.yaw_body = 0; + + // XXX disable yaw control, loiter + + } else { + + att_sp.roll_body = manual_sp.roll; + att_sp.pitch_body = manual_sp.pitch; + att_sp.yaw_body = 0; + att_sp.thrust = manual_sp.throttle; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* attitude control */ + control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* pass through flaps */ + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + + } else { + actuators.control[4] = 0.0f; + } + + } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + /* directly pass through values */ + actuators.control[0] = manual_sp.roll; + /* positive pitch means negative actuator -> pull up */ + actuators.control[1] = manual_sp.pitch; + actuators.control[2] = manual_sp.yaw; + actuators.control[3] = manual_sp.throttle; + + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + + } else { + actuators.control[4] = 0.0f; + } + } + } + + /* publish rates */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + + /* sanity check and publish actuator outputs */ + if (isfinite(actuators.control[0]) && + isfinite(actuators.control[1]) && + isfinite(actuators.control[2]) && + isfinite(actuators.control[3])) { + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } + } + } + } + + printf("[ex_fixedwing_control] exiting, stopping all motors.\n"); + thread_running = false; + + /* kill all outputs */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + actuators.control[i] = 0.0f; + + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + fflush(stdout); + + return 0; +} + +/* Startup Functions */ + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); + exit(1); +} + +/** + * The daemon 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 ex_fixedwing_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("ex_fixedwing_control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("ex_fixedwing_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + 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("\tex_fixedwing_control is running\n"); + + } else { + printf("\tex_fixedwing_control not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + + + diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk new file mode 100644 index 000000000..d2c48934f --- /dev/null +++ b/src/examples/fixedwing_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Fixedwing Attitude Control Demo / Example Application +# + +MODULE_COMMAND = ex_fixedwing_control + +SRCS = main.c \ + params.c diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c new file mode 100644 index 000000000..8042c74f5 --- /dev/null +++ b/src/examples/fixedwing_control/params.c @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 params.c + * + * Parameters for fixedwing demo + */ + +#include "params.h" + +/* controller parameters, use max. 15 characters for param name! */ + +/** + * + */ +PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f); + +/** + * + */ +PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f); + +/** + * + */ +PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f); + +int parameters_init(struct param_handles *h) +{ + /* PID parameters */ + h->hdng_p = param_find("EXFW_HDNG_P"); + h->roll_p = param_find("EXFW_ROLL_P"); + h->pitch_p = param_find("EXFW_PITCH_P"); + + return OK; +} + +int parameters_update(const struct param_handles *h, struct params *p) +{ + param_get(h->hdng_p, &(p->hdng_p)); + param_get(h->roll_p, &(p->roll_p)); + param_get(h->pitch_p, &(p->pitch_p)); + + return OK; +} diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h new file mode 100644 index 000000000..4ae8e90ec --- /dev/null +++ b/src/examples/fixedwing_control/params.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 params.h + * + * Definition of parameters for fixedwing example + */ + +#include + +struct params { + float hdng_p; + float roll_p; + float pitch_p; +}; + +struct param_handles { + param_t hdng_p; + param_t roll_p; + param_t pitch_p; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct param_handles *h, struct params *p); -- cgit v1.2.3 From 1ff6c80866775b48bf38de5cdf5dea5f99691fc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 May 2013 08:28:36 +0200 Subject: More example fixes --- makefiles/config_px4fmu_default.mk | 2 +- src/examples/px4_daemon_app/module.mk | 2 +- src/examples/px4_mavlink_debug/module.mk | 2 +- src/examples/px4_simple_app/module.mk | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/examples') diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 27bf0f973..ae62b7034 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -107,7 +107,7 @@ MODULES += modules/uORB # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +MODULES += examples/fixedwing_control # # Transitional support - add commands from the NuttX export archive. diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk index d23c19b75..5f8aa73d5 100644 --- a/src/examples/px4_daemon_app/module.mk +++ b/src/examples/px4_daemon_app/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = px4_daemon_app -SRCS = px4_daemon_app.c +SRCS = px4_daemon_app.c diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk index 3d400fdbf..fefd61496 100644 --- a/src/examples/px4_mavlink_debug/module.mk +++ b/src/examples/px4_mavlink_debug/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = px4_mavlink_debug -SRCS = px4_mavlink_debug.c \ No newline at end of file +SRCS = px4_mavlink_debug.c \ No newline at end of file diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk index 2c102fa50..32b06c3a5 100644 --- a/src/examples/px4_simple_app/module.mk +++ b/src/examples/px4_simple_app/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = px4_simple_app -SRCS = px4_simple_app.c +SRCS = px4_simple_app.c -- cgit v1.2.3