diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 5e4028bbb8..40a13b5d17 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index bbe3b7e288..9b664d63ed 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index d5782a89b8..7b9f41bf6d 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -52,11 +52,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 1c85e04f2a..0cc07ad343 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 8c5e4b6a8f..344d784224 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 4f843e9aa1..abe378b22f 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,46 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] then - echo "Using FMU_RET mixer from sd card" + echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix else - echo "Using standard FMU_RET mixer" + echo "Using /etc/mixers/FMU_RET.mix" mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index cef86c34d0..c616da9886 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,39 +59,27 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index 81ea292aae..467b56bbf9 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,10 +59,7 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -83,7 +80,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals +# Set disarmed, min and max PWM signals (for DJI ESCs) # pwm disarmed -c 1234 -p 900 pwm min -c 1234 -p 1200 diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 4dbf76cee7..818f9375e4 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200 pwm max -c 1234 -p 1800 # -# Start common for all multirotors apps +# Start common multirotor apps # sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex index 0f0bb05ced..f83f6cfd05 100644 --- a/ROMFS/px4fmu_common/init.d/12-13_hex +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -61,10 +61,6 @@ then usleep 5000 sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 900 900 - px4io min 1200 1200 1200 1200 1200 1200 - px4io max 1900 1900 1900 1900 1900 1900 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -77,12 +73,19 @@ fi # # Load mixer # -mixer load /dev/pwm_output $MIXER +mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix # # Set PWM output frequency to 400 Hz # -pwm -u 400 -m 0xff +pwm rate -c 123456 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 123456 -p 900 +pwm min -c 123456 -p 1100 +pwm max -c 123456 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index bd6189a6d3..c79e9d2830 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) @@ -66,11 +64,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix pwm rate -c 1234 -r 400 # -# Set disarmed, min and max PWM signals (for DJI ESCs) +# Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index d8cc0e9132..f6b071cf1b 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -31,7 +31,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - + set EXIT_ON_END no # @@ -43,8 +43,6 @@ then mavlink start usleep 5000 - commander start - sh /etc/init.d/rc.io else # Start MAVLink (on UART1 / ttyS0) @@ -69,8 +67,7 @@ pwm rate -c 1234 -r 400 # Set disarmed, min and max PWM signals # pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 +pwm min -c 1234 -p 1050 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 191d8cd95f..8a8bc15903 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,41 +37,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 6528337454..63cd7f9b2e 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -52,8 +52,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -61,41 +59,29 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fw_att_control start -fw_pos_control_l1 start +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then exit -fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 index cd7677112d..1e752f13a2 100644 --- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -30,8 +30,6 @@ then # Start MAVLink (depends on orb) mavlink start - commander start - sh /etc/init.d/rc.io # Limit to 100 Hz updates and (implicit) 50 Hz PWM px4io limit 100 @@ -39,32 +37,10 @@ else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 - commander start - fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) @@ -77,8 +53,11 @@ else echo "Using /etc/mixers/FMU_Q.mix" mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fi -fw_att_control start -fw_pos_control_l1 start + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing if [ $EXIT_ON_END == yes ] then diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index eae37098bd..1ee84b9b01 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -1,6 +1,6 @@ #!nsh -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with PWM outputs" +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" # # Load default params for this platform @@ -33,17 +33,27 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 + +set EXIT_ON_END no # -# Start PWM output +# Start and configure PX4IO or FMU interface # -fmu mode_pwm +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi # # Load mixer @@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # pwm rate -c 1234 -r 400 +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 + # # Start common for all multirotors apps # sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index a63d0e5f1d..8e0914d634 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -84,10 +84,10 @@ then sh /etc/init.d/rc.io else - fmu mode_pwm # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 usleep 5000 + fmu mode_pwm param set BAT_V_SCALING 0.004593 set EXIT_ON_END yes fi @@ -102,13 +102,6 @@ else mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix fi -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - # # Start common for all multirotors apps # diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc index 0c0cfa53d3..045e41e529 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -60,9 +60,7 @@ then # Start MAVLink (depends on orb) mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 - - commander start - + sh /etc/init.d/rc.io else fmu mode_pwm diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing new file mode 100644 index 0000000000..f028510064 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -0,0 +1,34 @@ +#!nsh +# +# Standard everything needed for fixedwing except mixer, actuator output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index dfff07198a..bc550ac5a1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -1,6 +1,6 @@ #!nsh # -# Standard everything needed for multirotors except mixer, output and mavlink +# Standard everything needed for multirotors except mixer, actuator output and mavlink # # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6e954f58a3..2592072764 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -105,6 +105,11 @@ then blinkm systemstate fi fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start if param compare SYS_AUTOSTART 1000 then diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0487848133..62c0d1f17c 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - uint16_t _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 964f5069c6..4b12b75f9e 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void) * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. * Keep the SPI2 init optional and conditionally initialize the ADC pins */ - spi2 = up_spiinitialize(2); - if (!spi2) { - message("[boot] Enabling IN12/13 instead of SPI2\n"); - /* no SPI2, use pins for ADC */ - stm32_configgpio(GPIO_ADC1_IN12); - stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - } else { + #ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); /* Default SPI2 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); SPI_SETBITS(spi2, 8); @@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - } + #else + spi2 = NULL; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + #endif /* Get the SPI port for the microSD slot */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9d8ad084e5..de371bf324 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -180,7 +180,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = diff_pres_pa; + report.differential_pressure_pa = (float)diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b3003fc1b0..3cd6d67203 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -36,6 +36,7 @@ * @author Lorenz Meier * @author Sarthak Kaingade * @author Simon Wilks + * @author Thomas Gubler * * Driver for the MEAS Spec series connected via I2C. * @@ -76,6 +77,7 @@ #include #include #include +#include #include #include @@ -184,7 +186,7 @@ MEASAirspeed::collect() //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; + dp_raw = 0x3FFF & dp_raw; //mask the used bits dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; @@ -193,7 +195,11 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value - uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); +// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + const float P_min = -1.0f; + const float P_max = 1.0f; + float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39b..2eb58abd66 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - - float dt = dt_micros / 1000000; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) @@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc _rate_error = _rate_setpoint - pitch_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc029..0b1ffa7a4d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_error = _rate_setpoint - roll_rate; - float ilimit_scaled = 0.0f; + float ilimit_scaled = _integrator_max * scaler; if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 33879892ee..18fb6dcbc9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -34,7 +34,7 @@ /** * @file KalmanNav.cpp * - * kalman filter navigation code + * Kalman filter navigation code */ #include @@ -228,10 +228,7 @@ void KalmanNav::update() updateSubscriptions(); // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate && - _sensors.accelerometer_counter > 10 && - _sensors.gyro_counter > 10 && - _sensors.magnetometer_counter > 10) { + if (!_attitudeInitialized && sensorsUpdate) { if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { @@ -643,7 +640,7 @@ int KalmanNav::correctAtt() if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:\n"); y.print(); + warnx("y:"); y.print(); } // update quaternions from euler diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66e..1809f96888 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -37,12 +37,15 @@ */ #include "airspeed_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include +#include #include #include #include +#include #include #include #include @@ -55,10 +58,13 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "dpress"; + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); const int calibration_count = 2500; @@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd) int calibration_counter = 0; float diff_pres_offset = 0.0f; + /* Reset sensor parameters */ + struct airspeed_scale airscale = { + 0.0f, + 1.0f, + }; + + bool paramreset_successful = false; + int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { + if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + paramreset_successful = true; + } + close(fd); + } + + if (!paramreset_successful) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } @@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } @@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } - mavlink_log_info(mavlink_fd, "airspeed calibration done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(); close(diff_pres_sub); return OK; } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271ca..ace13bb784 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -116,6 +116,8 @@ extern struct system_load_s system_load; #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define MAVLINK_OPEN_INTERVAL 50000 + #define STICK_ON_OFF_LIMIT 0.75f #define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 @@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - if (mavlink_fd < 0) { - /* try again later */ - usleep(20000); - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); - } - } - /* Main state machine */ /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); @@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { + if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* update parameters */ orb_check(param_changed_sub, &updated); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index eb67874db4..00a0dcd619 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + if (isfinite(roll_sp) && isfinite(pitch_sp)) { - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); - rates_sp.timestamp = hrt_absolute_time(); + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + rates_sp.timestamp = hrt_absolute_time(); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } } } else { diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index d37b18a195..fa23f996f3 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -64,6 +64,7 @@ #include #include +#include "geo/geo.h" #include "waypoints.h" #include "orb_topics.h" #include "missionlib.h" diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index f6860930c4..abc91d34fa 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -75,6 +75,7 @@ struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_effective_0; struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; +struct airspeed_s airspeed; struct mavlink_subscriptions mavlink_subs; @@ -92,6 +93,8 @@ static unsigned int gps_counter; */ static uint64_t last_sensor_timestamp; +static hrt_abstime last_sent_vfr = 0; + static void *uorb_receive_thread(void *arg); struct listener { @@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l) /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); - if (gcs_link) + if (gcs_link) { /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, @@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l) att.pitchspeed, att.yawspeed); + /* limit VFR message rate to 10Hz */ + hrt_abstime t = hrt_absolute_time(); + if (t >= last_sent_vfr + 100000) { + last_sent_vfr = t; + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + } + } + attitude_counter++; } @@ -681,17 +695,7 @@ l_home(const struct listener *l) void l_airspeed(const struct listener *l) { - struct airspeed_s airspeed; - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); - - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.relative_alt; - float climb = -global_pos.vz; - - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } void @@ -849,7 +853,7 @@ uorb_receive_start(void) mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ - /* --- AIRSPEED / VFR / HUD --- */ + /* --- AIRSPEED --- */ mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index adb63186c0..86ac0e4ff0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,31 +59,23 @@ #include #include -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; float yawrate_d; float yawrate_i; - //float yawrate_awu; - //float yawrate_lim; float attrate_p; float attrate_d; float attrate_i; - //float attrate_awu; - //float attrate_lim; float rate_lim; }; @@ -93,14 +85,10 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_d; - //param_t yawrate_awu; - //param_t yawrate_lim; param_t attrate_p; param_t attrate_i; param_t attrate_d; - //param_t attrate_awu; - //param_t attrate_lim; }; /** @@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h) h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_d = param_find("MC_YAWRATE_D"); - //h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - //h->yawrate_lim = param_find("MC_YAWRATE_LIM"); h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_d = param_find("MC_ATTRATE_D"); - //h->attrate_awu = param_find("MC_ATTRATE_AWU"); - //h->attrate_lim = param_find("MC_ATTRATE_LIM"); return OK; } @@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_d, &(p->yawrate_d)); - //param_get(h->yawrate_awu, &(p->yawrate_awu)); - //param_get(h->yawrate_lim, &(p->yawrate_lim)); param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_d, &(p->attrate_d)); - //param_get(h->attrate_awu, &(p->attrate_awu)); - //param_get(h->attrate_lim, &(p->attrate_lim)); return OK; } @@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static PID_t pitch_rate_controller; static PID_t roll_rate_controller; + static PID_t yaw_rate_controller; static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } /* load new parameters with lower rate */ @@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); } /* reset integrals if needed */ if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); - // TODO pid_reset_integral(&yaw_rate_controller); + pid_reset_integral(&yaw_rate_controller); } - /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); - - /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); - - /* control yaw rate */ //XXX use library here - float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); - - /* increase resilience to faulty control inputs */ - if (!isfinite(yaw_rate_control)) { - yaw_rate_control = 0.0f; - warnx("rej. NaN ctrl yaw"); - } + /* run pitch, roll and yaw controllers */ + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; + actuators->control[2] = yaw_control; actuators->control[3] = rate_sp->thrust; motor_skip_counter++; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 36dd370fb2..3d23d0c096 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } - att_sp.yaw_body = global_pos_sp.yaw; + /* update yaw setpoint only if value is valid */ + if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { + att_sp.yaw_body = global_pos_sp.yaw; + } mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..6d38b98ec1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) warn("WARNING: failed to set scale / offsets for airspeed sensor"); + close(fd); } #if 0 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60ff..1a479c2058 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -59,7 +59,7 @@ float calc_indicated_airspeed(float differential_pressure) { - if (differential_pressure > 0) { + if (differential_pressure > 0.0f) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ceb..5d94d42883 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,8 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - uint16_t differential_pressure_pa; /**< Differential pressure reading */ - uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 09a9344008..898c04cd57 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -395,6 +395,17 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); + /* get current servo values */ + struct pwm_output_values last_spos; + + for (unsigned i = 0; i < servo_count; i++) { + + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_GET(%d)", i); + } + /* perform PWM output */ /* Open console directly to grab CTRL-C signal */ @@ -420,6 +431,14 @@ pwm_main(int argc, char *argv[]) read(0, &c, 1); if (c == 0x03 || c == 0x63 || c == 'q') { + /* reset output to the last value */ + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<