diff options
Diffstat (limited to 'apps')
89 files changed, 4146 insertions, 2159 deletions
diff --git a/apps/COPYING b/apps/COPYING new file mode 100644 index 000000000..7e8fd8407 --- /dev/null +++ b/apps/COPYING @@ -0,0 +1,137 @@ +COPYING -- Describes the terms under which Nuttx is distributed. A +copy of the BSD-style licensing is included in this file. In my +words -- I believe that you should free to use NuttX in any +environment, private, private, commercial, open, closed, etc. +provided only that you repect the modest copyright notices as +described in license (below). Please feel free to contact me if you +have any licensing concerns. + +NuttX +^^^^^^ + +License for NuttX in general (authorship of individual files may vary): + + Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + Author: Gregory Nutt <gnutt@nuttx.org> + +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. + +uIP +^^^ + +Some lower-level networking middle-ware components of NuttX +derive from uIP which has a similar BSD style license: + + Copyright (c) 2001-2003, Adam Dunkels. + All rights reserved. + +FreeModbus +^^^^^^^^^ + +FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU. +Copyright (c) 2006 Christian Walter <wolti@sil.at> +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. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + +THTTPD +^^^^^^ + +Derived from the original THTTPD package: + + Copyright © 1995,1998,1999,2000,2001 by Jef Poskanzer <jef@mail.acme.com>. + 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. + +THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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. + +cJSON +^^^^^ + +Derives from the cJSON Project which has an MIT license: + + Copyright (c) 2009 Dave Gamble + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index 7375adccf..c1c5189c4 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -349,7 +349,7 @@ * apps/NxWidgets/Kconfig: Add option to turn on the memory monitor feature of the NxWidgets/NxWM unit tests. -6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org> +6.23 2012-11-05 Gregory Nutt <gnutt@nuttx.org> * vsn: Moved all NSH commands from vsn/ to system/. Deleted the vsn/ directory. @@ -368,3 +368,57 @@ recent check-ins (Darcy Gong). * apps/netutils/webclient/webclient.c: Fix another but that I introduced when I was trying to add correct handling for loss of connection (Darcy Gong) + * apps/nshlib/nsh_telnetd.c: Add support for login to Telnet session via + username and password (Darcy Gong). + * apps/netutils/resolv/resolv.c (and files using the DNS resolver): Various + DNS address resolution improvements from Darcy Gong. + * apps/nshlib/nsh_netcmds.c: The ping command now passes a maximum round + trip time to uip_icmpping(). This allows pinging of hosts on complex + networks where the ICMP ECHO round trip time may exceed the ping interval. + * apps/examples/nxtext/nxtext_main.c: Fix bad conditional compilation + when CONFIG_NX_KBD is not defined. Submitted by Petteri Aimonen. + * apps/examples/nximage/nximage_main.c: Add a 5 second delay after the + NX logo is presented so that there is time for the image to be verified. + Suggested by Petteri Aimonen. + * apps/Makefile: Small change that reduces the number of shell invocations + by one (Mike Smith). + * apps/examples/elf: Test example for the ELF loader. + * apps/examples/elf: The ELF module test example appears fully functional. + * apps/netutils/json: Add a snapshot of the cJSON project. Contributed by + Darcy Gong. + * apps/examples/json: Test example for cJSON from Darcy Gong + * apps/nshlib/nsh_netinit.c: Fix static IP DNS problem (Darcy Gong) + * apps/netutils/resolv/resolv.c: DNS fixes from Darcy Gong. + * COPYING: Licensing information added. + * apps/netutils/codec and include/netutils/urldecode.h, base64.h, and md5.h: + A port of the BASE46, MD5 and URL CODEC library from Darcy Gong. + * nsnlib/nsh_codeccmd.c: NSH commands to use the CODEC library. + Contributed by Darcy Gong. + * apps/examples/wgetjson: Test example contributed by Darcy Gong + * apps/examples/cxxtest: A test for the uClibc++ library provided by + Qiang Yu and the RGMP team. + * apps/netutils/webclient, apps/netutils.codes, and apps/examples/wgetjson: + Add support for wget POST interface. Contributed by Darcy Gong. + * apps/examples/relays: A relay example contributed by Darcy Gong. + * apps/nshlib/nsh_netcmds: Add ifup and ifdown commands (from Darcy + Gong). + * apps/nshlib/nsh_netcmds: Extend the ifconfig command so that it + supports setting IP addresses, network masks, name server addresses, + and hardware address (from Darcy Gong). + +6.24 2012-xx-xx Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/ostest/roundrobin.c: Replace large tables with + algorithmic prime number generation. This allows the roundrobin + test to run on platforms with minimal SRAM (Freddie Chopin). + * apps/nshlib/nsh_dbgcmds.c: Add hexdump command to dump the contents + of a file (or character device) to the console Contributed by Petteri + Aimonen. + * apps/examples/modbus: Fixes from Freddie Chopin + * apps/examples/modbus/Kconfig: Kconfig logic for FreeModBus contributed + by Freddie Chopin. + * Makefile, */Makefile: Various fixes for Windows native build. Now uses + make foreach loops instead of shell loops. + * apps/examples/elf/test/*/Makefile: OSX doesn't support install -D, use + mkdir -p then install without the -D. From Mike Smith. + diff --git a/apps/Makefile b/apps/Makefile index 19ad1c18b..0a0a31954 100644 --- a/apps/Makefile +++ b/apps/Makefile @@ -35,7 +35,6 @@ ############################################################################ -include $(TOPDIR)/Make.defs --include $(TOPDIR)/.config APPDIR = ${shell pwd} @@ -107,7 +106,7 @@ endif # Create the list of available applications (INSTALLED_APPS) define ADD_BUILTIN -INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} + INSTALLED_APPS += $(if $(wildcard $1$(DELIM)Makefile),$1,) endef $(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN)))) @@ -117,8 +116,10 @@ $(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN)))) # provided by the user (possibly as a symbolic link) to add libraries and # applications to the standard build from the repository. -INSTALLED_APPS += ${shell if [ -r external/Makefile ]; then echo "external"; fi} -SUBDIRS += ${shell if [ -r external/Makefile ]; then echo "external"; fi} +EXTERNAL_DIR := $(dir $(wildcard external$(DELIM)Makefile)) + +INSTALLED_APPS += $(EXTERNAL_DIR) +SUBDIRS += $(EXTERNAL_DIR) # The final build target @@ -130,48 +131,81 @@ all: $(BIN) .PHONY: $(INSTALLED_APPS) context depend clean distclean $(INSTALLED_APPS): - @$(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; + $(Q) $(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" $(BIN): $(INSTALLED_APPS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $@, $${obj}); \ - done ; ) .context: - @for dir in $(INSTALLED_APPS) ; do \ +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + $(Q) for %%G in ($(INSTALLED_APPS)) do ( \ + if exist %%G\.context del /f /q %%G\.context \ + $(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context \ + ) +else + $(Q) for dir in $(INSTALLED_APPS) ; do \ rm -f $$dir/.context ; \ - $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \ + $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \ done - @touch $@ +endif + $(Q) touch $@ context: .context .depend: context Makefile $(SRCS) - @for dir in $(INSTALLED_APPS) ; do \ +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + $(Q) for %%G in ($(INSTALLED_APPS)) do ( \ + if exist %%G\.depend del /f /q %%G\.depend \ + $(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend \ + ) +else + $(Q) for dir in $(INSTALLED_APPS) ; do \ rm -f $$dir/.depend ; \ - $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \ + $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \ done - @touch $@ +endif + $(Q) touch $@ depend: .depend clean: - @for dir in $(SUBDIRS) ; do \ +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + $(Q) for %%G in ($(SUBDIRS)) do ( \ + $(MAKE) -C %%G clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \ + ) +else + $(Q) for dir in $(SUBDIRS) ; do \ $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ done - @rm -f $(BIN) *~ .*.swp *.o +endif + $(call DELFILE, $(BIN)) $(call CLEAN) distclean: # clean - @for dir in $(SUBDIRS) ; do \ +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + $(Q) for %%G in ($(SUBDIRS)) do ( \ + $(MAKE) -C %%G distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \ + ) + $(call DELFILE, .config) + $(call DELFILE, .context) + $(call DELFILE, .depend) + $(Q) ( if exist external ( \ + echo ********************************************************" \ + echo * The external directory/link must be removed manually *" \ + echo ********************************************************" \ + ) +else + $(Q) for dir in $(SUBDIRS) ; do \ $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ done - @rm -f .config .context .depend - @( if [ -e external ]; then \ + $(call DELFILE, .config) + $(call DELFILE, .context) + $(call DELFILE, .depend) + $(Q) ( if [ -e external ]; then \ echo "********************************************************"; \ echo "* The external directory/link must be removed manually *"; \ echo "********************************************************"; \ fi; \ ) +endif diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7277e9fa4..e0ee500dc 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -72,10 +72,12 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> @@ -93,6 +95,9 @@ PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); #include <systemlib/cpuload.h> extern struct system_load_s system_load; @@ -150,6 +155,7 @@ static int led_on(int led); static int led_off(int led); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -177,7 +183,7 @@ static int buzzer_init() buzzer = open("/dev/tone_alarm", O_WRONLY); if (buzzer < 0) { - fprintf(stderr, "[commander] Buzzer: open fail\n"); + fprintf(stderr, "[cmd] Buzzer: open fail\n"); return ERROR; } @@ -195,12 +201,12 @@ static int led_init() leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { - fprintf(stderr, "[commander] LED: open fail\n"); + fprintf(stderr, "[cmd] LED: open fail\n"); return ERROR; } if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - fprintf(stderr, "[commander] LED: ioctl fail\n"); + fprintf(stderr, "[cmd] LED: ioctl fail\n"); return ERROR; } @@ -266,6 +272,30 @@ void tune_confirm(void) { ioctl(buzzer, TONE_SET_ALARM, 3); } +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + if(save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + mavlink_log_info(mavlink_fd, "[cmd] trim calibration done"); +} + void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { @@ -308,7 +338,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) }; if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "[commander] failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "[cmd] failed to set scale / offsets for mag"); } /* calibrate range */ @@ -356,7 +386,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "[cmd] Please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); tune_confirm(); @@ -371,7 +401,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // if ((axis_left / 1000) == 0 && axis_left > 0) { // char buf[50]; - // sprintf(buf, "[commander] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); // mavlink_log_info(mavlink_fd, buf); // } @@ -408,7 +438,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] mag cal canceled"); + mavlink_log_info(mavlink_fd, "[cmd] mag cal canceled"); break; } } @@ -444,27 +474,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting X mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting Y mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); + fprintf(stderr, "[cmd] Setting Z mag offset failed!\n"); } if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - fprintf(stderr, "[commander] Setting X mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting X mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - fprintf(stderr, "[commander] Setting Y mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting Y mag scale failed!\n"); } if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - fprintf(stderr, "[commander] Setting Z mag scale failed!\n"); + fprintf(stderr, "[cmd] Setting Z mag scale failed!\n"); } /* auto-save to EEPROM */ @@ -487,7 +517,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) (double)mscale.y_scale, (double)mscale.z_scale); mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] mag calibration done"); tune_confirm(); sleep(2); @@ -496,7 +526,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] mag calibration FAILED (NaN)"); } /* disable calibration mode */ @@ -547,7 +577,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration aborted, retry"); return; } } @@ -563,15 +593,15 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X gyro offset failed!"); } if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y gyro offset failed!"); } if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z gyro offset failed!"); } /* set offsets to actual value */ @@ -597,7 +627,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // char buf[50]; // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration done"); tune_confirm(); sleep(2); @@ -605,7 +635,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) sleep(2); /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] gyro calibration FAILED (NaN)"); } close(sub_sensor_combined); @@ -615,7 +645,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); + mavlink_log_info(mavlink_fd, "[cmd] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -653,7 +683,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); + mavlink_log_info(mavlink_fd, "[cmd] acceleration calibration aborted"); return; } } @@ -672,27 +702,27 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) float scale = 9.80665f / total_len; if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!"); } if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting X accel offset failed!"); } if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Y accel offset failed!"); } if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!"); + mavlink_log_critical(mavlink_fd, "[cmd] Setting Z accel offset failed!"); } fd = open(ACCEL_DEVICE_PATH, 0); @@ -715,9 +745,9 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) } //char buf[50]; - //sprintf(buf, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "[commander] accel calibration done"); + mavlink_log_info(mavlink_fd, "[cmd] accel calibration done"); tune_confirm(); sleep(2); @@ -725,7 +755,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) sleep(2); /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "[cmd] accel calibration FAILED (NaN)"); } /* exit accel calibration mode */ @@ -851,15 +881,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD starting gyro calibration"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD finished gyro calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING gyro calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -871,15 +901,50 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD starting mag calibration"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD finished mag calibration"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = MAV_RESULT_ACCEPTED; + } else { + mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "[cmd] zero altitude not implemented"); + tune_confirm(); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration"); + result = MAV_RESULT_DENIED; + } + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "[cmd] CMD starting mag calibration"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "[cmd] CMD finished mag calibration"); tune_confirm(); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration"); + mavlink_log_critical(mavlink_fd, "[cmd] CMD REJECTING mag calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -891,15 +956,15 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD starting accel calibration"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); - mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); + mavlink_log_info(mavlink_fd, "[cmd] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING accel calibration"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING accel calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -907,8 +972,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { - //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); + //fprintf(stderr, "[cmd] refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "[cmd] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } @@ -1167,8 +1232,10 @@ int commander_thread_main(int argc, char *argv[]) failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + param_t _param_sys_type = param_find("MAV_TYPE"); + /* welcome user */ - printf("[commander] I am in command now!\n"); + printf("[cmd] I am in command now!\n"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1176,17 +1243,17 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n"); + fprintf(stderr, "[cmd] ERROR: Failed to initialize leds\n"); } if (buzzer_init() != 0) { - fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n"); + fprintf(stderr, "[cmd] ERROR: Failed to initialize buzzer\n"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + fprintf(stderr, "[cmd] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); } /* make sure we are in preflight state */ @@ -1199,6 +1266,10 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1206,11 +1277,11 @@ int commander_thread_main(int argc, char *argv[]) state_machine_publish(stat_pub, ¤t_status, mavlink_fd); if (stat_pub < 0) { - printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); + printf("[cmd] ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); } - mavlink_log_info(mavlink_fd, "[commander] system is running"); + mavlink_log_info(mavlink_fd, "[cmd] system is running"); /* create pthreads */ pthread_attr_t subsystem_info_attr; @@ -1249,19 +1320,32 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + sensors.battery_voltage_v = 0.0f; + sensors.battery_voltage_valid = false; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1274,6 +1358,8 @@ int commander_thread_main(int argc, char *argv[]) uint64_t failsave_ll_start_time = 0; bool state_changed = true; + bool param_init_forced = true; + while (!thread_should_exit) { @@ -1288,8 +1374,13 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); if (new_data) { @@ -1299,6 +1390,46 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(stat_pub, ¤t_status, &cmd); } + /* update parameters */ + orb_check(param_changed_sub, &new_data); + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == MAV_TYPE_QUADROTOR || + current_status.system_type == MAV_TYPE_HEXAROTOR || + current_status.system_type == MAV_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + } else { + current_status.flag_external_manual_override_ok = true; + } + } else { + printf("ARMED, rejecting sys type change\n"); + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } battery_voltage = sensors.battery_voltage_v; battery_voltage_valid = sensors.battery_voltage_valid; @@ -1376,14 +1507,18 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); } low_voltage_counter++; @@ -1393,7 +1528,7 @@ int commander_thread_main(int argc, char *argv[]) else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } @@ -1406,6 +1541,76 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + } else { + current_status.flag_local_position_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + /* Check if last transition deserved an audio event */ // #warning This code depends on state that is no longer? maintained // #if 0 @@ -1471,8 +1676,82 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* check if left stick is in lower left position --> switch to standby state */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.manual_mode_switch)) { + + /* this switch is not properly mapped, set default */ + if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back to SAS */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + } else { + + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + } + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set direct mode for vehicles supporting it */ + if ((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, fall back to SAS */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + } else { + + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + } + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set SAS for all vehicle types */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + + } else { + /* center stick position, set rate control */ + current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + } + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == MAV_TYPE_QUADROTOR) || + (current_status.system_type == MAV_TYPE_HEXAROTOR) || + (current_status.system_type == MAV_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1484,7 +1763,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1494,24 +1773,31 @@ int commander_thread_main(int argc, char *argv[]) stick_off_counter = 0; } } - //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + } else { + /* check auto mode switch for correct mode */ + if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + /* enable stabilized mode */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } else { + update_state_machine_mode_hold(stat_pub, ¤t_status, mavlink_fd); + } } /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[commander] DETECTED RC SIGNAL FIRST TIME."); + mavlink_log_critical(mavlink_fd, "[cmd] DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } current_status.rc_signal_cutting_off = false; @@ -1524,7 +1810,7 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL - NO REMOTE SIGNAL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ @@ -1583,10 +1869,10 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; tune_confirm(); - mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); + mavlink_log_critical(mavlink_fd, "[cmd] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); } else { if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL"); + mavlink_log_critical(mavlink_fd, "[cmd] OK:RECOVERY OFFBOARD CONTROL"); state_changed = true; tune_confirm(); } @@ -1610,7 +1896,7 @@ int commander_thread_main(int argc, char *argv[]) /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!"); + mavlink_log_critical(mavlink_fd, "[cmd] CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ @@ -1669,11 +1955,11 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); - close(gps_sub); + close(global_position_sub); close(sensor_sub); close(cmd_sub); - printf("[commander] exiting..\n"); + printf("[cmd] exiting..\n"); fflush(stdout); thread_running = false; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 46793c89b..4152142df 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!"); + fprintf(stderr, "[cmd] EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!"); break; case SYSTEM_STATE_EMCY_CUTOFF: @@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!"); + fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!"); break; case SYSTEM_STATE_GROUND_ERROR: @@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* prevent actuators from arming */ current_status->flag_system_armed = false; - fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system"); + fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system"); break; case SYSTEM_STATE_PREFLIGHT: @@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state"); } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state"); } break; @@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM"); + mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM"); usleep(500000); up_systemreset(); /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { invalid_state = true; - mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT"); + mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT"); } break; @@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* standby enforces disarmed */ current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state"); break; case SYSTEM_STATE_GROUND_READY: @@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* ground ready has motors / actuators armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state"); break; case SYSTEM_STATE_AUTO: @@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* auto is airborne and in auto mode, motors armed */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode"); break; case SYSTEM_STATE_STABILIZED: @@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode"); break; case SYSTEM_STATE_MANUAL: @@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con /* set system flags according to state */ current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode"); + mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode"); break; default: @@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con ret = OK; } if (invalid_state) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition"); + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition"); ret = ERROR; } return ret; @@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } void publish_armed_status(const struct vehicle_status_s *current_status) { @@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) { */ void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - fprintf(stderr, "[commander] EMERGENCY HANDLER\n"); + fprintf(stderr, "[cmd] EMERGENCY HANDLER\n"); /* Depending on the current state go to one of the error states */ if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { @@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); } else { - fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine); } } @@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); } else { - //global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); } } @@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[commander] arming\n"); + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } } @@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[commander] going standby\n"); + printf("[cmd] going standby\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] MISSION ABORT!\n"); + printf("[cmd] MISSION ABORT!\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); } } @@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; @@ -525,46 +526,101 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] manual mode\n"); + printf("[cmd] manual mode\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); } } void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } +} + +void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. HOLD MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[commander] stabilized mode\n"); + printf("[cmd] stabilized mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[commander] auto mode\n"); + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); } } uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) { - printf("[commander] Requested new mode: %d\n", (int)mode); uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!") + } else { + + mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } /* vehicle is disarmed, mode requests arming */ if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { @@ -573,7 +629,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; - printf("[commander] arming due to command request\n"); + printf("[cmd] arming due to command request\n"); } } @@ -583,26 +639,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_ if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); ret = OK; - printf("[commander] disarming due to command request\n"); + printf("[cmd] disarming due to command request\n"); } } - /* Switch on HIL if in standby and not already in HIL mode */ - if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.") - } - /* NEVER actually switch off HIL without reboot */ if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL"); ret = ERROR; } @@ -627,7 +671,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { printf("system will reboot\n"); - //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + mavlink_log_critical(mavlink_fd, "[cmd] Rebooting.."); + usleep(200000); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); ret = 0; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index c4d1b78a5..815645089 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -128,6 +128,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); /** + * Handle state machine if mode switch is hold + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** * Handle state machine if mode switch is auto * * @param status_pub file descriptor for state update topic publication diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c index 987894163..b55af486d 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c @@ -67,10 +67,10 @@ /* Identifying number of each ADC channel: Variable Resistor. */ #ifdef CONFIG_STM32_ADC3 -static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards +static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11, 12, 13}; /* Configurations of pins used byte each ADC channels */ -static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; +static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; #endif /************************************************************************************ diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 57ffb77d3..bc047aa4f 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -150,9 +150,7 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER hrt_init(); -#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -160,27 +158,21 @@ __EXPORT int nsh_archinitialize(void) #endif /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); // initial LED state drv_led_start(); @@ -209,8 +201,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ + /* Get the SPI port for the microsd slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); @@ -233,7 +224,6 @@ __EXPORT int nsh_archinitialize(void) } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42..276a642fd 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -// #include <drivers/boards/HIL/HIL_internal.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_mixer.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } @@ -396,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index ed79440cc..55b7cfa85 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -446,8 +446,12 @@ MPU6000::init() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); - /* do CDev init for the gyro device node */ - ret = _gyro->init(); + /* do CDev init for the gyro device node, keep it optional */ + int gyro_ret = _gyro->init(); + + if (gyro_ret != OK) { + _gyro_topic = -1; + } return ret; } @@ -938,7 +942,9 @@ MPU6000::measure() /* and publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + if (_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + } /* stop measuring */ perf_end(_sample_perf); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 61dd418f8..a672bd2fb 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,12 +58,15 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> #include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> @@ -96,6 +99,7 @@ private: int _t_actuators; int _t_armed; orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -110,9 +114,9 @@ private: void task_main() __attribute__((noreturn)); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -161,6 +165,7 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_armed(-1), _t_outputs(0), + _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -177,6 +182,7 @@ PX4FMU::~PX4FMU() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -212,6 +218,7 @@ PX4FMU::init() /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (ret == OK) { log("default PWM output device"); _primary_pwm_device = true; @@ -245,7 +252,7 @@ PX4FMU::task_main_trampoline(int argc, char *argv[]) int PX4FMU::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -279,6 +286,7 @@ PX4FMU::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -315,6 +323,13 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -331,8 +346,18 @@ PX4FMU::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } + orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -358,20 +383,39 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } } @@ -388,6 +432,7 @@ PX4FMU::task_main() } ::close(_t_actuators); + ::close(_t_actuators_effective); ::close(_t_armed); /* make sure servos are off */ @@ -404,9 +449,9 @@ PX4FMU::task_main() int PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -424,15 +469,17 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); + if (ret != -ENOTTY) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: ret = pwm_ioctl(filp, cmd, arg); break; + default: debug("not in a PWM mode"); break; @@ -550,8 +597,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { ret = -ENOMEM; + } else { debug("loading mixers from %s", path); @@ -582,7 +631,7 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip + * Setup default GPIO config - all pins as GPIOs, GPIO driver chip * to input mode. */ for (unsigned i = 0; i < _ngpio; i++) @@ -609,17 +658,20 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1<<i)) { + if (gpios & (1 << i)) { switch (function) { case GPIO_SET_INPUT: stm32_configgpio(_gpio_tab[i].input); break; + case GPIO_SET_OUTPUT: stm32_configgpio(_gpio_tab[i].output); break; + case GPIO_SET_ALT_1: if (_gpio_tab[i].alt != 0) stm32_configgpio(_gpio_tab[i].alt); + break; } } @@ -636,7 +688,7 @@ PX4FMU::gpio_write(uint32_t gpios, int function) int value = (function == GPIO_SET) ? 1 : 0; for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1<<i)) + if (gpios & (1 << i)) stm32_gpiowrite(_gpio_tab[i].output, value); } @@ -660,7 +712,7 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) lock(); switch (cmd) { - + case GPIO_RESET: gpio_reset(); break; @@ -762,6 +814,7 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); + if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) g_fmu->set_pwm_rate(update_rate); @@ -800,13 +853,18 @@ test(void) fd = open(PWM_OUTPUT_DEVICE_PATH, 0); - if (fd < 0) { - puts("open fail"); - exit(1); - } + if (fd < 0) + errx(1, "open fail"); + + if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - ioctl(fd, PWM_SERVO_ARM, 0); - ioctl(fd, PWM_SERVO_SET(0), 1000); + if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + + if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); close(fd); @@ -816,10 +874,8 @@ test(void) void fake(int argc, char *argv[]) { - if (argc < 5) { - puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); - exit(1); - } + if (argc < 5) + errx(1, "fmu fake <roll> <pitch> <yaw> <thrust> (values -100 .. 100)"); actuator_controls_s ac; @@ -833,10 +889,18 @@ fake(int argc, char *argv[]) orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); - if (handle < 0) { - puts("advertise failed"); - exit(1); - } + if (handle < 0) + errx(1, "advertise failed"); + + actuator_armed_s aa; + + aa.armed = true; + aa.lockdown = false; + + handle = orb_advertise(ORB_ID(actuator_armed), &aa); + + if (handle < 0) + errx(1, "advertise failed 2"); exit(0); } @@ -890,15 +954,17 @@ fmu_main(int argc, char *argv[]) if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { if (argc > i + 1) { pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument for pwm update rate (-u)"); return 1; } + } else { - fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); } } - } + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNSET) { @@ -915,5 +981,5 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command, try:\n"); fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); - return -EINVAL; + exit(1); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index d662e634f..99f573d1d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -55,6 +55,7 @@ #include <unistd.h> #include <termios.h> #include <fcntl.h> +#include <math.h> #include <arch/board/board.h> @@ -70,9 +71,12 @@ #include <systemlib/hx_stream.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> +#include <systemlib/scheduling_priorities.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/rc_channels.h> #include <px4io/protocol.h> @@ -89,8 +93,17 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + /** + * Set the PWM via serial update rate + * @warning this directly affects CPU load + */ + int set_pwm_rate(int hz); + + bool dump_one; + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; + int _update_rate; ///< serial send rate in Hz int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -102,8 +115,13 @@ private: int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + int _t_vstatus; ///< system / vehicle status + vehicle_status_s _vstatus; ///< overall system state orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values @@ -178,13 +196,17 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), + _update_rate(50), _serial_fd(-1), _io_stream(nullptr), _task(-1), _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), + _t_vstatus(-1), _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), @@ -238,7 +260,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); return -errno; @@ -260,6 +282,17 @@ PX4IO::init() return OK; } +int +PX4IO::set_pwm_rate(int hz) +{ + if (hz > 0 && hz <= 400) { + _update_rate = hz; + return OK; + } else { + return -EINVAL; + } +} + void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -270,6 +303,7 @@ void PX4IO::task_main() { log("starting"); + int update_rate_in_ms; /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); @@ -307,12 +341,20 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); /* convert the update rate in hz to milliseconds, rounding down if necessary */ - //int update_rate_in_ms = int(1000 / _update_rate); - orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ + update_rate_in_ms = int(1000 / _update_rate); + orb_set_interval(_t_actuators, update_rate_in_ms); _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -323,13 +365,15 @@ PX4IO::task_main() _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); /* poll descriptor */ - pollfd fds[3]; + pollfd fds[4]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; fds[1].fd = _t_actuators; fds[1].events = POLLIN; fds[2].fd = _t_armed; fds[2].events = POLLIN; + fds[3].fd = _t_vstatus; + fds[3].events = POLLIN; log("ready"); @@ -358,16 +402,37 @@ PX4IO::task_main() if (fds[1].revents & POLLIN) { /* get controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* mix */ if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); + _outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators); + + // XXX output actual limited values + memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); /* convert to PWM values */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _outputs.output[i]); + for (unsigned i = 0; i < _max_actuators; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < _outputs.noutputs && + isfinite(_outputs.output[i]) && + _outputs.output[i] >= -1.0f && + _outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + _outputs.output[i] = 1500 + (600 * _outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = 900; + } + } /* and flag for update */ _send_needed = true; @@ -379,6 +444,11 @@ PX4IO::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } + + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); + _send_needed = true; + } } /* send an update to IO if required */ @@ -482,6 +552,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) _send_needed = true; + /* if monitoring, dump the received info */ + if (dump_one) { + dump_one = false; + + printf("IO: %s armed ", rep->armed ? "" : "not"); + for (unsigned i = 0; i < rep->channel_count; i++) + printf("%d: %d ", i, rep->rc_channel[i]); + printf("\n"); + } + out: unlock(); } @@ -499,14 +579,22 @@ PX4IO::io_send() cmd.servo_command[i] = _outputs.output[i]; /* publish as we send */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + _outputs.timestamp = hrt_absolute_time(); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); + /* update relays */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; - /* armed and not locked down */ + /* armed and not locked down -> arming ok */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); + /* indicate that full autonomous position control / vector flight mode is available */ + cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; + /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ + cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; + /* set desired PWM output rate */ + cmd.servo_rate = _update_rate; ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); if (ret) @@ -695,6 +783,31 @@ test(void) exit(0); } +void +monitor(void) +{ + unsigned cancels = 3; + printf("Hit <enter> three times to exit monitor mode\n"); + + for (;;) { + pollfd fds[1]; + + fds[0].fd = 0; + fds[0].events = POLLIN; + poll(fds, 1, 500); + + if (fds[0].revents == POLLIN) { + int c; + read(0, &c, 1); + if (cancels-- == 0) + exit(0); + } + + if (g_dev != nullptr) + g_dev->dump_one = true; + } +} + } int @@ -716,6 +829,16 @@ px4io_main(int argc, char *argv[]) errx(1, "driver init failed"); } + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 + 1) { + g_dev->set_pwm_rate(atoi(argv[2 + 1])); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + } + exit(0); } @@ -770,8 +893,11 @@ px4io_main(int argc, char *argv[]) !strcmp(argv[1], "rx_sbus") || !strcmp(argv[1], "rx_ppm")) errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + if (!strcmp(argv[1], "test")) test(); + if (!strcmp(argv[1], "monitor")) + monitor(); - errx(1, "need a command, try 'start', 'test', 'rx_ppm', 'rx_dsm', 'rx_sbus' or 'update'"); + errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'"); } diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 50aa34d81..954b458f5 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -60,13 +60,13 @@ #include "drv_pwm_servo.h" -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> /* default rate (in Hz) of PWM updates */ @@ -143,27 +143,27 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { case 1: - rCCMR1(timer) |= (6 << 4); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; rCCR1(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 0); + rCCER(timer) |= GTIM_CCER_CC1E; break; case 2: - rCCMR1(timer) |= (6 << 12); + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; rCCR2(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 4); + rCCER(timer) |= GTIM_CCER_CC2E; break; case 3: - rCCMR2(timer) |= (6 << 4); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; rCCR3(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 8); + rCCER(timer) |= GTIM_CCER_CC3E; break; case 4: - rCCMR2(timer) |= (6 << 12); + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; rCCR4(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 12); + rCCER(timer) |= GTIM_CCER_CC4E; break; } } @@ -288,17 +288,20 @@ up_pwm_servo_set_rate(unsigned rate) void up_pwm_servo_arm(bool armed) { - /* - * XXX this is inelgant and in particular will either jam outputs at whatever level - * they happen to be at at the time the timers stop or generate runts. - * The right thing is almost certainly to kill auto-reload on the timers so that - * they just stop at the end of their count for disable, and to reset/restart them - * for enable. - */ - /* iterate timers and arm/disarm appropriately */ for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - rCR1(i) = armed ? GTIM_CR1_CEN : 0; + if (pwm_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + /* on disarm, just stop auto-reload so we don't generate runts */ + rCR1(i) &= ~GTIM_CR1_ARPE; + } + } } } diff --git a/apps/examples/Kconfig b/apps/examples/Kconfig index 865268add..c0d126ad4 100644 --- a/apps/examples/Kconfig +++ b/apps/examples/Kconfig @@ -3,206 +3,59 @@ # see misc/tools/kconfig-language.txt. # -menu "ADC Example" source "$APPSDIR/examples/adc/Kconfig" -endmenu - -menu "Buttons Example" source "$APPSDIR/examples/buttons/Kconfig" -endmenu - -menu "CAN Example" source "$APPSDIR/examples/can/Kconfig" -endmenu - -menu "USB CDC/ACM Class Driver Example" source "$APPSDIR/examples/cdcacm/Kconfig" -endmenu - -menu "USB composite Class Driver Example" source "$APPSDIR/examples/composite/Kconfig" -endmenu - -menu "DHCP Server Example" +source "$APPSDIR/examples/cxxtest/Kconfig" source "$APPSDIR/examples/dhcpd/Kconfig" -endmenu - -menu "FTP Client Example" +source "$APPSDIR/examples/elf/Kconfig" source "$APPSDIR/examples/ftpc/Kconfig" -endmenu - -menu "FTP Server Example" source "$APPSDIR/examples/ftpd/Kconfig" -endmenu - -menu "\"Hello, World!\" Example" source "$APPSDIR/examples/hello/Kconfig" -endmenu - -menu "\"Hello, World!\" C++ Example" source "$APPSDIR/examples/helloxx/Kconfig" -endmenu - -menu "USB HID Keyboard Example" +source "$APPSDIR/examples/json/Kconfig" source "$APPSDIR/examples/hidkbd/Kconfig" -endmenu - -menu "IGMP Example" source "$APPSDIR/examples/igmp/Kconfig" -endmenu - -menu "LCD Read/Write Example" source "$APPSDIR/examples/lcdrw/Kconfig" -endmenu - -menu "Memory Management Example" source "$APPSDIR/examples/mm/Kconfig" -endmenu - -menu "File System Mount Example" source "$APPSDIR/examples/mount/Kconfig" -endmenu - -menu "FreeModBus Example" source "$APPSDIR/examples/modbus/Kconfig" -endmenu - -menu "Network Test Example" source "$APPSDIR/examples/nettest/Kconfig" -endmenu - -menu "NuttShell (NSH) Example" source "$APPSDIR/examples/nsh/Kconfig" -endmenu - -menu "NULL Example" source "$APPSDIR/examples/null/Kconfig" -endmenu - -menu "NX Graphics Example" source "$APPSDIR/examples/nx/Kconfig" -endmenu - -menu "NxConsole Example" source "$APPSDIR/examples/nxconsole/Kconfig" -endmenu - -menu "NXFFS File System Example" source "$APPSDIR/examples/nxffs/Kconfig" -endmenu - -menu "NXFLAT Example" source "$APPSDIR/examples/nxflat/Kconfig" -endmenu - -menu "NX Graphics \"Hello, World!\" Example" source "$APPSDIR/examples/nxhello/Kconfig" -endmenu - -menu "NX Graphics image Example" source "$APPSDIR/examples/nximage/Kconfig" -endmenu - -menu "NX Graphics lines Example" source "$APPSDIR/examples/nxlines/Kconfig" -endmenu - -menu "NX Graphics Text Example" source "$APPSDIR/examples/nxtext/Kconfig" -endmenu - -menu "OS Test Example" source "$APPSDIR/examples/ostest/Kconfig" -endmenu - -menu "Pascal \"Hello, World!\"example" source "$APPSDIR/examples/pashello/Kconfig" -endmenu - -menu "Pipe Example" source "$APPSDIR/examples/pipe/Kconfig" -endmenu - -menu "Poll Example" source "$APPSDIR/examples/poll/Kconfig" -endmenu - -menu "Pulse Width Modulation (PWM) Example" source "$APPSDIR/examples/pwm/Kconfig" -endmenu - -menu "Quadrature Encoder Example" source "$APPSDIR/examples/qencoder/Kconfig" -endmenu - -menu "RGMP Example" +source "$APPSDIR/examples/relays/Kconfig" source "$APPSDIR/examples/rgmp/Kconfig" -endmenu - -menu "ROMFS Example" source "$APPSDIR/examples/romfs/Kconfig" -endmenu - -menu "sendmail Example" source "$APPSDIR/examples/sendmail/Kconfig" -endmenu - -menu "Serial Loopback Example" source "$APPSDIR/examples/serloop/Kconfig" -endmenu - -menu "Telnet Daemon Example" source "$APPSDIR/examples/telnetd/Kconfig" -endmenu - -menu "THTTPD Web Server Example" source "$APPSDIR/examples/thttpd/Kconfig" -endmenu - -menu "TIFF Generation Example" source "$APPSDIR/examples/tiff/Kconfig" -endmenu - -menu "Touchscreen Example" source "$APPSDIR/examples/touchscreen/Kconfig" -endmenu - -menu "UDP Example" source "$APPSDIR/examples/udp/Kconfig" -endmenu - -menu "UDP Discovery Daemon Example" source "$APPSDIR/examples/discover/Kconfig" -endmenu - -menu "uIP Web Server Example" source "$APPSDIR/examples/uip/Kconfig" -endmenu - -menu "USB Serial Test Example" source "$APPSDIR/examples/usbserial/Kconfig" -endmenu - -menu "USB Mass Storage Class Example" source "$APPSDIR/examples/usbstorage/Kconfig" -endmenu - -menu "USB Serial Terminal Example" source "$APPSDIR/examples/usbterm/Kconfig" -endmenu - -menu "Watchdog timer Example" source "$APPSDIR/examples/watchdog/Kconfig" -endmenu - -menu "wget Example" source "$APPSDIR/examples/wget/Kconfig" -endmenu - -menu "WLAN Example" +source "$APPSDIR/examples/wgetjson/Kconfig" source "$APPSDIR/examples/wlan/Kconfig" -endmenu - -menu "XML RPC Example" source "$APPSDIR/examples/xmlrpc/Kconfig" -endmenu diff --git a/apps/examples/Make.defs b/apps/examples/Make.defs index d03b976d2..3d95ccb16 100644 --- a/apps/examples/Make.defs +++ b/apps/examples/Make.defs @@ -54,6 +54,10 @@ ifeq ($(CONFIG_EXAMPLES_COMPOSITE),y) CONFIGURED_APPS += examples/composite endif +ifeq ($(CONFIG_EXAMPLES_CXXTEST),y) +CONFIGURED_APPS += examples/cxxtest +endif + ifeq ($(CONFIG_EXAMPLES_DHCPD),y) CONFIGURED_APPS += examples/dhcpd endif @@ -62,6 +66,10 @@ ifeq ($(CONFIG_EXAMPLES_DISCOVER),y) CONFIGURED_APPS += examples/discover endif +ifeq ($(CONFIG_EXAMPLES_ELF),y) +CONFIGURED_APPS += examples/elf +endif + ifeq ($(CONFIG_EXAMPLES_FTPC),y) CONFIGURED_APPS += examples/ftpc endif @@ -86,6 +94,10 @@ ifeq ($(CONFIG_EXAMPLES_IGMP),y) CONFIGURED_APPS += examples/igmp endif +ifeq ($(CONFIG_EXAMPLES_JSON),y) +CONFIGURED_APPS += examples/json +endif + ifeq ($(CONFIG_EXAMPLES_LCDRW),y) CONFIGURED_APPS += examples/lcdrw endif @@ -94,6 +106,10 @@ ifeq ($(CONFIG_EXAMPLES_MM),y) CONFIGURED_APPS += examples/mm endif +ifeq ($(CONFIG_EXAMPLES_MODBUS),y) +CONFIGURED_APPS += examples/modbus +endif + ifeq ($(CONFIG_EXAMPLES_MOUNT),y) CONFIGURED_APPS += examples/mount endif @@ -166,6 +182,10 @@ ifeq ($(CONFIG_EXAMPLES_QENCODER),y) CONFIGURED_APPS += examples/qencoder endif +ifeq ($(CONFIG_EXAMPLES_RELAYS),y) +CONFIGURED_APPS += examples/relays +endif + ifeq ($(CONFIG_EXAMPLES_RGMP),y) CONFIGURED_APPS += examples/rgmp endif @@ -226,6 +246,10 @@ ifeq ($(CONFIG_EXAMPLES_WGET),y) CONFIGURED_APPS += examples/wget endif +ifeq ($(CONFIG_EXAMPLES_WGETJSON),y) +CONFIGURED_APPS += examples/wgetjson +endif + ifeq ($(CONFIG_EXAMPLES_WLAN),y) CONFIGURED_APPS += examples/wlan endif diff --git a/apps/examples/Makefile b/apps/examples/Makefile index 453f99ce7..9d20e9312 100644 --- a/apps/examples/Makefile +++ b/apps/examples/Makefile @@ -37,12 +37,12 @@ # Sub-directories -SUBDIRS = adc buttons can cdcacm composite dhcpd discover ftpc ftpd hello -SUBDIRS += helloxx hidkbd igmp lcdrw mm modbus mount nettest nsh null nx -SUBDIRS += nxconsole nxffs nxflat nxhello nximage nxlines nxtext ostest -SUBDIRS += pashello pipe poll pwm qencoder rgmp romfs serloop telnetd -SUBDIRS += thttpd tiff touchscreen udp uip usbserial sendmail usbstorage -SUBDIRS += usbterm watchdog wget wlan +SUBDIRS = adc buttons can cdcacm composite cxxtest dhcpd discover elf ftpc +SUBDIRS += ftpd hello helloxx hidkbd igmp json lcdrw mm modbus mount +SUBDIRS += nettest nsh null nx nxconsole nxffs nxflat nxhello nximage +SUBDIRS += nxlines nxtext ostest pashello pipe poll pwm qencoder relays +SUBDIRS += rgmp romfs serloop telnetd thttpd tiff touchscreen udp uip +SUBDIRS += usbserial sendmail usbstorage usbterm watchdog wget wgetjson wlan # Sub-directories that might need context setup. Directories may need # context setup for a variety of reasons, but the most common is because @@ -57,8 +57,8 @@ SUBDIRS += usbterm watchdog wget wlan CNTXTDIRS = pwm ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) -CNTXTDIRS += adc can cdcacm composite discover ftpd dhcpd modbus nettest -CNTXTDIRS += qencoder telnetd watchdog +CNTXTDIRS += adc can cdcacm composite cxxtestdhcpd discover ftpd json +CNTXTDIRS += modbus nettest nxlines relays qencoder telnetd watchdog wgetjson endif ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y) @@ -79,9 +79,6 @@ endif ifeq ($(CONFIG_EXAMPLES_NXIMAGE_BUILTIN),y) CNTXTDIRS += nximage endif -ifeq ($(CONFIG_EXAMPLES_LINES_BUILTIN),y) -CNTXTDIRS += nxlines -endif ifeq ($(CONFIG_EXAMPLES_NXTEXT_BUILTIN),y) CNTXTDIRS += nxtext endif @@ -105,27 +102,25 @@ all: nothing .PHONY: nothing context depend clean distclean +define SDIR_template +$(1)_$(2): + $(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" +endef + +$(foreach SDIR, $(CNTXTDIRS), $(eval $(call SDIR_template,$(SDIR),context))) +$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),depend))) +$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),clean))) +$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),distclean))) + nothing: -context: - @for dir in $(CNTXTDIRS) ; do \ - $(MAKE) -C $$dir context TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done - -depend: - @for dir in $(SUBDIRS) ; do \ - $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done - -clean: - @for dir in $(SUBDIRS) ; do \ - $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done - -distclean: clean - @for dir in $(SUBDIRS) ; do \ - $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +context: $(foreach SDIR, $(CNTXTDIRS), $(SDIR)_context) + +depend: $(foreach SDIR, $(SUBDIRS), $(SDIR)_depend) + +clean: $(foreach SDIR, $(SUBDIRS), $(SDIR)_clean) + +distclean: clean $(foreach SDIR, $(SUBDIRS), $(SDIR)_distclean) -include Make.dep diff --git a/apps/examples/README.txt b/apps/examples/README.txt index 763427e32..1463b0253 100644 --- a/apps/examples/README.txt +++ b/apps/examples/README.txt @@ -239,6 +239,29 @@ examples/composite CONFIG_EXAMPLES_COMPOSITE_TRACEINTERRUPTS Show interrupt-related events. +examples/cxxtest +^^^^^^^^^^^^^^^^ + + This is a test of the C++ standard library. At present a port of the uClibc++ + C++ library is available. Due to licensinging issues, the uClibc++ C++ library + is not included in the NuttX source tree by default, but must be installed + (see misc/uClibc++/README.txt for installation). + + The NuttX setting that are required include: + + CONFIG_HAVE_CXX=y + CONFIG_HAVE_CXXINITIALIZE=y + CONFIG_UCLIBCXX=y + + Additional uClibc++ settings may be required in your build environment. + + The uClibc++ test includes simple test of: + + - iostreams, + - STL, + - RTTI, and + - Exceptions + examples/dhcpd ^^^^^^^^^^^^^^ @@ -297,6 +320,68 @@ examples/discover CONFIG_EXAMPLES_DISCOVER_DRIPADDR - Router IP address CONFIG_EXAMPLES_DISCOVER_NETMASK - Network Mask +examples/elf +^^^^^^^^^^^^ + + This example builds a small ELF loader test case. This includes several + test programs under examples/elf tests. These tests are build using + the relocatable ELF format and installed in a ROMFS file system. At run time, + each program in the ROMFS file system is executed. Requires CONFIG_ELF. + Other configuration options: + + CONFIG_EXAMPLES_ELF_DEVMINOR - The minor device number of the ROMFS block. + For example, the N in /dev/ramN. Used for registering the RAM block driver + that will hold the ROMFS file system containing the ELF executables to be + tested. Default: 0 + + CONFIG_EXAMPLES_ELF_DEVPATH - The path to the ROMFS block driver device. This + must match EXAMPLES_ELF_DEVMINOR. Used for registering the RAM block driver + that will hold the ROMFS file system containing the ELF executables to be + tested. Default: "/dev/ram0" + + NOTES: + + 1. CFLAGS should be provided in CELFFLAGS. RAM and FLASH memory regions + may require long allcs. For ARM, this might be: + + CELFFLAGS = $(CFLAGS) -mlong-calls + + Similarly for C++ flags which must be provided in CXXELFFLAGS. + + 2. Your top-level nuttx/Make.defs file must alos include an approproate definition, + LDELFFLAGS, to generate a relocatable ELF object. With GNU LD, this should + include '-r' and '-e main' (or _main on some platforms). + + LDELFFLAGS = -r -e main + + If you use GCC to link, you make also need to include '-nostdlib' or + '-nostartfiles' and '-nodefaultlibs'. + + 3. This example also requires genromfs. genromfs can be build as part of the + nuttx toolchain. Or can built from the genromfs sources that can be found + at misc/tools/genromfs-0.5.2.tar.gz. In any event, the PATH variable must + include the path to the genromfs executable. + + 4. ELF size: The ELF files in this example are, be default, quite large + because they include a lot of "build garbage". You can greatly reduce the + size of the ELF binaries are using the 'objcopy --strip-unneeded' command to + remove un-necessary information from the ELF files. + + 5. Simulator. You cannot use this example with the the NuttX simulator on + Cygwin. That is because the Cygwin GCC does not generate ELF file but + rather some Windows-native binary format. + + If you really want to do this, you can create a NuttX x86 buildroot toolchain + and use that be build the ELF executables for the ROMFS file system. + + 6. Linker scripts. You might also want to use a linker scripts to combine + sections better. An example linker script is at nuttx/binfmt/libelf/gnu-elf.ld. + That example might have to be tuned for your particular linker output to + position additional sections correctly. The GNU LD LDELFFLAGS then might + be: + + LDELFFLAGS = -r -e main -T$(TOPDIR)/binfmt/libelf/gnu-elf.ld + examples/ftpc ^^^^^^^^^^^^^ @@ -482,6 +567,19 @@ examples/igmp CONFIGURED_APPS += uiplib +examples/json +^^^^^^^^^^^^^ + + This example exercises the cJSON implementation at apps/netutils/json. + This example contains logic taken from the cJSON project: + + http://sourceforge.net/projects/cjson/ + + The example corresponds to SVN revision r42 (with lots of changes for + NuttX coding standards). As of r42, the SVN repository was last updated + on 2011-10-10 so I presume that the code is stable and there is no risk + of maintaining duplicate logic in the NuttX repository. + examples/lcdrw ^^^^^^^^^^^^^^ @@ -496,6 +594,11 @@ examples/lcdrw * CONFIG_EXAMPLES_LDCRW_YRES LCD Y resolution. Default: 320 + NOTE: This test exercises internal lcd driver interfaces. As such, it + relies on internal OS interfaces that are not normally available to a + user-space program. As a result, this example cannot be used if a + NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL). + examples/mm ^^^^^^^^^^^ @@ -838,8 +941,6 @@ examplex/nxlines The following configuration options can be selected: - CONFIG_EXAMPLES_NXLINES_BUILTIN -- Build the NXLINES example as a "built-in" - that can be executed from the NSH command line CONFIG_EXAMPLES_NXLINES_VPLANE -- The plane to select from the frame- buffer driver for use in the test. Default: 0 CONFIG_EXAMPLES_NXLINES_DEVNO - The LCD device to select from the LCD @@ -877,6 +978,9 @@ examplex/nxlines FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); #endif + CONFIG_NSH_BUILTIN_APPS - Build the NX lines examples as an NSH built-in + function. + examples/nxtext ^^^^^^^^^^^^^^^ @@ -984,6 +1088,17 @@ examples/ostest Specifies the number of threads to create in the barrier test. The default is 8 but a smaller number may be needed on systems without sufficient memory to start so many threads. + * CONFIG_EXAMPLES_OSTEST_RR_RANGE + During round-robin scheduling test two threads are created. Each of the threads + searches for prime numbers in the configurable range, doing that configurable + number of times. + This value specifies the end of search range and together with number of runs + allows to configure the length of this test - it should last at least a few + tens of seconds. Allowed values [1; 32767], default 10000 + * CONFIG_EXAMPLES_OSTEST_RR_RUNS + During round-robin scheduling test two threads are created. Each of the threads + searches for prime numbers in the configurable range, doing that configurable + number of times. examples/pashello ^^^^^^^^^^^^^^^^^ @@ -1110,17 +1225,28 @@ examples/qencoder Specific configuration options for this example include: - CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default: - /dev/qe0 - CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS - is defined, then the number of samples is provided on the command line - and this value is ignored. Otherwise, this number of samples is - collected and the program terminates. Default: Samples are collected - indefinitely. - CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in - milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS - is defined, then this value is the default delay if no other delay is - provided on the command line. Default: 100 milliseconds + CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default: + /dev/qe0 + CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS + is defined, then the number of samples is provided on the command line + and this value is ignored. Otherwise, this number of samples is + collected and the program terminates. Default: Samples are collected + indefinitely. + CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in + milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS + is defined, then this value is the default delay if no other delay is + provided on the command line. Default: 100 milliseconds + +examples/relays +^^^^^^^^^^^^^^^ + + Requires CONFIG_ARCH_RELAYS. + Contributed by Darcy Gong. + + NOTE: This test exercises internal relay driver interfaces. As such, it + relies on internal OS interfaces that are not normally available to a + user-space program. As a result, this example cannot be used if a + NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL). examples/rgmp ^^^^^^^^^^^^^ @@ -1672,7 +1798,16 @@ examples/wget CONFIGURED_APPS += resolv CONFIGURED_APPS += webclient +examples/wget +^^^^^^^^^^^^^ + + Uses wget to get a JSON encoded file, then decodes the file. + + CONFIG_EXAMPLES_WDGETJSON_MAXSIZE - Max. JSON Buffer Size + CONFIG_EXAMPLES_EXAMPLES_WGETJSON_URL - wget URL + examples/xmlrpc +^^^^^^^^^^^^^^^ This example exercises the "Embeddable Lightweight XML-RPC Server" which is discussed at: diff --git a/apps/examples/adc/Makefile b/apps/examples/adc/Makefile index 6357dfc3d..69862b383 100644 --- a/apps/examples/adc/Makefile +++ b/apps/examples/adc/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/adc/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -90,16 +92,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/adc/adc_main.c b/apps/examples/adc/adc_main.c index 404fba8c1..553658fee 100644 --- a/apps/examples/adc/adc_main.c +++ b/apps/examples/adc/adc_main.c @@ -289,7 +289,7 @@ int adc_main(int argc, char *argv[]) { message("adc_main: open %s failed: %d\n", g_adcstate.devpath, errno); errval = 2; - goto errout_with_dev; + goto errout; } /* Now loop the appropriate number of times, displaying the collected @@ -357,6 +357,11 @@ int adc_main(int argc, char *argv[]) } } + close(fd); + return OK; + + /* Error exits */ + errout_with_dev: close(fd); diff --git a/apps/examples/buttons/Makefile b/apps/examples/buttons/Makefile index 25d1ef2c2..77c1cd67d 100644 --- a/apps/examples/buttons/Makefile +++ b/apps/examples/buttons/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/buttons/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -90,16 +92,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/can/Makefile b/apps/examples/can/Makefile index c6dc5af84..8924797e3 100644 --- a/apps/examples/can/Makefile +++ b/apps/examples/can/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/can/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -90,16 +92,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/cdcacm/Makefile b/apps/examples/cdcacm/Makefile index 3fa886d56..e8d03807d 100644 --- a/apps/examples/cdcacm/Makefile +++ b/apps/examples/cdcacm/Makefile @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -80,9 +84,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -93,17 +95,18 @@ $(COBJS): %$(OBJEXT): %.c context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/hello/Makefile b/apps/examples/hello/Makefile index 1d78d723e..560b0da35 100644 --- a/apps/examples/hello/Makefile +++ b/apps/examples/hello/Makefile @@ -54,10 +54,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -90,16 +92,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/helloxx/Makefile b/apps/examples/helloxx/Makefile index 8e85eab23..062da7d58 100644 --- a/apps/examples/helloxx/Makefile +++ b/apps/examples/helloxx/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/helloxx/Makefile # -# Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -50,10 +50,14 @@ CXXOBJS = $(CXXSRCS:.cxx=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS) OBJS = $(AOBJS) $(COBJS) $(CXXOBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -69,7 +73,7 @@ STACKSIZE = 2048 VPATH = all: .built -.PHONY: clean depend disclean chkcxx +.PHONY: clean depend distclean chkcxx chkcxx: ifneq ($(CONFIG_HAVE_CXX),y) @@ -93,9 +97,7 @@ $(CXXOBJS): %$(OBJEXT): %.cxx $(call COMPILEXX, $<, $@) .built: chkcxx $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -107,16 +109,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/mm/Makefile b/apps/examples/mm/Makefile index 24ed4926f..5ba7f4eec 100644 --- a/apps/examples/mm/Makefile +++ b/apps/examples/mm/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/mm/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/mount/Makefile b/apps/examples/mount/Makefile index 69cf970cf..133bdfa1f 100644 --- a/apps/examples/mount/Makefile +++ b/apps/examples/mount/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/Makefile # -# Copyright (C) 2007-2008, 2010-2010 Gregory Nutt. All rights reserved. +# Copyright (C) 2007-2008, 2010-2010, 2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile index bad40fb2e..c7d212fc2 100644 --- a/apps/examples/nsh/Makefile +++ b/apps/examples/nsh/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/nsh/Makefile # -# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2007-2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/null/Makefile b/apps/examples/null/Makefile index 634120600..47ec4cdaf 100644 --- a/apps/examples/null/Makefile +++ b/apps/examples/null/Makefile @@ -1,7 +1,7 @@ ############################################################################ # examples/null/Makefile # -# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2007-2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/ostest/Kconfig b/apps/examples/ostest/Kconfig index 0da7e4ce3..c3fe8f21d 100644 --- a/apps/examples/ostest/Kconfig +++ b/apps/examples/ostest/Kconfig @@ -39,4 +39,31 @@ config EXAMPLES_OSTEST_NBARRIER_THREADS is 8 but a smaller number may be needed on systems without sufficient memory to start so many threads. +config EXAMPLES_OSTEST_RR_RANGE + int "Round-robin test - end of search range" + default 10000 + range 1 32767 + ---help--- + During round-robin scheduling test two threads are created. Each of the threads + searches for prime numbers in the configurable range, doing that configurable + number of times. + + This value specifies the end of search range and together with number of runs + allows to configure the length of this test - it should last at least a few + tens of seconds. Allowed values [1; 32767], default 10000 + +config EXAMPLES_OSTEST_RR_RUNS + int "Round-robin test - number of runs" + default 10 + range 1 32767 + ---help--- + During round-robin scheduling test two threads are created. Each of the threads + searches for prime numbers in the configurable range, doing that configurable + number of times. + + This value specifies the number of times the thread searches the range for + prime numbers and together with end of search range allows to configure the + length of this test - it should last at least a few tens of seconds. Allowed + values [1; 32767], default 10 + endif diff --git a/apps/examples/ostest/Makefile b/apps/examples/ostest/Makefile index 374964b39..3e78c64e8 100644 --- a/apps/examples/ostest/Makefile +++ b/apps/examples/ostest/Makefile @@ -98,10 +98,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -120,9 +124,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -134,16 +136,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/ostest/roundrobin.c b/apps/examples/ostest/roundrobin.c index 5167a857e..bfd344df3 100644 --- a/apps/examples/ostest/roundrobin.c +++ b/apps/examples/ostest/roundrobin.c @@ -1,7 +1,7 @@ /******************************************************************************** * examples/ostest/roundrobin.c * - * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Copyright (C) 2007, 2008, 2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt <gnutt@nuttx.org> * * Redistribution and use in source and binary forms, with or without @@ -39,6 +39,7 @@ #include <nuttx/config.h> #include <stdio.h> +#include <stdbool.h> #include "ostest.h" #if CONFIG_RR_INTERVAL > 0 @@ -47,115 +48,87 @@ * Definitions ********************************************************************************/ -/* This number may need to be tuned for different processor speeds. Since these - * arrays must be large to very correct SCHED_RR behavior, this test may require - * too much memory on many targets. - */ +/* This numbers should be tuned for different processor speeds via .config file. + * With default values the test takes about 30s on Cortex-M3 @ 24MHz. With 32767 + * range and 10 runs it takes ~320s. */ -/* #define CONFIG_NINTEGERS 32768 Takes forever on 60Mhz ARM7 */ - -#define CONFIG_NINTEGERS 2048 - -/******************************************************************************** - * Private Data - ********************************************************************************/ +#ifndef CONFIG_EXAMPLES_OSTEST_RR_RANGE +# define CONFIG_EXAMPLES_OSTEST_RR_RANGE 10000 +# warning "CONFIG_EXAMPLES_OSTEST_RR_RANGE undefined, using default value = 10000" +#elif (CONFIG_EXAMPLES_OSTEST_RR_RANGE < 1) || (CONFIG_EXAMPLES_OSTEST_RR_RANGE > 32767) +# define CONFIG_EXAMPLES_OSTEST_RR_RANGE 10000 +# warning "Invalid value of CONFIG_EXAMPLES_OSTEST_RR_RANGE, using default value = 10000" +#endif -static int prime1[CONFIG_NINTEGERS]; -static int prime2[CONFIG_NINTEGERS]; +#ifndef CONFIG_EXAMPLES_OSTEST_RR_RUNS +# define CONFIG_EXAMPLES_OSTEST_RR_RUNS 10 +# warning "CONFIG_EXAMPLES_OSTEST_RR_RUNS undefined, using default value = 10" +#elif (CONFIG_EXAMPLES_OSTEST_RR_RUNS < 1) || (CONFIG_EXAMPLES_OSTEST_RR_RUNS > 32767) +# define CONFIG_EXAMPLES_OSTEST_RR_RUNS 10 +# warning "Invalid value of CONFIG_EXAMPLES_OSTEST_RR_RUNS, using default value = 10" +#endif /******************************************************************************** * Private Functions ********************************************************************************/ /******************************************************************************** - * Name: dosieve + * Name: get_primes * * Description - * This implements a "sieve of aristophanes" algorithm for finding prime number. - * Credit for this belongs to someone, but I am not sure who anymore. Anyway, - * the only purpose here is that we need some algorithm that takes a long period - * of time to execute. - * + * This function searches for prime numbers in the most primitive way possible. ********************************************************************************/ -static void dosieve(int *prime) +static void get_primes(int *count, int *last) { - int a,d; - int i; - int j; - - a = 2; - d = a; - - for (i = 0; i < CONFIG_NINTEGERS; i++) - { - prime[i] = i+2; - } - - for (i = 1; i < 10; i++) + int number; + int local_count = 0; + *last = 0; // to make compiler happy + + for (number = 1; number < CONFIG_EXAMPLES_OSTEST_RR_RANGE; number++) + { + int div; + bool is_prime = true; + + for (div = 2; div <= number / 2; div++) + if (number % div == 0) + { + is_prime = false; + break; + } + + if (is_prime) { - for (j = 0; j < CONFIG_NINTEGERS; j++) - { - d = a + d; - if (d < CONFIG_NINTEGERS) - { - prime[d]=0; - } - } - a++; - d = a; - i++; - } - + local_count++; + *last = number; #if 0 /* We don't really care what the numbers are */ - for (i = 0, j= 0; i < CONFIG_NINTEGERS; i++) - { - if (prime[i] != 0) - { - printf(" Prime %d: %d\n", j, prime[i]); - j++; - } - } + printf(" Prime %d: %d\n", local_count, number); #endif -} - -/******************************************************************************** - * Name: sieve1 - ********************************************************************************/ - -static void *sieve1(void *parameter) -{ - int i; - - printf("sieve1 started\n"); - - for (i = 0; i < 1000; i++) - { - dosieve(prime1); } + } - printf("sieve1 finished\n"); - - pthread_exit(NULL); - return NULL; /* To keep some compilers happy */ + *count = local_count; } /******************************************************************************** - * Name: sieve2 + * Name: get_primes_thread ********************************************************************************/ -static void *sieve2(void *parameter) +static void *get_primes_thread(void *parameter) { - int i; + int id = (int)parameter; + int i, count, last; - printf("sieve2 started\n"); + printf("get_primes_thread id=%d started, looking for primes < %d, doing %d run(s)\n", + id, CONFIG_EXAMPLES_OSTEST_RR_RANGE, CONFIG_EXAMPLES_OSTEST_RR_RUNS); - for (i = 0; i < 1000; i++) + for (i = 0; i < CONFIG_EXAMPLES_OSTEST_RR_RUNS; i++) { - dosieve(prime2); + get_primes(&count, &last); } - printf("sieve2 finished\n"); + printf("get_primes_thread id=%d finished, found %d primes, last one was %d\n", + id, count, last); pthread_exit(NULL); return NULL; /* To keep some compilers happy */ @@ -171,14 +144,13 @@ static void *sieve2(void *parameter) void rr_test(void) { - pthread_t sieve1_thread; - pthread_t sieve2_thread; + pthread_t get_primes1_thread; + pthread_t get_primes2_thread; struct sched_param sparam; pthread_attr_t attr; pthread_addr_t result; int status; - printf("rr_test: Starting sieve1 thread \n"); status = pthread_attr_init(&attr); if (status != OK) { @@ -203,29 +175,31 @@ void rr_test(void) } else { - printf("rr_test: Set thread policty to SCHED_RR\n"); + printf("rr_test: Set thread policy to SCHED_RR\n"); } - status = pthread_create(&sieve1_thread, &attr, sieve1, NULL); + printf("rr_test: Starting first get_primes_thread\n"); + + status = pthread_create(&get_primes1_thread, &attr, get_primes_thread, (void*)1); if (status != 0) { printf("rr_test: Error in thread 1 creation, status=%d\n", status); } - printf("rr_test: Starting sieve1 thread \n"); + printf("rr_test: Starting second get_primes_thread\n"); - status = pthread_create(&sieve2_thread, &attr, sieve2, NULL); + status = pthread_create(&get_primes2_thread, &attr, get_primes_thread, (void*)2); if (status != 0) { printf("rr_test: Error in thread 2 creation, status=%d\n", status); } - printf("rr_test: Waiting for sieves to complete -- this should take awhile\n"); + printf("rr_test: Waiting for threads to complete -- this should take awhile\n"); printf("rr_test: If RR scheduling is working, they should start and complete at\n"); printf("rr_test: about the same time\n"); - pthread_join(sieve2_thread, &result); - pthread_join(sieve1_thread, &result); + pthread_join(get_primes2_thread, &result); + pthread_join(get_primes1_thread, &result); printf("rr_test: Done\n"); } diff --git a/apps/examples/pipe/Makefile b/apps/examples/pipe/Makefile index 956c911b3..bed319085 100644 --- a/apps/examples/pipe/Makefile +++ b/apps/examples/pipe/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/pipe/Makefile # -# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/poll/Makefile b/apps/examples/poll/Makefile index aef61d199..13173f125 100644 --- a/apps/examples/poll/Makefile +++ b/apps/examples/poll/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/poll/Makefile # -# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,25 +74,25 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ # Register application depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend host + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + $(call DELFILE, host$(HOSTEXEEXT)) -include Make.dep diff --git a/apps/examples/pwm/Makefile b/apps/examples/pwm/Makefile index efbdb048e..3a6f2520a 100644 --- a/apps/examples/pwm/Makefile +++ b/apps/examples/pwm/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/pwm/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -88,16 +90,17 @@ $(COBJS): %$(OBJEXT): %.c context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/pwm/pwm_main.c b/apps/examples/pwm/pwm_main.c index 775bdba6b..a46c10f55 100644 --- a/apps/examples/pwm/pwm_main.c +++ b/apps/examples/pwm/pwm_main.c @@ -48,6 +48,7 @@ #include <fcntl.h> #include <errno.h> #include <debug.h> +#include <string.h> #include <nuttx/pwm.h> diff --git a/apps/examples/qencoder/Makefile b/apps/examples/qencoder/Makefile index 3f3fc9def..7d2427c6b 100644 --- a/apps/examples/qencoder/Makefile +++ b/apps/examples/qencoder/Makefile @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -90,16 +92,17 @@ endif context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/romfs/Makefile b/apps/examples/romfs/Makefile index ba930b77d..2b02952ed 100644 --- a/apps/examples/romfs/Makefile +++ b/apps/examples/romfs/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/romfs/Makefile # -# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -61,7 +65,7 @@ ROOTDEPPATH = --dep-path . VPATH = all: .built -.PHONY: checkgenromfs clean depend disclean +.PHONY: checkgenromfs clean depend distclean $(AOBJS): %$(OBJEXT): %.S $(call ASSEMBLE, $<, $@) @@ -86,26 +90,26 @@ romfs_testdir.h : testdir.img @xxd -i $< >$@ || { echo "xxd of $< failed" ; exit 1 ; } .built: romfs_testdir.h $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ # Register application depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend testdir.img + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + $(call DELFILE, testdir.img) -include Make.dep diff --git a/apps/examples/serloop/Makefile b/apps/examples/serloop/Makefile index e1c415cdd..4a262884b 100644 --- a/apps/examples/serloop/Makefile +++ b/apps/examples/serloop/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/examples/serloop/Makefile # -# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -70,26 +74,25 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ # Register application depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/examples/watchdog/Makefile b/apps/examples/watchdog/Makefile index d2739dbb0..9890959fb 100644 --- a/apps/examples/watchdog/Makefile +++ b/apps/examples/watchdog/Makefile @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built .context: @@ -88,16 +90,17 @@ $(COBJS): %$(OBJEXT): %.c context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c index 334b95226..957036b41 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c @@ -113,6 +113,7 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp) { static int counter = 0; @@ -145,12 +146,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* Roll (P) */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + /* Pitch (P) */ - float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body; - rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0); + + /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ + float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); + /* set pitch plus feedforward roll compensation */ + rates_sp->pitch = pid_calculate(&pitch_controller, + att_sp->pitch_body + pitch_sp_rollcompensation, + att->pitch, 0, 0); /* Yaw (from coordinated turn constraint or lateral force) */ - //TODO + rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) + / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); + +// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); counter++; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/apps/fixedwing_att_control/fixedwing_att_control_att.h index ca7c14b43..11c932800 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_att.h +++ b/apps/fixedwing_att_control/fixedwing_att_control_att.h @@ -41,9 +41,11 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_global_position.h> int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + const float speed_body[], struct vehicle_rates_setpoint_s *rates_sp); #endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 38f58ac33..9c450e68e 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -58,40 +58,16 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> - #include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_att.h> -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - /* Prototypes */ /** * Deamon management function. @@ -126,7 +102,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing att control] started\n"); /* declare and safely initialize all structs */ struct vehicle_attitude_s att; @@ -135,14 +111,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) 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)); -// static struct debug_key_value_s debug_output; -// memset(&debug_output, 0, sizeof(debug_output)); - /* output structs */ struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -153,18 +128,18 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); -// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); -// debug_output.key[0] = '1'; - + 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)); /* 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 = { .fd = att_sub, .events = POLLIN }; while(!thread_should_exit) @@ -172,9 +147,35 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* wait for a sensor update, check for exit condition every 500 ms */ poll(&fds, 1, 500); + /* Check if there is a new position measurement or attitude setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool att_sp_updated; + orb_check(att_sp_sub, &att_sp_updated); + /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + if(att_sp_updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_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; + + printf("FW ATT CONTROL: 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); @@ -182,33 +183,86 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* Control */ + /* control */ if (vstatus.state_machine == SYSTEM_STATE_AUTO) { - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &rates_sp); + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - /* Attitude Rate Control */ + /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - //REMOVEME XXX - actuators.control[3] = 0.7f; - - // debug_output.value = rates_sp.pitch; - // orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output); - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - /* 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; + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; + + } else { + 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) { + + // XXX define failsafe throttle param + //param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.5f; + + // 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 */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &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 output */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* 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("[fixedwing_att_control] exiting, stopping all motors.\n"); @@ -224,6 +278,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) close(att_sub); close(actuator_pub); + close(rates_pub); fflush(stdout); exit(0); @@ -268,7 +323,7 @@ int fixedwing_att_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_att_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index b81d50168..f3e36c0ec 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,8 @@ /** * @file fixedwing_att_control_rate.c * Implementation of a fixed wing attitude controller. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include <fixedwing_att_control_rate.h> @@ -59,9 +61,33 @@ #include <systemlib/geo/geo.h> #include <systemlib/systemlib.h> - - - +/* + * Controller parameters, accessible via MAVLink + * + */ +// Roll control parameters +PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); +PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller +PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); +PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); + +//Pitch control parameters +PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); +PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); +PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller +PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); + +//Yaw control parameters //XXX TODO this is copy paste, asign correct values +PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); +PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec + +/* feedforward compensation */ +PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ struct fw_rate_control_params { float rollrate_p; @@ -73,7 +99,7 @@ struct fw_rate_control_params { float yawrate_p; float yawrate_i; float yawrate_awu; - + float pitch_thr_ff; }; struct fw_rate_control_param_handles { @@ -86,7 +112,7 @@ struct fw_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_awu; - + param_t pitch_thr_ff; }; @@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru static int parameters_init(struct fw_rate_control_param_handles *h) { /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); + h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing + h->rollrate_i = param_find("FW_ROLLR_I"); + h->rollrate_awu = param_find("FW_ROLLR_AWU"); - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); + h->pitchrate_p = param_find("FW_PITCHR_P"); + h->pitchrate_i = param_find("FW_PITCHR_I"); h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->yawrate_p = param_find("FW_YAWR_P"); + h->yawrate_i = param_find("FW_YAWR_I"); + h->yawrate_awu = param_find("FW_YAWR_AWU"); + h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); return OK; } @@ -124,7 +151,7 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_awu, &(p->yawrate_awu)); - + param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); return OK; } @@ -167,12 +194,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } - /* Roll Rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); - - - actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... - actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now + /* roll rate (PI) */ + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + /* pitch rate (PI) */ + actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ + actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; + /* yaw rate (PI) */ + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); counter++; diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile deleted file mode 100644 index 02d4463dd..000000000 --- a/apps/fixedwing_control/Makefile +++ /dev/null @@ -1,44 +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. -# -############################################################################ - -# -# fixedwing_control Application -# - -APPNAME = fixedwing_control -PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 4096 - -CSRCS = fixedwing_control.c - -include $(APPDIR)/mk/app.mk diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c deleted file mode 100644 index e04033481..000000000 --- a/apps/fixedwing_control/fixedwing_control.c +++ /dev/null @@ -1,566 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Ivan Ovinnikov <oivan@ethz.ch> - * Modifications: Doug Weibel <douglas.weibel@colorado.edu> - * - * 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 fixedwing_control.c - * Implementation of a fixed wing attitude and position controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> -#include <uORB/topics/debug_key_value.h> - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f); -// Need to add functionality to suppress integrator windup while on the ground -// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place -PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec -PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - -struct fw_att_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float rollrate_lim; - float roll_p; - float roll_lim; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float pitchrate_lim; - float pitch_p; - float pitch_lim; -}; - -struct fw_att_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t rollrate_lim; - param_t roll_p; - param_t roll_lim; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t pitchrate_lim; - param_t pitch_p; - param_t pitch_lim; -}; - - -// TO_DO - Navigation control will be moved to a separate app -// Attitude control will just handle the inner angle and rate loops -// to control pitch and roll, and turn coordination via rudder and -// possibly throttle compensation for battery voltage sag. - -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f); - -struct fw_pos_control_params { - float heading_p; - float heading_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t heading_lim; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int att_parameters_init(struct fw_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p); - -/** - * Initialize all parameter handles and values - * - */ -static int pos_parameters_init(struct fw_pos_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - - -/** - * The fixed wing control main thread. - * - * The main loop executes continously and calculates the control - * response. - * - * @param argc number of arguments - * @param argv argument array - * - * @return 0 - * - */ -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 */ - printf("[fixedwing control] started\n"); - - /* output structs */ - struct actuator_controls_s actuators; - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* 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 att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - /* Subscribe to global position, attitude and rc */ - /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); - struct vehicle_attitude_s att; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* subscribe to attitude, motor setpoints and system state */ - struct vehicle_global_position_s global_pos; - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_setpoint_s global_setpoint; - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* Mainloop setup */ - unsigned int loopcounter = 0; - - uint64_t last_run = 0; - uint64_t last_run_pos = 0; - - bool global_sp_updated_set_once = false; - - struct fw_att_control_params p; - struct fw_att_control_param_handles h; - - struct fw_pos_control_params ppos; - struct fw_pos_control_param_handles hpos; - - /* initialize the pid controllers */ - att_parameters_init(&h); - att_parameters_update(&h, &p); - - pos_parameters_init(&hpos); - pos_parameters_update(&hpos, &ppos); - -// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere - PID_t roll_rate_controller; - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu, - p.rollrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t roll_angle_controller; - pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f, - p.roll_lim,PID_MODE_DERIVATIV_NONE); - - PID_t pitch_rate_controller; - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu, - p.pitchrate_lim,PID_MODE_DERIVATIV_NONE); - PID_t pitch_angle_controller; - pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f, - p.pitch_lim,PID_MODE_DERIVATIV_NONE); - - PID_t heading_controller; - pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, - 100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit - - // XXX remove in production - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - // This is the top of the main loop - while(!thread_should_exit) { - - struct pollfd fds[1] = { - { .fd = att_sub, .events = POLLIN }, - }; - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n"); - } else { - - // FIXME SUBSCRIBE - if (loopcounter % 100 == 0) { - att_parameters_update(&h, &p); - pos_parameters_update(&hpos, &ppos); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, - p.rollrate_awu, p.rollrate_lim); - pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, - 0.0f, p.roll_lim); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, - p.pitchrate_awu, p.pitchrate_lim); - pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, - 0.0f, p.pitch_lim); - pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f); -//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n", -//p.rollrate_p, p.rollrate_i, p.rollrate_lim); - } - - /* if position updated, run position control loop */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - if (global_sp_updated) { - global_sp_updated_set_once = true; - } - /* checking has to happen before the read, as the read clears the changed flag */ - - /* get a local copy of system state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - //orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - // XXX update to switch between external attitude reference and the - // attitude calculated here - - char name[10]; - - if (pos_updated) { - - /* get position */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (global_sp_updated_set_once) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - - /* calculate delta T */ - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* calculate bearing error */ - float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, - global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); - - /* shift error to prevent wrapping issues */ - float bearing_error = target_bearing - att.yaw; - - if (loopcounter % 2 == 0) { - sprintf(name, "hdng err1"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - if (bearing_error < M_PI_F) { - bearing_error += 2.0f * M_PI_F; - } - - if (bearing_error > M_PI_F) { - bearing_error -= 2.0f * M_PI_F; - } - - if (loopcounter % 2 != 0) { - sprintf(name, "hdng err2"); - memcpy(dbg.key, name, sizeof(name)); - dbg.value = bearing_error; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - /* calculate roll setpoint, do this artificially around zero */ - att_sp.roll_body = pid_calculate(&heading_controller, bearing_error, - 0.0f, att.yawspeed, deltaT); - - /* limit roll angle output */ - if (att_sp.roll_body > ppos.heading_lim) { - att_sp.roll_body = ppos.heading_lim; - heading_controller.saturated = 1; - } - - if (att_sp.roll_body < -ppos.heading_lim) { - att_sp.roll_body = -ppos.heading_lim; - heading_controller.saturated = 1; - } - - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - - } else { - /* no setpoint, maintain level flight */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - } - - att_sp.thrust = 0.7f; - } - - /* calculate delta T */ - const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f; - last_run_pos = hrt_absolute_time(); - - if (verbose && (loopcounter % 20 == 0)) { - printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body); - } - - // actuator control[0] is aileron (or elevon roll control) - // Commanded roll rate from P controller on roll angle - float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body, - att.roll, 0.0f, deltaTpos); - // actuator control from PI controller on roll rate - actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command, - att.rollspeed, 0.0f, deltaTpos); - - // actuator control[1] is elevator (or elevon pitch control) - // Commanded pitch rate from P controller on pitch angle - float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body, - att.pitch, 0.0f, deltaTpos); - // actuator control from PI controller on pitch rate - actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command, - att.pitchspeed, 0.0f, deltaTpos); - - // actuator control[3] is throttle - actuators.control[3] = att_sp.thrust; - - /* publish attitude setpoint (for MAVLink) */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - /* publish actuator setpoints (for mixer) */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - loopcounter++; - - } - } - - printf("[fixedwing_control] exiting.\n"); - thread_running = false; - - return 0; -} - -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_spawn("fixedwing_control", - SCHED_DEFAULT, - 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); -} - -static int att_parameters_init(struct fw_att_control_param_handles *h) -{ - /* PID parameters */ - - h->rollrate_p = param_find("FW_ROLLRATE_P"); - h->rollrate_i = param_find("FW_ROLLRATE_I"); - h->rollrate_awu = param_find("FW_ROLLRATE_AWU"); - h->rollrate_lim = param_find("FW_ROLLRATE_LIM"); - h->roll_p = param_find("FW_ROLL_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitchrate_p = param_find("FW_PITCHRATE_P"); - h->pitchrate_i = param_find("FW_PITCHRATE_I"); - h->pitchrate_awu = param_find("FW_PITCHRATE_AWU"); - h->pitchrate_lim = param_find("FW_PITCHRATE_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->roll_p, &(p->roll_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - -static int pos_parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->heading_lim = param_find("FW_HEADING_LIM"); - - return OK; -} - -static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->heading_lim, &(p->heading_lim)); - - return OK; -} diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index a70b9bd30..fbd6138de 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -57,24 +57,31 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> #include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> /* * Controller parameters, accessible via MAVLink * */ -PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); +PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians - +PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ struct fw_pos_control_params { float heading_p; + float headingr_p; + float headingr_i; + float headingr_lim; float xtrack_p; float altitude_p; float roll_lim; @@ -83,11 +90,13 @@ struct fw_pos_control_params { struct fw_pos_control_param_handles { param_t heading_p; + param_t headingr_p; + param_t headingr_i; + param_t headingr_lim; param_t xtrack_p; param_t altitude_p; param_t roll_lim; param_t pitch_lim; - }; @@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ - h->heading_p = param_find("FW_HEADING_P"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - + h->heading_p = param_find("FW_HEAD_P"); + h->headingr_p = param_find("FW_HEADR_P"); + h->headingr_i = param_find("FW_HEADR_I"); + h->headingr_lim = param_find("FW_HEADR_LIM"); + h->xtrack_p = param_find("FW_XTRACK_P"); + h->altitude_p = param_find("FW_ALT_P"); + h->roll_lim = param_find("FW_ROLL_LIM"); + h->pitch_lim = param_find("FW_PITCH_LIM"); return OK; } @@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h) static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { param_get(h->heading_p, &(p->heading_p)); + param_get(h->headingr_p, &(p->headingr_p)); + param_get(h->headingr_i, &(p->headingr_i)); + param_get(h->headingr_lim, &(p->headingr_lim)); param_get(h->xtrack_p, &(p->xtrack_p)); param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); @@ -171,7 +185,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* welcome user */ - printf("[fixedwing att_control] started\n"); + printf("[fixedwing pos control] started\n"); /* declare and safely initialize all structs */ struct vehicle_global_position_s global_pos; @@ -184,6 +198,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct crosstrack_error_s xtrack_err; memset(&xtrack_err, 0, sizeof(xtrack_err)); + struct parameter_update_s param_update; + memset(¶m_update, 0, sizeof(param_update)); /* output structs */ struct vehicle_attitude_setpoint_s attitude_setpoint; @@ -193,129 +209,196 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) attitude_setpoint.roll_body = 0.0f; attitude_setpoint.pitch_body = 0.0f; attitude_setpoint.yaw_body = 0.0f; + attitude_setpoint.thrust = 0.0f; orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); /* subscribe */ int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + struct pollfd fds[2] = { + { .fd = param_sub, .events = POLLIN }, + { .fd = att_sub, .events = POLLIN } + }; bool global_sp_updated_set_once = false; float psi_track = 0.0f; - while(!thread_should_exit) - { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - static int counter = 0; - static bool initialized = false; - - static struct fw_pos_control_params p; - static struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t altitude_controller; - - if(!initialized) - { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE); - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - - } - - /* Check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* Load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if(pos_updated) - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - - if(global_sp_updated) { - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - printf("psi_track: %0.4f\n", (double)psi_track); - } - - /* Control */ - - - /* Simple Horizontal Control */ - if(global_sp_updated_set_once) - { -// if (counter % 100 == 0) -// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - if(!(distance_res != OK || xtrack_err.past_end)) { + int counter = 0; - float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards + struct fw_pos_control_params p; + struct fw_pos_control_param_handles h; - if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F) - delta_psi_c = 60.0f*M_DEG_TO_RAD_F; + PID_t heading_controller; + PID_t heading_rate_controller; + PID_t offtrack_controller; + PID_t altitude_controller; - if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F) - delta_psi_c = -60.0f*M_DEG_TO_RAD_F; + parameters_init(&h); + parameters_update(&h, &p); + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f*M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value - float psi_c = psi_track + delta_psi_c; + /* error and performance monitoring */ + perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); + perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - float psi_e = psi_c - att.yaw; - - /* shift error to prevent wrapping issues */ - psi_e = _wrap_pi(psi_e); - - /* calculate roll setpoint, do this artificially around zero */ - attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - -// if (counter % 100 == 0) -// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); - } - else { - if (counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + while(!thread_should_exit) + { + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(fw_err_perf); + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), param_sub, &update); + + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f*M_DEG_TO_RAD); //TODO: remove hardcoded value } - } - - /* Very simple Altitude Control */ - if(global_sp_updated_set_once && pos_updated) - { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* check if there is a new position or setpoint */ + bool pos_updated; + orb_check(global_pos_sub, &pos_updated); + bool global_sp_updated; + orb_check(global_setpoint_sub, &global_sp_updated); + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + } + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); + start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) + global_sp_updated_set_once = true; + psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + printf("next wp direction: %0.4f\n", (double)psi_track); + } + + /* Simple Horizontal Control */ + if(global_sp_updated_set_once) + { + // if (counter % 100 == 0) + // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, + (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); + + // XXX what is xtrack_err.past_end? + if(distance_res == OK /*&& !xtrack_err.past_end*/) { + + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + + float psi_c = psi_track + delta_psi_c; + float psi_e = psi_c - att.yaw; + + /* wrap difference back onto -pi..pi range */ + psi_e = _wrap_pi(psi_e); + + if (verbose) { + printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); + printf("delta_psi_c %.4f ", (double)delta_psi_c); + printf("psi_c %.4f ", (double)psi_c); + printf("att.yaw %.4f ", (double)att.yaw); + printf("psi_e %.4f ", (double)psi_e); + } + + /* calculate roll setpoint, do this artificially around zero */ + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following + float psi_rate_c = delta_psi_rate_c + psi_rate_track; + + /* limit turn rate */ + if(psi_rate_c > p.headingr_lim) { + psi_rate_c = p.headingr_lim; + } else if(psi_rate_c < -p.headingr_lim) { + psi_rate_c = -p.headingr_lim; + } + + float psi_rate_e = psi_rate_c - att.yawspeed; + + // XXX sanity check: Assume 10 m/s stall speed and no stall condition + float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + + if (ground_speed < 10.0f) { + ground_speed = 10.0f; + } + + float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g + + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + + if (verbose) { + printf("psi_rate_c %.4f ", (double)psi_rate_c); + printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); + printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); + } + + if (verbose && counter % 100 == 0) + printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c); + } else { + if (verbose && counter % 100 == 0) + printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); + } + + /* Very simple Altitude Control */ + if(pos_updated) + { + + //TODO: take care of relative vs. ab. altitude + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + + } + + // XXX need speed control + attitude_setpoint.thrust = 0.7f; + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); + + /* measure in what intervals the controller runs */ + perf_count(fw_interval_perf); + + counter++; + + } else { + // XXX no setpoint, decent default needed (loiter?) + } + } } - /*Publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - counter++; } printf("[fixedwing_pos_control] exiting.\n"); @@ -367,7 +450,7 @@ int fixedwing_pos_control_main(int argc, char *argv[]) deamon_task = task_spawn("fixedwing_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4096, + 2048, fixedwing_pos_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; diff --git a/apps/interpreters/Makefile b/apps/interpreters/Makefile index 5901fc830..867d45f99 100644 --- a/apps/interpreters/Makefile +++ b/apps/interpreters/Makefile @@ -33,7 +33,7 @@ # ############################################################################ --include $(TOPDIR)/.config # Current configuration +-include $(TOPDIR)/.config # Sub-directories containing interpreter runtime @@ -41,30 +41,36 @@ SUBDIRS = pcode ficl # Create the list of installed runtime modules (INSTALLED_DIRS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) +define ADD_DIRECTORY +INSTALLED_DIRS += ${shell if exist $1\Makefile (echo $1)} +endef +else define ADD_DIRECTORY INSTALLED_DIRS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} endef +endif $(foreach DIR, $(SUBDIRS), $(eval $(call ADD_DIRECTORY,$(DIR)))) all: nothing .PHONY: nothing context depend clean distclean +define SDIR_template +$(1)_$(2): + $(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" +endef + +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),depend))) +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),clean))) +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),distclean))) + nothing: context: -depend: - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +depend: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_depend) -clean: - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +clean: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_clean) -distclean: clean - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +distclean: clean $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_distclean) diff --git a/apps/interpreters/ficl/Makefile b/apps/interpreters/ficl/Makefile index fb953964e..990630fb8 100644 --- a/apps/interpreters/ficl/Makefile +++ b/apps/interpreters/ficl/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/interpreters/ficl/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -35,14 +35,11 @@ BUILDDIR := ${shell pwd | sed -e 's/ /\\ /g'} --include $(TOPDIR)/.config -include $(TOPDIR)/Make.defs include $(APPDIR)/Make.defs # Tools -INCDIR = $(TOPDIR)/tools/incdir.sh - ifeq ($(WINTOOL),y) INCDIROPT = -w endif @@ -69,10 +66,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOT_DEPPATH = --dep-path . @@ -95,24 +96,24 @@ debug: @#echo "CFLAGS: $(CFLAGS)" .built: debug $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built context: .depend: debug Makefile $(SRCS) - @$(MKDEP) $(ROOT_DEPPATH) $(SRC_DEPPATH) $(FICL_DEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOT_DEPPATH) $(SRC_DEPPATH) $(FICL_DEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .context) + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 81b907bcd..3351d9cfd 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -189,10 +189,6 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -231,11 +227,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) case SYSTEM_STATE_STABILIZED: *mavlink_state = MAV_STATE_ACTIVE; *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case SYSTEM_STATE_AUTO: *mavlink_state = MAV_STATE_ACTIVE; *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case SYSTEM_STATE_MISSION_ABORT: @@ -470,14 +469,15 @@ mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) void mavlink_update_system(void) { static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + initialized = true; } /* update system and component id */ @@ -745,6 +745,7 @@ int mavlink_main(int argc, char *argv[]) thread_should_exit = true; while (thread_running) { usleep(200000); + printf("."); } warnx("terminated."); exit(0); diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 7cb1582b4..9d9b9914a 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0; */ void mavlink_pm_callback(void *arg, param_t param); -/** - * Save parameters to EEPROM. - * - * Stores the parameters to /eeprom/parameters - */ -static int mavlink_pm_save_eeprom(void); - -/** - * Load parameters from EEPROM. - * - * Loads the parameters from /eeprom/parameters - */ -static int mavlink_pm_load_eeprom(void); - void mavlink_pm_callback(void *arg, param_t param) { mavlink_pm_send_param(param); @@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } break; - - // case MAVLINK_MSG_ID_COMMAND_LONG: { - // mavlink_command_long_t cmd_mavlink; - // mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - // uint8_t result = MAV_RESULT_UNSUPPORTED; - - // if (cmd_mavlink.target_system == mavlink_system.sysid && - // ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - - // // XXX move this to LOW PRIO THREAD of commander app - - // /* preflight parameter load / store */ - // if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { - // /* Read all parameters from EEPROM to RAM */ - - // if (((int)(cmd_mavlink.param1)) == 0) { - - // /* read all parameters from EEPROM to RAM */ - // int read_ret = param_load_default(); - // if (read_ret == OK) { - // //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); - // mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - // } else if (read_ret == 1) { - // mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - // } else { - // if (read_ret < -1) { - // mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file()); - // } else { - // mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file()); - // } - // result = MAV_RESULT_FAILED; - // } - - // } else if (((int)(cmd_mavlink.param1)) == 1) { - - // /* write all parameters from RAM to EEPROM */ - // int write_ret = param_save_default(); - // if (write_ret == OK) { - // mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file()); - // result = MAV_RESULT_ACCEPTED; - - // } else { - // if (write_ret < -1) { - // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); - // } else { - // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); - // } - // result = MAV_RESULT_FAILED; - // } - - // } else { - // //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); - // mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request"); - // result = MAV_RESULT_UNSUPPORTED; - // } - // } - // } - - // /* send back command result */ - // //mavlink_msg_command_ack_send(chan, cmd.command, result); - // } break; } } diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed..58761e89c 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg) offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX; if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { ml_armed = false; @@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if (mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -305,6 +325,7 @@ handle_message(mavlink_message_t *msg) hil_global_pos.vy = hil_state.vy / 100.0f; hil_global_pos.vz = hil_state.vz / 100.0f; + /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index a067460d8..971ba6a8e 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -114,6 +114,7 @@ static void l_manual_control_setpoint(struct listener *l); static void l_vehicle_attitude_controls(struct listener *l); static void l_debug_key_value(struct listener *l); static void l_optical_flow(struct listener *l); +static void l_vehicle_rates_setpoint(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -136,6 +137,7 @@ struct listener listeners[] = { {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, + {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -409,6 +411,23 @@ l_attitude_setpoint(struct listener *l) } void +l_vehicle_rates_setpoint(struct listener *l) +{ + struct vehicle_rates_setpoint_s rates_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + rates_sp.timestamp/1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); +} + +void l_actuator_outputs(struct listener *l) { struct actuator_outputs_s act_outputs; @@ -487,20 +506,19 @@ l_actuator_outputs(struct listener *l) mavlink_mode, 0); } else { - float rudder, throttle; - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; } mavlink_msg_hil_controls_send(chan, @@ -547,28 +565,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_controls_s actuators; + struct actuator_controls_effective_s actuators; - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl0 ", - actuators.control[0]); + "eff ctrl0 ", + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl1 ", - actuators.control[1]); + "eff ctrl1 ", + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl2 ", - actuators.control[2]); + "eff ctrl2 ", + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - "ctrl3 ", - actuators.control[3]); + "eff ctrl3 ", + actuators.control_effective[3]); } } @@ -695,6 +713,10 @@ uorb_receive_start(void) mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + /* --- ACTUATOR OUTPUTS --- */ mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); @@ -716,7 +738,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 61e664401..4650c4991 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -79,6 +80,7 @@ struct mavlink_subscriptions { int debug_key_value; int input_rc_sub; int optical_flow; + int rates_setpoint_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ce1e52c1b..f184859a3 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -232,19 +232,9 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - /* decide wether we want rate or position input */ - } - else if (state.flag_control_manual_enabled) { - - /* manual inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { - rates_sp.roll = manual.roll; + - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + } else if (state.flag_control_manual_enabled) { if (state.flag_control_attitude_enabled) { @@ -258,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if(state.rc_signal_lost) { + if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.0f; @@ -285,41 +275,66 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; } - /* only move setpoint if manual input is != 0 */ + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) { - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + control_yaw_position = true; } - control_yaw_position = true; } - } else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); } - } - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d..b98736cee 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -197,7 +197,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,6 +206,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } + /* reset integral if on ground */ + if(att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ diff --git a/apps/namedapp/Makefile b/apps/namedapp/Makefile index 6b0fd6a05..a88c73567 100644 --- a/apps/namedapp/Makefile +++ b/apps/namedapp/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/nshlib/Makefile # -# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. # Author: Gregory Nutt <gnutt@nuttx.org> # # Redistribution and use in source and binary forms, with or without @@ -54,10 +54,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -75,32 +79,32 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built .context: @echo "/* List of application requirements, generated during make context. */" > namedapp_list.h @echo "/* List of application entry points, generated during make context. */" > namedapp_proto.h - @touch $@ + $(Q) touch $@ context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f .context Make.dep .depend - @rm -f namedapp_list.h - @rm -f namedapp_proto.h + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + $(call DELFILE, namedapp_list.h) + $(call DELFILE, namedapp_proto.h) -include Make.dep diff --git a/apps/nshlib/Kconfig b/apps/nshlib/Kconfig index d12a32973..17b107b8f 100644 --- a/apps/nshlib/Kconfig +++ b/apps/nshlib/Kconfig @@ -23,122 +23,194 @@ config NSH_BUILTIN_APPS (NAMEDAPP). menu "Disable Individual commands" + +config NSH_DISABLE_BASE64DEC + bool "Disable base64dec" + default n + depends on NETUTILS_CODECS && CODECS_BASE64 + +config NSH_DISABLE_BASE64ENC + bool "Disable base64enc" + default n + depends on NETUTILS_CODECS && CODECS_BASE64 + config NSH_DISABLE_CAT bool "Disable cat" default n + config NSH_DISABLE_CD bool "Disable cd" default n + config NSH_DISABLE_CP bool "Disable cp" default n + config NSH_DISABLE_DD bool "Disable dd" default n + config NSH_DISABLE_ECHO bool "Disable echo" default n + config NSH_DISABLE_EXEC bool "Disable exec" default n + config NSH_DISABLE_EXIT bool "Disable exit" default n + config NSH_DISABLE_FREE bool "Disable free" default n + config NSH_DISABLE_GET bool "Disable get" default n + config NSH_DISABLE_HELP bool "Disable help" default n + +config NSH_DISABLE_HEXDUMP + bool "Disable hexdump" + default n + config NSH_DISABLE_IFCONFIG bool "Disable ifconfig" default n + config NSH_DISABLE_KILL bool "Disable kill" default n + config NSH_DISABLE_LOSETUP bool "Disable losetup" default n + config NSH_DISABLE_LS bool "Disable ls" default n + config NSH_DISABLE_MB bool "Disable mb" default n + +config NSH_DISABLE_MD5 + bool "Disable md5" + default n + depends on NETUTILS_CODECS && CODECS_HASH_MD5 + config NSH_DISABLE_MKDIR bool "Disable mkdir" default n + config NSH_DISABLE_MKFATFS bool "Disable mkfatfs" default n + config NSH_DISABLE_MKFIFO bool "Disable mkfifo" default n + config NSH_DISABLE_MKRD bool "Disable mkrd" default n + config NSH_DISABLE_MH bool "Disable mh" default n + config NSH_DISABLE_MOUNT bool "Disable mount" default n + config NSH_DISABLE_MW bool "Disable mw" default n + config NSH_DISABLE_NSFMOUNT bool "Disable nfsmount" default n + config NSH_DISABLE_PS bool "Disable ps" default n + config NSH_DISABLE_PING bool "Disable ping" default n + config NSH_DISABLE_PUT bool "Disable put" default n + config NSH_DISABLE_PWD bool "Disable pwd" default n + config NSH_DISABLE_RM bool "Disable rm" default n + config NSH_DISABLE_RMDIR bool "Disable rmdir" default n + config NSH_DISABLE_SET bool "Disable set" default n + config NSH_DISABLE_SH bool "Disable sh" default n + config NSH_DISABLE_SLEEP bool "Disable sleep" default n + config NSH_DISABLE_TEST bool "Disable test" default n + config NSH_DISABLE_UMOUNT bool "Disable umount" default n + config NSH_DISABLE_UNSET bool "Disable unset" default n + +config NSH_DISABLE_URLDECODE + bool "Disable urldecode" + default n + depends on NETUTILS_CODECS && CODECS_URLCODE + +config NSH_DISABLE_URLENCODE + bool "Disable urlencode" + default n + depends on NETUTILS_CODECS && CODECS_URLCODE + config NSH_DISABLE_USLEEP bool "Disable usleep" default n + config NSH_DISABLE_WGET bool "Disable wget" default n + config NSH_DISABLE_XD bool "Disable xd" default n + endmenu +config NSH_CODECS_BUFSIZE + int "File buffer size used by CODEC commands" + default 128 + config NSH_FILEIOSIZE int "NSH I/O buffer size" default 1024 @@ -490,7 +562,7 @@ config NSH_DHCPC config NSH_IPADDR hex "Target IP address" - default 0x10000002 + default 0xa0000002 depends on NSH_LIBRARY && NET && !NSH_DHCPC ---help--- If NSH_DHCPC is NOT set, then the static IP address must be provided. @@ -499,7 +571,7 @@ config NSH_IPADDR config NSH_DRIPADDR hex "Router IP address" - default 0x10000001 + default 0xa0000001 depends on NSH_LIBRARY && NET && !NSH_DHCPC ---help--- Default router IP address (aka, Gateway). This is a 32-bit integer @@ -513,6 +585,21 @@ config NSH_NETMASK Network mask. This is a 32-bit integer value in host order. So, as an example, 0xffffff00 would be 255.255.255.0. +config NSH_DNS + bool "Use DNS" + default n + depends on NSH_LIBRARY && NET && NET_UDP && NET_BROADCAST + ---help--- + Configure to use a DNS. + +config NSH_DNSIPADDR + hex "DNS IP address" + default 0xa0000001 + depends on NSH_DNS + ---help--- + Configure the DNS address. This is a 32-bit integer value in host + order. So, as an example, 0xa0000001 would be 10.0.0.1. + config NSH_NOMAC bool "Hardware has no MAC address" default n @@ -520,3 +607,12 @@ config NSH_NOMAC ---help--- Set if your ethernet hardware has no built-in MAC address. If set, a bogus MAC will be assigned. + +config NSH_MAX_ROUNDTRIP + int "Max Ping Round-Trip (DSEC)" + default 20 + depends on NSH_LIBRARY && NET && !NSH_DISABLE_PING + ---help--- + This is the maximum round trip for a response to a ICMP ECHO request. + It is in units of deciseconds. The default is 20 (2 seconds). + diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile index f616374bf..73325e899 100644 --- a/apps/nshlib/Makefile +++ b/apps/nshlib/Makefile @@ -39,64 +39,72 @@ include $(APPDIR)/Make.defs # NSH Library -ASRCS = -CSRCS = nsh_init.c nsh_parse.c nsh_console.c nsh_fscmds.c nsh_ddcmd.c \ +ASRCS = +CSRCS = nsh_init.c nsh_parse.c nsh_console.c nsh_fscmds.c nsh_ddcmd.c \ nsh_proccmds.c nsh_mmcmds.c nsh_envcmds.c nsh_dbgcmds.c ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) -CSRCS += nsh_apps.c +CSRCS += nsh_apps.c endif ifeq ($(CONFIG_NSH_ROMFSETC),y) -CSRCS += nsh_romfsetc.c +CSRCS += nsh_romfsetc.c endif ifeq ($(CONFIG_NET),y) -CSRCS += nsh_netinit.c nsh_netcmds.c +CSRCS += nsh_netinit.c nsh_netcmds.c endif ifeq ($(CONFIG_RTC),y) -CSRCS += nsh_timcmds.c +CSRCS += nsh_timcmds.c endif ifneq ($(CONFIG_DISABLE_MOUNTPOINT),y) -CSRCS += nsh_mntcmds.c +CSRCS += nsh_mntcmds.c endif ifeq ($(CONFIG_NSH_CONSOLE),y) -CSRCS += nsh_consolemain.c +CSRCS += nsh_consolemain.c endif ifeq ($(CONFIG_NSH_TELNET),y) -CSRCS += nsh_telnetd.c +CSRCS += nsh_telnetd.c endif ifneq ($(CONFIG_NSH_DISABLESCRIPT),y) -CSRCS += nsh_test.c +CSRCS += nsh_test.c endif ifeq ($(CONFIG_USBDEV),y) -CSRCS += nsh_usbdev.c +CSRCS += nsh_usbdev.c endif -AOBJS = $(ASRCS:.S=$(OBJEXT)) -COBJS = $(CSRCS:.c=$(OBJEXT)) +ifeq ($(CONFIG_NETUTILS_CODECS),y) +CSRCS += nsh_codeccmd.c +endif + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) -SRCS = $(ASRCS) $(CSRCS) -OBJS = $(AOBJS) $(COBJS) +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../libapps$(LIBEXT) +endif endif -ROOTDEPPATH = --dep-path . -VPATH = +ROOTDEPPATH = --dep-path . +VPATH = # Build targets -all: .built +all: .built .PHONY: context .depend depend clean distclean $(AOBJS): %$(OBJEXT): %.S @@ -106,26 +114,25 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) + $(call ARCHIVE, $(BIN), $(OBJS)) @touch .built context: .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) \ - $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep @touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/nshlib/README.txt b/apps/nshlib/README.txt index 0f6aee759..59f0538f0 100644 --- a/apps/nshlib/README.txt +++ b/apps/nshlib/README.txt @@ -235,6 +235,10 @@ o test <expression> integer -gt integer | integer -le integer | integer -lt integer | integer -ne integer +o base64dec [-w] [-f] <string or filepath> + +o base64dec [-w] [-f] <string or filepath> + o cat <path> [<path> [<path> ...]] This command copies and concatentates all of the files at <path> @@ -381,7 +385,11 @@ o help [-v] [<cmd>] <cmd> Show full command usage only for this command -o ifconfig +o hexdump <file or device> + + Dump data in hexadecimal format from a file or character device. + +o ifconfig [nic_name [ip]] [dr|gw|gateway <dr-address>] [netmask <net-mask>] [dns <dns-address>] [hw <hw-mac>] Show the current configuration of the network, for example: @@ -392,6 +400,22 @@ o ifconfig if uIP statistics are enabled (CONFIG_NET_STATISTICS), then this command will also show the detailed state of uIP. +o ifdown <nic-name> + + Take down the interface identified by the name <nic-name>. + + Example: + + ifdown eth0 + +o ifup <nic-name> + + Bring up down the interface identified by the name <nic-name>. + + Example: + + ifup eth0 + o kill -<signal> <pid> Send the <signal> to the task identified by <pid>. @@ -449,6 +473,8 @@ o ls [-lRs] <dir-path> -l Show size and mode information along with the filenames in the listing. +o md5 [-f] <string or filepath> + o mb <hex-address>[=<hex-value>][ <hex-byte-count>] o mh <hex-address>[=<hex-value>][ <hex-byte-count>] o mw <hex-address>[=<hex-value>][ <hex-byte-count>] @@ -781,6 +807,10 @@ o unset <name> nsh> + o urldecode [-f] <string or filepath> + + o urlencode [-f] <string or filepath> + o usleep <usec> Pause execution (sleep) of <usec> microseconds. @@ -826,6 +856,8 @@ Command Dependencies on Configuration Settings Command Depends on Configuration ---------- -------------------------- [ !CONFIG_NSH_DISABLESCRIPT + base64dec CONFIG_NETUTILS_CODECS && CONFIG_CODECS_BASE64 + base64enc CONFIG_NETUTILS_CODECS && CONFIG_CODECS_BASE64 cat CONFIG_NFILE_DESCRIPTORS > 0 cd !CONFIG_DISABLE_ENVIRON && CONFIG_NFILE_DESCRIPTORS > 0 cp CONFIG_NFILE_DESCRIPTORS > 0 @@ -837,10 +869,14 @@ Command Dependencies on Configuration Settings free -- get CONFIG_NET && CONFIG_NET_UDP && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NET_BUFSIZE >= 558 (see note 1) help -- + hexdump CONFIG_NFILE_DESCRIPTORS > 0 ifconfig CONFIG_NET + ifdown CONFIG_NET + ifup CONFIG_NET kill !CONFIG_DISABLE_SIGNALS losetup !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 ls CONFIG_NFILE_DESCRIPTORS > 0 + md5 CONFIG_NETUTILS_CODECS && CONFIG_CODECS_HASH_MD5 mb,mh,mw --- mkdir !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) mkfatfs !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_FAT @@ -861,6 +897,8 @@ Command Dependencies on Configuration Settings test !CONFIG_NSH_DISABLESCRIPT umount !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_READABLE unset !CONFIG_DISABLE_ENVIRON + urldecode CONFIG_NETUTILS_CODECS && CONFIG_CODECS_URLCODE + urlencode CONFIG_NETUTILS_CODECS && CONFIG_CODECS_URLCODE usleep !CONFIG_DISABLE_SIGNALS get CONFIG_NET && CONFIG_NET_TCP && CONFIG_NFILE_DESCRIPTORS > 0 xd --- @@ -880,20 +918,22 @@ In addition, each NSH command can be individually disabled via one of the follow settings. All of these settings make the configuration of NSH potentially complex but also allow it to squeeze into very small memory footprints. - CONFIG_NSH_DISABLE_CAT, CONFIG_NSH_DISABLE_CD, CONFIG_NSH_DISABLE_CP, - CONFIG_NSH_DISABLE_DD, CONFIG_NSH_DISABLE_DF, CONFIG_NSH_DISABLE_ECHO, - CONFIG_NSH_DISABLE_EXEC, CONFIG_NSH_DISABLE_EXIT, CONFIG_NSH_DISABLE_FREE, - CONFIG_NSH_DISABLE_GET, CONFIG_NSH_DISABLE_HELP, CONFIG_NSH_DISABLE_IFCONFIG, - CONFIG_NSH_DISABLE_KILL, CONFIG_NSH_DISABLE_LOSETUP, CONFIG_NSH_DISABLE_LS, - CONFIG_NSH_DISABLE_MB, CONFIG_NSH_DISABLE_MKDIR, CONFIG_NSH_DISABLE_MKFATFS, - CONFIG_NSH_DISABLE_MKFIFO, CONFIG_NSH_DISABLE_MKRD, CONFIG_NSH_DISABLE_MH, - CONFIG_NSH_DISABLE_MOUNT, CONFIG_NSH_DISABLE_MW, CONFIG_NSH_DISABLE_MV, - CONFIG_NSH_DISABLE_NFSMOUNT, CONFIG_NSH_DISABLE_PS, CONFIG_NSH_DISABLE_PING, - CONFIG_NSH_DISABLE_PUT, CONFIG_NSH_DISABLE_PWD, CONFIG_NSH_DISABLE_RM, - CONFIG_NSH_DISABLE_RMDIR, CONFIG_NSH_DISABLE_SET, CONFIG_NSH_DISABLE_SH, - CONFIG_NSH_DISABLE_SLEEP, CONFIG_NSH_DISABLE_TEST, CONFIG_NSH_DISABLE_UMOUNT, - CONFIG_NSH_DISABLE_UNSET, CONFIG_NSH_DISABLE_USLEEP, CONFIG_NSH_DISABLE_WGET, - CONFIG_NSH_DISABLE_XD + CONFIG_NSH_DISABLE_BASE64DEC, CONFIG_NSH_DISABLE_BASE64ENC, CONFIG_NSH_DISABLE_CAT, + CONFIG_NSH_DISABLE_CD, CONFIG_NSH_DISABLE_CP, CONFIG_NSH_DISABLE_DD, + CONFIG_NSH_DISABLE_DF, CONFIG_NSH_DISABLE_ECHO, CONFIG_NSH_DISABLE_EXEC, + CONFIG_NSH_DISABLE_EXIT, CONFIG_NSH_DISABLE_FREE, CONFIG_NSH_DISABLE_GET, + CONFIG_NSH_DISABLE_HELP, CONFIG_NSH_DISABLE_HEXDUMP, CONFIG_NSH_DISABLE_IFCONFIG, + CONFIG_NSH_DISABLE_IFUPDOWN, CONFIG_NSH_DISABLE_KILL, CONFIG_NSH_DISABLE_LOSETUP, + CONFIG_NSH_DISABLE_LS, CONFIG_NSH_DISABLE_MD5 CONFIG_NSH_DISABLE_MB, + CONFIG_NSH_DISABLE_MKDIR, CONFIG_NSH_DISABLE_MKFATFS, CONFIG_NSH_DISABLE_MKFIFO, + CONFIG_NSH_DISABLE_MKRD, CONFIG_NSH_DISABLE_MH, CONFIG_NSH_DISABLE_MOUNT, + CONFIG_NSH_DISABLE_MW, CONFIG_NSH_DISABLE_MV, CONFIG_NSH_DISABLE_NFSMOUNT, + CONFIG_NSH_DISABLE_PS, CONFIG_NSH_DISABLE_PING, CONFIG_NSH_DISABLE_PUT, + CONFIG_NSH_DISABLE_PWD, CONFIG_NSH_DISABLE_RM, CONFIG_NSH_DISABLE_RMDIR, + CONFIG_NSH_DISABLE_SET, CONFIG_NSH_DISABLE_SH, CONFIG_NSH_DISABLE_SLEEP, + CONFIG_NSH_DISABLE_TEST, CONFIG_NSH_DISABLE_UMOUNT, CONFIG_NSH_DISABLE_UNSET, + CONFIG_NSH_DISABLE_URLDECODE, CONFIG_NSH_DISABLE_URLENCODE, CONFIG_NSH_DISABLE_USLEEP, + CONFIG_NSH_DISABLE_WGET, CONFIG_NSH_DISABLE_XD Verbose help output can be suppressed by defining CONFIG_NSH_HELP_TERSE. In that case, the help command is still available but will be slightly smaller. @@ -1084,6 +1124,10 @@ NSH-Specific Configuration Settings Set if your ethernet hardware has no built-in MAC address. If set, a bogus MAC will be assigned. + * CONFIG_NSH_MAX_ROUNDTRIP + This is the maximum round trip for a response to a ICMP ECHO request. + It is in units of deciseconds. The default is 20 (2 seconds). + If you use DHCPC, then some special configuration network options are required. These include: diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h index 7188477ce..a046a384f 100644 --- a/apps/nshlib/nsh.h +++ b/apps/nshlib/nsh.h @@ -47,6 +47,7 @@ #include <stdio.h> #include <stdint.h> #include <stdbool.h> +#include <unistd.h> #include <errno.h> #include <nuttx/usb/usbdev_trace.h> @@ -215,6 +216,15 @@ #endif /* CONFIG_NSH_TELNET_LOGIN */ +/* CONFIG_NSH_MAX_ROUNDTRIP - This is the maximum round trip for a response to + * a ICMP ECHO request. It is in units of deciseconds. The default is 20 + * (2 seconds). + */ + +#ifndef CONFIG_NSH_MAX_ROUNDTRIP +# define CONFIG_NSH_MAX_ROUNDTRIP 20 +#endif + /* Verify support for ROMFS /etc directory support options */ #ifdef CONFIG_NSH_ROMFSETC @@ -258,12 +268,36 @@ # undef CONFIG_NSH_ROMFSSECTSIZE #endif -/* This is the maximum number of arguments that will be accepted for a command */ -#ifdef CONFIG_NSH_MAX_ARGUMENTS -# define NSH_MAX_ARGUMENTS CONFIG_NSH_MAX_ARGUMENTS -#else -# define NSH_MAX_ARGUMENTS 10 +/* This is the maximum number of arguments that will be accepted for a + * command. Here we attempt to select the smallest number possible depending + * upon the of commands that are available. Most commands use six or fewer + * arguments, but there are a few that require more. + * + * This value is also configurable with CONFIG_NSH_MAXARGUMENTS. This + * configurability is necessary since there may also be external, "built-in" + * commands that require more commands than NSH is aware of. + */ + +#ifndef CONFIG_NSH_MAXARGUMENTS +# define CONFIG_NSH_MAXARGUMENTS 6 #endif + +#if CONFIG_NSH_MAXARGUMENTS < 11 +# if defined(CONFIG_NET) && !defined(CONFIG_NSH_DISABLE_IFCONFIG) +# undef CONFIG_NSH_MAXARGUMENTS +# define CONFIG_NSH_MAXARGUMENTS 11 +# endif +#endif + +#if CONFIG_NSH_MAXARGUMENTS < 7 +# if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +# if !defined(CONFIG_NSH_DISABLE_GET) || !defined(CONFIG_NSH_DISABLE_PUT) +# undef CONFIG_NSH_MAXARGUMENTS +# define CONFIG_NSH_MAXARGUMENTS 7 +# endif +# endif +#endif + /* strerror() produces much nicer output but is, however, quite large and * will only be used if CONFIG_NSH_STRERROR is defined. Note that the strerror * interface must also have been enabled with CONFIG_LIBC_STRERROR. @@ -507,7 +541,7 @@ void nsh_usbtrace(void); #ifndef CONFIG_NSH_DISABLE_XD int cmd_xd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); #endif - + #if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) int cmd_test(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); int cmd_lbracket(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); @@ -529,6 +563,9 @@ void nsh_usbtrace(void); # ifndef CONFIG_NSH_DISABLE_DD int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif +# ifndef CONFIG_NSH_DISABLE_HEXDUMP + int cmd_hexdump(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif # ifndef CONFIG_NSH_DISABLE_LS int cmd_ls(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif @@ -595,6 +632,10 @@ void nsh_usbtrace(void); # ifndef CONFIG_NSH_DISABLE_IFCONFIG int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif +# ifndef CONFIG_NSH_DISABLE_IFUPDOWN + int cmd_ifup(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); + int cmd_ifdown(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif #if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 # ifndef CONFIG_NSH_DISABLE_GET int cmd_get(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); @@ -643,4 +684,28 @@ void nsh_usbtrace(void); # endif #endif /* CONFIG_DISABLE_SIGNALS */ +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_BASE64) +# ifndef CONFIG_NSH_DISABLE_BASE64DEC + int cmd_base64decode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_BASE64ENC + int cmd_base64encode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif + +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_HASH_MD5) +# ifndef CONFIG_NSH_DISABLE_MD5 + int cmd_md5(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif + +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_URLCODE) +# ifndef CONFIG_NSH_DISABLE_URLDECODE + int cmd_urlencode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_URLENCODE + int cmd_urldecode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif + #endif /* __APPS_NSHLIB_NSH_H */ diff --git a/apps/nshlib/nsh_codeccmd.c b/apps/nshlib/nsh_codeccmd.c new file mode 100644 index 000000000..8c1f5adbd --- /dev/null +++ b/apps/nshlib/nsh_codeccmd.c @@ -0,0 +1,538 @@ +/****************************************************************************
+ * apps/nshlib/nsh_apps.c
+ *
+ * This file is part of NuttX, contributed by Darcy Gong
+ *
+ * Copyright (C) 2012 Gregory Nutt. All rights reserved.
+ * Author: Darcy Gong 2012-10-30
+ *
+ * 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
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#ifdef CONFIG_NETUTILS_CODECS
+
+#include <sys/stat.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sched.h>
+#include <fcntl.h>
+#include <libgen.h>
+#include <errno.h>
+#include <debug.h>
+
+#if defined(CONFIG_NSH_DISABLE_URLENCODE) && defined(CONFIG_NSH_DISABLE_URLDECODE)
+# undef CONFIG_CODECS_URLCODE
+#endif
+
+#ifdef CONFIG_CODECS_URLCODE
+#include <apps/netutils/urldecode.h>
+#endif
+
+#if defined(CONFIG_NSH_DISABLE_BASE64ENC) && defined(CONFIG_NSH_DISABLE_BASE64ENC)
+# undef CONFIG_CODECS_BASE64
+#endif
+
+#ifdef CONFIG_CODECS_BASE64
+#include <apps/netutils/base64.h>
+#endif
+
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+#include <apps/netutils/md5.h>
+#endif
+
+#include "nsh.h"
+#include "nsh_console.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef CONFIG_NSH_CODECS_BUFSIZE
+# define CONFIG_NSH_CODECS_BUFSIZE 128
+#endif
+
+#define CODEC_MODE_URLENCODE 1
+#define CODEC_MODE_URLDECODE 2
+#define CODEC_MODE_BASE64ENC 3
+#define CODEC_MODE_BASE64DEC 4
+#define CODEC_MODE_HASH_MD5 5
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+typedef void (*codec_callback_t)(FAR char *src_buff, int src_buff_len,
+ FAR char *dst_buff, FAR int *dst_buff_len,
+ int mode);
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: urlencode_cb
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_URLCODE) && !defined(CONFIG_NSH_DISABLE_URLENCODE)
+static void urlencode_cb(FAR char *src_buff, int src_buff_len,
+ FAR char *dst_buff, FAR int *dst_buff_len, int mode)
+{
+ urlencode(src_buff,src_buff_len,dst_buff,dst_buff_len);
+}
+#endif
+
+/****************************************************************************
+ * Name: urldecode_cb
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_URLCODE) && !defined(CONFIG_NSH_DISABLE_URLDECODE)
+static void urldecode_cb(FAR char *src_buff, int src_buff_len, FAR char *dst_buff,
+ FAR int *dst_buff_len, int mode)
+{
+ urldecode(src_buff,src_buff_len,dst_buff,dst_buff_len);
+}
+#endif
+
+/****************************************************************************
+ * Name: b64enc_cb
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_BASE64) && !defined(CONFIG_NSH_DISABLE_BASE64ENC)
+static void b64enc_cb(FAR char *src_buff, int src_buff_len, FAR char *dst_buff,
+ FAR int *dst_buff_len, int mode)
+{
+ if (mode == 0)
+ {
+ //dst_buff =
+ base64_encode((unsigned char *)src_buff, src_buff_len,
+ (unsigned char *)dst_buff, (size_t *)dst_buff_len);
+ }
+ else
+ {
+ //dst_buff =
+ base64w_encode((unsigned char *)src_buff, src_buff_len,
+ (unsigned char *)dst_buff, (size_t *)dst_buff_len);
+ }
+}
+#endif
+
+/****************************************************************************
+ * Name: b64dec_cb
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_BASE64) && !defined(CONFIG_NSH_DISABLE_BASE64DEC)
+static void b64dec_cb(FAR char *src_buff, int src_buff_len, FAR char *dst_buff,
+ FAR int *dst_buff_len, int mode)
+{
+ if (mode == 0)
+ {
+ //dst_buff =
+ base64_decode((unsigned char *)src_buff, src_buff_len,
+ (unsigned char *)dst_buff, (size_t *)dst_buff_len);
+ }
+ else
+ {
+ //dst_buff =
+ base64w_decode((unsigned char *)src_buff, src_buff_len,
+ (unsigned char *)dst_buff,(size_t *)dst_buff_len);
+ }
+}
+#endif
+
+/****************************************************************************
+ * Name: md5_cb
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+static void md5_cb(FAR char *src_buff, int src_buff_len, FAR char *dst_buff,
+ FAR int *dst_buff_len, int mode)
+{
+ MD5Update((MD5_CTX *)dst_buff, (unsigned char *)src_buff, src_buff_len);
+}
+#endif
+
+/****************************************************************************
+ * Name: calc_codec_buffsize
+ ****************************************************************************/
+
+static int calc_codec_buffsize(int src_buffsize, uint8_t mode)
+{
+ switch (mode)
+ {
+ case CODEC_MODE_URLENCODE:
+ return src_buffsize*3+1;
+ case CODEC_MODE_URLDECODE:
+ return src_buffsize+1;
+ case CODEC_MODE_BASE64ENC:
+ return ((src_buffsize + 2)/ 3 * 4)+1;
+ case CODEC_MODE_BASE64DEC:
+ return (src_buffsize / 4 * 3 + 2)+1;
+ case CODEC_MODE_HASH_MD5:
+ return 32+1;
+ default:
+ return src_buffsize+1;
+ }
+}
+
+/****************************************************************************
+ * Name: cmd_codecs_proc
+ ****************************************************************************/
+
+static int cmd_codecs_proc(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv,
+ uint8_t mode, codec_callback_t func)
+{
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+ static const unsigned char hex_chars[] = "0123456789abcdef";
+ MD5_CTX ctx;
+ unsigned char mac[16];
+ char *pSrc;
+ char *pDest;
+#endif
+
+ char *localfile = NULL;
+ char *src_buffer = NULL;
+ char *buffer = NULL;
+ char *fullpath = NULL;
+ const char *fmt;
+ char *s_data;
+ bool badarg = false;
+ bool is_file = false;
+ bool is_websafe=false;
+ int option;
+ int fd = -1;
+ int buff_len = 0;
+ int src_buff_len = 0;
+ int i = 0;
+ int ret = OK;
+
+ /* Get the command options */
+
+ while ((option = getopt(argc, argv, ":fw")) != ERROR)
+ {
+ switch (option)
+ {
+ case 'f':
+ is_file = true;
+ break;
+
+#ifdef CONFIG_CODECS_BASE64
+ case 'w':
+ is_websafe = true;
+
+ if (!(mode == CODEC_MODE_BASE64ENC || mode == CODEC_MODE_BASE64DEC))
+ {
+ badarg = true;
+ }
+ break;
+#endif
+ case ':':
+ nsh_output(vtbl, g_fmtargrequired, argv[0]);
+ badarg = true;
+ break;
+
+ case '?':
+ default:
+ nsh_output(vtbl, g_fmtarginvalid, argv[0]);
+ badarg = true;
+ break;
+ }
+ }
+
+ /* If a bad argument was encountered, then return without processing the command */
+
+ if (badarg)
+ {
+ return ERROR;
+ }
+
+ /* There should be exactly on parameter left on the command-line */
+
+ if (optind == argc-1)
+ {
+ s_data = argv[optind];
+ }
+ else if (optind >= argc)
+ {
+ fmt = g_fmttoomanyargs;
+ goto errout;
+ }
+ else
+ {
+ fmt = g_fmtargrequired;
+ goto errout;
+ }
+
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+ if (mode == CODEC_MODE_HASH_MD5)
+ {
+ MD5Init(&ctx);
+ }
+#endif
+
+ if (is_file)
+ {
+ /* Get the local file name */
+
+ localfile = s_data;
+
+ /* Get the full path to the local file */
+
+ fullpath = nsh_getfullpath(vtbl, localfile);
+
+ /* Open the local file for writing */
+
+ fd = open(fullpath, O_RDONLY|O_TRUNC, 0644);
+ if (fd < 0)
+ {
+ nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO);
+ ret = ERROR;
+ goto exit;
+ }
+
+ src_buffer = malloc(CONFIG_NSH_CODECS_BUFSIZE+2);
+#if defined(CONFIG_CODECS_BASE64) && !defined(CONFIG_NSH_DISABLE_BASE64ENC)
+ if (mode == CODEC_MODE_BASE64ENC)
+ {
+ src_buff_len = CONFIG_NSH_CODECS_BUFSIZE / 3 * 3;
+ }
+ else
+#endif
+ {
+ src_buff_len = CONFIG_NSH_CODECS_BUFSIZE;
+ }
+
+ buff_len = calc_codec_buffsize(src_buff_len+2, mode);
+ buffer = malloc(buff_len);
+ while(true)
+ {
+ memset(src_buffer, 0, src_buff_len+2);
+ ret=read(fd, src_buffer, src_buff_len);
+ if (ret < 0)
+ {
+ nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO);
+ ret = ERROR;
+ goto exit;
+ }
+ else if(ret==0)
+ {
+ break;
+ }
+
+#if defined(CONFIG_CODECS_URLCODE) && !defined(CONFIG_NSH_DISABLE_URLDECODE)
+ if (mode == CODEC_MODE_URLDECODE)
+ {
+ if (src_buffer[src_buff_len-1]=='%')
+ {
+ ret += read(fd,&src_buffer[src_buff_len],2);
+ }
+ else if (src_buffer[src_buff_len-2]=='%')
+ {
+ ret += read(fd,&src_buffer[src_buff_len],1);
+ }
+ }
+#endif
+ memset(buffer, 0, buff_len);
+ if (func)
+ {
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+ if (mode == CODEC_MODE_HASH_MD5)
+ {
+ func(src_buffer, ret, (char *)&ctx, &buff_len,0);
+ }
+ else
+#endif
+ {
+ func(src_buffer, ret, buffer, &buff_len,(is_websafe)?1:0);
+ nsh_output(vtbl, "%s", buffer);
+ }
+ }
+
+ buff_len = calc_codec_buffsize(src_buff_len+2, mode);
+ }
+
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+ if (mode == CODEC_MODE_HASH_MD5)
+ {
+ MD5Final(mac, &ctx);
+ pSrc = (char *)&mac;
+ pDest = buffer;
+ for(i=0;i<16;i++,pSrc++)
+ {
+ *pDest++ = hex_chars[(*pSrc) >> 4];
+ *pDest++ = hex_chars[(*pSrc) & 0x0f];
+ }
+
+ *pDest='\0';
+ nsh_output(vtbl, "%s\n", buffer);
+ }
+#endif
+ ret = OK;
+ goto exit;
+ }
+ else
+ {
+ src_buffer = s_data;
+ src_buff_len = strlen(s_data);
+ buff_len = calc_codec_buffsize(src_buff_len, mode);
+ buffer = malloc(buff_len);
+ buffer[0]=0;
+ if (!buffer)
+ {
+ fmt = g_fmtcmdoutofmemory;
+ goto errout;
+ }
+
+ memset(buffer, 0, buff_len);
+ if (func)
+ {
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+ if (mode == CODEC_MODE_HASH_MD5)
+ {
+ func(src_buffer, src_buff_len, (char *)&ctx, &buff_len, 0);
+ MD5Final(mac, &ctx);
+ pSrc = (char *)&mac;
+ pDest = buffer;
+ for(i=0;i<16;i++,pSrc++)
+ {
+ *pDest++ = hex_chars[(*pSrc) >> 4];
+ *pDest++ = hex_chars[(*pSrc) & 0x0f];
+ }
+
+ *pDest='\0';
+ }
+ else
+#endif
+ {
+ func(src_buffer, src_buff_len, buffer, &buff_len,(is_websafe)?1:0);
+ }
+ }
+
+ nsh_output(vtbl, "%s\n",buffer);
+ src_buffer = NULL;
+ goto exit;
+ }
+
+exit:
+ if (fd >= 0)
+ {
+ close(fd);
+ }
+
+ if (fullpath)
+ {
+ free(fullpath);
+ }
+
+ if (src_buffer)
+ {
+ free(src_buffer);
+ }
+
+ if (buffer)
+ {
+ free(buffer);
+ }
+
+ return ret;
+
+errout:
+ nsh_output(vtbl, fmt, argv[0]);
+ ret = ERROR;
+ goto exit;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cmd_urlencode
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_URLCODE) && !defined(CONFIG_NSH_DISABLE_URLENCODE)
+int cmd_urlencode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
+{
+ return cmd_codecs_proc(vtbl, argc, argv, CODEC_MODE_URLENCODE, urlencode_cb);
+}
+#endif
+
+/****************************************************************************
+ * Name: cmd_urldecode
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_URLCODE) && !defined(CONFIG_NSH_DISABLE_URLDECODE)
+int cmd_urldecode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
+{
+ return cmd_codecs_proc(vtbl, argc, argv, CODEC_MODE_URLDECODE, urldecode_cb);
+}
+#endif
+
+/****************************************************************************
+ * Name: cmd_base64encode
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_BASE64) && !defined(CONFIG_NSH_DISABLE_BASE64ENC)
+int cmd_base64encode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
+{
+ return cmd_codecs_proc(vtbl, argc, argv, CODEC_MODE_BASE64ENC, b64enc_cb);
+}
+#endif
+
+/****************************************************************************
+ * Name: cmd_base64decode
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_BASE64) && !defined(CONFIG_NSH_DISABLE_BASE64DEC)
+int cmd_base64decode(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
+{
+ return cmd_codecs_proc(vtbl, argc, argv, CODEC_MODE_BASE64DEC, b64dec_cb);
+}
+#endif
+
+/****************************************************************************
+ * Name: cmd_md5
+ ****************************************************************************/
+
+#if defined(CONFIG_CODECS_HASH_MD5) && !defined(CONFIG_NSH_DISABLE_MD5)
+int cmd_md5(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
+{
+ return cmd_codecs_proc(vtbl,argc,argv,CODEC_MODE_HASH_MD5,md5_cb);
+}
+#endif
+
+#endif /* CONFIG_NETUTILS_CODECS */
diff --git a/apps/nshlib/nsh_dbgcmds.c b/apps/nshlib/nsh_dbgcmds.c index 384b377f3..5463e0f5a 100644 --- a/apps/nshlib/nsh_dbgcmds.c +++ b/apps/nshlib/nsh_dbgcmds.c @@ -46,6 +46,10 @@ #include <string.h> #include <errno.h> +#if CONFIG_NFILE_DESCRIPTORS > 0 +# include <fcntl.h> +#endif + #include "nsh.h" #include "nsh_console.h" @@ -327,7 +331,7 @@ void nsh_dumpbuffer(FAR struct nsh_vtbl_s *vtbl, const char *msg, } /**************************************************************************** - * Name: cmd_xd + * Name: cmd_xd, hex dump of memory ****************************************************************************/ #ifndef CONFIG_NSH_DISABLE_XD @@ -353,3 +357,58 @@ int cmd_xd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) return OK; } #endif + +/**************************************************************************** + * Name: cmd_hexdump, hex dump of files + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_HEXDUMP +int cmd_hexdump(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + uint8_t buffer[IOBUFFERSIZE]; + char msg[32]; + int position; + int fd; + int ret = OK; + + /* Open the file for reading */ + + fd = open(argv[1], O_RDONLY); + if (fd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, "hexdump", "open", NSH_ERRNO); + return ERROR; + } + + position = 0; + for (;;) + { + int nbytesread = read(fd, buffer, IOBUFFERSIZE); + + /* Check for read errors */ + + if (nbytesread < 0) + { + int errval = errno; + nsh_output(vtbl, g_fmtcmdfailed, "hexdump", "read", NSH_ERRNO_OF(errval)); + ret = ERROR; + break; + } + else if (nbytesread > 0) + { + snprintf(msg, sizeof(msg), "%s at %08x", argv[1], position); + nsh_dumpbuffer(vtbl, msg, buffer, nbytesread); + position += nbytesread; + } + else + { + break; // EOF + } + } + + (void)close(fd); + return ret; +} +#endif +#endif diff --git a/apps/nshlib/nsh_netcmds.c b/apps/nshlib/nsh_netcmds.c index cfea5a08a..d28fe873d 100644 --- a/apps/nshlib/nsh_netcmds.c +++ b/apps/nshlib/nsh_netcmds.c @@ -51,6 +51,7 @@ #include <fcntl.h> /* Needed for open */ #include <libgen.h> /* Needed for basename */ #include <errno.h> +#include <debug.h> #include <nuttx/net/net.h> #include <nuttx/clock.h> @@ -80,6 +81,12 @@ # endif #endif +#ifdef CONFIG_HAVE_GETHOSTBYNAME +# include <netdb.h> +#else +# include <apps/netutils/resolv.h> +#endif + #include "nsh.h" #include "nsh_console.h" @@ -87,8 +94,16 @@ * Definitions ****************************************************************************/ +/* Size of the ECHO data */ + #define DEFAULT_PING_DATALEN 56 +/* Get the larger value */ + +#ifndef MAX +# define MAX(a,b) (a > b ? a : b) +#endif + /**************************************************************************** * Private Types ****************************************************************************/ @@ -262,14 +277,34 @@ int ifconfig_callback(FAR struct uip_driver_s *dev, void *arg) { struct nsh_vtbl_s *vtbl = (struct nsh_vtbl_s*)arg; struct in_addr addr; + bool is_running = false; + int ret; + + ret = uip_getifstatus(dev->d_ifname,&is_running); + if (ret != OK) + { + nsh_output(vtbl, "\tGet %s interface flags error: %d\n", + dev->d_ifname, ret); + } + + nsh_output(vtbl, "%s\tHWaddr %s at %s\n", + dev->d_ifname, ether_ntoa(&dev->d_mac), (is_running)?"UP":"DOWN"); - nsh_output(vtbl, "%s\tHWaddr %s\n", dev->d_ifname, ether_ntoa(&dev->d_mac)); addr.s_addr = dev->d_ipaddr; nsh_output(vtbl, "\tIPaddr:%s ", inet_ntoa(addr)); + addr.s_addr = dev->d_draddr; nsh_output(vtbl, "DRaddr:%s ", inet_ntoa(addr)); + addr.s_addr = dev->d_netmask; - nsh_output(vtbl, "Mask:%s\n\n", inet_ntoa(addr)); + nsh_output(vtbl, "Mask:%s\n", inet_ntoa(addr)); + +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_DNS) + resolv_getserver(&addr); + nsh_output(vtbl, "\tDNSaddr:%s\n", inet_ntoa(addr)); +#endif + + nsh_output(vtbl, "\n"); return OK; } @@ -469,6 +504,54 @@ int cmd_get(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) #endif /**************************************************************************** + * Name: cmd_ifup + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_IFUPDOWN +int cmd_ifup(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + FAR char *intf = NULL; + int ret; + + if (argc != 2) + { + nsh_output(vtbl, "Please select nic_name:\n"); + netdev_foreach(ifconfig_callback, vtbl); + return OK; + } + + intf = argv[1]; + ret = uip_ifup(intf); + nsh_output(vtbl, "ifup %s...%s\n", intf, (ret == OK) ? "OK" : "Failed"); + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_ifdown + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_IFUPDOWN +int cmd_ifdown(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + FAR char *intf = NULL; + int ret; + + if (argc != 2) + { + nsh_output(vtbl, "Please select nic_name:\n"); + netdev_foreach(ifconfig_callback, vtbl); + return OK; + } + + intf = argv[1]; + ret = uip_ifdown(intf); + nsh_output(vtbl, "ifdown %s...%s\n", intf, (ret == OK) ? "OK" : "Failed"); + return ret; +} +#endif + +/**************************************************************************** * Name: cmd_ifconfig ****************************************************************************/ @@ -476,7 +559,17 @@ int cmd_get(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) { struct in_addr addr; - in_addr_t ip; + in_addr_t gip; + int i; + FAR char *intf = NULL; + FAR char *hostip = NULL; + FAR char *gwip = NULL; + FAR char *mask = NULL; + FAR char *tmp = NULL; + FAR char *hw = NULL; + FAR char *dns = NULL; + bool badarg=false; + uint8_t mac[6]; /* With one or no arguments, ifconfig simply shows the status of ethernet * device: @@ -498,24 +591,142 @@ int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) * ifconfig nic_name ip_address */ + if (argc > 2) + { + for(i = 0; i < argc; i++) + { + if (i == 1) + { + intf = argv[i]; + } + else if (i == 2) + { + hostip = argv[i]; + } + else + { + tmp = argv[i]; + if (!strcmp(tmp, "dr") || !strcmp(tmp, "gw") || !strcmp(tmp, "gateway")) + { + if (argc-1 >= i+1) + { + gwip = argv[i+1]; + i++; + } + else + { + badarg = true; + } + } + else if(!strcmp(tmp, "netmask")) + { + if (argc-1 >= i+1) + { + mask = argv[i+1]; + i++; + } + else + { + badarg = true; + } + } + else if(!strcmp(tmp, "hw")) + { + if (argc-1>=i+1) + { + hw = argv[i+1]; + i++; + badarg = !uiplib_hwmacconv(hw, mac); + } + else + { + badarg = true; + } + } + else if(!strcmp(tmp, "dns")) + { + if (argc-1 >= i+1) + { + dns = argv[i+1]; + i++; + } + else + { + badarg = true; + } + } + } + } + } + + if (badarg) + { + nsh_output(vtbl, g_fmtargrequired, argv[0]); + return ERROR; + } + + /* Set Hardware ethernet MAC addr */ + + if (hw) + { + ndbg("HW MAC: %s\n", hw); + uip_setmacaddr(intf, mac); + } + /* Set host ip address */ - ip = addr.s_addr = inet_addr(argv[2]); - uip_sethostaddr(argv[1], &addr); + ndbg("Host IP: %s\n", hostip); + gip = addr.s_addr = inet_addr(hostip); + uip_sethostaddr(intf, &addr); /* Set gateway */ - ip = NTOHL(ip); - ip &= ~0x000000ff; - ip |= 0x00000001; + if (gwip) + { + ndbg("Gateway: %s\n", gwip); + gip = addr.s_addr = inet_addr(gwip); + } + else + { + ndbg("Gateway: default\n"); + gip = NTOHL(gip); + gip &= ~0x000000ff; + gip |= 0x00000001; + gip = HTONL(gip); + addr.s_addr = gip; + } + + uip_setdraddr(intf, &addr); - addr.s_addr = HTONL(ip); - uip_setdraddr(argv[1], &addr); + /* Set network mask */ - /* Set netmask */ + if (mask) + { + ndbg("Netmask: %s\n",mask); + addr.s_addr = inet_addr(mask); + } + else + { + ndbg("Netmask: Default\n"); + addr.s_addr = inet_addr("255.255.255.0"); + } - addr.s_addr = inet_addr("255.255.255.0"); - uip_setnetmask(argv[1], &addr); + uip_setnetmask(intf, &addr); + +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_DNS) + if (dns) + { + ndbg("DNS: %s\n", dns); + addr.s_addr = inet_addr(dns); + } + else + { + ndbg("DNS: Default\n"); + addr.s_addr = gip; + } + + resolv_conf(&addr); +#endif return OK; } @@ -536,6 +747,7 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) uint32_t start; uint32_t next; uint32_t dsec = 10; + uint32_t maxwait; uint16_t id; bool badarg = false; int count = 10; @@ -599,7 +811,7 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) if (optind == argc-1) { staddr = argv[optind]; - if (!uiplib_ipaddrconv(staddr, (FAR unsigned char*)&ipaddr)) + if (dns_gethostip(staddr, &ipaddr) < 0) { goto errout; } @@ -619,16 +831,26 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) id = ping_newid(); + /* The maximum wait for a response will be the larger of the inter-ping time and + * the configured maximum round-trip time. + */ + + maxwait = MAX(dsec, CONFIG_NSH_MAX_ROUNDTRIP); + /* Loop for the specified count */ - nsh_output(vtbl, "PING %s %d bytes of data\n", staddr, DEFAULT_PING_DATALEN); + nsh_output(vtbl, "PING %d.%d.%d.%d %d bytes of data\n", + (ipaddr ) & 0xff, (ipaddr >> 8 ) & 0xff, + (ipaddr >> 16 ) & 0xff, (ipaddr >> 24 ) & 0xff, + DEFAULT_PING_DATALEN); + start = g_system_timer; for (i = 1; i <= count; i++) { /* Send the ECHO request and wait for the response */ next = g_system_timer; - seqno = uip_ping(ipaddr, id, i, DEFAULT_PING_DATALEN, dsec); + seqno = uip_ping(ipaddr, id, i, DEFAULT_PING_DATALEN, maxwait); /* Was any response returned? We can tell if a non-negative sequence * number was returned. @@ -636,7 +858,7 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) if (seqno >= 0 && seqno <= i) { - /* Get the elpased time from the time that the request was + /* Get the elapsed time from the time that the request was * sent until the response was received. If we got a response * to an earlier request, then fudge the elpased time. */ @@ -644,7 +866,7 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) elapsed = TICK2MSEC(g_system_timer - next); if (seqno < i) { - elapsed += 100*dsec*(i - seqno); + elapsed += 100 * dsec * (i - seqno); } /* Report the receipt of the reply */ @@ -662,7 +884,7 @@ int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) elapsed = TICK2DSEC(g_system_timer - next); if (elapsed < dsec) { - usleep(100000*dsec); + usleep(100000 * (dsec - elapsed)); } } diff --git a/apps/nshlib/nsh_netinit.c b/apps/nshlib/nsh_netinit.c index bc845c4ed..bb1c73dff 100644 --- a/apps/nshlib/nsh_netinit.c +++ b/apps/nshlib/nsh_netinit.c @@ -47,7 +47,7 @@ #include <net/if.h> #include <apps/netutils/uiplib.h> -#if defined(CONFIG_NSH_DHCPC) +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_DNS) # include <apps/netutils/resolv.h> # include <apps/netutils/dhcpc.h> #endif @@ -60,6 +60,10 @@ * Definitions ****************************************************************************/ +#if defined(CONFIG_NSH_DRIPADDR) && !defined(CONFIG_NSH_DNSIPADDR) +# define CONFIG_NSH_DNSIPADDR CONFIG_NSH_DRIPADDR +#endif + /**************************************************************************** * Private Types ****************************************************************************/ @@ -125,10 +129,14 @@ int nsh_netinit(void) addr.s_addr = HTONL(CONFIG_NSH_NETMASK); uip_setnetmask("eth0", &addr); -#if defined(CONFIG_NSH_DHCPC) +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_DNS) /* Set up the resolver */ resolv_init(); +#if defined(CONFIG_NSH_DNS) + addr.s_addr = HTONL(CONFIG_NSH_DNSIPADDR); + resolv_conf(&addr); +#endif #endif #if defined(CONFIG_NSH_DHCPC) diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c index df2f7c3e3..abdf5c321 100644 --- a/apps/nshlib/nsh_parse.c +++ b/apps/nshlib/nsh_parse.c @@ -73,19 +73,19 @@ /* Argument list size * * argv[0]: The command name. - * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[1]: The beginning of argument (up to CONFIG_NSH_MAXARGUMENTS) * argv[argc-3]: Possibly '>' or '>>' * argv[argc-2]: Possibly <file> * argv[argc-1]: Possibly '&' (if pthreads are enabled) * argv[argc]: NULL terminating pointer * - * Maximum size is NSH_MAX_ARGUMENTS+5 + * Maximum size is CONFIG_NSH_MAXARGUMENTS+5 */ #ifndef CONFIG_NSH_DISABLEBG -# define MAX_ARGV_ENTRIES (NSH_MAX_ARGUMENTS+5) +# define MAX_ARGV_ENTRIES (CONFIG_NSH_MAXARGUMENTS+5) #else -# define MAX_ARGV_ENTRIES (NSH_MAX_ARGUMENTS+4) +# define MAX_ARGV_ENTRIES (CONFIG_NSH_MAXARGUMENTS+4) #endif /* Help command summary layout */ @@ -146,16 +146,25 @@ static const char g_failure[] = "1"; static const struct cmdmap_s g_cmdmap[] = { #if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) - { "[", cmd_lbracket, 4, NSH_MAX_ARGUMENTS, "<expression> ]" }, + { "[", cmd_lbracket, 4, CONFIG_NSH_MAXARGUMENTS, "<expression> ]" }, #endif #ifndef CONFIG_NSH_DISABLE_HELP { "?", cmd_help, 1, 1, NULL }, #endif +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_BASE64) +# ifndef CONFIG_NSH_DISABLE_BASE64DEC + { "base64dec", cmd_base64decode, 2, 4, "[-w] [-f] <string or filepath>" }, +# endif +# ifndef CONFIG_NSH_DISABLE_BASE64ENC + { "base64enc", cmd_base64encode, 2, 4, "[-w] [-f] <string or filepath>" }, +# endif +#endif + #if CONFIG_NFILE_DESCRIPTORS > 0 # ifndef CONFIG_NSH_DISABLE_CAT - { "cat", cmd_cat, 2, NSH_MAX_ARGUMENTS, "<path> [<path> [<path> ...]]" }, + { "cat", cmd_cat, 2, CONFIG_NSH_MAXARGUMENTS, "<path> [<path> [<path> ...]]" }, # endif #ifndef CONFIG_DISABLE_ENVIRON # ifndef CONFIG_NSH_DISABLE_CD @@ -187,9 +196,9 @@ static const struct cmdmap_s g_cmdmap[] = #ifndef CONFIG_NSH_DISABLE_ECHO # ifndef CONFIG_DISABLE_ENVIRON - { "echo", cmd_echo, 0, NSH_MAX_ARGUMENTS, "[<string|$name> [<string|$name>...]]" }, + { "echo", cmd_echo, 0, CONFIG_NSH_MAXARGUMENTS, "[<string|$name> [<string|$name>...]]" }, # else - { "echo", cmd_echo, 0, NSH_MAX_ARGUMENTS, "[<string> [<string>...]]" }, + { "echo", cmd_echo, 0, CONFIG_NSH_MAXARGUMENTS, "[<string> [<string>...]]" }, # endif #endif @@ -217,10 +226,20 @@ static const struct cmdmap_s g_cmdmap[] = { "help", cmd_help, 1, 3, "[-v] [<cmd>]" }, # endif #endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_HEXDUMP + { "hexdump", cmd_hexdump, 2, 2, "<file or device>" }, +#endif +#endif #ifdef CONFIG_NET # ifndef CONFIG_NSH_DISABLE_IFCONFIG - { "ifconfig", cmd_ifconfig, 1, 3, "[nic_name [ip]]" }, + { "ifconfig", cmd_ifconfig, 1, 11, "[nic_name [ip]] [dr|gw|gateway <dr-address>] [netmask <net-mask>] [dns <dns-address>] [hw <hw-mac>]" }, +# endif +# ifndef CONFIG_NSH_DISABLE_IFUPDOWN + { "ifdown", cmd_ifdown, 2, 2, "<nic_name>" }, + { "ifup", cmd_ifup, 2, 2, "<nic_name>" }, # endif #endif @@ -246,6 +265,12 @@ static const struct cmdmap_s g_cmdmap[] = { "mb", cmd_mb, 2, 3, "<hex-address>[=<hex-value>][ <hex-byte-count>]" }, #endif +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_HASH_MD5) +# ifndef CONFIG_NSH_DISABLE_MD5 + { "md5", cmd_md5, 2, 3, "[-f] <string or filepath>" }, +# endif +#endif + #if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) # ifndef CONFIG_NSH_DISABLE_MKDIR { "mkdir", cmd_mkdir, 2, 2, "<path>" }, @@ -348,7 +373,7 @@ static const struct cmdmap_s g_cmdmap[] = #endif #if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) - { "test", cmd_test, 3, NSH_MAX_ARGUMENTS, "<expression>" }, + { "test", cmd_test, 3, CONFIG_NSH_MAXARGUMENTS, "<expression>" }, #endif #if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_READABLE) @@ -363,6 +388,15 @@ static const struct cmdmap_s g_cmdmap[] = # endif #endif +#if defined(CONFIG_NETUTILS_CODECS) && defined(CONFIG_CODECS_URLCODE) +# ifndef CONFIG_NSH_DISABLE_URLDECODE + { "urldecode", cmd_urldecode, 2, 3, "[-f] <string or filepath>" }, +# endif +# ifndef CONFIG_NSH_DISABLE_URLENCODE + { "urlencode", cmd_urlencode, 2, 3, "[-f] <string or filepath>" }, +# endif +#endif + #ifndef CONFIG_DISABLE_SIGNALS # ifndef CONFIG_NSH_DISABLE_USLEEP { "usleep", cmd_usleep, 2, 2, "<usec>" }, @@ -378,6 +412,7 @@ static const struct cmdmap_s g_cmdmap[] = #ifndef CONFIG_NSH_DISABLE_XD { "xd", cmd_xd, 3, 3, "<hex-address> <byte-count>" }, #endif + { NULL, NULL, 1, 1, NULL } }; @@ -711,7 +746,7 @@ static int nsh_execute(FAR struct nsh_vtbl_s *vtbl, int argc, char *argv[]) * * argv[0]: The command name. This is argv[0] when the arguments * are, finally, received by the command vtblr - * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[1]: The beginning of argument (up to CONFIG_NSH_MAXARGUMENTS) * argv[argc]: NULL terminating pointer */ @@ -1318,13 +1353,13 @@ int nsh_parse(FAR struct nsh_vtbl_s *vtbl, char *cmdline) * of argv is: * * argv[0]: The command name. - * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[1]: The beginning of argument (up to CONFIG_NSH_MAXARGUMENTS) * argv[argc-3]: Possibly '>' or '>>' * argv[argc-2]: Possibly <file> * argv[argc-1]: Possibly '&' * argv[argc]: NULL terminating pointer * - * Maximum size is NSH_MAX_ARGUMENTS+5 + * Maximum size is CONFIG_NSH_MAXARGUMENTS+5 */ argv[0] = cmd; @@ -1398,7 +1433,7 @@ int nsh_parse(FAR struct nsh_vtbl_s *vtbl, char *cmdline) /* Check if the maximum number of arguments was exceeded */ - if (argc > NSH_MAX_ARGUMENTS) + if (argc > CONFIG_NSH_MAXARGUMENTS) { nsh_output(vtbl, g_fmttoomanyargs, cmd); } diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index ffb7fd679..2b8f95534 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -52,6 +52,7 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> @@ -100,8 +101,8 @@ comms_main(void) debug("FMU: ready"); for (;;) { - /* wait for serial data, but no more than 100ms */ - poll(&fds, 1, 100); + /* wait for serial data, but no more than 10ms */ + poll(&fds, 1, 10); /* * Pull bytes from FMU and feed them to the HX engine. @@ -130,15 +131,11 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (int i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) { report.rc_channel[i] = system_state.rc_channel_data[i]; - - if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) { - report.channel_count = system_state.rc_channels; - } else { - report.channel_count = 0; } - + + report.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ @@ -174,22 +171,44 @@ comms_handle_command(const void *buffer, size_t length) irqstate_t flags = irqsave(); /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) + for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { system_state.fmu_channel_data[i] = cmd->servo_command[i]; + } - /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ + /* if IO is armed and FMU gets disarmed, IO must also disarm */ if (system_state.arm_ok && !cmd->arm_ok) { system_state.armed = false; } system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; - system_state.fmu_data_received = true; + system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; + system_state.manual_override_ok = cmd->manual_override_ok; + system_state.mixer_fmu_available = true; + system_state.fmu_data_received_time = hrt_absolute_time(); + + /* set PWM update rate if changed (after limiting) */ + uint16_t new_servo_rate = cmd->servo_rate; + /* reject faster than 500 Hz updates */ + if (new_servo_rate > 500) { + new_servo_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (new_servo_rate < 50) { + new_servo_rate = 50; + } + if (system_state.servo_rate != new_servo_rate) { + up_pwm_servo_set_rate(new_servo_rate); + system_state.servo_rate = new_servo_rate; + } - /* handle changes signalled by FMU */ -// if (!system_state.arm_ok && system_state.armed) -// system_state.armed = false; + /* + * update servo values immediately. + * the updates are done in addition also + * in the mainloop, since this function will only + * update with a connected FMU. + */ + mixer_tick(); /* handle relay state changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d4eace3df..48370d9d0 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -55,19 +55,27 @@ #include <drivers/drv_hrt.h> #include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> +#include <systemlib/ppm_decode.h> #define DEBUG #include "px4io.h" +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 1700 +#define RC_CHANNEL_LOW_THRESH 1300 + +static void ppm_input(void); + void controls_main(void) { struct pollfd fds[2]; + /* DSM input */ fds[0].fd = dsm_init("/dev/ttyS0"); fds[0].events = POLLIN; - + /* S.bus input */ fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; @@ -75,14 +83,103 @@ controls_main(void) /* run this loop at ~100Hz */ poll(fds, 2, 10); + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + bool locked = false; + + /* + * Store RC channel count to detect switch to RC loss sooner + * than just by timeout + */ + unsigned rc_channels = system_state.rc_channels; + + /* + * Track if any input got an update in this round + */ + bool rc_updated; + if (fds[0].revents & POLLIN) - dsm_input(); + locked |= dsm_input(); if (fds[1].revents & POLLIN) - sbus_input(); + locked |= sbus_input(fds[1].fd, PX4IO_INPUT_CHANNELS, &system_state.rc_channel_data, + &system_state.rc_channels, &system_state.rc_channels_timestamp, &rc_updated); + + /* + * If we don't have lock from one of the serial receivers, + * look for PPM. It shares an input with S.bus, so there's + * a possibility it will mis-parse an S.bus frame. + * + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have an alternate + * receiver lock. + */ + if (!locked) + ppm_input(); + + /* check for manual override status */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + /* force manual input override */ + system_state.mixer_manual_override = true; + } else { + /* override not engaged, use FMU */ + system_state.mixer_manual_override = false; + } + + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input and tell FMU. + */ + if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { - /* XXX do ppm processing, bypass mode, etc. here */ + /* set the number of channels to zero - no inputs */ + system_state.rc_channels = 0; + rc_updated = true; + } - /* do PWM output updates */ + /* + * If there was a RC update OR the RC signal status (lost / present) has + * just changed, request an update immediately. + */ + system_state.fmu_report_due |= rc_updated; + + /* + * PWM output updates are performed in addition on each comm update. + * the updates here are required to ensure operation if FMU is not started + * or stopped responding. + */ mixer_tick(); } } + +static void +ppm_input(void) +{ + /* + * Look for new PPM input. + */ + if (ppm_last_valid_decode != 0) { + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* PPM data exists, copy it */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + + /* copy the timestamp and clear it */ + system_state.rc_channels_timestamp = ppm_last_valid_decode; + ppm_last_valid_decode = 0; + + irqrestore(state); + + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; + } +} diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 744dac3d6..2611f3a03 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -104,7 +104,7 @@ dsm_init(const char *device) return dsm_fd; } -void +bool dsm_input(void) { ssize_t ret; @@ -141,7 +141,7 @@ dsm_input(void) /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -153,7 +153,7 @@ dsm_input(void) * If we don't have a full frame, return */ if (partial_frame_count < DSM_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and @@ -161,6 +161,12 @@ dsm_input(void) */ dsm_decode(now); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } static bool @@ -275,10 +281,13 @@ dsm_decode(hrt_abstime frame_time) */ if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) dsm_guess_format(true); + + /* we have received something we think is a frame */ last_frame_time = frame_time; + + /* if we don't know the frame format, update the guessing state machine */ if (channel_shift == 0) { dsm_guess_format(false); - system_state.dsm_input_ok = false; return; } @@ -293,10 +302,6 @@ dsm_decode(hrt_abstime frame_time) * seven channels are being transmitted. */ - const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS; - - uint16_t dsm_channels[dsm_chancount]; - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; @@ -311,31 +316,40 @@ dsm_decode(hrt_abstime frame_time) continue; /* update the decoded channel count */ - if (channel > ppm_decoded_channels) - ppm_decoded_channels = channel; + if (channel >= system_state.rc_channels) + system_state.rc_channels = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ if (channel_shift == 11) value /= 2; - - /* stuff the decoded channel into the PPM input buffer */ - dsm_channels[channel] = 988 + value; + value += 998; + + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; + case 1: + channel = 0; + break; + case 2: + channel = 1; + default: + break; + } + system_state.rc_channel_data[channel] = value; } - /* DSM input is valid */ - system_state.dsm_input_ok = true; - - /* check if no S.BUS data is available */ - if (!system_state.sbus_input_ok) { + /* and note that we have received data from the R/C controller */ + /* XXX failsafe will cause problems here - need a strategy for detecting it */ + system_state.rc_channels_timestamp = frame_time; - for (unsigned i = 0; i < dsm_chancount; i++) { - system_state.rc_channel_data[i] = dsm_channels[i]; - } - - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - system_state.rc_channels_timestamp = frame_time; - system_state.rc_channels = dsm_chancount; - system_state.fmu_report_due = true; - } + /* trigger an immediate report to the FMU */ + system_state.fmu_report_due = true; } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d..9cdb98e23 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -48,21 +48,17 @@ #include <unistd.h> #include <fcntl.h> -#include <drivers/drv_pwm_output.h> +#include <debug.h> -#include <systemlib/ppm_decode.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> #include "px4io.h" /* - * Count of periodic calls in which we have no FMU input. - */ -static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 20 -/* - * Collect RC input data from the controller source(s). + * Maximum interval in us before FMU signal is considered lost */ -static void mixer_get_rc_input(void); +#define FMU_INPUT_DROP_LIMIT_US 200000 /* * Update a mixer based on the current control signals. @@ -88,42 +84,47 @@ mixer_tick(void) int i; bool should_arm; - /* - * Start by looking for R/C control inputs. - * This updates system_state with any control inputs received. - */ - mixer_get_rc_input(); + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("\nRX timeout\n"); + } /* * Decide which set of inputs we're using. */ - if (system_state.mixer_use_fmu) { - /* we have recent control data from the FMU */ - control_count = PX4IO_OUTPUT_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - /* check that we are receiving fresh data from the FMU */ - if (!system_state.fmu_data_received) { - fmu_input_drops++; - - /* too many frames without FMU input, time to go to failsafe */ - if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_use_fmu = false; - } + /* this is for planes, where manual override makes sense */ + if(system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; } else { - fmu_input_drops = 0; - system_state.fmu_data_received = false; - } + /* we have no control input (no FMU, no RC) */ - } else if (system_state.rc_channels > 0) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + // XXX builtin failsafe would activate here + control_count = 0; + } + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input */ - control_count = 0; + /* if the fmu is available whe are good */ + if(system_state.mixer_fmu_available) { + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } + /* * Tickle each mixer, if we have control data. */ @@ -141,9 +142,10 @@ mixer_tick(void) /* * Decide whether the servos should be armed right now. + * A sufficient reason is armed state and either FMU or RC control inputs */ - should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu; + should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -166,30 +168,3 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) mixers[mixer].current_value = 0; } } - -static void -mixer_get_rc_input(void) -{ - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ - if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - - /* input was ok and timed out, mark as update */ - if (system_state.ppm_input_ok) { - system_state.ppm_input_ok = false; - system_state.fmu_report_due = true; - } - return; - } - - /* mark PPM as valid */ - system_state.ppm_input_ok = true; - - /* check if no DSM and S.BUS data is available */ - if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) { - /* otherwise, copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - system_state.fmu_report_due = true; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b1201..0bf6d4997 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -52,9 +52,12 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */ + uint16_t servo_rate; /**< PWM output rate in Hz */ + bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ + bool arm_ok; /**< FMU allows full arming */ + bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ + bool manual_override_ok; /**< if true, IO performs a direct manual override */ }; /* config message from FMU to IO */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 77524797f..4f68b3d2d 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -44,6 +44,7 @@ #include <debug.h> #include <stdlib.h> #include <errno.h> +#include <string.h> #include <nuttx/clock.h> @@ -60,6 +61,8 @@ int user_start(int argc, char *argv[]) { /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); + /* default to 50 Hz PWM outputs */ + system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc8..9cfd8f716 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,50 +69,72 @@ struct sys_state_s { - bool armed; /* IO armed */ - bool arm_ok; /* FMU says OK to arm */ + bool armed; /* IO armed */ + bool arm_ok; /* FMU says OK to arm */ + uint16_t servo_rate; /* output rate of servos in Hz */ - bool ppm_input_ok; /* valid PPM input data */ - bool dsm_input_ok; /* valid Spektrum DSM data */ - bool sbus_input_ok; /* valid Futaba S.Bus data */ - - /* + /** * Data from the remote control input(s) */ - int rc_channels; + unsigned rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; uint64_t rc_channels_timestamp; - /* + /** * Control signals from FMU. */ uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; - /* + /** * Relay controls */ bool relays[PX4IO_RELAY_CHANNELS]; - /* - * If true, we are using the FMU controls. + /** + * If true, we are using the FMU controls, else RC input if available. + */ + bool mixer_manual_override; + + /** + * If true, FMU input is available. */ - bool mixer_use_fmu; + bool mixer_fmu_available; - /* + /** * If true, state that should be reported to FMU has been updated. */ bool fmu_report_due; - /* - * If true, new control data from the FMU has been received. + /** + * Last FMU receive time, in microseconds since system boot */ - bool fmu_data_received; + uint64_t fmu_data_received_time; - /* + /** * Current serial interface mode, per the serial_rx_mode parameter * in the config packet. */ uint8_t serial_rx_mode; + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * If true, the connection to FMU has been lost for more than a timeout interval + */ + bool fmu_lost; + + /** + * If true, FMU is ready for autonomous position control. Only used for LED indication + */ + bool vector_flight_mode_ok; + + /** + * If true, IO performs an on-board manual override with the RC channel values + */ + bool manual_override_ok; }; extern struct sys_state_s system_state; @@ -169,9 +191,10 @@ extern void comms_main(void) __attribute__((noreturn)); */ extern void controls_main(void); extern int dsm_init(const char *device); -extern void dsm_input(void); +extern bool dsm_input(void); extern int sbus_init(const char *device); -extern void sbus_input(void); +extern bool sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated); /* * Assertion codes diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 60d20905a..3314ef513 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -58,20 +58,27 @@ static struct hrt_call failsafe_call; * Count the number of times in a row that we see the arming button * held down. */ -static unsigned counter; +static unsigned counter = 0; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0xffff // always on -#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking -#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking -#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink +#define LED_PATTERN_SAFE 0xffff /**< always on */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */ +#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ +#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ static unsigned blink_counter = 0; +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ #define ARM_COUNTER_THRESHOLD 10 -#define DISARM_COUNTER_THRESHOLD 2 static bool safety_button_pressed; @@ -101,12 +108,16 @@ safety_check_button(void *arg) */ safety_button_pressed = BUTTON_SAFETY; - if(safety_button_pressed) { - //printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter); - } - - /* Keep pressed for a while to arm */ + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ if (safety_button_pressed && !system_state.armed) { + if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { @@ -117,9 +128,10 @@ safety_check_button(void *arg) } /* Disarm quickly */ } else if (safety_button_pressed && system_state.armed) { - if (counter < DISARM_COUNTER_THRESHOLD) { + + if (counter < ARM_COUNTER_THRESHOLD) { counter++; - } else if (counter == DISARM_COUNTER_THRESHOLD) { + } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ system_state.armed = false; counter++; @@ -139,6 +151,8 @@ safety_check_button(void *arg) } } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; + } else if (system_state.vector_flight_mode_ok) { + pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } /* Turn the LED on if we have a 1 at the current bit position */ @@ -165,7 +179,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_use_fmu) { + if (!system_state.mixer_fmu_available) { failsafe = !failsafe; } else { failsafe = false; diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index c3949f2b0..0de0593e7 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -49,15 +49,13 @@ #define DEBUG #include "px4io.h" -#include "protocol.h" #include "debug.h" #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 -static int sbus_fd = -1; - static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; static uint8_t frame[SBUS_FRAME_SIZE]; @@ -65,11 +63,14 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static void sbus_decode(hrt_abstime frame_time); +static int sbus_decode(hrt_abstime frame_time, unsigned max_channels, + uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time); int sbus_init(const char *device) { + static int sbus_fd = -1; + if (sbus_fd < 0) sbus_fd = open(device, O_RDONLY); @@ -94,8 +95,9 @@ sbus_init(const char *device) return sbus_fd; } -void -sbus_input(void) +bool +sbus_input(int fd, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, + uint64_t *receive_time, bool *updated) { ssize_t ret; hrt_abstime now; @@ -127,11 +129,11 @@ sbus_input(void) * Fetch bytes, but no more than we would need to complete * the current frame. */ - ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + ret = read(fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ if (ret < 1) - return; + goto out; last_rx_time = now; /* @@ -143,14 +145,20 @@ sbus_input(void) * If we don't have a full frame, return */ if (partial_frame_count < SBUS_FRAME_SIZE) - return; + goto out; /* * Great, it looks like we might have a frame. Go ahead and - * decode it. + * decode it, report if the receiver got something. */ - sbus_decode(now); + *updated = (sbus_decode(now, max_channels, channel_data, channel_count, receive_time) == OK); partial_frame_count = 0; + +out: + /* + * If we have seen a frame in the last 200ms, we consider ourselves 'locked' + */ + return (now - last_frame_time) < 200000; } /* @@ -189,23 +197,32 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { /* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} } }; -static void -sbus_decode(hrt_abstime frame_time) +static int +sbus_decode(hrt_abstime frame_time, unsigned max_channels, uint16_t *channel_data, unsigned *channel_count, uint64_t *receive_time) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; - system_state.sbus_input_ok = false; - return; + return 1; } - /* if the failsafe bit is set, we consider that a loss of RX signal */ - if (frame[23] & (1 << 4)) { - system_state.sbus_input_ok = false; - return; + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + *channel_count = 0; + + return 1; } - unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + /* decode failsafe and RC status */ + + /* we have received something we think is a frame */ + last_frame_time = frame_time; + + unsigned chancount = (max_channels > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_channels; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { @@ -227,15 +244,19 @@ sbus_decode(hrt_abstime frame_time) system_state.rc_channel_data[channel] = (value / 2) + 998; } - if (PX4IO_INPUT_CHANNELS >= 18) { - /* decode two switch channels */ - chancount = 18; + /* decode switch channels if data fields are wide enough */ + if (chancount > 17) { + /* channel 17 (index 16) */ + system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998; } - system_state.rc_channels = chancount; - system_state.sbus_input_ok = true; - system_state.fmu_report_due = true; + /* note the number of channels decoded */ + *channel_count = chancount; /* and note that we have received data from the R/C controller */ - system_state.rc_channels_timestamp = frame_time; + *receive_time = frame_time; + + return 0; } diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index cc74ae705..1546fb56d 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -69,58 +69,100 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); PARAM_DEFINE_FLOAT(RC5_MAX, 2000); PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); +// PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); -PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); - -PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA +// PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC9_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC10_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC11_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC13_EXP, 0.0f); + +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); @@ -129,12 +171,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); -PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); -PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); + +PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); + +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); - +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 07a9015fe..10d25d347 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -95,6 +95,8 @@ #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ +#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) + /** * Sensor app start / stop handling function * @@ -123,7 +125,7 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ #if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ @@ -167,7 +169,7 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - float ex[_rc_max_chan_count]; + // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -182,11 +184,27 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_mode_sw; + + int rc_map_manual_override_sw; + int rc_map_auto_mode_sw; + + int rc_map_manual_mode_sw; + int rc_map_sas_mode_sw; + int rc_map_rtl_sw; + int rc_map_offboard_ctrl_mode_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; float rc_scale_roll; float rc_scale_pitch; float rc_scale_yaw; + float rc_scale_flaps; float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -197,9 +215,11 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t ex[_rc_max_chan_count]; + // param_t ex[_rc_max_chan_count]; param_t rc_type; + param_t rc_demix; + param_t gyro_offset[3]; param_t accel_offset[3]; param_t accel_scale[3]; @@ -210,11 +230,27 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_mode_sw; + + param_t rc_map_manual_override_sw; + param_t rc_map_auto_mode_sw; + + param_t rc_map_manual_mode_sw; + param_t rc_map_sas_mode_sw; + param_t rc_map_rtl_sw; + param_t rc_map_offboard_ctrl_mode_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; param_t rc_scale_roll; param_t rc_scale_pitch; param_t rc_scale_yaw; + param_t rc_scale_flaps; param_t battery_voltage_scaling; } _parameter_handles; /**< handles for interesting parameters */ @@ -377,22 +413,43 @@ Sensors::Sensors() : sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles.dz[i] = param_find(nbuf); - /* channel exponential gain */ - sprintf(nbuf, "RC%d_EXP", i + 1); - _parameter_handles.ex[i] = param_find(nbuf); + // /* channel exponential gain */ + // sprintf(nbuf, "RC%d_EXP", i + 1); + // _parameter_handles.ex[i] = param_find(nbuf); } _parameter_handles.rc_type = param_find("RC_TYPE"); + // _parameter_handles.rc_demix = param_find("RC_DEMIX"); + + /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); + _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); + _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); + _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -449,10 +506,10 @@ Sensors::~Sensors() int Sensors::parameters_update() { - const unsigned int nchans = 8; + bool rc_valid = true; /* rc values */ - for (unsigned int i = 0; i < nchans; i++) { + for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); @@ -469,25 +526,27 @@ Sensors::parameters_update() if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { warnx("Failed getting dead zone for chan %d", i); } - if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { - warnx("Failed getting exponential gain for chan %d", i); - } + // if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) { + // warnx("Failed getting exponential gain for chan %d", i); + // } _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + if (!isfinite(_parameters.scaling_factor[i]) || + _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || + _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + + /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; + rc_valid = false; } } - /* update RC function mappings */ - _rc.function[0] = _parameters.rc_map_throttle - 1; - _rc.function[1] = _parameters.rc_map_roll - 1; - _rc.function[2] = _parameters.rc_map_pitch - 1; - _rc.function[3] = _parameters.rc_map_yaw - 1; - _rc.function[4] = _parameters.rc_map_mode_sw - 1; + /* handle wrong values */ + if (!rc_valid) + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); /* remote control type */ if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { @@ -507,8 +566,44 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { + warnx("Failed getting override sw chan index"); + } + if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { + warnx("Failed getting auto mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { + warnx("Failed getting manual mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { + warnx("Failed getting rtl sw chan index"); + } + if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { + warnx("Failed getting sas mode sw chan index"); + } + if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { + warnx("Failed getting offboard control mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { + warnx("Failed getting mode aux 1 index"); + } + if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { + warnx("Failed getting mode aux 2 index"); + } + if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { + warnx("Failed getting mode aux 3 index"); + } + if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { + warnx("Failed getting mode aux 4 index"); + } + if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { + warnx("Failed getting mode aux 5 index"); } if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { @@ -520,6 +615,31 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { warnx("Failed getting rc scaling for yaw"); } + if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { + warnx("Failed getting rc scaling for flaps"); + } + + /* update RC function mappings */ + _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[ROLL] = _parameters.rc_map_roll - 1; + _rc.function[PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; + _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + + _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; + _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; + _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; + + _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -879,7 +999,7 @@ Sensors::ppm_poll() */ if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned int i = 0; i < ppm_decoded_channels; i++) { raw.values[i] = ppm_buffer[i]; } @@ -905,8 +1025,23 @@ Sensors::ppm_poll() struct manual_control_setpoint_s manual_control; - /* get a copy first, to prevent altering values */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control); + /* initialize to default values */ + manual_control.roll = NAN; + manual_control.pitch = NAN; + manual_control.yaw = NAN; + manual_control.throttle = NAN; + + manual_control.manual_mode_switch = NAN; + manual_control.manual_sas_switch = NAN; + manual_control.return_to_launch_switch = NAN; + manual_control.auto_offboard_input_switch = NAN; + + manual_control.flaps = NAN; + manual_control.aux1 = NAN; + manual_control.aux2 = NAN; + manual_control.aux3 = NAN; + manual_control.aux4 = NAN; + manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ if (rc_input.channel_count < 4) @@ -953,45 +1088,116 @@ Sensors::ppm_poll() manual_control.timestamp = rc_input.timestamp; + // /* check if input needs to be de-mixed */ + // if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) { + // // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS + + // /* roll input - mixed roll and pitch channels */ + // manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled); + // pitch input - mixed roll and pitch channels + // manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); + // /* yaw input - stick right is positive and positive rotation */ + // manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; + // /* throttle input */ + // manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + + // /* direct pass-through of manual input */ + // } else { + /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } - /* * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, * so reverse sign. */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); /* throttle input */ manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* override switch input */ + manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); + /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + + /* flaps */ + if (_rc.function[FLAPS] >= 0) { + + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } + + if (_rc.function[MANUAL_MODE] >= 0) { + manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + } + + if (_rc.function[SAS_MODE] >= 0) { + manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + } + + if (_rc.function[RTL] >= 0) { + manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + } + + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } + + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } + + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* check if ready for publishing */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + } else { + /* advertise the rc topic */ + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + /* check if ready for publishing */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } } } @@ -1040,7 +1246,7 @@ Sensors::task_main() memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.battery_voltage_v = BAT_VOL_INITIAL; - raw.adc_voltage_v[0] = 0.9f; + raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.battery_voltage_counter = 0; @@ -1057,27 +1263,6 @@ Sensors::task_main() /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - /* advertise the manual_control topic */ - struct manual_control_setpoint_s manual_control; - manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS; - manual_control.roll = 0.0f; - manual_control.pitch = 0.0f; - manual_control.yaw = 0.0f; - manual_control.throttle = 0.0f; - manual_control.aux1_cam_pan_flaps = 0.0f; - manual_control.aux2_cam_tilt = 0.0f; - manual_control.aux3_cam_zoom = 0.0f; - manual_control.aux4_cam_roll = 0.0f; - - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - - /* advertise the rc topic */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - } - /* wakeup source(s) */ struct pollfd fds[1]; diff --git a/apps/system/Makefile b/apps/system/Makefile index d64bb54c6..9955a6b2c 100644 --- a/apps/system/Makefile +++ b/apps/system/Makefile @@ -41,31 +41,36 @@ SUBDIRS = free i2c install readline poweroff ramtron sdcard sysinfo # Create the list of installed runtime modules (INSTALLED_DIRS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) define ADD_DIRECTORY -INSTALLED_DIRS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} + INSTALLED_DIRS += $(if $(wildcard .\$1\Makefile),$1,) endef +else +define ADD_DIRECTORY + INSTALLED_DIRS += $(if $(wildcard ./$1/Makefile),$1,) +endef +endif $(foreach DIR, $(SUBDIRS), $(eval $(call ADD_DIRECTORY,$(DIR)))) all: nothing .PHONY: nothing context depend clean distclean +define SDIR_template +$(1)_$(2): + $(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" +endef + +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),depend))) +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),clean))) +$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),distclean))) + nothing: context: -depend: - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done - -clean: - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +depend: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_depend) -distclean: clean - @for dir in $(INSTALLED_DIRS) ; do \ - $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ - done +clean: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_clean) +distclean: clean $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_distclean) diff --git a/apps/system/free/Makefile b/apps/system/free/Makefile index 7f911d81c..dada00d99 100644 --- a/apps/system/free/Makefile +++ b/apps/system/free/Makefile @@ -1,7 +1,7 @@ ############################################################################ # apps/system/free/Makefile # -# Copyright (C) 2011 Uros Platise. All rights reserved. +# Copyright (C) 2011-2012 Uros Platise. All rights reserved. # Author: Uros Platise <uros.platise@isotel.eu> # Gregory Nutt <gnutt@nuttx.org> # @@ -61,10 +61,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -83,32 +87,32 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built # Register application .context: $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) - @touch $@ + $(Q) touch $@ context: .context # Create dependencies .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f .context Make.dep .depend + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile index 00db91bb7..029d2b6fe 100644 --- a/apps/system/i2c/Makefile +++ b/apps/system/i2c/Makefile @@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -73,30 +77,29 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built .context: $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) - @touch $@ + $(Q) touch $@ context: .context .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) \ - $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f Make.dep .depend + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/system/install/Makefile b/apps/system/install/Makefile index 71d82f34c..6a02d859f 100644 --- a/apps/system/install/Makefile +++ b/apps/system/install/Makefile @@ -2,6 +2,7 @@ # apps/system/install/Makefile # # Copyright (C) 2011 Uros Platise. All rights reserved. +# Copyright (C) 2012 Gregory Nutt. All rights reserved. # Author: Uros Platise <uros.platise@isotel.eu> # Gregory Nutt <gnutt@nuttx.org> # @@ -61,10 +62,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -83,32 +88,32 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built # Register application .context: $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) - @touch $@ + $(Q) touch $@ context: .context # Create dependencies .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f .context Make.dep .depend + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/system/readline/Makefile b/apps/system/readline/Makefile index 34fab7e81..3a48d324e 100644 --- a/apps/system/readline/Makefile +++ b/apps/system/readline/Makefile @@ -52,10 +52,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT)) SRCS = $(ASRCS) $(CSRCS) OBJS = $(AOBJS) $(COBJS) +ifeq ($(CONFIG_WINDOWS_NATIVE),y) + BIN = ..\..\libapps$(LIBEXT) +else ifeq ($(WINTOOL),y) - BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" + BIN = ..\\..\\libapps$(LIBEXT) else - BIN = "$(APPDIR)/libapps$(LIBEXT)" + BIN = ../../libapps$(LIBEXT) +endif endif ROOTDEPPATH = --dep-path . @@ -74,10 +78,8 @@ $(COBJS): %$(OBJEXT): %.c $(call COMPILE, $<, $@) .built: $(OBJS) - @( for obj in $(OBJS) ; do \ - $(call ARCHIVE, $(BIN), $${obj}); \ - done ; ) - @touch .built + $(call ARCHIVE, $(BIN), $(OBJS)) + $(Q) touch .built # Context build phase target @@ -86,18 +88,20 @@ context: # Dependency build phase target .depend: Makefile $(SRCS) - @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep - @touch $@ + $(Q) $(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ depend: .depend # Housekeeping targets clean: - @rm -f *.o *~ .*.swp .built + $(call DELFILE, .built) $(call CLEAN) distclean: clean - @rm -f .context Make.dep .depend + $(call DELFILE, .context) + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) -include Make.dep diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25..49315cdc9 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -183,3 +183,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo return pid->last_output; } + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index b51afef9e..64d668867 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -72,6 +72,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT void pid_reset_integral(PID_t *pid); #endif /* PID_H_ */ diff --git a/apps/systemlib/scheduling_priorities.h b/apps/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/apps/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/sched.h> + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index aad2c4d9b..40278c56c 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_controls_effective.h * * Actuator control topics - mixer inputs. * diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560af..bbe429073 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 1368cb716..261a8a4ad 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -48,29 +48,33 @@ * @{ */ -enum MANUAL_CONTROL_MODE -{ - MANUAL_CONTROL_MODE_DIRECT = 0, - MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1, - MANUAL_CONTROL_MODE_ATT_YAW_POS = 2, - MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ -}; - struct manual_control_setpoint_s { uint64_t timestamp; - enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ + float roll; /**< ailerons roll / roll rate input */ + float pitch; /**< elevator / pitch / pitch rate */ + float yaw; /**< rudder / yaw rate / yaw */ + float throttle; /**< throttle / collective thrust / altitude */ + + float manual_override_switch; /**< manual override mode (mandatory) */ + float auto_mode_switch; /**< auto mode switch (mandatory) */ + + /** + * Any of the channels below may not be available and be set to NaN + * to indicate that it does not contain valid data. + */ + float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ + float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ + float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ + float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float override_mode_switch; + float flaps; /**< flap position */ - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ }; /**< manual control inputs */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index fef6ef2b3..9dd54df91 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,6 +50,13 @@ * @{ */ +/** + * The number of RC channel inputs supported. + * Current (Q1/2013) radios support up to 18 channels, + * leaving at a sane value of 14. + */ +#define RC_CHANNELS_MAX 14 + /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -62,14 +69,18 @@ enum RC_CHANNELS_FUNCTION PITCH = 2, YAW = 3, OVERRIDE = 4, - FUNC_0 = 5, - FUNC_1 = 6, - FUNC_2 = 7, - FUNC_3 = 8, - FUNC_4 = 9, - FUNC_5 = 10, - FUNC_6 = 11, - RC_CHANNELS_FUNCTION_MAX = 12 + AUTO_MODE = 5, + MANUAL_MODE = 6, + SAS_MODE = 7, + RTL = 8, + OFFBOARD_MODE = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; struct rc_channels_s { @@ -78,14 +89,13 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_FUNCTION_MAX]; - uint8_t chan_count; /**< maximum number of valid channels */ + } chan[RC_CHANNELS_MAX]; + uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - uint8_t function[RC_CHANNELS_FUNCTION_MAX]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ - bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 23172d7cf..230521f53 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -79,7 +79,7 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, VEHICLE_MODE_FLAG_TEST_ENABLED = 2, @@ -87,16 +87,23 @@ enum VEHICLE_MODE_FLAG { }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */ - VEHICLE_FLIGHT_MODE_ATTITUDE, /**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */ - VEHICLE_FLIGHT_MODE_STABILIZED, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ + VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ }; -enum VEHICLE_ATTITUDE_MODE { - VEHICLE_ATTITUDE_MODE_DIRECT, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_ATTITUDE_MODE_RATES, /**< body rates control mode */ - VEHICLE_ATTITUDE_MODE_ATTITUDE /**< tait-bryan attitude control mode */ +enum VEHICLE_MANUAL_CONTROL_MODE { + VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ + VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +}; + +enum VEHICLE_MANUAL_SAS_MODE { + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ + VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ + VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ }; /** @@ -115,10 +122,9 @@ struct vehicle_status_s commander_state_machine_t state_machine; /**< current flight state, main state machine */ enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - - // uint8_t mode; - + enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + int32_t system_type; /**< system type, inspired by MAVLinks MAV_TYPE enum */ /* system flags - these represent the state predicates */ @@ -164,9 +170,12 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; -// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ - bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - + bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool flag_local_position_valid; + bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_valid_launch_position; /**< indicates a valid launch position */ }; /** |