From 8144a84bc3b0e9c0e5582c6a8db335cfdae3d8eb Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 1 May 2015 13:43:27 +0200 Subject: [PATCH 01/41] Make sure we have a boolog.txt entry for critical failures. --- ROMFS/px4fmu_common/init.d/rcS | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1237d9bb11..e100e6b4b7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -233,11 +233,11 @@ then set IO_PRESENT yes else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR: PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi else - echo "PX4IO update failed" >> $LOG_FILE + echo "ERROR: PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -246,6 +246,7 @@ then if [ $IO_PRESENT == no ] then echo "[i] ERROR: PX4IO not found" + echo "ERROR: PX4IO not found" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -328,6 +329,7 @@ then then sh /etc/init.d/rc.io else + echo "ERROR: PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -339,6 +341,7 @@ then echo "[i] FMU mode_$FMU_MODE started" else echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + echo "ERROR: FMU start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -372,6 +375,7 @@ then echo "[i] MK started" else echo "[i] ERROR: MK start failed" + echo "ERROR: MK start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi unset MKBLCTRL_ARG @@ -385,6 +389,7 @@ then echo "[i] HIL output started" else echo "[i] ERROR: HIL output start failed" + echo "ERROR: HIL output start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -402,6 +407,7 @@ then sh /etc/init.d/rc.io else echo "[i] ERROR: PX4IO start failed" + echo "ERROR: PX4IO start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -413,6 +419,7 @@ then echo "[i] FMU mode_$FMU_MODE started" else echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + echo "ERROR: FMU mode_$FMU_MODE start failed" >> $LOG_FILE tone_alarm $TUNE_ERR fi From bf23ca70c593ea27d5d3ab125f9f9678157cbf81 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 May 2015 10:17:56 +0200 Subject: [PATCH 02/41] Added log entries for the rc.interfaces file as well. --- ROMFS/px4fmu_common/init.d/rc.interface | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 7a424970f6..71b670888e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -48,6 +48,7 @@ then echo "[i] Mixer: $MIXER_FILE on $OUTPUT_DEV" else echo "[i] Error loading mixer: $MIXER_FILE" + echo "ERROR: Could not load mixer: $MIXER_FILE" >> $LOG_FILE tone_alarm $TUNE_ERR fi @@ -56,6 +57,7 @@ else if [ $MIXER != skip ] then echo "[i] Mixer not defined" + echo "ERROR: Mixer not defined" >> $LOG_FILE tone_alarm $TUNE_ERR fi fi @@ -125,8 +127,10 @@ then echo "[i] Mixer: $MIXER_AUX_FILE on $OUTPUT_AUX_DEV" else echo "[i] Error loading mixer: $MIXER_AUX_FILE" + echo "ERROR: Could not load mixer: $MIXER_AUX_FILE" >> $LOG_FILE fi else + echo "ERROR: Could not start: fmu mode_pwm" >> $LOG_FILE tone_alarm $TUNE_ERR fi From 542dc9f65d05291b8b9661374044f8c51a4dd209 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 13 May 2015 15:28:02 +0200 Subject: [PATCH 03/41] added default rate I gain for multicopters. added default gains for FireFly6 in mc mode (from Simon Wilks) --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- .../init.d/10017_steadidrone_qu4d | 4 ++-- ROMFS/px4fmu_common/init.d/13002_firefly6 | 20 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 4 ++-- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 6 +++--- 7 files changed, 33 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index caa8e28fc5..68428be6f6 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -12,11 +12,11 @@ then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.5 param set MC_YAWRATE_P 0.25 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 5e41d6957d..8ab65be7b9 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -12,11 +12,11 @@ then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 0.5 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 191c58da4c..ed90dabf41 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -7,6 +7,26 @@ sh /etc/init.d/rc.vtol_defaults +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.002 + param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_FF 0.0 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.002 + param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_FF 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_FF 0.5 + param set MC_YAWRATE_P 0.22 + param set MC_YAWRATE_I 0.02 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_FF 0.0 +fi + set MIXER firefly6 set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 8e5dc76b1f..d07e926a79 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -11,11 +11,11 @@ if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ea35b3ba9b..9b3954be6f 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -12,11 +12,11 @@ then # TODO REVIEW param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index b1db1dd9aa..05b4355138 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -12,11 +12,11 @@ then # TODO REVIEW param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index a1de19d5df..aec0c62f8f 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -13,15 +13,15 @@ if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 66b87ac7618d2e886b5389e3eef50af182b32f25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 May 2015 16:04:33 +0200 Subject: [PATCH 04/41] Fix state-level HIL. Reported by @s3erjaeh --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0cd163a0c9..988232dc21 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1360,8 +1360,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; + hil_global_pos.lat = hil_state.lat / ((double)1e7); + hil_global_pos.lon = hil_state.lon / ((double)1e7); hil_global_pos.alt = hil_state.alt / 1000.0f; hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; From 90c3aec1fdc640ef15a610991251fa8a6ab202e8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 13 May 2015 16:23:29 +0200 Subject: [PATCH 05/41] make main LED show low and critical battery status --- src/modules/commander/commander.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50846ff4d0..f274879325 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2157,10 +2157,12 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->failsafe) { + rgbled_set_color(RGBLED_COLOR_PURPLE); + } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ - + } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { + rgbled_set_color(RGBLED_COLOR_RED); } else { if (status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); From 16aa03e30a8844b03b9594bbf2008ff6151db4eb Mon Sep 17 00:00:00 2001 From: Andrea Nistico Date: Wed, 13 May 2015 17:22:10 +0200 Subject: [PATCH 06/41] mpc parameter INAV_W_Z_VIS_P increased --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 96b12f9e09..382e9e46d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); /** * Z axis weight for sonar From b4e7b041cac7a937df3ea4e84dc943bff809fb55 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Wed, 13 May 2015 11:19:29 -0400 Subject: [PATCH 07/41] Fix potential null pointer deref if Mavlink start fails before task_main loop LL_APPEND is called just before the loop spins up but various error conditions can cause the task to exit before then. When that happens Mavlink::start_helper calls delete on the instance which tries to prune it from the global list. If this is the first Mavlink instance to attempt starting the list head is null and we hardfault in the Mavlink dtor. Only call LL_DELETE after checking the list head for a null pointer. --- src/modules/mavlink/mavlink_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 326b0b5ab4..a76abc298d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -249,7 +249,9 @@ Mavlink::~Mavlink() } while (_task_running); } - LL_DELETE(_mavlink_instances, this); + if (_mavlink_instances) { + LL_DELETE(_mavlink_instances, this); + } } void From b0efb24468564850e1fdb9bf7692fd4e2b939235 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:21:59 +0200 Subject: [PATCH 08/41] add USART6 on PC6(TX) and PC7(RX) to discovery --- .../px4-stm32f4discovery/include/board.h | 18 ++++++++-- .../px4-stm32f4discovery/nsh/defconfig | 35 +++++++++++++------ 2 files changed, 40 insertions(+), 13 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h index 0412c3aa6d..432d574092 100644 --- a/nuttx-configs/px4-stm32f4discovery/include/board.h +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -34,8 +34,8 @@ * ************************************************************************************/ -#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H -#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H /************************************************************************************ * Included Files @@ -200,6 +200,18 @@ #define GPIO_USART2_RX GPIO_USART2_RX_1 #define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* UART6: + * + * The STM32F4 Discovery has no on-board serial devices, PC6 (TX) and PC7 (RX) + * for connection to an external serial device. + */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* USART6 require a RX DMA configuration */ +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 /* SPI - There is a MEMS device on SPI1 using these pins: */ @@ -270,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset); #endif #endif /* __ASSEMBLY__ */ -#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig index 6a2470bea9..03092256e9 100644 --- a/nuttx-configs/px4-stm32f4discovery/nsh/defconfig +++ b/nuttx-configs/px4-stm32f4discovery/nsh/defconfig @@ -91,6 +91,8 @@ CONFIG_ARCH_HAVE_MPU=y # ARMV7M Configuration Options # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y @@ -128,6 +130,7 @@ CONFIG_SERIAL_TERMIOS=y # CONFIG_ARCH_CHIP_STM32F100VC is not set # CONFIG_ARCH_CHIP_STM32F100VD is not set # CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F102CB is not set # CONFIG_ARCH_CHIP_STM32F103C4 is not set # CONFIG_ARCH_CHIP_STM32F103C8 is not set # CONFIG_ARCH_CHIP_STM32F103RET6 is not set @@ -173,7 +176,6 @@ CONFIG_ARCH_CHIP_STM32F407VG=y # CONFIG_STM32_STM32F20XX is not set # CONFIG_STM32_STM32F30XX is not set CONFIG_STM32_STM32F40XX=y -# CONFIG_STM32_STM32F427 is not set # CONFIG_STM32_DFU is not set # @@ -207,9 +209,6 @@ CONFIG_STM32_PWR=y CONFIG_STM32_SPI1=y # CONFIG_STM32_SPI2 is not set # CONFIG_STM32_SPI3 is not set -# CONFIG_STM32_SPI4 is not set -# CONFIG_STM32_SPI5 is not set -# CONFIG_STM32_SPI6 is not set CONFIG_STM32_SYSCFG=y CONFIG_STM32_TIM1=y # CONFIG_STM32_TIM2 is not set @@ -230,9 +229,7 @@ CONFIG_STM32_USART2=y # CONFIG_STM32_USART3 is not set # CONFIG_STM32_UART4 is not set # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set -# CONFIG_STM32_UART7 is not set -# CONFIG_STM32_UART8 is not set +CONFIG_STM32_USART6=y # CONFIG_STM32_IWDG is not set # CONFIG_STM32_WWDG is not set CONFIG_STM32_ADC=y @@ -268,6 +265,8 @@ CONFIG_STM32_USART=y # # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y @@ -336,8 +335,10 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD="" +CONFIG_ARCH_BOARD_STM32F4_DISCOVERY=y +# CONFIG_ARCH_BOARD_MIKROE_STM32F4 is not set +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="stm32f4discovery" # # Common Board Options @@ -431,7 +432,7 @@ CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set -# CONFIG_RTC is not set +# CONFIG_RTC= is not set CONFIG_WATCHDOG=y # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set @@ -449,10 +450,12 @@ CONFIG_SERIAL=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 CONFIG_USART2_SERIAL_CONSOLE=y +# CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set # @@ -466,6 +469,18 @@ CONFIG_USART2_PARITY=0 CONFIG_USART2_2STOP=0 # CONFIG_USART2_IFLOWCONTROL is not set # CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set # CONFIG_SERIAL_IFLOWCONTROL is not set # CONFIG_SERIAL_OFLOWCONTROL is not set CONFIG_USBDEV=y From 52fca5b7a153e21970d9e8f62e24d76a1ca49cf5 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:24:32 +0200 Subject: [PATCH 09/41] correct comments --- src/drivers/boards/px4-stm32f4discovery/board_config.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index dd66abc19f..c4f469caa1 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -60,9 +60,10 @@ __BEGIN_DECLS ****************************************************************************************************/ /* Configuration ************************************************************************************/ -/* PX4FMU GPIOs ***********************************************************************************/ -/* LEDs */ +/* PX4-STM32F4Discovery GPIOs ***********************************************************************************/ /* LEDs */ +// LED1 green, LED2 orange, LED3 red, LED4 blue + #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) From 169f112f0d1b883b26e7d8297a4d898cf709cf06 Mon Sep 17 00:00:00 2001 From: nopeppermint Date: Wed, 13 May 2015 22:59:30 +0200 Subject: [PATCH 10/41] change name in board.h --- nuttx-configs/px4-stm32f4discovery/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h index 432d574092..95feca5506 100644 --- a/nuttx-configs/px4-stm32f4discovery/include/board.h +++ b/nuttx-configs/px4-stm32f4discovery/include/board.h @@ -34,8 +34,8 @@ * ************************************************************************************/ -#ifndef __ARCH_BOARD_BOARD_H -#define __ARCH_BOARD_BOARD_H +#ifndef __CONFIG_DISCOVERY_INCLUDE_BOARD_H +#define __CONFIG_DISCOVERY_INCLUDE_BOARD_H /************************************************************************************ * Included Files @@ -282,4 +282,4 @@ EXTERN void stm32_setleds(uint8_t ledset); #endif #endif /* __ASSEMBLY__ */ -#endif /* __ARCH_BOARD_BOARD_H */ +#endif /* __CONFIG_DISCOVERY_INCLUDE_BOARD_H */ From 8ac81515ce30100cda801134ad7c4cd9a9be6bd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:11:36 +0200 Subject: [PATCH 11/41] EKF estimator: Remove unused params --- .../attitude_estimator_ekf_main.cpp | 13 +------------ .../attitude_estimator_ekf_params.c | 14 -------------- 2 files changed, 1 insertion(+), 26 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 3faf63a27f..fb7cda8c42 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -397,16 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) hrt_abstime vel_t = 0; bool vel_valid = false; - if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - vel_valid = true; - if (gps_updated) { - vel_t = gps.timestamp_velocity; - vel(0) = gps.vel_n_m_s; - vel(1) = gps.vel_e_m_s; - vel(2) = gps.vel_d_m_s; - } - - } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; @@ -487,8 +478,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); - } else { - mag_decl = ekf_params.mag_decl; } /* update mag declination rotation matrix */ diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index e981c6eb74..13a9fa5f6f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -107,11 +107,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); */ PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1); -/* magnetic declination, in degrees */ -PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); - -PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); - /** * Moment of inertia matrix diagonal entry (1, 1) * @@ -159,10 +154,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->mag_decl = param_find("ATT_MAG_DECL"); - - h->acc_comp = param_find("ATT_ACC_COMP"); - h->moment_inertia_J[0] = param_find("ATT_J11"); h->moment_inertia_J[1] = param_find("ATT_J22"); h->moment_inertia_J[2] = param_find("ATT_J33"); @@ -182,11 +173,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI_F / 180.0f; - - param_get(h->acc_comp, &(p->acc_comp)); - for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } From 33338ee2d0523867088c534bf0428ec1a60de950 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:11:56 +0200 Subject: [PATCH 12/41] Q attitude estimator: Comment undocumented params --- .../attitude_estimator_q_params.c | 68 +++++++++++++++++-- 1 file changed, 64 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index fa15923076..730d7a4e8b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -41,10 +41,70 @@ #include +/** + * Complimentary filter accelerometer weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); + +/** + * Complimentary filter magnetometer weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); + +/** + * Complimentary filter gyroscope bias weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); -PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); ///< magnetic declination, in degrees -PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); ///< automatic GPS based magnetic declination -PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); ///< acceleration compensation -PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s + +/** + * Magnetic declination, in degrees + * + * This parameter is not used in normal operation, + * as the declination is looked up based on the + * GPS coordinates of the vehicle. + * + * @group Attitude Q estimator + * @unit degrees + */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + +/** + * Enable automatic GPS based declination compensation + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); + +/** + * Enable acceleration compensation based on GPS + * velocity. + * + * @group Attitude Q estimator + * @min 1 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); + +/** + * Gyro bias limit + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + * @unit rad/s + */ +PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); From e7c30322187068d76cf6ef9ea9935856134a3c6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:31:05 +0200 Subject: [PATCH 13/41] Mag calibration: Accept less rotation to start the UI, ensure calibration abort does indeed abort. --- src/modules/commander/mag_calibration.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1a8de86c41..04e66a5cb6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -209,7 +209,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta float gyro_y_integral = 0.0f; float gyro_z_integral = 0.0f; - const float gyro_int_thresh_rad = 1.0f; + const float gyro_int_thresh_rad = 0.5f; int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); @@ -220,7 +220,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; - break; + close(sub_gyro); + return result; } /* abort with timeout */ From 8a73f91ad6fb18c451f04cfe7a976b66f806031a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 17:33:07 +0200 Subject: [PATCH 14/41] Commander: Be less verbose about param saves to focus the user attention to relevant output --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f274879325..7fbb0a2dac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2794,7 +2794,7 @@ void *commander_low_prio_loop(void *arg) need_param_autosave_timeout = 0; } - mavlink_log_info(mavlink_fd, "settings saved"); + /* do not spam MAVLink, but provide the answer / green led mechanism */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { From dc4d5619eae2bb6eebfe2f11ee97f5734a35d731 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 14 May 2015 12:36:40 -1000 Subject: [PATCH 15/41] Reduced the amount of memory used by params to only that that is needed Conflicts: src/modules/systemlib/param/param.c --- src/modules/systemlib/param/param.c | 42 ++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 4ec885ab3e..544eeaa39d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -57,6 +57,7 @@ #include "systemlib/param/param.h" #include "systemlib/uthash/utarray.h" #include "systemlib/bson/tinybson.h" +#include #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" @@ -91,8 +92,23 @@ struct param_wbuf_s { bool unsaved; }; -// XXX this should be param_info_count, but need to work out linking -uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {}; + +uint8_t *param_changed_storage = 0; +int size_param_changed_storage_bytes = 0; +const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); + + +static unsigned +get_param_info_count(void) +{ + // Singleton + if (!param_changed_storage) + { + size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit ) + 1; + param_changed_storage = zalloc(size_param_changed_storage_bytes); + } + return param_info_count; +} /** flexible array holding modified parameter values */ UT_array *param_values; @@ -140,7 +156,7 @@ param_assert_locked(void) static bool handle_in_range(param_t param) { - return (param < param_info_count); + return (param < get_param_info_count()); } /** @@ -245,15 +261,17 @@ param_find_no_notification(const char *name) unsigned param_count(void) { - return param_info_count; + return get_param_info_count(); } unsigned param_count_used(void) { + // ensure the allocation has been done + get_param_info_count(); unsigned count = 0; - for (unsigned i = 0; i < sizeof(param_changed_storage) / sizeof(param_changed_storage[0]); i++) { - for (unsigned j = 0; j < 8; j++) { + for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { count++; } @@ -265,7 +283,7 @@ param_count_used(void) param_t param_for_index(unsigned index) { - if (index < param_info_count) { + if (index < get_param_info_count()) return (param_t)index; } @@ -323,7 +341,7 @@ param_get_used_index(param_t param) int count = 0; for (unsigned i = 0; i < (unsigned)param + 1; i++) { - for (unsigned j = 0; j < 8; j++) { + for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { if (param_storage_index == i) { @@ -559,8 +577,8 @@ param_used(param_t param) return false; } - unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); - return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (1 << bitindex); + unsigned bitindex = param_index - (param_index / bits_per_allocation_unit); + return param_changed_storage[param_index / bits_per_allocation_unit] & (1 << bitindex); } void param_set_used_internal(param_t param) @@ -570,8 +588,8 @@ void param_set_used_internal(param_t param) return; } - unsigned bitindex = param_index - (param_index / sizeof(param_changed_storage[0])); - param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex); + unsigned bitindex = param_index - (param_index / bits_per_allocation_unit); + param_changed_storage[param_index / bits_per_allocation_unit] |= (1 << bitindex); } int From 8e9fdc61473f851f8747f379353bd6725e173720 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 14 May 2015 16:44:55 -1000 Subject: [PATCH 16/41] Use stdlib's calloc for compaiblity --- src/modules/systemlib/param/param.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 544eeaa39d..26568266e7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -105,7 +106,7 @@ get_param_info_count(void) if (!param_changed_storage) { size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit ) + 1; - param_changed_storage = zalloc(size_param_changed_storage_bytes); + param_changed_storage = calloc(size_param_changed_storage_bytes, 1); } return param_info_count; } From 6667e6e078e305df1b739dd70876423aa0455e7f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 14 May 2015 18:03:23 -1000 Subject: [PATCH 17/41] Update param.c Not used px4_macros.h anyway --- src/modules/systemlib/param/param.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 26568266e7..010f40c883 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -58,7 +58,6 @@ #include "systemlib/param/param.h" #include "systemlib/uthash/utarray.h" #include "systemlib/bson/tinybson.h" -#include #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" From a0af91d05cd801852f08041ddeffa881ae2cd1fb Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 14 May 2015 18:15:10 -1000 Subject: [PATCH 18/41] Missing Brace --- src/modules/systemlib/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 010f40c883..14e7299e60 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -283,7 +283,7 @@ param_count_used(void) param_t param_for_index(unsigned index) { - if (index < get_param_info_count()) + if (index < get_param_info_count()) { return (param_t)index; } From fa8dc5723610c5eafddb47526775038cc2a5d77b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 14 May 2015 20:00:06 +0200 Subject: [PATCH 19/41] mavlink app: Only send params if the system has booted. --- src/modules/mavlink/mavlink_parameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 9c8794017b..524effb205 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -181,8 +181,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested */ - if (_send_all_index >= 0) { + /* send all parameters if requested, but only after the system has booted */ + if (_send_all_index >= 0 && t > 4 * 1000 * 1000) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) { From 535eb7dbd954cf8eae984d5b347568f966e5dde8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 09:11:28 +0200 Subject: [PATCH 20/41] param lib: Fix use of array size --- src/modules/systemlib/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 14e7299e60..e86f09ffae 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -298,7 +298,7 @@ param_for_used_index(unsigned index) /* walk all params and count */ int count = 0; - for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) { + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { From 2bcfd4f6f7c03b40ec39083a657a2208fd9a8a65 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 15 May 2015 09:26:07 +0200 Subject: [PATCH 21/41] rename parameters in launch file to match new MP parameters --- launch/ardrone.launch | 16 ++++++++-------- launch/iris.launch | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 56030dd1f4..f43bbf4709 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -6,14 +6,14 @@ - - - - - - - - + + + + + + + + diff --git a/launch/iris.launch b/launch/iris.launch index bf5b099b64..5231e3215b 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -7,14 +7,14 @@ - - - - - - - - + + + + + + + + From 4fb91f47cdc5cb7925c7bf2feb46c64551081883 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 11:15:04 +0200 Subject: [PATCH 22/41] Fix mavlink FTP list stack overflow. Fixes #2180, not flight-critical --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 988232dc21..544d343b43 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1581,7 +1581,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 1800); + pthread_attr_setstacksize(&receiveloop_attr, 2100); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); From f154f6e5e7598b02c2a5c5bb87e646a3425421a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 09:42:35 +0200 Subject: [PATCH 23/41] MAVLink transmission: Allow faster overall transmissions. --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- src/modules/mavlink/mavlink_main.cpp | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index e456eff7f1..8c62abb129 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 30000 -d /dev/ttyACM0 -x +mavlink start -r 80000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 4ccc5dacb6..8b551aa0e8 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,7 +664,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=2048 +CONFIG_CDCACM_TXBUFSIZE=4096 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a76abc298d..0df0c3b5bc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" -#define MAX_DATA_RATE 60000 ///< max data rate in bytes/s +#define MAX_DATA_RATE 1000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate #define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. @@ -990,7 +990,7 @@ void Mavlink::adjust_stream_rates(const float multiplier) { /* do not allow to push us to zero */ - if (multiplier < 0.01f) { + if (multiplier < 0.0005f) { return; } @@ -1001,9 +1001,9 @@ Mavlink::adjust_stream_rates(const float multiplier) unsigned interval = stream->get_interval(); interval /= multiplier; - /* allow max ~600 Hz */ + /* allow max ~2000 Hz */ if (interval < 1600) { - interval = 1600; + interval = 500; } /* set new interval */ @@ -1375,7 +1375,7 @@ Mavlink::task_main(int argc, char *argv[]) /* MAVLINK_FTP stream */ _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); - _mavlink_ftp->set_interval(interval_from_rate(120.0f)); + _mavlink_ftp->set_interval(interval_from_rate(1000.0f)); LL_APPEND(_streams, _mavlink_ftp); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on @@ -1425,7 +1425,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; + _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); From 9bd2e376f6bdd4aa31f7c00ba8161fffa293121a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:56:39 +0200 Subject: [PATCH 24/41] NuttX for FMUv2: Larger USB buffer --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 8b551aa0e8..a688a1bf9b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -663,8 +663,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=4096 +CONFIG_CDCACM_RXBUFSIZE=1000 +CONFIG_CDCACM_TXBUFSIZE=5000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From dabcf63c506f5ead7978d49df039e4e193cc3a15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:28:07 +0200 Subject: [PATCH 25/41] mavlink FTP: Drop rate to realistic value --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0df0c3b5bc..e5cddfafb6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1375,7 +1375,7 @@ Mavlink::task_main(int argc, char *argv[]) /* MAVLINK_FTP stream */ _mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this); - _mavlink_ftp->set_interval(interval_from_rate(1000.0f)); + _mavlink_ftp->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _mavlink_ftp); /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on From 8f8184560b2a35e64bb27f9386a60a7650d8dec1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 10:16:21 +0200 Subject: [PATCH 26/41] sdlog2: Log at 100 Hz as default --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 454af8da7a..6bfbfea396 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -11,7 +11,7 @@ then then fi else - if sdlog2 start -r 200 -a -b 22 -t + if sdlog2 start -r 100 -a -b 22 -t then fi fi From 5c53d38652dc6c97e216ea6b70215a95890df572 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 11:38:06 +0200 Subject: [PATCH 27/41] FMUv2 config: Increase USB TX buf size further to speed up MAVLink FTP transfers --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a688a1bf9b..ab8f9eef62 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -664,7 +664,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=1000 -CONFIG_CDCACM_TXBUFSIZE=5000 +CONFIG_CDCACM_TXBUFSIZE=8000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From c6bc3153efcd345d411695e449c1a1475f84655a Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 15 May 2015 03:58:04 -1000 Subject: [PATCH 28/41] Reviewd - fixed indexing that was wrong, code clean up ran astyle --- src/modules/systemlib/param/param.c | 126 +++++++++++++++------------- 1 file changed, 70 insertions(+), 56 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e86f09ffae..6de03125ca 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -72,13 +72,13 @@ * Array of static parameter info. */ #ifdef _UNIT_TEST - extern struct param_info_s param_array[]; - extern struct param_info_s *param_info_base; - extern struct param_info_s *param_info_limit; +extern struct param_info_s param_array[]; +extern struct param_info_s *param_info_base; +extern struct param_info_s *param_info_limit; #else - extern char __param_start, __param_end; - static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; - static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; +extern char __param_start, __param_end; +static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; +static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; #endif #define param_info_count ((unsigned)(param_info_limit - param_info_base)) @@ -101,13 +101,13 @@ const int bits_per_allocation_unit = (sizeof(*param_changed_storage) * 8); static unsigned get_param_info_count(void) { - // Singleton - if (!param_changed_storage) - { - size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit ) + 1; - param_changed_storage = calloc(size_param_changed_storage_bytes, 1); - } - return param_info_count; + /* Singleton creation of and array of bits to track changed values */ + if (!param_changed_storage) { + size_param_changed_storage_bytes = (param_info_count / bits_per_allocation_unit) + 1; + param_changed_storage = calloc(size_param_changed_storage_bytes, 1); + } + + return param_info_count; } /** flexible array holding modified parameter values */ @@ -170,11 +170,13 @@ param_compare_values(const void *a, const void *b) struct param_wbuf_s *pa = (struct param_wbuf_s *)a; struct param_wbuf_s *pb = (struct param_wbuf_s *)b; - if (pa->param < pb->param) + if (pa->param < pb->param) { return -1; + } - if (pa->param > pb->param) + if (pa->param > pb->param) { return 1; + } return 0; } @@ -187,7 +189,8 @@ param_compare_values(const void *a, const void *b) * NULL if the parameter has not been modified. */ static struct param_wbuf_s * -param_find_changed(param_t param) { +param_find_changed(param_t param) +{ struct param_wbuf_s *s = NULL; param_assert_locked(); @@ -200,8 +203,9 @@ param_find_changed(param_t param) { #else while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { - if (s->param == param) + if (s->param == param) { break; + } } #endif @@ -238,6 +242,7 @@ param_find_internal(const char *name, bool notification) if (notification) { param_set_used_internal(param); } + return param; } } @@ -267,9 +272,10 @@ param_count(void) unsigned param_count_used(void) { - // ensure the allocation has been done - get_param_info_count(); + // ensure the allocation has been done + get_param_info_count(); unsigned count = 0; + for (unsigned i = 0; i < size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { @@ -277,6 +283,7 @@ param_count_used(void) } } } + return count; } @@ -359,10 +366,7 @@ param_get_used_index(param_t param) const char * param_name(param_t param) { - if (handle_in_range(param)) - return param_info_base[param].name; - - return NULL; + return handle_in_range(param) ? param_info_base[param].name : NULL;; } bool @@ -375,29 +379,22 @@ bool param_value_unsaved(param_t param) { static struct param_wbuf_s *s; - s = param_find_changed(param); - - if (s && s->unsaved) - return true; - - return false; + return (s && s->unsaved) ? true : false; } enum param_type_e -param_type(param_t param) -{ - if (handle_in_range(param)) - return param_info_base[param].type; - - return PARAM_TYPE_UNKNOWN; +param_type(param_t param) { + return handle_in_range(param) ? param_info_base[param].type : PARAM_TYPE_UNKNOWN; } size_t param_size(param_t param) { if (handle_in_range(param)) { + switch (param_type(param)) { + case PARAM_TYPE_INT32: case PARAM_TYPE_FLOAT: return 4; @@ -442,8 +439,9 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) >= PARAM_TYPE_STRUCT - && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { + if (param_type(param) >= PARAM_TYPE_STRUCT && + param_type(param) <= PARAM_TYPE_STRUCT_MAX) { + result = v->p; } else { @@ -481,8 +479,9 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ param_lock(); - if (param_values == NULL) + if (param_values == NULL) { utarray_new(param_values, ¶m_icd); + } if (param_values == NULL) { debug("failed to allocate modified values array"); @@ -512,6 +511,7 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ /* update the changed value */ switch (param_type(param)) { + case PARAM_TYPE_INT32: s->val.i = *(int32_t *)val; break; @@ -551,8 +551,9 @@ out: * If we set something, now that we have unlocked, go ahead and advertise that * a thing has been set. */ - if (params_changed && notify_changes) + if (params_changed && notify_changes) { param_notify_changes(); + } return result; } @@ -573,23 +574,25 @@ bool param_used(param_t param) { int param_index = param_get_index(param); + if (param_index < 0) { return false; } - unsigned bitindex = param_index - (param_index / bits_per_allocation_unit); - return param_changed_storage[param_index / bits_per_allocation_unit] & (1 << bitindex); + return param_changed_storage[param_index / bits_per_allocation_unit] & + (1 << param_index % bits_per_allocation_unit); } void param_set_used_internal(param_t param) { int param_index = param_get_index(param); + if (param_index < 0) { return; } - unsigned bitindex = param_index - (param_index / bits_per_allocation_unit); - param_changed_storage[param_index / bits_per_allocation_unit] |= (1 << bitindex); + param_changed_storage[param_index / bits_per_allocation_unit] |= + (1 << param_index % bits_per_allocation_unit); } int @@ -616,8 +619,9 @@ param_reset(param_t param) param_unlock(); - if (s != NULL) + if (s != NULL) { param_notify_changes(); + } return (!param_found); } @@ -640,28 +644,28 @@ param_reset_all(void) } void -param_reset_excludes(const char* excludes[], int num_excludes) +param_reset_excludes(const char *excludes[], int num_excludes) { param_lock(); param_t param; for (param = 0; handle_in_range(param); param++) { - const char* name = param_name(param); + const char *name = param_name(param); bool exclude = false; for (int index = 0; index < num_excludes; index ++) { int len = strlen(excludes[index]); - if((excludes[index][len - 1] == '*' - && strncmp(name, excludes[index], len - 1) == 0) - || strcmp(name, excludes[index]) == 0) { + if ((excludes[index][len - 1] == '*' + && strncmp(name, excludes[index], len - 1) == 0) + || strcmp(name, excludes[index]) == 0) { exclude = true; break; } } - if(!exclude) { + if (!exclude) { param_reset(param); } } @@ -675,14 +679,17 @@ static const char *param_default_file = "/eeprom/parameters"; static char *param_user_file = NULL; int -param_set_default_file(const char* filename) +param_set_default_file(const char *filename) { if (param_user_file != NULL) { free(param_user_file); param_user_file = NULL; } - if (filename) + + if (filename) { param_user_file = strdup(filename); + } + return 0; } @@ -733,6 +740,7 @@ param_load_default(void) warn("open '%s' for reading failed", param_get_default_file()); return -1; } + return 1; } @@ -773,13 +781,16 @@ param_export(int fd, bool only_unsaved) * If we are only saving values changed since last save, and this * one hasn't, then skip it */ - if (only_unsaved && !s->unsaved) + if (only_unsaved && !s->unsaved) { continue; + } s->unsaved = false; /* append the appropriate BSON type object */ + switch (param_type(s->param)) { + case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -823,8 +834,9 @@ param_export(int fd, bool only_unsaved) out: param_unlock(); - if (result == 0) + if (result == 0) { result = bson_encoder_fini(&encoder); + } return result; } @@ -934,8 +946,9 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) out: - if (tmp != NULL) + if (tmp != NULL) { free(tmp); + } return result; } @@ -961,8 +974,9 @@ param_import_internal(int fd, bool mark_saved) out: - if (result < 0) + if (result < 0) { debug("BSON error decoding parameters"); + } return result; } From 97b8e7a20e1bf64b7677911e6bbab300ff471358 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 May 2015 21:14:34 +0200 Subject: [PATCH 29/41] mavlink FTP: Speed up transfers by longer bursts --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++++++++-- src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 10b94b6b26..6a87458f72 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -491,6 +491,7 @@ MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id) // Setup for streaming sends _session_info.stream_download = true; _session_info.stream_offset = payload->offset; + _session_info.stream_chunk_transmitted = 0; _session_info.stream_seq_number = payload->seq_number + 1; _session_info.stream_target_system_id = target_system_id; @@ -872,6 +873,7 @@ void MavlinkFTP::send(const hrt_abstime t) } else { payload->size = bytes_read; _session_info.stream_offset += bytes_read; + _session_info.stream_chunk_transmitted += bytes_read; } } @@ -890,8 +892,12 @@ void MavlinkFTP::send(const hrt_abstime t) #ifndef MAVLINK_FTP_UNIT_TEST if (max_bytes_to_send < (get_size()*2)) { more_data = false; - payload->burst_complete = true; - _session_info.stream_download = false; + /* perform transfers in 35K chunks - this is determined empirical */ + if (_session_info.stream_chunk_transmitted > 35000) { + payload->burst_complete = true; + _session_info.stream_download = false; + _session_info.stream_chunk_transmitted = 0; + } } else { #endif more_data = true; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index af8740e481..33b8996f71 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -165,6 +165,7 @@ private: uint32_t stream_offset; uint16_t stream_seq_number; uint8_t stream_target_system_id; + unsigned stream_chunk_transmitted; }; struct SessionInfo _session_info; ///< Session info, fd=-1 for no active session From 2432418751b341aae150bccb275a7908ae107ee7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 May 2015 11:37:41 +0200 Subject: [PATCH 30/41] FTP: Better error reporting, ignore hidden directories --- src/modules/mavlink/mavlink_ftp.cpp | 43 ++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6a87458f72..6d806415ce 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -301,23 +301,32 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req) MavlinkFTP::ErrorCode MavlinkFTP::_workList(PayloadHeader* payload) { - char dirPath[kMaxDataLength]; - strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); - + char dirPath[kMaxDataLength]; + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); + + ErrorCode errorCode = kErrNone; + unsigned offset = 0; + DIR *dp = opendir(dirPath); if (dp == nullptr) { - warnx("FTP: can't open path '%s'", dirPath); - return kErrFailErrno; + _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)"); + _mavlink->send_statustext_critical(dirPath); + // this is not an FTP error, abort directory read and continue + + payload->data[offset++] = kDirentSkip; + *((char *)&payload->data[offset]) = '\0'; + offset++; + payload->size = offset; + + return errorCode; } - + #ifdef MAVLINK_FTP_DEBUG warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif - ErrorCode errorCode = kErrNone; struct dirent entry, *result = nullptr; - unsigned offset = 0; // move to the requested offset seekdir(dp, payload->offset); @@ -325,9 +334,16 @@ MavlinkFTP::_workList(PayloadHeader* payload) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { - warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrFailErrno; - break; + _mavlink->send_statustext_critical("FTP: list readdir_r failure"); + _mavlink->send_statustext_critical(dirPath); + + payload->data[offset++] = kDirentSkip; + *((char *)&payload->data[offset]) = '\0'; + offset++; + payload->size = offset; + closedir(dp); + + return errorCode; } // no more entries? @@ -357,7 +373,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // XXX @DonLakeFlyer: Remove the first condition for the test setup + if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { From 03ef6a30ec5fa7c09b0de3ba00d577ccca5d39e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 May 2015 11:43:45 +0200 Subject: [PATCH 31/41] Speed up param transmit now that we are faster on USB --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 8c62abb129..0442637941 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ mavlink start -r 80000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 +mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 From 8a891591a8eee665fe31f5ff4a1d7dfe47ff00a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 May 2015 19:05:31 +0200 Subject: [PATCH 32/41] make constrain airspeed actually constrain the airspeed --- src/lib/ecl/attitude_fw/ecl_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 9cd08a50d4..95ccf4130f 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -132,9 +132,9 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m float airspeed_result = airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (minspeed + maxspeed); + airspeed_result = 0.5f * (minspeed + maxspeed); } else if (airspeed < minspeed) { - airspeed = minspeed; + airspeed_result = minspeed; } return airspeed_result; } From 3032afb80399a78fe56417858d7b026db8206634 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 10:48:12 +0200 Subject: [PATCH 33/41] mavlink FTP: Fix build system error for unit test --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6d806415ce..a6c92ac623 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -310,8 +310,12 @@ MavlinkFTP::_workList(PayloadHeader* payload) DIR *dp = opendir(dirPath); if (dp == nullptr) { +#ifdef MAVLINK_FTP_UNIT_TEST + warnx("File open failed"); +#else _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)"); _mavlink->send_statustext_critical(dirPath); +#endif // this is not an FTP error, abort directory read and continue payload->data[offset++] = kDirentSkip; @@ -334,8 +338,12 @@ MavlinkFTP::_workList(PayloadHeader* payload) for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { +#ifdef MAVLINK_FTP_UNIT_TEST + warnx("readdir_r failed"); +#else _mavlink->send_statustext_critical("FTP: list readdir_r failure"); _mavlink->send_statustext_critical(dirPath); +#endif payload->data[offset++] = kDirentSkip; *((char *)&payload->data[offset]) = '\0'; From fcb1bb2bb6af0d9f46ea86f124c5eb98d47b0bf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Erik=20J=C3=A4hne?= Date: Sun, 17 May 2015 14:50:17 +0200 Subject: [PATCH 34/41] FIX HIL GPS conversion --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 544d343b43..40adc6b054 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1386,7 +1386,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); + map_projection_init(&_hil_local_proj_ref, lat, lon); hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_lat = lat; hil_local_pos.ref_lon = lon; From 83fdb9931a0a9dd209566102bd873a274f4db6ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 17:44:32 +0200 Subject: [PATCH 35/41] commander / mavlink: Add battery status to output BATTERY_STATUS MAVLink message --- msg/vehicle_status.msg | 2 ++ src/modules/commander/commander.cpp | 2 ++ src/modules/commander/commander_helper.cpp | 4 ++++ src/modules/commander/commander_helper.h | 2 ++ src/modules/mavlink/mavlink_messages.cpp | 21 +++++++++++++++++++++ 5 files changed, 31 insertions(+) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2a7e493a39..430f7dd768 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -147,6 +147,8 @@ float32 load # processor load from 0 to 1 float32 battery_voltage float32 battery_current float32 battery_remaining +float32 battery_discharged_mah +uint32 battery_cell_count uint8 battery_warning # current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum uint16 drop_rate_comm diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7fbb0a2dac..9676ad53e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1502,7 +1502,9 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; + status.battery_discharged_mah = battery.discharged_mah; status.condition_battery_voltage_valid = true; + status.battery_cell_count = battery_get_n_cells(); /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 99b926be4d..c0f8561fda 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -330,6 +330,10 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } +unsigned battery_get_n_cells() { + return bat_n_cells; +} + float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index bf0c0505d2..d2aace2a40 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -89,4 +89,6 @@ int battery_init(); */ float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); +unsigned battery_get_n_cells(); + #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7ddc52fd1a..3883cb84a9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -543,6 +543,27 @@ protected: status.battery_remaining * 100.0f : -1; _mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg); + + /* battery status message with higher resolution */ + mavlink_battery_status_t bat_msg; + bat_msg.id = 0; + bat_msg.battery_function = MAV_BATTERY_FUNCTION_ALL; + bat_msg.type = MAV_BATTERY_TYPE_LIPO; + bat_msg.temperature = INT16_MAX; + for (unsigned i = 0; i < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); i++) { + if (i < status.battery_cell_count) { + bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f; + } else { + bat_msg.voltages[i] = 0; + } + } + bat_msg.current_battery = status.battery_current * 100.0f; + bat_msg.current_consumed = status.battery_discharged_mah; + bat_msg.energy_consumed = -1.0f; + bat_msg.battery_remaining = (status.battery_voltage > 0) ? + status.battery_remaining * 100.0f : -1; + + _mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg); } } }; From e279e8bb2a87e56d5356fc255d800516c8094cc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 22:58:52 +0200 Subject: [PATCH 36/41] Fix param changed count logic, speed up logic for unused params --- src/modules/systemlib/param/param.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 6de03125ca..f2d5f81b16 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -338,20 +338,19 @@ param_get_index(param_t param) int param_get_used_index(param_t param) { - int param_storage_index = param_get_index(param); - - if (param_storage_index < 0) { + /* this tests for out of bounds and does a constant time lookup */ + if (!param_used(param)) { return -1; } - /* walk all params and count */ + /* walk all params and count, now knowing that it has a valid index */ int count = 0; - for (unsigned i = 0; i < (unsigned)param + 1; i++) { - for (unsigned j = 0; j < bits_per_allocation_unit; j++) { + for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { + for (unsigned j = 0; j < 8; j++) { if (param_changed_storage[i] & (1 << j)) { - if (param_storage_index == i) { + if ((unsigned)param == i * 8 + j) { return count; } From ff4be81976dfe178ed4ddc3ea12c23c00f1dcc18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 23:05:32 +0200 Subject: [PATCH 37/41] Param: do not set a param as used just because its value is non-default. --- src/modules/systemlib/param/param.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index f2d5f81b16..26696dc0d0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -542,8 +542,6 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_ } out: - param_set_used_internal(param); - param_unlock(); /* From 2f5e27c18013c1e8c96c4e2266c3a63f774c5962 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 23:08:10 +0200 Subject: [PATCH 38/41] param lib: Fix code style --- src/modules/systemlib/param/param.c | 2 +- src/modules/systemlib/param/param.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 26696dc0d0..910f9da07f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -365,7 +365,7 @@ param_get_used_index(param_t param) const char * param_name(param_t param) { - return handle_in_range(param) ? param_info_base[param].name : NULL;; + return handle_in_range(param) ? param_info_base[param].name : NULL; } bool diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 9cbe3570b6..07c6dd6bf2 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -249,7 +249,7 @@ __EXPORT void param_reset_all(void); * at the end to exclude parameters with a certain prefix. * @param num_excludes The number of excludes provided. */ - __EXPORT void param_reset_excludes(const char* excludes[], int num_excludes); +__EXPORT void param_reset_excludes(const char *excludes[], int num_excludes); /** * Export changed parameters to a file. @@ -306,16 +306,16 @@ __EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, * exist. * @return Zero on success. */ -__EXPORT int param_set_default_file(const char* filename); +__EXPORT int param_set_default_file(const char *filename); /** * Get the default parameter file name. * * @return The path to the current default parameter file; either as - * a result of a call to param_set_default_file, or the + * a result of a call to param_set_default_file, or the * built-in default. */ -__EXPORT const char* param_get_default_file(void); +__EXPORT const char *param_get_default_file(void); /** * Save parameters to the default file. From fa12a61765c1732e47b812b66bd3b78360f9450e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 23:23:26 +0200 Subject: [PATCH 39/41] sdlog2: Fix command handling / interpretation of command parameters --- src/modules/sdlog2/sdlog2.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c76569cf8f..504a1a4f6e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -286,7 +286,7 @@ sdlog2_usage(const char *reason) fprintf(stderr, "%s\n", reason); } - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t -x\n" + errx(1, "usage: sdlog2 {start|stop|status|on|off} [-r ] [-b ] -e -a -t -x\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" @@ -349,6 +349,8 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "on")) { struct vehicle_command_s cmd; cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; + cmd.param1 = -1; + cmd.param2 = -1; cmd.param3 = 1; int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); close(fd); @@ -358,7 +360,9 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "off")) { struct vehicle_command_s cmd; cmd.command = VEHICLE_CMD_PREFLIGHT_STORAGE; - cmd.param3 = 0; + cmd.param1 = -1; + cmd.param2 = -1; + cmd.param3 = 2; int fd = orb_advertise(ORB_ID(vehicle_command), &cmd); close(fd); return 0; @@ -1873,7 +1877,7 @@ void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); if (!logging_enabled) { - warnx("standing by"); + warnx("not logging"); } else { float kibibytes = log_bytes_written / 1024.0f; @@ -1975,7 +1979,7 @@ void handle_command(struct vehicle_command_s *cmd) if (param == 1) { sdlog2_start_log(); - } else if (param == -1) { + } else if (param == 2) { sdlog2_stop_log(); } else { // Silently ignore non-matching command values, as they could be for params. From 635b7fa01d5627ef998c01abad4e5cda89f086f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 May 2015 00:17:09 +0200 Subject: [PATCH 40/41] param API: Ensure info count method is called --- src/modules/systemlib/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 910f9da07f..bca92dc6bc 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -300,7 +300,7 @@ param_for_index(unsigned index) param_t param_for_used_index(unsigned index) { - if (index < param_info_count) { + if (index < get_param_info_count()) { /* walk all params and count */ int count = 0; From 937289a3ef6a893725e1674c4b591a669d36e6a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 May 2015 18:28:13 +0200 Subject: [PATCH 41/41] Give user a choice which time source to pick --- src/modules/sdlog2/sdlog2.c | 89 ++++++++++++++++++++++++++++--------- 1 file changed, 69 insertions(+), 20 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 504a1a4f6e..37234ca5d3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -144,6 +144,19 @@ PARAM_DEFINE_INT32(SDLOG_RATE, -1); */ PARAM_DEFINE_INT32(SDLOG_EXT, -1); +/** + * Use timestamps only if GPS 3D fix is available + * + * A value of 1 constrains the log folder creation + * to only use the time stamp if a 3D GPS lock is + * present. + * + * @min 0 + * @max 1 + * @group SD Logging + */ +PARAM_DEFINE_INT32(SDLOG_GPSTIME, 0); + #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ @@ -163,6 +176,7 @@ static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; +static bool _gpstime_only = false; #define MOUNTPOINT "/fs/microsd" static const char *mountpoint = MOUNTPOINT; @@ -272,6 +286,11 @@ static void handle_status(struct vehicle_status_s *cmd); */ static int create_log_dir(void); +/** + * Get the time struct from the currently preferred time source + */ +static bool get_log_time_utc_tt(struct tm *tt, bool boot_time); + /** * Select first free log file name and open it. */ @@ -372,20 +391,41 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } +bool get_log_time_utc_tt(struct tm *tt, bool boot_time) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ + time_t utc_time_sec; + + if (_gpstime_only) { + utc_time_sec = gps_time / 1e6; + } else { + utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + } + if (utc_time_sec > PX4_EPOCH_SECS) { + + /* strip the time elapsed since boot */ + if (boot_time) { + utc_time_sec -= hrt_absolute_time() / 1e6; + } + + struct tm *ttp = gmtime_r(&utc_time_sec, tt); + return (ttp != NULL); + } else { + return false; + } +} + int create_log_dir() { /* create dir on sdcard if needed */ uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, true); - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); @@ -434,15 +474,11 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, false); /* start logging if we have a valid time and the time is not in the past */ - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); @@ -487,14 +523,10 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */ - time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; - struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + bool time_ok = get_log_time_utc_tt(&tt, false); - if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + if (log_name_timestamp && time_ok) { strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); @@ -966,6 +998,22 @@ int sdlog2_thread_main(int argc, char *argv[]) } + param_t log_gpstime_ph = param_find("SDLOG_GPSTIME"); + + if (log_gpstime_ph != PARAM_INVALID) { + + int32_t param_log_gpstime; + param_get(log_gpstime_ph, ¶m_log_gpstime); + + if (param_log_gpstime > 0) { + _gpstime_only = true; + } else if (param_log_gpstime == 0) { + _gpstime_only = false; + } + /* any other value means to ignore the parameter, so no else case */ + + } + if (check_free_space() != OK) { errx(1, "ERR: MicroSD almost full"); @@ -1207,7 +1255,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_utc_usec; + gps_time = buf_gps_pos.time_utc_usec / 1e6; } } @@ -1235,7 +1283,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_utc_usec; + gps_time = buf_gps_pos.time_utc_usec / 1e6; } if (!logging_enabled) { @@ -1876,6 +1924,7 @@ int sdlog2_thread_main(int argc, char *argv[]) void sdlog2_status() { warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF"); + warnx("time: gps: %u seconds", (unsigned)gps_time); if (!logging_enabled) { warnx("not logging"); } else {