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
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
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
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index e456eff7f1..0442637941 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,9 +3,9 @@
# 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 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
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
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 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
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/nuttx-configs/px4-stm32f4discovery/include/board.h b/nuttx-configs/px4-stm32f4discovery/include/board.h
index 0412c3aa6d..95feca5506 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 __CONFIG_DISCOVERY_INCLUDE_BOARD_H
+#define __CONFIG_DISCOVERY_INCLUDE_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 /* __CONFIG_DISCOVERY_INCLUDE_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
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 4ccc5dacb6..ab8f9eef62 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=2048
+CONFIG_CDCACM_RXBUFSIZE=1000
+CONFIG_CDCACM_TXBUFSIZE=8000
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"
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)
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;
}
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]));
}
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);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 50846ff4d0..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;
@@ -2157,10 +2159,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);
@@ -2792,7 +2796,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 {
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/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 */
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 10b94b6b26..a6c92ac623 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,36 @@ 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;
+#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;
+ *((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 +338,20 @@ 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;
+#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';
+ offset++;
+ payload->size = offset;
+ closedir(dp);
+
+ return errorCode;
}
// no more entries?
@@ -357,7 +381,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 {
@@ -491,6 +516,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 +898,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 +917,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
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 326b0b5ab4..e5cddfafb6 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.
@@ -249,7 +249,9 @@ Mavlink::~Mavlink()
} while (_task_running);
}
- LL_DELETE(_mavlink_instances, this);
+ if (_mavlink_instances) {
+ LL_DELETE(_mavlink_instances, this);
+ }
}
void
@@ -988,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;
}
@@ -999,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 */
@@ -1373,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(80.0f));
LL_APPEND(_streams, _mavlink_ftp);
/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
@@ -1423,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);
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);
}
}
};
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()) {
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0cd163a0c9..40adc6b054 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;
@@ -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;
@@ -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);
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
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c76569cf8f..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.
*/
@@ -286,7 +305,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 +368,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 +379,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;
@@ -368,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);
@@ -430,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);
@@ -483,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);
@@ -962,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");
@@ -1203,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;
}
}
@@ -1231,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) {
@@ -1872,8 +1924,9 @@ 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("standing by");
+ warnx("not logging");
} else {
float kibibytes = log_bytes_written / 1024.0f;
@@ -1975,7 +2028,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.
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 4ec885ab3e..bca92dc6bc 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
@@ -71,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))
@@ -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 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 */
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());
}
/**
@@ -154,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;
}
@@ -171,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();
@@ -184,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
@@ -222,6 +242,7 @@ param_find_internal(const char *name, bool notification)
if (notification) {
param_set_used_internal(param);
}
+
return param;
}
}
@@ -245,27 +266,31 @@ 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++;
}
}
}
+
return count;
}
param_t
param_for_index(unsigned index)
{
- if (index < param_info_count) {
+ if (index < get_param_info_count()) {
return (param_t)index;
}
@@ -275,12 +300,12 @@ 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;
- 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)) {
@@ -313,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 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;
}
@@ -341,10 +365,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
@@ -357,29 +378,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;
@@ -424,8 +438,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 {
@@ -463,8 +478,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");
@@ -494,6 +510,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;
@@ -525,16 +542,15 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
}
out:
- param_set_used_internal(param);
-
param_unlock();
/*
* 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;
}
@@ -555,23 +571,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 / sizeof(param_changed_storage[0]));
- return param_changed_storage[param_index / sizeof(param_changed_storage[0])] & (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 / sizeof(param_changed_storage[0]));
- param_changed_storage[param_index / sizeof(param_changed_storage[0])] |= (1 << bitindex);
+ param_changed_storage[param_index / bits_per_allocation_unit] |=
+ (1 << param_index % bits_per_allocation_unit);
}
int
@@ -598,8 +616,9 @@ param_reset(param_t param)
param_unlock();
- if (s != NULL)
+ if (s != NULL) {
param_notify_changes();
+ }
return (!param_found);
}
@@ -622,28 +641,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);
}
}
@@ -657,14 +676,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;
}
@@ -715,6 +737,7 @@ param_load_default(void)
warn("open '%s' for reading failed", param_get_default_file());
return -1;
}
+
return 1;
}
@@ -755,13 +778,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);
@@ -805,8 +831,9 @@ param_export(int fd, bool only_unsaved)
out:
param_unlock();
- if (result == 0)
+ if (result == 0) {
result = bson_encoder_fini(&encoder);
+ }
return result;
}
@@ -916,8 +943,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;
}
@@ -943,8 +971,9 @@ param_import_internal(int fd, bool mark_saved)
out:
- if (result < 0)
+ if (result < 0) {
debug("BSON error decoding parameters");
+ }
return result;
}
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.