fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into stable

Browse Source
This commit is contained in:
Lorenz Meier
2015-05-18 11:03:34 +02:00
parent d188f8012f 4345204064
commit 045a598a97
36 changed files with 455 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/13002_firefly6
+20
View File
@@ -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
ROMFS/px4fmu_common/init.d/4010_dji_f330
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/4011_dji_f450
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/4012_quad_x_can
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
+3 -3
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.interface
+4
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.logging
+1 -1
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.usb
+2 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
+9 -2
View File
@@ -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
launch/ardrone.launch
+8 -8
View File
@@ -6,14 +6,14 @@
</include>
<group ns="$(arg ns)">
<param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.1" />
<param name="MPC_XY_VEL_P" type="double" value="0.01" />
<param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPC_Z_VEL_P" type="double" value="0.3" />
<param name="MPC_Z_P" type="double" value="2" />
<param name="MPP_XY_P" type="double" value="1.0" />
<param name="MPP_XY_FF" type="double" value="0.1" />
<param name="MPP_XY_VEL_P" type="double" value="0.01" />
<param name="MPP_XY_VEL_I" type="double" value="0.0" />
<param name="MPP_XY_VEL_D" type="double" value="0.01" />
<param name="MPP_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPP_Z_VEL_P" type="double" value="0.3" />
<param name="MPP_Z_P" type="double" value="2" />
<param name="vehicle_model" type="string" value="ardrone" />
</group>
launch/iris.launch
+8 -8
View File
@@ -7,14 +7,14 @@
<group ns="$(arg ns)">
<param name="mixer" type="string" value="i" />
<param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.0" />
<param name="MPC_XY_VEL_P" type="double" value="0.01" />
<param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPC_Z_VEL_P" type="double" value="0.3" />
<param name="MPC_Z_P" type="double" value="2" />
<param name="MPP_XY_P" type="double" value="1.0" />
<param name="MPP_XY_FF" type="double" value="0.0" />
<param name="MPP_XY_VEL_P" type="double" value="0.01" />
<param name="MPP_XY_VEL_I" type="double" value="0.0" />
<param name="MPP_XY_VEL_D" type="double" value="0.01" />
<param name="MPP_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPP_Z_VEL_P" type="double" value="0.3" />
<param name="MPP_Z_P" type="double" value="2" />
<param name="vehicle_model" type="string" value="iris" />
</group>
msg/vehicle_status.msg
+2
View File
@@ -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
nuttx-configs/px4-stm32f4discovery/include/board.h
+15 -3
View File
@@ -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 */
nuttx-configs/px4-stm32f4discovery/nsh/defconfig
+25 -10
View File
@@ -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
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -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"
src/drivers/boards/px4-stm32f4discovery/board_config.h
+3 -2
View File
@@ -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)
src/lib/ecl/attitude_fw/ecl_controller.cpp
+2 -2
View File
@@ -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;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+1 -12
View File
@@ -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 */
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
-14
View File
@@ -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]));
}
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+64 -4
View File
@@ -41,10 +41,70 @@
#include <systemlib/param/param.h>
/**
* 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);
src/modules/commander/commander.cpp
+8 -4
View File
@@ -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 {
src/modules/commander/commander_helper.cpp
+4
View File
@@ -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;
src/modules/commander/commander_helper.h
+2
View File
@@ -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_ */
src/modules/commander/mag_calibration.cpp
+3 -2
View File
@@ -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 */
src/modules/mavlink/mavlink_ftp.cpp
+46 -15
View File
@@ -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;
src/modules/mavlink/mavlink_ftp.h
+2 -1
View File
@@ -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
src/modules/mavlink/mavlink_main.cpp
+9 -7
View File
@@ -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);
src/modules/mavlink/mavlink_messages.cpp
+21
View File
@@ -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);
}
}
};
src/modules/mavlink/mavlink_parameters.cpp
+2 -2
View File
@@ -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()) {
src/modules/mavlink/mavlink_receiver.cpp
+4 -4
View File
@@ -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, &param);
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);
src/modules/position_estimator_inav/position_estimator_inav_params.c
+1 -1
View File
@@ -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
src/modules/sdlog2/sdlog2.c
+77 -24
View File
@@ -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 <log rate>] [-b <buffer size>] -e -a -t -x\n"
errx(1, "usage: sdlog2 {start|stop|status|on|off} [-r <log rate>] [-b <buffer size>] -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, &param_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.
src/modules/systemlib/param/param.c
+93 -64
View File
@@ -44,6 +44,7 @@
#include <debug.h>
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
@@ -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 = &param_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, &param_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;
}
src/modules/systemlib/param/param.h
+4 -4
View File
@@ -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.