diff options
Diffstat (limited to 'apps')
92 files changed, 3462 insertions, 1772 deletions
diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt index 7375adccf..0695747f8 100644 --- a/apps/ChangeLog.txt +++ b/apps/ChangeLog.txt @@ -33,7 +33,7 @@ 6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org> - * apps/interpreter: Add a directory to hold interpreters. The Pascal add- + * apps/interpreter: Add a directory to hold interpreters. The Pascal add- on module now installs and builds under this directory. * apps/interpreter/ficl: Added logic to build Ficl (the "Forth Inspired Command Language"). See http://ficl.sourceforge.net/. @@ -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,70 @@ 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-12-20 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. + * apps/examples/relays/Makefile: Reduced stack requirement (Darcy Gong). + * apps/nshlib and apps/netutils/dhcpc: Extend the NSH ifconfig command plus + various DHCPC improvements(Darcy Gong). + * apps/nshlib/nsh_apps.c: Fix compilation errors when CONFIG_NSH_DISABLEBG=y. + From Freddie Chopin. + * Rename CONFIG_PCODE and CONFIG_FICL as CONFIG_INTERPRETERS_PCODE and + CONFIG_INTERPRETERS_FICL for consistency with other configuration naming. + * apps/examples/keypadtest: A keypad test example contributed by Denis + Carikli. + * apps/examples/elf and nxflat: If CONFIG_BINFMT_EXEPATH is defined, these + tests will now use a relative path to the program and expect the binfmt/ + logic to find the absolute path to the program using the PATH variable. + +6.25 2013-xx-xx Gregory Nutt <gnutt@nuttx.org> diff --git a/apps/Makefile b/apps/Makefile index 11d95bc19..f0de58e25 100644 --- a/apps/Makefile +++ b/apps/Makefile @@ -107,7 +107,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)))) @@ -120,8 +120,10 @@ INSTALLED_APPS += $(EXTERNAL_APPS) # 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 @@ -133,48 +135,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 c05948402..7654b3426 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -72,8 +72,10 @@ #include <uORB/topics/battery_status.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> @@ -1251,6 +1253,7 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; + bool home_position_set = false; /* set parameters */ failsafe_lowlevel_timeout_ms = 0; @@ -1302,6 +1305,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish current state machine */ state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + if (stat_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed.\n"); exit(ERROR); @@ -1319,9 +1327,9 @@ int commander_thread_main(int argc, char *argv[]) uint16_t counter = 0; uint8_t flight_env; - /* Initialize to 3.0V to make sure the low-pass loads below valid threshold */ - float battery_voltage = 12.0f; - bool battery_voltage_valid = true; + /* Initialize to 0.0V */ + float battery_voltage = 0.0f; + bool battery_voltage_valid = false; bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; @@ -1332,10 +1340,6 @@ int commander_thread_main(int argc, char *argv[]) uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; - float hdop = 65535.0f; - - int gps_quality_good_counter = 0; - /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1356,6 +1360,15 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1411,9 +1424,6 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - - } else { - sensors.battery_voltage_valid = false; } orb_check(cmd_sub, &new_data); @@ -1473,8 +1483,8 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* update battery status */ orb_check(battery_sub, &new_data); - if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); battery_voltage = battery.voltage_v; @@ -1663,65 +1673,57 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } + if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { - /* Check if last transition deserved an audio event */ -// #warning This code depends on state that is no longer? maintained -// #if 0 -// trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); -// #endif + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* only check gps fix if we are outdoor */ -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// -// hdop = (float)(gps.eph) / 100.0f; -// -// /* check if gps fix is ok */ -// if (gps.fix_type == GPS_FIX_TYPE_3D) { //TODO: is 2d-fix ok? //see http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29 -// -// if (gotfix_counter >= GPS_GOTFIX_COUNTER_REQUIRED) { //TODO: add also a required time? -// update_state_machine_got_position_fix(stat_pub, ¤t_status); -// gotfix_counter = 0; -// } else { -// gotfix_counter++; -// } -// nofix_counter = 0; -// -// if (hdop < 5.0f) { //TODO: this should be a parameter -// if (gps_quality_good_counter > GPS_QUALITY_GOOD_COUNTER_LIMIT) { -// current_status.gps_valid = true;//--> position estimator can use the gps measurements -// } -// -// gps_quality_good_counter++; -// -// -//// if(counter%10 == 0)//for testing only -//// warnx("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only -// -// } else { -// gps_quality_good_counter = 0; -// current_status.gps_valid = false;//--> position estimator can not use the gps measurements -// } -// -// } else { -// gps_quality_good_counter = 0; -// current_status.gps_valid = false;//--> position estimator can not use the gps measurements -// -// if (nofix_counter > GPS_NOFIX_COUNTER_LIMIT) { //TODO: add also a timer limit? -// update_state_machine_no_position_fix(stat_pub, ¤t_status); -// nofix_counter = 0; -// } else { -// nofix_counter++; -// } -// gotfix_counter = 0; -// } -// -// } -// -// -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests - //update_state_machine_got_position_fix(stat_pub, ¤t_status, mavlink_fd); - /* end: check gps */ + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph / 100.0f; + float vdop_m = gps_position.epv / 100.0f; + + /* check if gps fix is ok */ + // XXX magic number + float dop_threshold_m = 2.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop < dop_threshold_m) + && (vdop_m < dop_threshold_m) + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp < 2000000) + && !current_status.flag_system_armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph = gps_position.eph; + home.epv = gps_position.epv; + + home.s_variance = gps_position.s_variance; + home.p_variance = gps_position.p_variance; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + /* mark home position as set */ + home_position_set = true; + tune_confirm(); + } + } /* ignore RC signals if in offboard control mode */ if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { @@ -1848,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[]) update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - /* check auto mode switch for correct mode */ - if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - /* enable guided mode */ - update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } + // } } else { /* center stick position, set SAS for all vehicle types */ @@ -2044,4 +2047,3 @@ int commander_thread_main(int argc, char *argv[]) return 0; } - diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index d4e88b146..bea388a10 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -134,7 +134,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con case SYSTEM_STATE_REBOOT: if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { invalid_state = false; /* set system flags according to state */ current_status->flag_system_armed = false; @@ -708,7 +709,9 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ case SYSTEM_STATE_REBOOT: printf("try to reboot\n"); - if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { + if (current_system_state == SYSTEM_STATE_STANDBY + || current_system_state == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { printf("system will reboot\n"); mavlink_log_critical(mavlink_fd, "Rebooting.."); usleep(200000); diff --git a/apps/controllib/Makefile b/apps/controllib/Makefile index 99cf91069..6749b805f 100644 --- a/apps/controllib/Makefile +++ b/apps/controllib/Makefile @@ -43,11 +43,4 @@ CXXSRCS = block/Block.cpp \ blocks.cpp \ fixedwing.cpp -CXXHDRS = block/Block.hpp \ - block/BlockParam.hpp \ - block/UOrbPublication.hpp \ - block/UOrbSubscription.hpp \ - blocks.hpp \ - fixedwing.hpp - include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 2a7bfe3d7..e88d2861e 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -226,7 +226,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN10); stm32_configgpio(GPIO_ADC1_IN11); stm32_configgpio(GPIO_ADC1_IN12); - //stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards return OK; } diff --git a/apps/drivers/stm32/adc/adc.cpp b/apps/drivers/stm32/adc/adc.cpp index 87dac1ef9..911def943 100644 --- a/apps/drivers/stm32/adc/adc.cpp +++ b/apps/drivers/stm32/adc/adc.cpp @@ -366,8 +366,8 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the minimum channel set for PX4FMU - should be configurable */ - g_adc = new ADC((1 << 10) | (1 << 11)); + /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/apps/examples/Kconfig b/apps/examples/Kconfig index 865268add..ae5f0a61a 100644 --- a/apps/examples/Kconfig +++ b/apps/examples/Kconfig @@ -3,206 +3,60 @@ # 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/keypadtest/Kconfig" 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..91f1331df 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,14 @@ ifeq ($(CONFIG_EXAMPLES_IGMP),y) CONFIGURED_APPS += examples/igmp endif +ifeq ($(CONFIG_EXAMPLES_JSON),y) +CONFIGURED_APPS += examples/json +endif + +ifeq ($(CONFIG_EXAMPLES_KEYPADTEST),y) +CONFIGURED_APPS += examples/keypadtest +endif + ifeq ($(CONFIG_EXAMPLES_LCDRW),y) CONFIGURED_APPS += examples/lcdrw endif @@ -94,6 +110,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 +186,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 +250,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..bdbfd4de8 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 keypadtest 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 cxxtest dhcpd discover ftpd json keypadtest +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..e40a63be9 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,32 @@ 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/keypadtest +^^^^^^^^^^^^^^^^^^^ + + This is a generic keypad test example. It is similar to the USB hidkbd + example, but makes no assumptions about the underlying keyboard interface. + It uses the interfaces of include/nuttx/input/keypad.h. + + CONFIG_EXAMPLES_KEYPADTEST - Selects the keypadtest example (only need + if the mconf/Kconfig tool is used. + + CONFIG_EXAMPLES_KEYPAD_DEVNAME - The name of the keypad device that will + be opened in order to perform the keypad test. Default: "/dev/keypad" + examples/lcdrw ^^^^^^^^^^^^^^ @@ -496,6 +607,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 +954,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 +991,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 +1101,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 +1238,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 +1811,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/control_demo/control_demo.cpp b/apps/examples/control_demo/control_demo.cpp index d9c773c05..e609f2f4b 100644 --- a/apps/examples/control_demo/control_demo.cpp +++ b/apps/examples/control_demo/control_demo.cpp @@ -108,7 +108,7 @@ int control_demo_main(int argc, char *argv[]) deamon_task = task_spawn("control_demo", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, - 4096, + 5120, control_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 4eec456fb..6525b70f5 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -6,58 +6,58 @@ // 16 is max name length // gyro low pass filter -PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq // yaw washout PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass // stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder // psi -> phi -> p -PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll -PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit +PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll +PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f); -PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f); -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); // h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f); -PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID +PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); // crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f); -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f); +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain // speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); -PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); -PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); +PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity // trim -PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f); +PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) 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/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index c2faa89de..7e89dffb2 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -42,10 +42,12 @@ #include "KalmanNav.hpp" // constants +// Titterton pg. 52 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R = 6.371000e6f; // earth radius, m -static const float RSq = 4.0589641e13f; // radius squared -static const float g = 9.8f; // gravitational accel. m/s^2 +static const float R0 = 6378137.0f; // earth radius, m +static const float g0 = 9.806f; // standard gravitational accel. m/s^2 +static const int8_t ret_ok = 0; // no error in function +static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), @@ -53,33 +55,36 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : F(9, 9), G(9, 6), P(9, 9), + P0(9, 9), V(6, 6), // attitude measurement ekf matrices HAtt(6, 9), RAtt(6, 6), - // gps measurement ekf matrices - HGps(6, 9), - RGps(6, 6), + // position measurement ekf matrices + HPos(5, 9), + RPos(5, 5), // attitude representations C_nb(), q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), - _fastTimeStamp(hrt_absolute_time()), - _slowTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), _attTimeStamp(hrt_absolute_time()), _outTimeStamp(hrt_absolute_time()), // frame count _navFrames(0), - // state + // miss counts + _miss(0), + // accelerations fN(0), fE(0), fD(0), + // state phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), @@ -87,36 +92,35 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _vGyro(this, "V_GYRO"), _vAccel(this, "V_ACCEL"), _rMag(this, "R_MAG"), - _rGpsV(this, "R_GPS_V"), - _rGpsGeo(this, "R_GPS_GEO"), + _rGpsVel(this, "R_GPS_VEL"), + _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), - _rAccel(this, "R_ACCEL") + _rAccel(this, "R_ACCEL"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), + _attitudeInitialized(false), + _positionInitialized(false), + _attitudeInitCounter(0) { using namespace math; // initial state covariance matrix - P = Matrix::identity(9) * 1.0f; - - // wait for gps lock - while (1) { - updateSubscriptions(); - - if (_gps.fix_type > 2) break; - - printf("[kalman_demo] waiting for gps lock\n"); - usleep(1000000); - } + P0 = Matrix::identity(9) * 0.01f; + P = P0; // initial state phi = 0.0f; theta = 0.0f; psi = 0.0f; - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); + vN = 0.0f; + vE = 0.0f; + vD = 0.0f; + lat = 0.0f; + lon = 0.0f; + alt = 0.0f; // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -124,16 +128,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // initialize dcm C_nb = Dcm(q); - // initialize F to identity - F = Matrix::identity(9); - - // HGps is constant - HGps(0, 3) = 1.0f; - HGps(1, 4) = 1.0f; - HGps(2, 5) = 1.0f; - HGps(3, 6) = 1.0f; - HGps(4, 7) = 1.0f; - HGps(5, 8) = 1.0f; + // HPos is constant + HPos(0, 3) = 1.0f; + HPos(1, 4) = 1.0f; + HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(4, 8) = 1.0f; // initialize all parameters updateParams(); @@ -143,68 +143,95 @@ void KalmanNav::update() { using namespace math; - struct pollfd fds[2]; + struct pollfd fds[1]; fds[0].fd = _sensors.getHandle(); fds[0].events = POLLIN; - fds[1].fd = _param_update.getHandle(); - fds[1].events = POLLIN; - // poll 20 milliseconds for new data - int ret = poll(fds, 2, 20); + // poll for new data + int ret = poll(fds, 1, 1000); - // check return value if (ret < 0) { // XXX this is seriously bad - should be an emergency return; } else if (ret == 0) { // timeout - // run anyway - } else if (ret > 0) { - // update params when requested - if (fds[1].revents & POLLIN) { - printf("updating params\n"); - updateParams(); - } - - // if no new sensor data, return - if (!(fds[0].revents & POLLIN)) return; + return; } // get new timestamp uint64_t newTimeStamp = hrt_absolute_time(); // check updated subscriptions + if (_param_update.updated()) updateParams(); + bool gpsUpdate = _gps.updated(); + bool sensorsUpdate = _sensors.updated(); // get new information from subscriptions // this clears update flag updateSubscriptions(); - // count fast frames - _navFrames += 1; - - // fast prediciton step - // note, using sensors timestamp so we can account - // for packet lag - float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; - _fastTimeStamp = _sensors.timestamp; - predictFast(dtFast); + // initialize attitude when sensors online + if (!_attitudeInitialized && sensorsUpdate && + _sensors.accelerometer_counter > 10 && + _sensors.gyro_counter > 10 && + _sensors.magnetometer_counter > 10) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; + + if (_attitudeInitCounter > 100) { + printf("[kalman_demo] initialized EKF attitude\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + _attitudeInitialized = true; + } + } - // slow prediction step - float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f; + // initialize position when gps received + if (!_positionInitialized && + _attitudeInitialized && // wait for attitude first + gpsUpdate && + _gps.fix_type > 2 && + _gps.counter_pos_valid > 10) { + vN = _gps.vel_n; + vE = _gps.vel_e; + vD = _gps.vel_d; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + _positionInitialized = true; + printf("[kalman_demo] initialized EKF state with GPS\n"); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); + } - if (dtSlow > 1.0f / 200) { // 200 Hz - _slowTimeStamp = _sensors.timestamp; - predictSlow(dtSlow); + // prediciton step + // using sensors timestamp so we can account for packet lag + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dt)); + _predictTimeStamp = _sensors.timestamp; + + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); + // count fast frames + _navFrames += 1; } + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; + // gps correction step - if (gpsUpdate) { - correctGps(); + if (_positionInitialized && gpsUpdate) { + correctPos(); } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + ) { _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -212,14 +239,17 @@ void KalmanNav::update() // publication if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz _pubTimeStamp = newTimeStamp; + updatePublications(); } // output - if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz\n", _navFrames); + printf("nav: %4d Hz, miss #: %4d\n", + _navFrames / 10, _miss / 10); _navFrames = 0; + _miss = 0; } } @@ -262,66 +292,87 @@ void KalmanNav::updatePublications() _att.q_valid = true; _att.counter = _navFrames; - // update publications - SuperBlock::updatePublications(); + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) + _pos.update(); + + if (_attitudeInitialized) + _att.update(); } -void KalmanNav::predictFast(float dt) +int KalmanNav::predictState(float dt) { using namespace math; - Vector3 w(_sensors.gyro_rad_s); - // attitude - q = q + q.derivative(w) * dt; + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSing = cosf(lat); - // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; } - // C_nb update - C_nb = Dcm(q); + // attitude prediction + if (_attitudeInitialized) { + Vector3 w(_sensors.gyro_rad_s); - // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + // attitude + q = q + q.derivative(w) * dt; - // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); + // renormalize quaternion if needed + if (fabsf(q.norm() - 1.0f) > 1e-4f) { + q = q.unit(); + } - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); + // C_nb update + C_nb = Dcm(q); + + // euler update + EulerAngles euler(C_nb); + phi = euler.getPhi(); + theta = euler.getTheta(); + psi = euler.getPsi(); + + // specific acceleration in nav frame + Vector3 accelB(_sensors.accelerometer_m_s2); + Vector3 accelN = C_nb * accelB; + fN = accelN(0); + fE = accelN(1); + fD = accelN(2); + } - // position update - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float LDot = vN / (R + float(alt)); - float lDot = vE / (cosL * (R + float(alt))); - float vNDot = fN - vE * (2 * omega + - lDot) * sinL + - vD * LDot; - float vDDot = fD - vE * (2 * omega + lDot) * cosL - - vN * LDot + g; - float vEDot = fE + vN * (2 * omega + lDot) * sinL + - vDDot * (2 * omega * cosL); - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); + // position prediction + if (_positionInitialized) { + // neglects angular deflections in local gravity + // see Titerton pg. 70 + float R = R0 + float(alt); + float LDot = vN / R; + float lDot = vE / (cosLSing * R); + float rotRate = 2 * omega + lDot; + float vNDot = fN - vE * rotRate * sinL + + vD * LDot; + float vDDot = fD - vE * rotRate * cosL - + vN * LDot + _g.get(); + float vEDot = fE + vN * rotRate * sinL + + vDDot * rotRate * cosL; + + // rectangular integration + vN += vNDot * dt; + vE += vEDot * dt; + vD += vDDot * dt; + lat += double(LDot * dt); + lon += double(lDot * dt); + alt += double(-vD * dt); + } + + return ret_ok; } -void KalmanNav::predictSlow(float dt) +int KalmanNav::predictStateCovariance(float dt) { using namespace math; @@ -331,90 +382,94 @@ void KalmanNav::predictSlow(float dt) float cosLSq = cosL * cosL; float tanL = tanf(lat); + // prepare for matrix + float R = R0 + float(alt); + float RSq = R * R; + // F Matrix // Titterton pg. 291 - // - // difference from Jacobian - // multiplity by dt for all elements - // add 1.0 to diagonal elements - - F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt; - F(0, 2) = (vN / R) * dt; - F(0, 4) = (1.0f / R) * dt; - F(0, 6) = (-omega * sinL) * dt; - F(0, 8) = (-vE / RSq) * dt; - - F(1, 0) = (omega * sinL + vE * tanL / R) * dt; - F(1, 2) = (omega * cosL + vE / R) * dt; - F(1, 3) = (-1.0f / R) * dt; - F(1, 8) = (vN / RSq) * dt; - - F(2, 0) = (-vN / R) * dt; - F(2, 1) = (-omega * cosL - vE / R) * dt; - F(2, 4) = (-tanL / R) * dt; - F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt; - F(2, 8) = (vE * tanL / RSq) * dt; - - F(3, 1) = (-fD) * dt; - F(3, 2) = (fE) * dt; - F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal - F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt; - F(3, 5) = (vN / R) * dt; - F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt; - F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt; - - F(4, 0) = (fD) * dt; - F(4, 2) = (-fN) * dt; - F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt; - F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal - F(4, 5) = (2 * omega * cosL + vE / R) * dt; - F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq)) * dt; - F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt; - - F(5, 0) = (-fE) * dt; - F(5, 1) = (fN) * dt; - F(5, 3) = (-2 * vN / R) * dt; - F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt; - F(5, 6) = (2 * omega * vE * sinL) * dt; - F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt; - - F(6, 3) = (1 / R) * dt; - F(6, 8) = (-vN / RSq) * dt; - - F(7, 4) = (1 / (R * cosL)) * dt; - F(7, 6) = (vE * tanL / (R * cosL)) * dt; - F(7, 8) = (-vE / (cosL * RSq)) * dt; - - F(8, 5) = (-1) * dt; + + F(0, 1) = -(omega * sinL + vE * tanL / R); + F(0, 2) = vN / R; + F(0, 4) = 1.0f / R; + F(0, 6) = -omega * sinL; + F(0, 8) = -vE / RSq; + + F(1, 0) = omega * sinL + vE * tanL / R; + F(1, 2) = omega * cosL + vE / R; + F(1, 3) = -1.0f / R; + F(1, 8) = vN / RSq; + + F(2, 0) = -vN / R; + F(2, 1) = -omega * cosL - vE / R; + F(2, 4) = -tanL / R; + F(2, 6) = -omega * cosL - vE / (R * cosLSq); + F(2, 8) = vE * tanL / RSq; + + F(3, 1) = -fD; + F(3, 2) = fE; + F(3, 3) = vD / R; + F(3, 4) = -2 * (omega * sinL + vE * tanL / R); + F(3, 5) = vN / R; + F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); + F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; + + F(4, 0) = fD; + F(4, 2) = -fN; + F(4, 3) = 2 * omega * sinL + vE * tanL / R; + F(4, 4) = (vN * tanL + vD) / R; + F(4, 5) = 2 * omega * cosL + vE / R; + F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + + vN * vE / (R * cosLSq); + F(4, 8) = -vE * (vN * tanL + vD) / RSq; + + F(5, 0) = -fE; + F(5, 1) = fN; + F(5, 3) = -2 * vN / R; + F(5, 4) = -2 * (omega * cosL + vE / R); + F(5, 6) = 2 * omega * vE * sinL; + F(5, 8) = (vN * vN + vE * vE) / RSq; + + F(6, 3) = 1 / R; + F(6, 8) = -vN / RSq; + + F(7, 4) = 1 / (R * cosL); + F(7, 6) = vE * tanL / (R * cosL); + F(7, 8) = -vE / (cosL * RSq); + + F(8, 5) = -1; // G Matrix // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0) * dt; - G(0, 1) = -C_nb(0, 1) * dt; - G(0, 2) = -C_nb(0, 2) * dt; - G(1, 0) = -C_nb(1, 0) * dt; - G(1, 1) = -C_nb(1, 1) * dt; - G(1, 2) = -C_nb(1, 2) * dt; - G(2, 0) = -C_nb(2, 0) * dt; - G(2, 1) = -C_nb(2, 1) * dt; - G(2, 2) = -C_nb(2, 2) * dt; - - G(3, 3) = C_nb(0, 0) * dt; - G(3, 4) = C_nb(0, 1) * dt; - G(3, 5) = C_nb(0, 2) * dt; - G(4, 3) = C_nb(1, 0) * dt; - G(4, 4) = C_nb(1, 1) * dt; - G(4, 5) = C_nb(1, 2) * dt; - G(5, 3) = C_nb(2, 0) * dt; - G(5, 4) = C_nb(2, 1) * dt; - G(5, 5) = C_nb(2, 2) * dt; - - // predict equations for kalman filter - P = F * P * F.transpose() + G * V * G.transpose(); + G(0, 0) = -C_nb(0, 0); + G(0, 1) = -C_nb(0, 1); + G(0, 2) = -C_nb(0, 2); + G(1, 0) = -C_nb(1, 0); + G(1, 1) = -C_nb(1, 1); + G(1, 2) = -C_nb(1, 2); + G(2, 0) = -C_nb(2, 0); + G(2, 1) = -C_nb(2, 1); + G(2, 2) = -C_nb(2, 2); + + G(3, 3) = C_nb(0, 0); + G(3, 4) = C_nb(0, 1); + G(3, 5) = C_nb(0, 2); + G(4, 3) = C_nb(1, 0); + G(4, 4) = C_nb(1, 1); + G(4, 5) = C_nb(1, 2); + G(5, 3) = C_nb(2, 0); + G(5, 4) = C_nb(2, 1); + G(5, 5) = C_nb(2, 2); + + // continuous predictioon equations + // for discrte time EKF + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + + return ret_ok; } -void KalmanNav::correctAtt() +int KalmanNav::correctAtt() { using namespace math; @@ -428,13 +483,14 @@ void KalmanNav::correctAtt() // mag measurement Vector3 zMag(_sensors.magnetometer_ga); + //float magNorm = zMag.norm(); zMag = zMag.unit(); // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); @@ -443,10 +499,25 @@ void KalmanNav::correctAtt() // accel measurement Vector3 zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.norm(); zAccel = zAccel.unit(); + // ignore accel correction when accel mag not close to g + Matrix RAttAdjust = RAtt; + + bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; + + if (ignoreAccel) { + RAttAdjust(3, 3) = 1.0e10; + RAttAdjust(4, 4) = 1.0e10; + RAttAdjust(5, 5) = 1.0e10; + + } else { + //printf("correcting attitude with accel\n"); + } + // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit(); + Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); // combined measurement Vector zAtt(6); @@ -498,8 +569,9 @@ void KalmanNav::correctAtt() HAtt(5, 1) = cosPhi * sinTheta; // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter Vector y = zAtt - zAttHat; // residual - Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance + Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance Matrix K = P * HAtt.transpose() * S.inverse(); Vector xCorrect = K * y; @@ -510,46 +582,65 @@ void KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return printf("[kalman_demo] numerical failure in att correction\n"); - return; + // reset P matrix to P0 + P = P0; + return ret_error; } } // correct state - phi += xCorrect(PHI); - theta += xCorrect(THETA); + if (!ignoreAccel) { + phi += xCorrect(PHI); + theta += xCorrect(THETA); + } + psi += xCorrect(PSI); + // attitude also affects nav velocities + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter P = P - K * HAtt * P; // fault detection float beta = y.dot(S.inverse() * y); - printf("attitude: beta = %8.4f\n", (double)beta); - if (beta > 10.0f) { - //printf("fault in attitude: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + if (beta > _faultAtt.get()) { + printf("fault in attitude: beta = %8.4f\n", (double)beta); + printf("y:\n"); y.print(); + printf("zMagHat:\n"); zMagHat.print(); + printf("zMag:\n"); zMag.print(); + printf("bNav:\n"); bNav.print(); } // update quaternions from euler // angle correction q = Quaternion(EulerAngles(phi, theta, psi)); + + return ret_ok; } -void KalmanNav::correctGps() +int KalmanNav::correctPos() { using namespace math; - Vector y(6); + + // residual + Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; - y(2) = _gps.vel_d - vD; - y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat; - y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon; - y(5) = double(_gps.alt) / 1.0e3 - alt; + y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; + y(4) = double(_gps.alt) / 1.0e3 - alt; // compute correction - Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance - Matrix K = P * HGps.transpose() * S.inverse(); + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance + Matrix K = P * HPos.transpose() * S.inverse(); Vector xCorrect = K * y; // check correction is sane @@ -566,7 +657,9 @@ void KalmanNav::correctGps() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); - return; + // reset P matrix to P0 + P = P0; + return ret_error; } } @@ -579,16 +672,23 @@ void KalmanNav::correctGps() alt += double(xCorrect(ALT)); // update state covariance - P = P - K * HGps * P; + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HPos * P; // fault detetcion float beta = y.dot(S.inverse() * y); - printf("gps: beta = %8.4f\n", (double)beta); - if (beta > 100.0f) { - //printf("fault in gps: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + if (beta > _faultPos.get()) { + printf("fault in gps: beta = %8.4f\n", (double)beta); + printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(y(0) / sqrtf(RPos(0, 0))), + double(y(1) / sqrtf(RPos(1, 1))), + double(y(2) / sqrtf(RPos(2, 2))), + double(y(3) / sqrtf(RPos(3, 3))), + double(y(4) / sqrtf(RPos(4, 4)))); } + + return ret_ok; } void KalmanNav::updateParams() @@ -608,20 +708,54 @@ void KalmanNav::updateParams() V(5, 5) = _vAccel.get(); // accel z // magnetometer noise - RAtt(0, 0) = _rMag.get(); // normalized direction - RAtt(1, 1) = _rMag.get(); - RAtt(2, 2) = _rMag.get(); + float noiseMin = 1e-6f; + float noiseMagSq = _rMag.get() * _rMag.get(); + + if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; + + RAtt(0, 0) = noiseMagSq; // normalized direction + RAtt(1, 1) = noiseMagSq; + RAtt(2, 2) = noiseMagSq; // accelerometer noise - RAtt(3, 3) = _rAccel.get(); // normalized direction - RAtt(4, 4) = _rAccel.get(); - RAtt(5, 5) = _rAccel.get(); + float noiseAccelSq = _rAccel.get() * _rAccel.get(); + + // bound noise to prevent singularities + if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; + + RAtt(3, 3) = noiseAccelSq; // normalized direction + RAtt(4, 4) = noiseAccelSq; + RAtt(5, 5) = noiseAccelSq; // gps noise - RGps(0, 0) = _rGpsV.get(); // vn, m/s - RGps(1, 1) = _rGpsV.get(); // ve - RGps(2, 2) = _rGpsV.get(); // vd - RGps(3, 3) = _rGpsGeo.get(); // L, rad - RGps(4, 4) = _rGpsGeo.get(); // l, rad - RGps(5, 5) = _rGpsAlt.get(); // h, m + float R = R0 + float(alt); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + float noiseVel = _rGpsVel.get(); + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + float noiseAlt = _rGpsAlt.get(); + + // bound noise to prevent singularities + if (noiseVel < noiseMin) noiseVel = noiseMin; + + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + + if (noiseAlt < noiseMin) noiseAlt = noiseMin; + + RPos(0, 0) = noiseVel * noiseVel; // vn + RPos(1, 1) = noiseVel * noiseVel; // ve + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon + RPos(4, 4) = noiseAlt * noiseAlt; // h + // XXX, note that RPos depends on lat, so updateParams should + // be called if lat changes significantly } diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index dc4a81f4a..7709ac697 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -60,53 +60,116 @@ #include <poll.h> #include <unistd.h> +/** + * Kalman filter navigation class + * http://en.wikipedia.org/wiki/Extended_Kalman_filter + * Discrete-time extended Kalman filter + */ class KalmanNav : public control::SuperBlock { public: + /** + * Constructor + */ KalmanNav(SuperBlock *parent, const char *name); + + /** + * Deconstuctor + */ + virtual ~KalmanNav() {}; + /** + * The main callback function for the class + */ void update(); + + + /** + * Publication update + */ virtual void updatePublications(); - void predictFast(float dt); - void predictSlow(float dt); - void correctAtt(); - void correctGps(); + + /** + * State prediction + * Continuous, non-linear + */ + int predictState(float dt); + + /** + * State covariance prediction + * Continuous, linear + */ + int predictStateCovariance(float dt); + + /** + * Attitude correction + */ + int correctAtt(); + + /** + * Position correction + */ + int correctPos(); + + /** + * Overloaded update parameters + */ virtual void updateParams(); protected: - math::Matrix F; - math::Matrix G; - math::Matrix P; - math::Matrix V; - math::Matrix HAtt; - math::Matrix RAtt; - math::Matrix HGps; - math::Matrix RGps; - math::Dcm C_nb; - math::Quaternion q; - control::UOrbSubscription<sensor_combined_s> _sensors; - control::UOrbSubscription<vehicle_gps_position_s> _gps; - control::UOrbSubscription<parameter_update_s> _param_update; - control::UOrbPublication<vehicle_global_position_s> _pos; - control::UOrbPublication<vehicle_attitude_s> _att; - uint64_t _pubTimeStamp; - uint64_t _fastTimeStamp; - uint64_t _slowTimeStamp; - uint64_t _attTimeStamp; - uint64_t _outTimeStamp; - uint16_t _navFrames; - float fN, fE, fD; + // kalman filter + math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix G; /**< noise shaping matrix for gyro/accel */ + math::Matrix P; /**< state covariance matrix */ + math::Matrix P0; /**< initial state covariance matrix */ + math::Matrix V; /**< gyro/ accel noise matrix */ + math::Matrix HAtt; /**< attitude measurement matrix */ + math::Matrix RAtt; /**< attitude measurement noise matrix */ + math::Matrix HPos; /**< position measurement jacobian matrix */ + math::Matrix RPos; /**< position measurement noise matrix */ + // attitude + math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ + control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + // time stamps + uint64_t _pubTimeStamp; /**< output data publication time stamp */ + uint64_t _predictTimeStamp; /**< prediction time stamp */ + uint64_t _attTimeStamp; /**< attitude correction time stamp */ + uint64_t _outTimeStamp; /**< output time stamp */ + // frame count + uint16_t _navFrames; /**< navigation frames completed in output cycle */ + // miss counts + uint16_t _miss; /**< number of times fast prediction loop missed */ + // accelerations + float fN, fE, fD; /**< navigation frame acceleration */ // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; - float phi, theta, psi; - float vN, vE, vD; - double lat, lon, alt; - control::BlockParam<float> _vGyro; - control::BlockParam<float> _vAccel; - control::BlockParam<float> _rMag; - control::BlockParam<float> _rGpsV; - control::BlockParam<float> _rGpsGeo; - control::BlockParam<float> _rGpsAlt; - control::BlockParam<float> _rAccel; + enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ + float phi, theta, psi; /**< 3-2-1 euler angles */ + float vN, vE, vD; /**< navigation velocity, m/s */ + double lat, lon, alt; /**< lat, lon, alt, radians */ + // parameters + control::BlockParam<float> _vGyro; /**< gyro process noise */ + control::BlockParam<float> _vAccel; /**< accelerometer process noise */ + control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ + control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ + control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ + control::BlockParam<float> _magDip; /**< magnetic inclination with level */ + control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam<float> _g; /**< gravitational constant */ + control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ + control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + // status + bool _attitudeInitialized; + bool _positionInitialized; + uint16_t _attitudeInitCounter; + // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 327e2cda6..3bfe01261 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,10 +1,16 @@ #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f); -PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); + 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/gps/gps.c b/apps/gps/gps.c index 00f6ee9f8..8a9512054 100644 --- a/apps/gps/gps.c +++ b/apps/gps/gps.c @@ -107,8 +107,8 @@ enum GPS_MODES { #define AUTO_DETECTION_COUNT 8 -const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400}; -const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea +const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600}; +const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea /**************************************************************************** * Private functions @@ -368,7 +368,6 @@ int gps_thread_main(int argc, char *argv[]) { args.thread_should_exit_ptr = &thread_should_exit; pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args); - sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver pthread_attr_t ubx_wd_attr; pthread_attr_init(&ubx_wd_attr); diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index e15ed4e00..50bf579a0 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -52,7 +52,7 @@ #define UBX_HEALTH_FAIL_COUNTER_LIMIT 3 #define UBX_HEALTH_PROBE_COUNTER_LIMIT 4 -#define UBX_BUFFER_SIZE 1000 +#define UBX_BUFFER_SIZE 500 extern bool gps_mode_try_all; extern bool gps_mode_success; @@ -63,18 +63,10 @@ extern int current_gps_speed; pthread_mutex_t *ubx_mutex; gps_bin_ubx_state_t *ubx_state; +enum UBX_CONFIG_STATE ubx_config_state; static struct vehicle_gps_position_s *ubx_gps; -//Definitions for ubx, last two bytes are checksum which is calculated below -uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00}; -uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00}; void ubx_decode_init(void) { @@ -100,15 +92,15 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) int ubx_parse(uint8_t b, char *gps_rx_buffer) { -// printf("b=%x\n",b); + //printf("b=%x\n",b); if (ubx_state->decode_state == UBX_DECODE_UNINIT) { - if (b == 0xb5) { + if (b == UBX_SYNC_1) { ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; } } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { - if (b == 0x62) { + if (b == UBX_SYNC_2) { ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; } else { @@ -122,16 +114,25 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) //check for known class switch (b) { - case UBX_CLASS_NAV: //NAV + case UBX_CLASS_ACK: + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = ACK; + break; + + case UBX_CLASS_NAV: ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = NAV; break; - case UBX_CLASS_RXM: //RXM + case UBX_CLASS_RXM: ubx_state->decode_state = UBX_DECODE_GOT_CLASS; ubx_state->message_class = RXM; break; + case UBX_CLASS_CFG: + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = CFG; + break; default: //unknown class: reset state machine ubx_decode_init(); break; @@ -193,9 +194,36 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) ubx_decode_init(); break; } + break; + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } break; + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } + break; default: //should not happen ubx_decode_init(); break; @@ -542,6 +570,75 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer) break; } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer; + + //Check if checksum is valid + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + ubx_config_state++; + break; + case UBX_CONFIG_STATE_NAV5: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) + ubx_config_state++; + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + case UBX_CONFIG_STATE_MSG_NAV_DOP: + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + case UBX_CONFIG_STATE_MSG_NAV_SOL: + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + ubx_config_state++; + break; + default: + break; + } + + + ret = 1; + + } else { + if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer; + + //Check if checksum is valid + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n"); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + default: //something went wrong ubx_decode_init(); @@ -574,53 +671,105 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length) message[length - 2] = ck_a; message[length - 1] = ck_b; - -// printf("[%x,%x]", ck_a, ck_b); - -// printf("[%x,%x]\n", message[length-2], message[length-1]); } int configure_gps_ubx(int *fd) { - //fflush(((FILE *)fd)); - - //TODO: write this in a loop once it is tested - //UBX_CFG_PRT_PART: - write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd); - - usleep(100000); - - //NAV_POSLLH: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_TIMEUTC: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_DOP: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_SOL: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd); - usleep(100000); - - - //NAV_SVINFO: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd); - usleep(100000); - - //NAV_VELNED: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd); - usleep(100000); - - - //RXM_SVSI: - write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd); - usleep(100000); - - return 0; + // only needed once like this + const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_PRT, + .length = UBX_CFG_PRT_LENGTH, + .portID = UBX_CFG_PRT_PAYLOAD_PORTID, + .mode = UBX_CFG_PRT_PAYLOAD_MODE, + .baudRate = current_gps_speed, + .inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK, + .outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK, + .ck_a = 0, + .ck_b = 0 + }; + + // only needed once like this + const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_NAV5, + .length = UBX_CFG_NAV5_LENGTH, + .mask = UBX_CFG_NAV5_PAYLOAD_MASK, + .dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL, + .fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE, + .ck_a = 0, + .ck_b = 0 + }; + + // this message is reusable for different configuration commands, so not const + type_gps_bin_cfg_msg_packet cfg_msg_packet = { + .clsID = UBX_CLASS_CFG, + .msgID = UBX_MESSAGE_CFG_MSG, + .length = UBX_CFG_MSG_LENGTH, + .rate = UBX_CFG_MSG_PAYLOAD_RATE + }; + + uint64_t time_before_config = hrt_absolute_time(); + + while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) { + +// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state); + + switch (ubx_config_state) { + case UBX_CONFIG_STATE_PRT: +// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate); + + write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd); + break; + case UBX_CONFIG_STATE_NAV5: + write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_MSG_RXM_SVSI: + cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd); + break; + case UBX_CONFIG_STATE_CONFIGURED: + if (gps_verbose) printf("[gps] ubx configuration finished\n"); + return OK; + break; + default: + break; + } + usleep(10000); + } + if (gps_verbose) printf("[gps] ubx configuration timeout\n"); + return ERROR; } @@ -637,22 +786,17 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) fds.events = POLLIN; // UBX GPS mode - // This blocks the task until there is something on the buffer while (1) { //check if the thread should terminate if (terminate_gps_thread == true) { -// printf("terminate_gps_thread=%u ", terminate_gps_thread); -// printf("exiting mtk thread\n"); -// fflush(stdout); ret = 1; break; } - if (poll(&fds, 1, 1000) > 0) { if (read(*fd, &c, 1) > 0) { - // printf("Read %x\n",c); +// printf("Read %x\n",c); if (rx_count >= buffer_size) { // The buffer is already full and we haven't found a valid ubx sentence. // Flush the buffer and note the overflow event. @@ -671,7 +815,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) int msg_read = ubx_parse(c, gps_rx_buffer); if (msg_read > 0) { - // printf("Found sequence\n"); + //printf("Found sequence\n"); break; } @@ -688,28 +832,41 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size) return ret; } -int write_config_message_ubx(uint8_t *message, size_t length, int fd) +int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd) { - //calculate and write checksum to the end uint8_t ck_a = 0; uint8_t ck_b = 0; unsigned int i; - for (i = 2; i < length; i++) { + uint8_t buffer[2]; + ssize_t result_write = 0; + + //calculate and write checksum to the end + for (i = 0; i < length-2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } -// printf("[%x,%x]\n", ck_a, ck_b); + // write sync bytes first + buffer[0] = UBX_SYNC_1; + buffer[1] = UBX_SYNC_2; + + // write config message without the checksum + result_write = write(*fd, buffer, sizeof(buffer)); + result_write += write(*fd, message, length-2); + + buffer[0] = ck_a; + buffer[1] = ck_b; - unsigned int result_write = write(fd, message, length); - result_write += write(fd, &ck_a, 1); - result_write += write(fd, &ck_b, 1); - fsync(fd); + // write the checksum + result_write += write(*fd, buffer, sizeof(buffer)); - return (result_write != length + 2); //return 0 as success + fsync(*fd); + if ((unsigned int)result_write != length + 2) + return ERROR; + return OK; } void *ubx_watchdog_loop(void *args) @@ -723,6 +880,9 @@ void *ubx_watchdog_loop(void *args) int *fd = arguments->fd_ptr; bool *thread_should_exit = arguments->thread_should_exit_ptr; + /* first try to configure the GPS anyway */ + configure_gps_ubx(fd); + /* GPS watchdog error message skip counter */ bool ubx_healthy = false; @@ -732,7 +892,7 @@ void *ubx_watchdog_loop(void *args) bool once_ok = false; int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - + ubx_config_state = UBX_CONFIG_STATE_PRT; //int err_skip_counter = 0; while (!(*thread_should_exit)) { @@ -833,23 +993,6 @@ void *ubx_loop(void *args) /* set parameters for ubx */ - if (configure_gps_ubx(fd) != 0) { - printf("[gps] Configuration of gps module to ubx failed\n"); - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present); - - } else { - if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n"); - - // XXX Shouldn't the system status only change if the module is known to work ok? - - /* Write shared variable sys_status */ - // TODO enable this again - //global_data_send_subsystem_info(&ubx_present_enabled); - } - struct vehicle_gps_position_s ubx_gps_d = {.counter = 0}; ubx_gps = &ubx_gps_d; diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h index f4f01df9a..e700fe388 100644 --- a/apps/gps/ubx.h +++ b/apps/gps/ubx.h @@ -49,15 +49,17 @@ //internal definitions (not depending on the ubx protocol -#define CONFIGURE_UBX_FINISHED 0 -#define CONFIGURE_UBX_MESSAGE_ACKNOWLEDGED 1 -#define CONFIGURE_UBX_MESSAGE_NOT_ACKNOWLEDGED 2 #define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */ -#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 500000 /**< Check for current state every 0.4 seconds */ +#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 1000000 /**< Check for current state every second */ + +#define UBX_CONFIG_TIMEOUT 500000 #define APPNAME "gps: ubx" +#define UBX_SYNC_1 0xB5 +#define UBX_SYNC_2 0x62 + //UBX Protocoll definitions (this is the subset of the messages that are parsed) #define UBX_CLASS_NAV 0x01 #define UBX_CLASS_RXM 0x02 @@ -72,6 +74,24 @@ #define UBX_MESSAGE_RXM_SVSI 0x20 #define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_PRT 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_CFG_MSG 0x01 + +#define UBX_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ + +#define UBX_CFG_NAV5_LENGTH 36 +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ + +#define UBX_CFG_MSG_LENGTH 8 +#define UBX_CFG_MSG_PAYLOAD_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */ // ************ @@ -216,7 +236,7 @@ typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t; typedef struct { uint8_t clsID; - uint8_t msgId; + uint8_t msgID; uint8_t ck_a; uint8_t ck_b; @@ -226,7 +246,7 @@ typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t; typedef struct { uint8_t clsID; - uint8_t msgId; + uint8_t msgID; uint8_t ck_a; uint8_t ck_b; @@ -234,15 +254,91 @@ typedef struct { typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t; +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t portID; + uint8_t res0; + uint16_t res1; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t pad; + + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_prt_packet; + +typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint32_t reserved2; + uint32_t reserved3; + uint32_t reserved4; + + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_nav5_packet; + +typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t msgClass_payload; + uint8_t msgID_payload; + uint8_t rate[6]; + + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_msg_packet; + +typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t; + // END the structures of the binary packets // ************ +enum UBX_CONFIG_STATE { + UBX_CONFIG_STATE_NONE = 0, + UBX_CONFIG_STATE_PRT = 1, + UBX_CONFIG_STATE_NAV5 = 2, + UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4, + UBX_CONFIG_STATE_MSG_NAV_DOP = 5, + UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6, + UBX_CONFIG_STATE_MSG_NAV_SOL = 7, + UBX_CONFIG_STATE_MSG_NAV_VELNED = 8, + UBX_CONFIG_STATE_MSG_RXM_SVSI = 9, + UBX_CONFIG_STATE_CONFIGURED = 10 +}; + enum UBX_MESSAGE_CLASSES { CLASS_UNKNOWN = 0, NAV = 1, RXM = 2, - ACK = 3 + ACK = 3, + CFG = 4 }; enum UBX_MESSAGE_IDS { @@ -254,7 +350,10 @@ enum UBX_MESSAGE_IDS { NAV_DOP = 4, NAV_SVINFO = 5, NAV_VELNED = 6, - RXM_SVSI = 7 + RXM_SVSI = 7, + CFG_NAV5 = 8, + ACK_ACK = 9, + ACK_NAK = 10 }; enum UBX_DECODE_STATES { @@ -304,7 +403,7 @@ int configure_gps_ubx(int *fd); int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size); -int write_config_message_ubx(uint8_t *message, size_t length, int fd); +int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd); void calculate_ubx_checksum(uint8_t *message, uint8_t length); diff --git a/apps/interpreters/Kconfig b/apps/interpreters/Kconfig index 34cbf2eee..6e7d1ac4f 100644 --- a/apps/interpreters/Kconfig +++ b/apps/interpreters/Kconfig @@ -7,7 +7,7 @@ comment "Interpreters" source "$APPSDIR/interpreters/ficl/Kconfig" -config PCODE +config INTERPRETERS_PCODE bool "Pascal p-code interpreter" default n ---help--- @@ -16,6 +16,6 @@ config PCODE configuration implies that you have performed the required installation of the Pascal run-time code. -if PCODE +if INTERPRETERS_PCODE endif diff --git a/apps/interpreters/Make.defs b/apps/interpreters/Make.defs index 2fc4b26d4..5d808d5d6 100644 --- a/apps/interpreters/Make.defs +++ b/apps/interpreters/Make.defs @@ -34,10 +34,10 @@ # ############################################################################ -ifeq ($(CONFIG_PCODE),y) +ifeq ($(CONFIG_INTERPRETERS_PCODE),y) CONFIGURED_APPS += interpreters/pcode endif -ifeq ($(CONFIG_FICL),y) +ifeq ($(CONFIG_INTERPRETERS_FICL),y) CONFIGURED_APPS += interpreters/ficl endif 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/Kconfig b/apps/interpreters/ficl/Kconfig index 1860a1591..ba6a7bc35 100644 --- a/apps/interpreters/ficl/Kconfig +++ b/apps/interpreters/ficl/Kconfig @@ -3,7 +3,7 @@ # see misc/tools/kconfig-language.txt. # -config FICL +config INTERPRETERS_FICL bool "Ficl Forth interpreter" default n ---help--- @@ -11,6 +11,6 @@ config FICL apps/interpreters/ficl directory. Use of this configuration assumes that you have performed the required installation of the Ficl run-time code. -if FICL +if INTERPRETERS_FICL endif 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/mathlib/CMSIS/Include/arm_math.h b/apps/mathlib/CMSIS/Include/arm_math.h index fabd98be7..f224d3eb0 100644 --- a/apps/mathlib/CMSIS/Include/arm_math.h +++ b/apps/mathlib/CMSIS/Include/arm_math.h @@ -5520,7 +5520,7 @@ extern "C" *pIa = Ialpha;
/* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
- *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
+ *pIb = -0.5f * Ialpha + (float32_t) 0.8660254039f *Ibeta;
}
@@ -5898,7 +5898,7 @@ extern "C" /* Iniatilize output for below specified range as least output value of table */
y = pYData[0];
}
- else if(i >= S->nValues)
+ else if((unsigned)i >= S->nValues)
{
/* Iniatilize output for above specified range as last output value of table */
y = pYData[S->nValues - 1];
diff --git a/apps/mathlib/CMSIS/Makefile b/apps/mathlib/CMSIS/Makefile index fa5de668a..9e28518bc 100644 --- a/apps/mathlib/CMSIS/Makefile +++ b/apps/mathlib/CMSIS/Makefile @@ -45,6 +45,13 @@ INCLUDES += $(DSPLIB_SRCDIR)/Include \ $(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \ $(DSPLIB_SRCDIR)/Device/ARM/ARMCM3/Include +# Suppress some warnings that ARM should fix, but haven't. +EXTRADEFINES += -Wno-unsuffixed-float-constants \ + -Wno-sign-compare \ + -Wno-shadow \ + -Wno-float-equal \ + -Wno-unused-variable + # # Override the default visibility for symbols, since the CMSIS DSPLib doesn't # have anything we can use to mark exported symbols. diff --git a/apps/mathlib/Makefile b/apps/mathlib/Makefile index e5fab1e35..7eebd6ae0 100644 --- a/apps/mathlib/Makefile +++ b/apps/mathlib/Makefile @@ -31,8 +31,6 @@ # ############################################################################ -include $(TOPDIR)/.config - # # Math library # @@ -44,27 +42,21 @@ CXXSRCS = math/test/test.cpp \ math/Dcm.cpp \ math/Matrix.cpp -CXXHDRS = math/test/test.hpp \ - math/Vector.hpp \ - math/Vector3.hpp \ - math/EulerAngles.hpp \ - math/Quaternion.hpp \ - math/Dcm.hpp \ - math/Matrix.hpp +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config -# XXX this really should be a CONFIG_* test ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) INCLUDES += math/arm CXXSRCS += math/arm/Vector.cpp \ math/arm/Matrix.cpp -CXXHDRS += math/arm/Vector.hpp \ - math/arm/Matrix.hpp else INCLUDES += math/generic CXXSRCS += math/generic/Vector.cpp \ math/generic/Matrix.cpp -CXXHDRS += math/generic/Vector.hpp \ - math/generic/Matrix.hpp endif include $(APPDIR)/mk/app.mk diff --git a/apps/mathlib/math/Dcm.cpp b/apps/mathlib/math/Dcm.cpp index 59f3d88ab..df0f09b20 100644 --- a/apps/mathlib/math/Dcm.cpp +++ b/apps/mathlib/math/Dcm.cpp @@ -52,6 +52,23 @@ Dcm::Dcm() : { } +Dcm::Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + dcm(0, 0) = c00; + dcm(0, 1) = c01; + dcm(0, 2) = c02; + dcm(1, 0) = c10; + dcm(1, 1) = c11; + dcm(1, 2) = c12; + dcm(2, 0) = c20; + dcm(2, 1) = c21; + dcm(2, 2) = c22; +} + Dcm::Dcm(const float *data) : Matrix(3, 3, data) { @@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) : Matrix(3, 3) { Dcm &dcm = *this; - float a = q.getA(); - float b = q.getB(); - float c = q.getC(); - float d = q.getD(); - float aSq = a * a; - float bSq = b * b; - float cSq = c * c; - float dSq = d * d; + double a = q.getA(); + double b = q.getB(); + double c = q.getC(); + double d = q.getD(); + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2 * (b * c - a * d); - dcm(0, 2) = 2 * (a * c + b * d); - dcm(1, 0) = 2 * (b * c + a * d); + dcm(0, 1) = 2.0 * (b * c - a * d); + dcm(0, 2) = 2.0 * (a * c + b * d); + dcm(1, 0) = 2.0 * (b * c + a * d); dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2 * (c * d - a * b); - dcm(2, 0) = 2 * (b * d - a * c); - dcm(2, 1) = 2 * (a * b + c * d); + dcm(1, 2) = 2.0 * (c * d - a * b); + dcm(2, 0) = 2.0 * (b * d - a * c); + dcm(2, 1) = 2.0 * (a * b + c * d); dcm(2, 2) = aSq - bSq - cSq + dSq; } @@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) : Matrix(3, 3) { Dcm &dcm = *this; - float cosPhi = cosf(euler.getPhi()); - float sinPhi = sinf(euler.getPhi()); - float cosThe = cosf(euler.getTheta()); - float sinThe = sinf(euler.getTheta()); - float cosPsi = cosf(euler.getPsi()); - float sinPsi = sinf(euler.getPsi()); + double cosPhi = cos(euler.getPhi()); + double sinPhi = sin(euler.getPhi()); + double cosThe = cos(euler.getTheta()); + double sinThe = sin(euler.getTheta()); + double cosPsi = cos(euler.getPsi()); + double sinPsi = sin(euler.getPsi()); dcm(0, 0) = cosThe * cosPsi; dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; @@ -116,18 +133,30 @@ Dcm::~Dcm() int __EXPORT dcmTest() { printf("Test DCM\t\t: "); - Vector3 vB(1, 2, 3); - ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)), + // default ctor + ASSERT(matrixEqual(Dcm(), Matrix::identity(3))); - ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)), - Matrix::identity(3))); - ASSERT(vectorEqual(Vector3(-2, 1, 3), - Dcm(EulerAngles(0, 0, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3, 2, -1), - Dcm(EulerAngles(0, M_PI_2_F, 0))*vB)); - ASSERT(vectorEqual(Vector3(1, -3, 2), - Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB)); - ASSERT(vectorEqual(Vector3(3, 2, -1), + // quaternion ctor + ASSERT(matrixEqual( + Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // euler angle ctor + ASSERT(matrixEqual( + Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // rotations + Vector3 vB(1, 2, 3); + ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), + Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), + Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), Dcm(EulerAngles( M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); printf("PASS\n"); diff --git a/apps/mathlib/math/Dcm.hpp b/apps/mathlib/math/Dcm.hpp index de69a7aa4..28d840b10 100644 --- a/apps/mathlib/math/Dcm.hpp +++ b/apps/mathlib/math/Dcm.hpp @@ -65,6 +65,13 @@ public: Dcm(); /** + * scalar ctor + */ + Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22); + + /** * data ctor */ Dcm(const float *data); diff --git a/apps/mathlib/math/EulerAngles.cpp b/apps/mathlib/math/EulerAngles.cpp index 8991d5623..2e96fef4c 100644 --- a/apps/mathlib/math/EulerAngles.cpp +++ b/apps/mathlib/math/EulerAngles.cpp @@ -97,23 +97,27 @@ EulerAngles::~EulerAngles() int __EXPORT eulerAnglesTest() { printf("Test EulerAngles\t: "); - EulerAngles euler(1, 2, 3); + EulerAngles euler(0.1f, 0.2f, 0.3f); // test ctor - ASSERT(vectorEqual(Vector3(1, 2, 3), euler)); - ASSERT(equal(euler.getPhi(), 1)); - ASSERT(equal(euler.getTheta(), 2)); - ASSERT(equal(euler.getPsi(), 3)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + ASSERT(equal(euler.getPhi(), 0.1f)); + ASSERT(equal(euler.getTheta(), 0.2f)); + ASSERT(equal(euler.getPsi(), 0.3f)); // test dcm ctor + euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test quat ctor + euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); // test assignment - euler.setPhi(4); - ASSERT(equal(euler.getPhi(), 4)); - euler.setTheta(5); - ASSERT(equal(euler.getTheta(), 5)); - euler.setPsi(6); - ASSERT(equal(euler.getPsi(), 6)); + euler.setPhi(0.4f); + euler.setTheta(0.5f); + euler.setPsi(0.6f); + ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); printf("PASS\n"); return 0; diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp index 68fe85300..78481b286 100644 --- a/apps/mathlib/math/Quaternion.cpp +++ b/apps/mathlib/math/Quaternion.cpp @@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) : Quaternion::Quaternion(const Dcm &dcm) : Vector(4) { - setA(0.5f * sqrtf(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); - setB((dcm(2, 1) - dcm(1, 2)) / - (4 * getA())); - setC((dcm(0, 2) - dcm(2, 0)) / - (4 * getA())); - setD((dcm(1, 0) - dcm(0, 1)) / - (4 * getA())); + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); } Quaternion::Quaternion(const EulerAngles &euler) : Vector(4) { - float cosPhi_2 = cosf(euler.getPhi() / 2.0f); - float cosTheta_2 = cosf(euler.getTheta() / 2.0f); - float cosPsi_2 = cosf(euler.getPsi() / 2.0f); - float sinPhi_2 = sinf(euler.getPhi() / 2.0f); - float sinTheta_2 = sinf(euler.getTheta() / 2.0f); - float sinPsi_2 = sinf(euler.getPsi() / 2.0f); + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 + + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } @@ -142,38 +144,29 @@ int __EXPORT quaternionTest() printf("Test Quaternion\t\t: "); // test default ctor Quaternion q; - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + ASSERT(equal(q.getA(), 1.0f)); + ASSERT(equal(q.getB(), 0.0f)); + ASSERT(equal(q.getC(), 0.0f)); + ASSERT(equal(q.getD(), 0.0f)); // test float ctor - q = Quaternion(0, 1, 0, 0); - ASSERT(equal(q.getA(), 0)); - ASSERT(equal(q.getB(), 1)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); + ASSERT(equal(q.getA(), 0.1825742f)); + ASSERT(equal(q.getB(), 0.3651484f)); + ASSERT(equal(q.getC(), 0.5477226f)); + ASSERT(equal(q.getD(), 0.7302967f)); // test euler ctor - q = Quaternion(EulerAngles(0, 0, 0)); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); // test dcm ctor q = Quaternion(Dcm()); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); // TODO test derivative // test accessors - q.setA(0.1); - q.setB(0.2); - q.setC(0.3); - q.setD(0.4); - ASSERT(equal(q.getA(), 0.1)); - ASSERT(equal(q.getB(), 0.2)); - ASSERT(equal(q.getC(), 0.3)); - ASSERT(equal(q.getD(), 0.4)); + q.setA(0.1f); + q.setB(0.2f); + q.setC(0.3f); + q.setD(0.4f); + ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); printf("PASS\n"); return 0; } diff --git a/apps/mathlib/math/nasa_rotation_def.pdf b/apps/mathlib/math/nasa_rotation_def.pdf Binary files differnew file mode 100644 index 000000000..eb67a4bfc --- /dev/null +++ b/apps/mathlib/math/nasa_rotation_def.pdf diff --git a/apps/mathlib/math/test_math.sce b/apps/mathlib/math/test_math.sce new file mode 100644 index 000000000..c3fba4729 --- /dev/null +++ b/apps/mathlib/math/test_math.sce @@ -0,0 +1,63 @@ +clc +clear +function out = float_truncate(in, digits) + out = round(in*10^digits) + out = out/10^digits +endfunction + +phi = 0.1 +theta = 0.2 +psi = 0.3 + +cosPhi = cos(phi) +cosPhi_2 = cos(phi/2) +sinPhi = sin(phi) +sinPhi_2 = sin(phi/2) + +cosTheta = cos(theta) +cosTheta_2 = cos(theta/2) +sinTheta = sin(theta) +sinTheta_2 = sin(theta/2) + +cosPsi = cos(psi) +cosPsi_2 = cos(psi/2) +sinPsi = sin(psi) +sinPsi_2 = sin(psi/2) + +C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; + cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; + -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] + +disp(C_nb) +//C_nb = float_truncate(C_nb,3) +//disp(C_nb) + +theta = asin(-C_nb(3,1)) +phi = atan(C_nb(3,2), C_nb(3,3)) +psi = atan(C_nb(2,1), C_nb(1,1)) +printf('phi %f\n', phi) +printf('theta %f\n', theta) +printf('psi %f\n', psi) + +q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; + sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; + cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; + cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] + +//q = float_truncate(q,3) + +a = q(1) +b = q(2) +c = q(3) +d = q(4) +printf('q: %f %f %f %f\n', a, b, c, d) +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); + 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); + 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] + +disp(C2_nb) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 2ec459ddc..ceb7c2554 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -77,7 +77,7 @@ /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); -PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR); +PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); @@ -98,7 +98,7 @@ static bool mavlink_link_termination_allowed = false; mavlink_system_t mavlink_system = { 100, 50, - MAV_TYPE_QUADROTOR, + MAV_TYPE_FIXED_WING, 0, 0, 0 @@ -148,6 +148,10 @@ set_hil_on_off(bool hil_enabled) pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + /* sensore level hil */ + pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h index 1eb8c32e9..8c7a5b514 100644 --- a/apps/mavlink/mavlink_hil.h +++ b/apps/mavlink/mavlink_hil.h @@ -43,8 +43,12 @@ extern bool mavlink_hil_enabled; extern struct vehicle_global_position_s hil_global_pos; extern struct vehicle_attitude_s hil_attitude; +extern struct sensor_combined_s hil_sensors; +extern struct vehicle_gps_position_s hil_gps; extern orb_advert_t pub_hil_global_pos; extern orb_advert_t pub_hil_attitude; +extern orb_advert_t pub_hil_sensors; +extern orb_advert_t pub_hil_gps; /** * Enable / disable Hardware in the Loop simulation mode. @@ -54,4 +58,4 @@ extern orb_advert_t pub_hil_attitude; * requested change could not be made or was * redundant. */ -extern int set_hil_on_off(bool hil_enabled);
\ No newline at end of file +extern int set_hil_on_off(bool hil_enabled); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 79452f515..9491ce7e3 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -86,8 +86,12 @@ static struct offboard_control_setpoint_s offboard_control_sp; struct vehicle_global_position_s hil_global_pos; struct vehicle_attitude_s hil_attitude; +struct vehicle_gps_position_s hil_gps; +struct sensor_combined_s hil_sensors; orb_advert_t pub_hil_global_pos = -1; orb_advert_t pub_hil_attitude = -1; +orb_advert_t pub_hil_gps = -1; +orb_advert_t pub_hil_sensors = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; @@ -302,6 +306,166 @@ handle_message(mavlink_message_t *msg) if (mavlink_hil_enabled) { + uint64_t timestamp = hrt_absolute_time(); + + if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { + + mavlink_raw_imu_t imu; + mavlink_msg_raw_imu_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = imu.time_usec; + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro; + hil_sensors.gyro_raw[1] = imu.ygyro; + hil_sensors.gyro_raw[2] = imu.zgyro; + hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; + hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; + hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc; + hil_sensors.accelerometer_raw[1] = imu.yacc; + hil_sensors.accelerometer_raw[2] = imu.zacc; + hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; + hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; + hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag; + hil_sensors.magnetometer_raw[1] = imu.ymag; + hil_sensors.magnetometer_raw[2] = imu.zmag; + hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; + hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; + hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { + + mavlink_gps_raw_int_t gps; + mavlink_msg_gps_raw_int_decode(msg, &gps); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* gps */ + hil_gps.timestamp = gps.time_usec; + hil_gps.counter = hil_counter++; + hil_gps.time_gps_usec = gps.time_usec; + hil_gps.lat = gps.lat; + hil_gps.lon = gps.lon; + hil_gps.alt = gps.alt; + hil_gps.counter_pos_valid = hil_counter++; + hil_gps.eph = gps.eph; + hil_gps.epv = gps.epv; + hil_gps.s_variance = 100; + hil_gps.p_variance = 100; + hil_gps.vel = gps.vel; + hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); + hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); + hil_gps.vel_d = 0.0f; + hil_gps.cog = gps.cog; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; + + /* publish */ + orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil gps at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { + + mavlink_raw_pressure_t press; + mavlink_msg_raw_pressure_decode(msg, &press); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = press.time_usec; + + /* baro */ + /* TODO, set ground_press/ temp during calib */ + static const float ground_press = 1013.25f; // mbar + static const float ground_tempC = 21.0f; + static const float ground_alt = 0.0f; + static const float T0 = 273.15; + static const float R = 287.05f; + static const float g = 9.806f; + + float tempC = press.temperature / 100.0f; + float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; + float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); + hil_sensors.baro_counter = hil_counter; + hil_sensors.baro_pres_mbar = press.press_abs; + hil_sensors.baro_alt_meter = h; + hil_sensors.baro_temp_celcius = tempC; + + /* publish */ + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + + // increment counters + hil_counter += 1 ; + hil_frames += 1 ; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil pressure at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { mavlink_hil_state_t hil_state; @@ -412,7 +576,7 @@ receive_thread(void *arg) int uart_fd = *((int *)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; @@ -423,13 +587,12 @@ receive_thread(void *arg) struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + /* non-blocking read */ + size_t nread = read(uart_fd, buf, sizeof(buf)); + ASSERT(nread > 0) - do { - nread = read(uart_fd, &ch, 1); - - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + for (size_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { //parse the char /* handle generic messages and commands */ handle_message(&msg); @@ -439,7 +602,7 @@ receive_thread(void *arg) /* Handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } - } while (nread > 0); + } } } @@ -452,6 +615,10 @@ receive_start(int uart) pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); + // set to non-blocking read + int flags = fcntl(uart, F_GETFL, 0); + fcntl(uart, F_SETFL, flags | O_NONBLOCK); + struct sched_param param; param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index f5253ea35..ec5149745 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -114,6 +114,7 @@ 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); +static void l_home(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -137,6 +138,7 @@ struct listener listeners[] = { {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}, + {l_home, &mavlink_subs.home_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -621,6 +623,16 @@ l_optical_flow(struct listener *l) flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } +void +l_home(struct listener *l) +{ + struct home_position_s home; + + orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); +} + static void * uorb_receive_thread(void *arg) { @@ -688,6 +700,10 @@ uorb_receive_start(void) mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + /* --- HOME POSITION --- */ + mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + /* --- SYSTEM STATE --- */ status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ @@ -702,7 +718,7 @@ uorb_receive_start(void) /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ /* --- LOCAL POS VALUE --- */ mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index 4650c4991..d61cd43dc 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -45,6 +45,7 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> @@ -81,6 +82,7 @@ struct mavlink_subscriptions { int input_rc_sub; int optical_flow; int rates_setpoint_sub; + int home_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 3832ebe70..a131b143b 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -408,9 +408,10 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ cur_wp->current = 0; if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - wpm->current_active_wp_id = 1; + /* the last waypoint was reached, if auto continue is + * activated restart the waypoint list from the beginning + */ + wpm->current_active_wp_id = 0; } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) diff --git a/apps/mk/app.mk b/apps/mk/app.mk index 1d8b0e44c..fa4a12cab 100644 --- a/apps/mk/app.mk +++ b/apps/mk/app.mk @@ -81,7 +81,9 @@ # Work out who included us so we can report decent errors # THIS_MAKEFILE := $(lastword $(MAKEFILE_LIST)) -PARENT_MAKEFILE := $(lastword $(filter-out $(THIS_MAKEFILE),$(MAKEFILE_LIST))) +ifeq ($(APP_MAKEFILE),) +APP_MAKEFILE := $(lastword $(filter-out $(THIS_MAKEFILE),$(MAKEFILE_LIST))) +endif ############################################################################ # Get configuration @@ -93,7 +95,7 @@ include $(APPDIR)/Make.defs ############################################################################ # Sanity-check the information we've been given and set any defaults # -SRCDIR ?= $(dir $(PARENT_MAKEFILE)) +SRCDIR ?= $(dir $(APP_MAKEFILE)) PRIORITY ?= SCHED_PRIORITY_DEFAULT STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT @@ -112,14 +114,14 @@ endif # there has to be a source file ifeq ($(ASRCS)$(CSRCS)$(CXXSRCS),) -$(error $(realpath $(PARENT_MAKEFILE)): at least one of ASRCS, CSRCS or CXXSRCS must be set) +$(error $(realpath $(APP_MAKEFILE)): at least one of ASRCS, CSRCS or CXXSRCS must be set) endif # check that C++ is configured if we have C++ source files and we are building ifneq ($(CXXSRCS),) ifneq ($(CONFIG_HAVE_CXX),y) ifeq ($(MAKECMDGOALS),build) -$(error $(realpath $(PARENT_MAKEFILE)): cannot set CXXSRCS if CONFIG_HAVE_CXX not set in configuration) +$(error $(realpath $(APP_MAKEFILE)): cannot set CXXSRCS if CONFIG_HAVE_CXX not set in configuration) endif endif endif @@ -153,6 +155,11 @@ COBJS = $(patsubst %.c,%.o,$(CSRCS)) CXXOBJS = $(patsubst %.cpp,%.o,$(CXXSRCS)) OBJS = $(AOBJS) $(COBJS) $(CXXOBJS) +# Automatic depdendency generation +DEPS = $(OBJS:$(OBJEXT)=.d) +CFLAGS += -MD +CXXFLAGS += -MD + # The prelinked object that we are ultimately going to build ifneq ($(APPNAME),) PRELINKOBJ = $(APPNAME).pre.o @@ -160,7 +167,7 @@ else PRELINKOBJ = $(LIBNAME).pre.o endif -# The archive that the object file will be placed in +# The archive the prelinked object will be linked into # XXX does WINTOOL ever get set? ifeq ($(WINTOOL),y) INCDIROPT = -w @@ -186,11 +193,8 @@ all: .built # # Source dependencies # -depend: .depend -.depend: $(MAKEFILE_LIST) $(SRCS) - @$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) $(CHDRS) >Make.dep - @$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) $(CXXHDRS) >>Make.dep - @touch $@ +depend: + @exit 0 ifneq ($(APPNAME),) # @@ -202,6 +206,7 @@ context: .context @touch $@ else context: + @exit 0 endif # @@ -210,23 +215,23 @@ endif $(PRELINKOBJ): $(OBJS) $(call PRELINK, $@, $(OBJS)) -$(AOBJS): %.o : %.S +$(AOBJS): %.o : %.S $(MAKEFILE_LIST) $(call ASSEMBLE, $<, $@) -$(COBJS): %.o : %.c +$(COBJS): %.o : %.c $(MAKEFILE_LIST) $(call COMPILE, $<, $@) -$(CXXOBJS): %.o : %.cpp +$(CXXOBJS): %.o : %.cpp $(MAKEFILE_LIST) $(call COMPILEXX, $<, $@) # # Tidying up # clean: - @rm -f $(OBJS) $(PRELINKOBJ) Make.dep .built + @rm -f $(OBJS) $(DEPS) $(PRELINKOBJ) .built $(call CLEAN) distclean: clean @rm -f Make.dep .depend --include Make.dep +-include $(DEPS) 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..bc626e699 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-address>|dhcp]] [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_apps.c b/apps/nshlib/nsh_apps.c index e335c2e2c..7dbaf9ba8 100644 --- a/apps/nshlib/nsh_apps.c +++ b/apps/nshlib/nsh_apps.c @@ -122,13 +122,28 @@ int nsh_execapp(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, ret = exec_namedapp(cmd, (FAR const char **)argv); if (ret >= 0) { - /* The application was successfully started (but still blocked because the - * scheduler is locked). If the application was not backgrounded, then we - * need to wait here for the application to exit. + /* The application was successfully started (but still blocked because + * the scheduler is locked). If the application was not backgrounded, + * then we need to wait here for the application to exit. These really + * only works works with the following options: + * + * - CONFIG_NSH_DISABLEBG - Do not run commands in background + * - CONFIG_SCHED_WAITPID - Required to run external commands in + * foreground + * + * These concepts do not apply cleanly to the external applications. */ #ifdef CONFIG_SCHED_WAITPID + + /* CONFIG_SCHED_WAITPID is selected, so we may run the command in + * foreground unless we were specifically requested to run the command + * in background (and running commands in background is enabled). + */ + +# ifndef CONFIG_NSH_DISABLEBG if (vtbl->np.np_bg == false) +# endif /* CONFIG_NSH_DISABLEBG */ { int rc = 0; @@ -155,8 +170,25 @@ int nsh_execapp(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, */ } } +# ifndef CONFIG_NSH_DISABLEBG else -#endif +# endif /* CONFIG_NSH_DISABLEBG */ +#endif /* CONFIG_SCHED_WAITPID */ + + /* We get here if either: + * + * - CONFIG_SCHED_WAITPID is not selected meaning that all commands + * have to be run in background, or + * - CONFIG_SCHED_WAITPID and CONFIG_NSH_DISABLEBG are both selected, but the + * user requested to run the command in background. + * + * NOTE that the case of a) CONFIG_SCHED_WAITPID is not selected and + * b) CONFIG_NSH_DISABLEBG selected cannot be supported. In that event, all + * commands will have to run in background. The waitpid() API must be + * available to support running the command in foreground. + */ + +#if !defined(CONFIG_SCHED_WAITPID) || !defined(CONFIG_NSH_DISABLEBG) { struct sched_param param; sched_getparam(0, ¶m); @@ -168,6 +200,7 @@ int nsh_execapp(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, ret = OK; } +#endif /* !CONFIG_SCHED_WAITPID || !CONFIG_NSH_DISABLEBG */ } sched_unlock(); diff --git a/apps/nshlib/nsh_consolemain.c b/apps/nshlib/nsh_consolemain.c index 6b51be470..f05447a64 100644 --- a/apps/nshlib/nsh_consolemain.c +++ b/apps/nshlib/nsh_consolemain.c @@ -160,11 +160,11 @@ int nsh_consolemain(int argc, char *argv[]) } } - /* Clean up */ + /* Clean up. We do not get here, but this is necessary to keep some + * compilers happy. But others will complain that this code is not + * reachable. + */ nsh_exit(&pstate->cn_vtbl, 0); - - /* We do not get here, but this is necessary to keep some compilers happy */ - return OK; } diff --git a/apps/nshlib/nsh_dbgcmds.c b/apps/nshlib/nsh_dbgcmds.c index 384b377f3..85a4ccb9c 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" @@ -99,7 +103,7 @@ int mem_parse(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv, pcvalue++; lvalue = (unsigned long)strtol(pcvalue, NULL, 16); - if (lvalue > 0xffffffff) + if (lvalue > 0xffffffffL) { return -EINVAL; } @@ -127,6 +131,7 @@ int mem_parse(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv, { mem->dm_count = 1; } + return OK; } @@ -327,7 +332,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 +358,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..371d30460 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,15 @@ # endif #endif +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_DNS) +# ifdef CONFIG_HAVE_GETHOSTBYNAME +# include <netdb.h> +# else +# include <apps/netutils/resolv.h> +# endif +# include <apps/netutils/dhcpc.h> +#endif + #include "nsh.h" #include "nsh_console.h" @@ -87,8 +97,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 +280,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 +507,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 +562,20 @@ 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[IFHWADDRLEN]; +#if defined(CONFIG_NSH_DHCPC) + FAR void *handle; +#endif /* With one or no arguments, ifconfig simply shows the status of ethernet * device: @@ -498,24 +597,201 @@ int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) * ifconfig nic_name ip_address */ - /* Set host 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); + } + +#if defined(CONFIG_NSH_DHCPC) + if (!strcmp(hostip, "dhcp")) + { + /* Set DHCP addr */ + + ndbg("DHCPC Mode\n"); + gip = addr.s_addr = 0; + } + else +#endif + { + /* Set host IP address */ + + ndbg("Host IP: %s\n", hostip); + gip = addr.s_addr = inet_addr(hostip); + } - ip = addr.s_addr = inet_addr(argv[2]); - uip_sethostaddr(argv[1], &addr); + 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 + { + if (gip) + { + ndbg("Gateway: default\n"); + gip = NTOHL(gip); + gip &= ~0x000000ff; + gip |= 0x00000001; + gip = HTONL(gip); + } + + addr.s_addr = gip; + } + + uip_setdraddr(intf, &addr); + + /* Set network mask */ + + 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"); + } + + uip_setnetmask(intf, &addr); - addr.s_addr = HTONL(ip); - uip_setdraddr(argv[1], &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 + +#if defined(CONFIG_NSH_DHCPC) + /* Get the MAC address of the NIC */ + + if (!gip) + { + uip_getmacaddr("eth0", mac); + + /* Set up the DHCPC modules */ + + handle = dhcpc_open(&mac, IFHWADDRLEN); + + /* Get an IP address. Note that there is no logic for renewing the IP address in this + * example. The address should be renewed in ds.lease_time/2 seconds. + */ + + if (handle) + { + struct dhcpc_state ds; - /* Set netmask */ + (void)dhcpc_request(handle, &ds); + uip_sethostaddr("eth0", &ds.ipaddr); - addr.s_addr = inet_addr("255.255.255.0"); - uip_setnetmask(argv[1], &addr); + if (ds.netmask.s_addr != 0) + { + uip_setnetmask("eth0", &ds.netmask); + } + + if (ds.default_router.s_addr != 0) + { + uip_setdraddr("eth0", &ds.default_router); + } + + if (ds.dnsaddr.s_addr != 0) + { + resolv_conf(&ds.dnsaddr); + } + + dhcpc_close(handle); + } + } +#endif return OK; } @@ -536,6 +812,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 +876,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 +896,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 +923,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 +931,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 +949,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..58d238312 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) @@ -148,7 +156,7 @@ int nsh_netinit(void) { struct dhcpc_state ds; (void)dhcpc_request(handle, &ds); - uip_sethostaddr("eth1", &ds.ipaddr); + uip_sethostaddr("eth0", &ds.ipaddr); if (ds.netmask.s_addr != 0) { uip_setnetmask("eth0", &ds.netmask); diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c index df2f7c3e3..27068acff 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-address>|dhcp]] [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/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index c5960e757..4c021303f 100644 --- a/apps/px4/tests/test_adc.c +++ b/apps/px4/tests/test_adc.c @@ -53,96 +53,40 @@ #include "tests.h" #include <nuttx/analog/adc.h> +#include <drivers/drv_adc.h> +#include <systemlib/err.h> int test_adc(int argc, char *argv[]) { - int fd0 = 0; - int ret = 0; - - #pragma pack(push,1) - struct adc_msg4_s { - uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ - int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ - uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ - int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ - uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ - int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ - uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ - int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ - }; - #pragma pack(pop) - - struct adc_msg4_s sample1; - - ssize_t nbytes; - int j; - int errval; - - fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK); - - if (fd0 <= 0) { - message("/dev/adc0 open fail: %d\n", errno); - return ERROR; - - } else { - message("opened /dev/adc0 successfully\n"); - } - usleep(10000); - - for (j = 0; j < 10; j++) { + int fd = open(ADC_DEVICE_PATH, O_RDONLY); - /* sleep 20 milliseconds */ - usleep(20000); - nbytes = read(fd0, &sample1, sizeof(sample1)); + if (fd < 0) + err(1, "can't open ADC device"); - /* Handle unexpected return values */ + for (unsigned i = 0; i < 5; i++) { + /* make space for a maximum of eight channels */ + struct adc_msg_s data[8]; + /* read all channels available */ + ssize_t count = read(fd, data, sizeof(data)); - if (nbytes < 0) { - errval = errno; + if (count < 0) + goto errout_with_dev; - if (errval != EINTR) { - message("reading /dev/adc0 failed: %d\n", errval); - errval = 3; - goto errout_with_dev; - } + unsigned channels = count / sizeof(data[0]); - message("\tinterrupted read..\n"); - - } else if (nbytes == 0) { - message("\tno data read, ignoring.\n"); - ret = ERROR; + for (unsigned j = 0; j < channels; j++) { + printf("%d: %u ", data[j].am_channel, data[j].am_data); } - /* Print the sample data on successful return */ - - else { - if (nbytes != sizeof(sample1)) { - message("\tsample 1 size %d is not matching struct size %d, ignoring\n", - nbytes, sizeof(sample1)); - ret = ERROR; - - } else { - - message("CYCLE %d:\n", j); - - message("channel: %d value: %d\n", - (int)sample1.am_channel1, sample1.am_data1); - message("channel: %d value: %d\n", - (int)sample1.am_channel2, sample1.am_data2); - message("channel: %d value: %d\n", - (int)sample1.am_channel3, sample1.am_data3); - message("channel: %d value: %d\n", - (int)sample1.am_channel4, sample1.am_data4); - } - } - fflush(stdout); + printf("\n"); + usleep(150000); } message("\t ADC test successful.\n"); errout_with_dev: - if (fd0 != 0) close(fd0); + if (fd != 0) close(fd); - return ret; + return OK; } diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c index e2a10ec29..a42c462ca 100644 --- a/apps/px4/tests/test_bson.c +++ b/apps/px4/tests/test_bson.c @@ -240,5 +240,5 @@ test_bson(int argc, char *argv[]) decode(&decoder); free(buf); - exit(0); + return OK; }
\ No newline at end of file diff --git a/apps/px4/tests/test_jig_voltages.c b/apps/px4/tests/test_jig_voltages.c index 51f9b9a5b..10c93b264 100644 --- a/apps/px4/tests/test_jig_voltages.c +++ b/apps/px4/tests/test_jig_voltages.c @@ -52,7 +52,8 @@ #include "tests.h" #include <nuttx/analog/adc.h> - +#include <drivers/drv_adc.h> +#include <systemlib/err.h> /**************************************************************************** * Pre-processor Definitions @@ -89,129 +90,79 @@ int test_jig_voltages(int argc, char *argv[]) { - int fd0 = 0; - int ret = OK; - const int nchannels = 4; - - struct adc_msg4_s - { - uint8_t am_channel1; /* The 8-bit ADC Channel */ - int32_t am_data1; /* ADC convert result (4 bytes) */ - uint8_t am_channel2; /* The 8-bit ADC Channel */ - int32_t am_data2; /* ADC convert result (4 bytes) */ - uint8_t am_channel3; /* The 8-bit ADC Channel */ - int32_t am_data3; /* ADC convert result (4 bytes) */ - uint8_t am_channel4; /* The 8-bit ADC Channel */ - int32_t am_data4; /* ADC convert result (4 bytes) */ - }__attribute__((__packed__));; - - struct adc_msg4_s sample1[4]; - - size_t readsize; - ssize_t nbytes; - int i = 0; - int j = 0; - int errval; - - char name[11]; - sprintf(name, "/dev/adc%d", j); - fd0 = open(name, O_RDONLY | O_NONBLOCK); - if (fd0 < 0) - { - printf("ADC: %s open fail\n", name); - return ERROR; - } else { - printf("Opened %s successfully\n", name); + int fd = open(ADC_DEVICE_PATH, O_RDONLY); + int ret = OK; + + if (fd < 0) { + warnx("can't open ADC device"); + return 1; + } + + /* make space for a maximum of eight channels */ + struct adc_msg_s data[8]; + /* read all channels available */ + ssize_t count = read(fd, data, sizeof(data)); + + if (count < 0) { + close(fd); + warnx("can't read from ADC driver. Forgot 'adc start' command?"); + return 1; + } + + unsigned channels = count / sizeof(data[0]); + + for (unsigned j = 0; j < channels; j++) { + printf("%d: %u ", data[j].am_channel, data[j].am_data); } + printf("\n"); + + warnx("\t ADC operational.\n"); /* Expected values */ - int16_t expected_min[] = {2700, 2700, 2200, 2000}; - int16_t expected_max[] = {3000, 3000, 2500, 2200}; - char* check_res[nchannels]; + int16_t expected_min[] = {2800, 2800, 1800, 800}; + int16_t expected_max[] = {3100, 3100, 2100, 1100}; + char *check_res[channels]; - /* first adc read round */ - readsize = 4 * sizeof(struct adc_msg_s); + if (channels < 4) { + close(fd); + warnx("not all four test channels available, aborting."); + return 1; - /* Empty all buffers */ - do { - nbytes = read(fd0, sample1, readsize); + } else { + /* Check values */ + check_res[0] = (expected_min[0] < data[0].am_data && expected_max[0] > data[0].am_data) ? "OK" : "FAIL"; + check_res[1] = (expected_min[1] < data[1].am_data && expected_max[1] > data[1].am_data) ? "OK" : "FAIL"; + check_res[2] = (expected_min[2] < data[2].am_data && expected_max[2] > data[2].am_data) ? "OK" : "FAIL"; + check_res[3] = (expected_min[3] < data[3].am_data && expected_max[3] > data[3].am_data) ? "OK" : "FAIL"; + + /* Accumulate result */ + ret += (expected_min[0] > data[0].am_data || expected_max[0] < data[0].am_data) ? 1 : 0; + ret += (expected_min[1] > data[1].am_data || expected_max[1] < data[1].am_data) ? 1 : 0; + ret += (expected_min[2] > data[2].am_data || expected_max[2] < data[2].am_data) ? 1 : 0; + ret += (expected_min[3] > data[3].am_data || expected_max[3] < data[3].am_data) ? 1 : 0; + + message("Sample:"); + message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + data[0].am_channel, (int)(data[0].am_data), expected_min[0], expected_max[0], check_res[0]); + message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + data[1].am_channel, (int)(data[1].am_data), expected_min[1], expected_max[1], check_res[1]); + message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + data[2].am_channel, (int)(data[2].am_data), expected_min[2], expected_max[2], check_res[2]); + message("channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + data[3].am_channel, (int)(data[3].am_data), expected_min[3], expected_max[3], check_res[3]); + + if (ret != OK) { + printf("\t JIG voltages test FAILED. Some channels where out of allowed range. Check supply voltages.\n"); + goto errout_with_dev; + } } - while (nbytes > 0); - - up_udelay(20000);//microseconds - /* Take measurements */ - nbytes = read(fd0, sample1, readsize); - - /* Handle unexpected return values */ - - if (nbytes <= 0) - { - errval = errno; - if (errval != EINTR) - { - message("read %s failed: %d\n", - name, errval); - errval = 3; - goto errout_with_dev; - } - - message("\tInterrupted read...\n"); - } - else if (nbytes == 0) - { - message("\tNo data read, Ignoring\n"); - } - - /* Print the sample data on successful return */ - - else - { - int nsamples = nbytes / sizeof(struct adc_msg_s); - if (nsamples * sizeof(struct adc_msg_s) != nbytes) - { - message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", - nbytes, sizeof(struct adc_msg_s)); - } - else - { - /* Check values */ - check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL"; - check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL"; - check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL"; - check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL"; - - /* Accumulate result */ - ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0; - // XXX Chan 11 not connected on test setup - //ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0; - ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0; - ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0; - - message("Sample:"); - message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", - i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]); - message("Sample:"); - message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", - i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]); - message("Sample:"); - message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", - i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]); - message("Sample:"); - message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", - i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]); - - if (ret != OK) { - printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n"); - goto errout_with_dev; - } - } - } - - printf("\t ADC test successful.\n"); - - errout_with_dev: - if (fd0 != 0) close(fd0); + + printf("\t JIG voltages test successful.\n"); + +errout_with_dev: + + if (fd != 0) close(fd); return ret; } diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c index c7b9d2833..ae682b542 100644 --- a/apps/px4/tests/test_sleep.c +++ b/apps/px4/tests/test_sleep.c @@ -90,9 +90,8 @@ int test_sleep(int argc, char *argv[]) printf("\t %d 100ms sleeps\n", nsleeps); fflush(stdout); - for (int i = 0; i < nsleeps; i++) { + for (unsigned int i = 0; i < nsleeps; i++) { usleep(100000); - //printf("\ttick\n"); } printf("\t Sleep test successful.\n"); diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c index 25bf02c31..8a164f3fc 100644 --- a/apps/px4/tests/test_time.c +++ b/apps/px4/tests/test_time.c @@ -95,7 +95,7 @@ cycletime(void) lasttime = cycles; - return (basetime + cycles) / 168; + return (basetime + cycles) / 168; /* XXX magic number */ } /**************************************************************************** @@ -133,9 +133,9 @@ int test_time(int argc, char *argv[]) lowdelta = abs(delta / 100); /* loop checking the time */ - for (unsigned i = 0; i < 100000; i++) { + for (unsigned i = 0; i < 100; i++) { - usleep(rand() * 10); + usleep(rand()); uint32_t flags = irqsave(); @@ -154,7 +154,7 @@ int test_time(int argc, char *argv[]) fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); } - printf("Maximum jitter %lld\n", maxdelta); + printf("Maximum jitter %lldus\n", maxdelta); return 0; } diff --git a/apps/px4/tests/tests_file.c b/apps/px4/tests/tests_file.c index 697410cee..6f75b9812 100644 --- a/apps/px4/tests/tests_file.c +++ b/apps/px4/tests/tests_file.c @@ -37,6 +37,7 @@ * File write test. */ +#include <sys/stat.h> #include <stdio.h> #include <unistd.h> #include <fcntl.h> @@ -51,6 +52,13 @@ int test_file(int argc, char *argv[]) { + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + uint8_t buf[512]; hrt_abstime start, end; perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c index 26f7ef96b..5bedd257b 100644 --- a/apps/px4/tests/tests_main.c +++ b/apps/px4/tests/tests_main.c @@ -78,42 +78,43 @@ static int test_jig(int argc, char *argv[]); * Private Data ****************************************************************************/ -struct { +const struct { const char *name; int (* fn)(int argc, char *argv[]); unsigned options; - int passed; #define OPT_NOHELP (1<<0) #define OPT_NOALLTEST (1<<1) #define OPT_NOJIGTEST (1<<2) } tests[] = { - {"led", test_led, 0, 0}, - {"int", test_int, 0, 0}, - {"float", test_float, 0, 0}, - {"sensors", test_sensors, 0, 0}, - {"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"adc", test_adc, OPT_NOJIGTEST, 0}, - {"jig_voltages", test_jig_voltages, OPT_NOALLTEST, 0}, - {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"tone", test_tone, 0, 0}, - {"sleep", test_sleep, OPT_NOJIGTEST, 0}, - {"time", test_time, OPT_NOJIGTEST, 0}, - {"perf", test_perf, OPT_NOJIGTEST, 0}, - {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0}, - {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, - {"param", test_param, 0, 0}, - {"bson", test_bson, 0, 0}, - {"file", test_file, 0, 0}, - {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0}, - {NULL, NULL, 0, 0} + {"led", test_led, 0}, + {"int", test_int, 0}, + {"float", test_float, 0}, + {"sensors", test_sensors, 0}, + {"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"adc", test_adc, OPT_NOJIGTEST}, + {"jig_voltages", test_jig_voltages, OPT_NOALLTEST}, + {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"tone", test_tone, 0}, + {"sleep", test_sleep, OPT_NOJIGTEST}, + {"time", test_time, OPT_NOJIGTEST}, + {"perf", test_perf, OPT_NOJIGTEST}, + {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST}, + {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"param", test_param, 0}, + {"bson", test_bson, 0}, + {"file", test_file, 0}, + {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, + {NULL, NULL, 0} }; +#define NTESTS (sizeof(tests) / sizeof(tests[0])) + static int test_help(int argc, char *argv[]) { @@ -133,6 +134,7 @@ test_all(int argc, char *argv[]) unsigned i; char *args[2] = {"all", NULL}; unsigned int failcount = 0; + bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); @@ -147,11 +149,11 @@ test_all(int argc, char *argv[]) fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); fflush(stderr); failcount++; - + passed[i] = false; } else { - tests[i].passed = 1; printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); + passed[i] = true; } } } @@ -195,7 +197,7 @@ test_all(int argc, char *argv[]) unsigned int k; for (k = 0; k < i; k++) { - if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOALLTEST)) { + if (!passed[k] && !(tests[k].options & OPT_NOALLTEST)) { printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); } } @@ -242,6 +244,7 @@ int test_jig(int argc, char *argv[]) unsigned i; char *args[2] = {"jig", NULL}; unsigned int failcount = 0; + bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); for (i = 0; tests[i].name; i++) { @@ -254,10 +257,11 @@ int test_jig(int argc, char *argv[]) fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); fflush(stderr); failcount++; + passed[i] = false; } else { - tests[i].passed = 1; printf(" [%s] \t\t\tPASS\n", tests[i].name); fflush(stdout); + passed[i] = true; } } } @@ -296,7 +300,7 @@ int test_jig(int argc, char *argv[]) unsigned int k; for (k = 0; k < i; k++) { - if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOJIGTEST)) + if (!passed[i] && !(tests[k].options & OPT_NOJIGTEST)) { printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); } diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index f8668a2e3..4fd5fea1b 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -51,6 +51,7 @@ #include <poll.h> #include <stdlib.h> #include <string.h> +#include <ctype.h> #include <systemlib/err.h> #include <unistd.h> #include <drivers/drv_hrt.h> @@ -73,6 +74,8 @@ #include <systemlib/systemlib.h> +#include <mavlink/mavlink_log.h> + #include "sdlog_ringbuffer.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -83,6 +86,7 @@ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ static const char *mountpoint = "/fs/microsd"; static const char *mfile_in = "/etc/logging/logconv.m"; int sysvector_file = -1; +int mavlink_fd = -1; struct sdlog_logbuffer lb; /* mutex / condition to synchronize threads */ @@ -118,6 +122,8 @@ static int file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); +static void handle_command(struct vehicle_command_s *cmd); + /** * Print the current status. */ @@ -134,7 +140,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n"); + errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n"); } // XXX turn this into a C++ class @@ -145,6 +151,9 @@ unsigned sysvector_bytes = 0; unsigned blackbox_file_bytes = 0; uint64_t starttime = 0; +/* logging on or off, default to true */ +bool logging_enabled = true; + /** * The sd log deamon app only briefly exists to start * the background job. The stack size assigned in the @@ -172,7 +181,7 @@ int sdlog_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT - 30, 4096, sdlog_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (const char **)argv); exit(0); } @@ -318,8 +327,54 @@ sysvector_write_start(struct sdlog_logbuffer *logbuf) int sdlog_thread_main(int argc, char *argv[]) { + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* log every n'th value (skip three per default) */ + int skip_value = 3; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + int ch; + + while ((ch = getopt(argc, argv, "s:r")) != EOF) { + switch (ch) { + case 's': + { + /* log only every n'th (gyro clocked) value */ + unsigned s = strtoul(optarg, NULL, 10); + + if (s < 1 || s > 250) { + errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); + } else { + skip_value = s; + } + } + break; - warnx("starting\n"); + case 'r': + /* log only on request, disable logging per default */ + logging_enabled = false; + break; + + case '?': + if (optopt == 'c') { + warnx("Option -%c requires an argument.\n", optopt); + } else if (isprint(optopt)) { + warnx("Unknown option `-%c'.\n", optopt); + } else { + warnx("Unknown option character `\\x%x'.\n", optopt); + } + + default: + usage("unrecognized flag"); + errx(1, "exiting."); + } + } if (file_exist(mountpoint) != OK) { errx(1, "logging mount point %s not present, exiting.", mountpoint); @@ -330,31 +385,15 @@ int sdlog_thread_main(int argc, char *argv[]) if (create_logfolder(folder_path)) errx(1, "unable to create logging folder, exiting."); - /* create sensorfile */ - int sensorfile = -1; - int actuator_outputs_file = -1; - int actuator_controls_file = -1; FILE *gpsfile; FILE *blackbox_file; - // FILE *vehiclefile; - char path_buf[64] = ""; // string to hold the path to the sensorfile + /* string to hold the path to the sensorfile */ + char path_buf[64] = ""; + /* only print logging path, important to find log file later */ warnx("logging to directory %s\n", folder_path); - /* set up file path: e.g. /mnt/sdcard/session0001/sensor_combined.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sensor_combined"); - - if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - - // /* set up file path: e.g. /mnt/sdcard/session0001/actuator_outputs0.bin */ - // sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_outputs0"); - // if (0 == (actuator_outputs_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - // errx(1, "opening %s failed.\n", path_buf); - // } - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); @@ -362,13 +401,6 @@ int sdlog_thread_main(int argc, char *argv[]) errx(1, "opening %s failed.\n", path_buf); } - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "actuator_controls0"); - - if (0 == (actuator_controls_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); @@ -385,6 +417,7 @@ int sdlog_thread_main(int argc, char *argv[]) errx(1, "opening %s failed.\n", path_buf); } + // XXX for fsync() calls int blackbox_file_no = fileno(blackbox_file); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ @@ -432,18 +465,24 @@ int sdlog_thread_main(int argc, char *argv[]) } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for sensors raw */ + /* subscribe to ORB for vehicle command */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GPS POSITION --- */ + /* subscribe to ORB for global position */ + subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + fds[fdsc_count].fd = subs.gps_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(subs.sensor_sub, 5); + /* do not rate limit, instead use skip counter (aliasing on rate limit) */ fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -496,13 +535,6 @@ int sdlog_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- VICON POSITION --- */ /* subscribe to ORB for vicon position */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); @@ -560,22 +592,13 @@ int sdlog_thread_main(int argc, char *argv[]) starttime = hrt_absolute_time(); - // XXX clock the log for now with the gyro output rate / 2 - struct pollfd gyro_fd; - gyro_fd.fd = subs.sensor_sub; - gyro_fd.events = POLLIN; - - /* log every 2nd value (skip one) */ - int skip_value = 0; /* track skipping */ int skip_count = 0; while (!thread_should_exit) { - // XXX only use gyro for now - int poll_ret = poll(&gyro_fd, 1, 1000); - - // int poll_ret = poll(fds, fdsc_count, timeout); + /* only poll for commands, gps and sensor_combined */ + int poll_ret = poll(fds, 3, 1000); /* handle the poll result */ if (poll_ret == 0) { @@ -584,159 +607,129 @@ int sdlog_thread_main(int argc, char *argv[]) /* XXX this is seriously bad - should be an emergency */ } else { - /* always copy sensors raw data into local buffer, since poll flags won't clear else */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - - if (skip_count < skip_value) { - skip_count++; - /* do not log data */ - continue; - } else { - /* log data, reset */ - skip_count = 0; + int ifds = 0; + + /* --- VEHICLE COMMAND VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy command into local buffer */ + orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + + /* always log to blackbox, even when logging disabled */ + blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, + buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, + (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); + + handle_command(&buf.cmd); + } + + /* --- VEHICLE GPS VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy gps position into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + /* if logging disabled, continue */ + if (logging_enabled) { + + /* write KML line */ + } + } + + /* --- SENSORS RAW VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + + // /* copy sensors raw data into local buffer */ + // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + // /* write out */ + // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); + + /* always copy sensors raw data into local buffer, since poll flags won't clear else */ + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); + orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); + orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); + + /* if skipping is on or logging is disabled, ignore */ + if (skip_count < skip_value || !logging_enabled) { + skip_count++; + /* do not log data */ + continue; + } else { + /* log data, reset */ + skip_count = 0; + } + + struct sdlog_sysvector sysvect = { + .timestamp = buf.raw.timestamp, + .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, + .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, + .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, + .baro = buf.raw.baro_pres_mbar, + .baro_alt = buf.raw.baro_alt_meter, + .baro_temp = buf.raw.baro_temp_celcius, + .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, + .actuators = { + buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], + buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] + }, + .vbat = buf.batt.voltage_v, + .bat_current = buf.batt.current_a, + .bat_discharged = buf.batt.discharged_mah, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, + .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, + .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, + .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, + .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, + .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, + .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, + .diff_pressure = buf.diff_pressure.differential_pressure_mbar, + .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, + .true_airspeed = buf.diff_pressure.true_airspeed_m_s + }; + + /* put into buffer for later IO */ + pthread_mutex_lock(&sysvector_mutex); + sdlog_logbuffer_write(&lb, &sysvect); + /* signal the other thread new data, but not yet unlock */ + if ((unsigned)lb.count > (lb.size / 2)) { + /* only request write if several packets can be written at once */ + pthread_cond_signal(&sysvector_cond); + } + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&sysvector_mutex); } - // int ifds = 0; - - // if (poll_count % 5000 == 0) { - // fsync(sensorfile); - // fsync(actuator_outputs_file); - // fsync(actuator_controls_file); - // fsync(blackbox_file_no); - // } - - - - // /* --- VEHICLE COMMAND VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy command into local buffer */ - // orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - // blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, - // buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, - // (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); - // } - - // /* --- SENSORS RAW VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - // /* write out */ - // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); - // } - - // /* --- ATTITUDE VALUE --- */ - // if (fds[ifds++].revents & POLLIN) { - - // /* copy attitude data into local buffer */ - // orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - - - // } - - // /* --- VEHICLE ATTITUDE SETPOINT --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy local position data into local buffer */ - // orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - - // } - - // /* --- ACTUATOR OUTPUTS 0 --- */ - // if (fds[ifds++].revents & POLLIN) { - // /* copy actuator data into local buffer */ - // orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - // /* write out */ - // // actuator_outputs_bytes += write(actuator_outputs_file, (const char*)&buf.act_outputs, sizeof(buf.act_outputs)); - // } - - // /* --- ACTUATOR CONTROL --- */ - // if (fds[ifds++].revents & POLLIN) { - // orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls); - // /* write out */ - // actuator_controls_bytes += write(actuator_controls_file, (const char*)&buf.act_controls, sizeof(buf.act_controls)); - // } - - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); - orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); - - struct sdlog_sysvector sysvect = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = { - buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] - }, - .vbat = buf.batt.voltage_v, - .bat_current = buf.batt.current_a, - .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pressure.differential_pressure_mbar, - .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, - .true_airspeed = buf.diff_pressure.true_airspeed_m_s - }; - - /* put into buffer for later IO */ - pthread_mutex_lock(&sysvector_mutex); - sdlog_logbuffer_write(&lb, &sysvect); - /* signal the other thread new data, but not yet unlock */ - pthread_cond_signal(&sysvector_cond); - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&sysvector_mutex); } } print_sdlog_status(); + /* wake up write thread one last time */ + pthread_mutex_lock(&sysvector_mutex); + pthread_cond_signal(&sysvector_cond); + /* unlock, now the writer thread may return */ + pthread_mutex_unlock(&sysvector_mutex); + /* wait for write thread to return */ (void)pthread_join(sysvector_pthread, NULL); pthread_mutex_destroy(&sysvector_mutex); pthread_cond_destroy(&sysvector_cond); - warnx("exiting.\n"); + warnx("exiting.\n\n"); - close(sensorfile); - close(actuator_outputs_file); - close(actuator_controls_file); + /* finish KML file */ + // XXX fclose(gpsfile); fclose(blackbox_file); @@ -803,4 +796,34 @@ int file_copy(const char *file_old, const char *file_new) return ret; } +void handle_command(struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd->command) { + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + if (((int)(cmd->param3)) == 1) { + + /* enable logging */ + mavlink_log_info(mavlink_fd, "[log] file:"); + mavlink_log_info(mavlink_fd, "logdir"); + logging_enabled = true; + } + if (((int)(cmd->param3)) == 0) { + + /* disable logging */ + mavlink_log_info(mavlink_fd, "[log] stopped."); + logging_enabled = false; + } + break; + + default: + /* silently ignore */ + break; + } + +} diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d00340173..2697cf3d9 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -43,7 +43,6 @@ #include <fcntl.h> #include <poll.h> -#include <nuttx/analog/adc.h> #include <unistd.h> #include <stdlib.h> #include <string.h> @@ -52,13 +51,15 @@ #include <errno.h> #include <math.h> -#include <drivers/drv_hrt.h> +#include <nuttx/analog/adc.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> #include <drivers/drv_rc_input.h> +#include <drivers/drv_adc.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -87,7 +88,7 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLATGE_CHANNEL 10 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define BAT_VOL_INITIAL 12.f #define BAT_VOL_LOWPASS_1 0.99f @@ -108,8 +109,8 @@ extern "C" __EXPORT int sensors_main(int argc, char *argv[]); class Sensors { public: - /** - * Constructor + /** + * Constructor */ Sensors(); @@ -125,7 +126,7 @@ public: */ int start(); -private: +private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ #if CONFIG_HRT_PPM @@ -233,7 +234,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - + param_t rc_map_manual_override_sw; param_t rc_map_auto_mode_sw; @@ -373,7 +374,7 @@ Sensors::Sensors() : _hil_enabled(false), _publishing(true), - /* subscriptions */ +/* subscriptions */ _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), @@ -383,13 +384,13 @@ Sensors::Sensors() : _params_sub(-1), _manual_control_sub(-1), - /* publications */ +/* publications */ _sensor_pub(-1), _manual_control_pub(-1), _rc_pub(-1), _battery_pub(-1), - /* performance counters */ +/* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) { @@ -487,6 +488,7 @@ Sensors::~Sensors() /* wait for a second for the task to quit at our request */ unsigned i = 0; + do { /* wait 20ms */ usleep(20000); @@ -513,15 +515,19 @@ Sensors::parameters_update() if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { warnx("Failed getting min for chan %d", i); } + if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) { warnx("Failed getting trim for chan %d", i); } + if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) { warnx("Failed getting max for chan %d", i); } + if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { warnx("Failed getting rev for chan %d", i); } + if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { warnx("Failed getting dead zone for chan %d", i); } @@ -530,8 +536,8 @@ Sensors::parameters_update() /* handle blowup in the scaling factor calculation */ if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + _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; @@ -553,18 +559,23 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); } + if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { warnx("Failed getting pitch chan index"); } + if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { warnx("Failed getting yaw chan index"); } + 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_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"); } @@ -576,12 +587,15 @@ Sensors::parameters_update() 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"); } @@ -589,15 +603,19 @@ Sensors::parameters_update() 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"); } @@ -605,12 +623,15 @@ Sensors::parameters_update() if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { warnx("Failed getting rc scaling for roll"); } + if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) { warnx("Failed getting rc scaling for pitch"); } + 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"); } @@ -673,9 +694,11 @@ Sensors::accel_init() int fd; fd = open(ACCEL_DEVICE_PATH, 0); + if (fd < 0) { warn("%s", ACCEL_DEVICE_PATH); errx(1, "FATAL: no accelerometer found"); + } else { /* set the accel internal sampling rate up to at leat 500Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 500); @@ -694,9 +717,11 @@ Sensors::gyro_init() int fd; fd = open(GYRO_DEVICE_PATH, 0); + if (fd < 0) { warn("%s", GYRO_DEVICE_PATH); errx(1, "FATAL: no gyro found"); + } else { /* set the gyro internal sampling rate up to at leat 500Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 500); @@ -715,6 +740,7 @@ Sensors::mag_init() int fd; fd = open(MAG_DEVICE_PATH, 0); + if (fd < 0) { warn("%s", MAG_DEVICE_PATH); errx(1, "FATAL: no magnetometer found"); @@ -735,6 +761,7 @@ Sensors::baro_init() int fd; fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { warn("%s", BARO_DEVICE_PATH); warnx("No barometer found, ignoring"); @@ -750,9 +777,10 @@ void Sensors::adc_init() { - _fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); + _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + if (_fd_adc < 0) { - warnx("/dev/adc0"); + warn(ADC_DEVICE_PATH); warnx("FATAL: no ADC found"); } } @@ -821,7 +849,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - + raw.magnetometer_counter++; } } @@ -853,6 +881,7 @@ Sensors::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); + if (vstatus_updated) { orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); @@ -880,8 +909,7 @@ Sensors::parameter_update_poll(bool forced) /* Check if any parameter has changed */ orb_check(_params_sub, ¶m_updated); - if (param_updated || forced) - { + if (param_updated || forced) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); @@ -891,7 +919,7 @@ Sensors::parameter_update_poll(bool forced) /* update sensor offsets */ int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { + struct gyro_scale gscale = { _parameters.gyro_offset[0], 1.0f, _parameters.gyro_offset[1], @@ -899,8 +927,10 @@ Sensors::parameter_update_poll(bool forced) _parameters.gyro_offset[2], 1.0f, }; + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) warn("WARNING: failed to set scale / offsets for gyro"); + close(fd); fd = open(ACCEL_DEVICE_PATH, 0); @@ -912,8 +942,10 @@ Sensors::parameter_update_poll(bool forced) _parameters.accel_offset[2], _parameters.accel_scale[2], }; + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) warn("WARNING: failed to set scale / offsets for accel"); + close(fd); fd = open(MAG_DEVICE_PATH, 0); @@ -925,62 +957,67 @@ Sensors::parameter_update_poll(bool forced) _parameters.mag_offset[2], _parameters.mag_scale[2], }; + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) warn("WARNING: failed to set scale / offsets for mag"); + close(fd); #if 0 - printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor*10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); - printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor*10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); - printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled*100), (int)(_rc.chan[1].scaled*100)); + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); + printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100)); fflush(stdout); usleep(5000); #endif - } + } } void Sensors::adc_poll(struct sensor_combined_s &raw) { - #pragma pack(push,1) - struct adc_msg4_s { - uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ - int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ - uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ - int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ - uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ - int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ - uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ - int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ - } buf_adc; - #pragma pack(pop) + /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { - read(_fd_adc, &buf_adc, sizeof(buf_adc)); + /* make space for a maximum of eight channels */ + struct adc_msg_s buf_adc[8]; + /* read all channels available */ + int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); + + /* look for battery channel */ + + for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { - /* Voltage in volts */ - float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling); + if (ret >= sizeof(buf_adc[0]) && ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + /* Voltage in volts */ + float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; + /* one-time initialization of low-pass value to avoid long init delays */ + if (_battery_status.voltage_v < 3.0f) { + _battery_status.voltage_v = voltage; + } - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* announce the battery voltage if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } - } - raw.battery_voltage_counter++; + _last_adc = hrt_absolute_time(); + break; + } } - _last_adc = hrt_absolute_time(); } } @@ -1012,6 +1049,7 @@ Sensors::ppm_poll() /* publish to object request broker */ if (rc_input_pub <= 0) { rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); + } else { orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } @@ -1052,6 +1090,7 @@ Sensors::ppm_poll() return; unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; @@ -1064,10 +1103,11 @@ Sensors::ppm_poll() /* scale around the mid point differently for lower and upper range */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - + } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; @@ -1078,6 +1118,7 @@ Sensors::ppm_poll() if ((int)_parameters.rev[i] == -1) { _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; } + } else { _rc.chan[i].scaled *= _parameters.rev[i]; } @@ -1103,7 +1144,9 @@ Sensors::ppm_poll() 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 */ @@ -1175,6 +1218,7 @@ Sensors::ppm_poll() /* 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); @@ -1183,6 +1227,7 @@ Sensors::ppm_poll() /* 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); } @@ -1260,7 +1305,7 @@ Sensors::task_main() fds[0].events = POLLIN; while (!_task_should_exit) { - + /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); @@ -1329,6 +1374,7 @@ Sensors::start() warn("task start failed"); return -errno; } + return OK; } @@ -1343,6 +1389,7 @@ int sensors_main(int argc, char *argv[]) errx(1, "sensors task already running"); sensors::g_sensors = new Sensors; + if (sensors::g_sensors == nullptr) errx(1, "sensors task alloc failed"); @@ -1351,12 +1398,14 @@ int sensors_main(int argc, char *argv[]) sensors::g_sensors = nullptr; err(1, "sensors task start failed"); } + exit(0); } if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) errx(1, "sensors task not running"); + delete sensors::g_sensors; sensors::g_sensors = nullptr; exit(0); @@ -1365,6 +1414,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { errx(0, "task is running"); + } else { errx(1, "task is not running"); } 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/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 781b01065..e34be44e3 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -163,6 +163,8 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, FAR const uint8_t *buf); static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); +void at24c_test(void); + /************************************************************************************ * Private Data ************************************************************************************/ @@ -219,6 +221,31 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc } /************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + +/************************************************************************************ * Name: at24c_bread ************************************************************************************/ diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index b4257cda9..49da51358 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -73,6 +73,7 @@ static void eeprom_erase(void); static void eeprom_ioctl(unsigned operation); static void eeprom_save(const char *name); static void eeprom_load(const char *name); +static void eeprom_test(void); static bool attached = false; static bool started = false; @@ -93,6 +94,9 @@ int eeprom_main(int argc, char *argv[]) if (!strcmp(argv[1], "erase")) eeprom_erase(); + if (!strcmp(argv[1], "test")) + eeprom_test(); + if (0) { /* these actually require a file on the filesystem... */ if (!strcmp(argv[1], "reformat")) @@ -250,3 +254,12 @@ eeprom_load(const char *name) exit(0); } + +extern void at24c_test(void); + +static void +eeprom_test(void) +{ + at24c_test(); + exit(0); +} diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 82893b3e9..3f3476f62 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -68,6 +68,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/home_position.h" +ORB_DEFINE(home_position, struct home_position_s); + #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h new file mode 100644 index 000000000..dec34b6ab --- /dev/null +++ b/apps/uORB/topics/home_position.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 home_position.h + * Definition of the GPS home position uORB topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef TOPIC_HOME_POSITION_H_ +#define TOPIC_HOME_POSITION_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GPS home position in WGS84 coordinates. + */ +struct home_position_s +{ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ + + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ + uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ + float s_variance; /**< speed accuracy estimate cm/s */ + float p_variance; /**< position accuracy estimate cm */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(home_position); + +#endif diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 1d25af35a..961ee8b4a 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -102,8 +102,6 @@ struct sensor_combined_s { float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ - uint32_t battery_voltage_counter; /**< Number of voltage measurements taken */ - bool battery_voltage_valid; /**< True if battery voltage can be measured */ }; |