fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 06:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into inav_alt_gps

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-11 14:48:50 +04:00
parent c3944f49b1 a1398c991e
commit e4f0c7d26a
41 changed files with 313 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
-5
View File
@@ -55,11 +55,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #
ROMFS/px4fmu_common/init.d/1001_rc_quad.hil
-5
View File
@@ -60,11 +60,6 @@ hil mode_pwm
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
-5
View File
@@ -52,11 +52,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
-5
View File
@@ -60,11 +60,6 @@ hil mode_pwm
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
-5
View File
@@ -55,11 +55,6 @@ hil mode_pwm
# #
param set MAV_TYPE 1 param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
# #
# Check if we got an IO # Check if we got an IO
# #
ROMFS/px4fmu_common/init.d/100_mpx_easystar
+7 -28
View File
@@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@@ -61,46 +59,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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) # Load mixer and start controllers (depends on px4io)
# #
if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
then 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 mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
else else
echo "Using standard FMU_RET mixer" echo "Using /etc/mixers/FMU_RET.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fi 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 ] if [ $EXIT_ON_END == yes ]
then then
ROMFS/px4fmu_common/init.d/101_hk_bixler
+13 -27
View File
@@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@@ -61,39 +59,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
fw_att_control start then
fw_pos_control_l1 start 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 ] if [ $EXIT_ON_END == yes ]
then then
ROMFS/px4fmu_common/init.d/10_dji_f330
+1 -4
View File
@@ -59,10 +59,7 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/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 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 disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200 pwm min -c 1234 -p 1200
ROMFS/px4fmu_common/init.d/11_dji_f450
+1 -1
View File
@@ -73,7 +73,7 @@ pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800 pwm max -c 1234 -p 1800
# #
# Start common for all multirotors apps # Start common multirotor apps
# #
sh /etc/init.d/rc.multirotor sh /etc/init.d/rc.multirotor
ROMFS/px4fmu_common/init.d/12-13_hex
+9 -6
View File
@@ -61,10 +61,6 @@ then
usleep 5000 usleep 5000
sh /etc/init.d/rc.io 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 else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
@@ -77,12 +73,19 @@ fi
# #
# Load mixer # 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 # 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 # Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/15_tbs_discovery
+3 -5
View File
@@ -43,8 +43,6 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
# Start MAVLink (on UART1 / ttyS0) # 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 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 disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200 pwm min -c 1234 -p 1100
pwm max -c 1234 -p 1800 pwm max -c 1234 -p 1900
# #
# Start common for all multirotors apps # Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/16_3dr_iris
-2
View File
@@ -43,8 +43,6 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
ROMFS/px4fmu_common/init.d/30_io_camflyer
+13 -27
View File
@@ -30,8 +30,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@@ -39,39 +37,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
fw_att_control start then
fw_pos_control_l1 start 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 ] if [ $EXIT_ON_END == yes ]
then then
ROMFS/px4fmu_common/init.d/31_io_phantom
+13 -27
View File
@@ -52,8 +52,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@@ -61,39 +59,27 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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) # Load mixer and start controllers (depends on px4io)
# #
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
fw_att_control start then
fw_pos_control_l1 start 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 ] if [ $EXIT_ON_END == yes ]
then then
ROMFS/px4fmu_common/init.d/32_skywalker_x5
+5 -26
View File
@@ -30,8 +30,6 @@ then
# Start MAVLink (depends on orb) # Start MAVLink (depends on orb)
mavlink start mavlink start
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM # Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100 px4io limit 100
@@ -39,33 +37,11 @@ else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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) # Load mixer and start controllers (depends on px4io)
# #
@@ -77,8 +53,11 @@ else
echo "Using /etc/mixers/FMU_Q.mix" echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi 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 ] if [ $EXIT_ON_END == yes ]
then then
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+29 -10
View File
@@ -1,6 +1,6 @@
#!nsh #!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 # Load default params for this platform
@@ -34,16 +34,26 @@ fi
# #
param set MAV_TYPE 2 param set MAV_TYPE 2
# set EXIT_ON_END no
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
# #
# 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 # Load mixer
@@ -55,10 +65,19 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
# #
pwm rate -c 1234 -r 400 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 # Start common for all multirotors apps
# #
sh /etc/init.d/rc.multirotor sh /etc/init.d/rc.multirotor
# Exit, because /dev/ttyS0 is needed for MAVLink if [ $EXIT_ON_END == yes ]
exit then
exit
fi
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+1 -1
View File
@@ -84,10 +84,10 @@ then
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0 mavlink start -d /dev/ttyS0
usleep 5000 usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593 param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes set EXIT_ON_END yes
fi fi
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
-2
View File
@@ -61,8 +61,6 @@ then
mavlink start -d /dev/ttyS1 -b 57600 mavlink start -d /dev/ttyS1 -b 57600
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
else else
fmu mode_pwm fmu mode_pwm
ROMFS/px4fmu_common/init.d/rc.fixedwing
+34
View File
@@ -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
ROMFS/px4fmu_common/init.d/rc.multirotor
+1 -1
View File
@@ -1,6 +1,6 @@
#!nsh #!nsh
# #
# Standard everything needed for multirotors except mixer, output and mavlink # Standard everything needed for multirotors except mixer, actuator output and mavlink
# #
# #
ROMFS/px4fmu_common/init.d/rcS
+5
View File
@@ -106,6 +106,11 @@ then
fi fi
fi fi
#
# Start the Commander (needs to be this early for in-air-restarts)
#
commander start
if param compare SYS_AUTOSTART 1000 if param compare SYS_AUTOSTART 1000
then then
sh /etc/init.d/1000_rc_fw_easystar.hil sh /etc/init.d/1000_rc_fw_easystar.hil
src/drivers/airspeed/airspeed.h
+1 -1
View File
@@ -119,7 +119,7 @@ protected:
virtual int collect() = 0; virtual int collect() = 0;
work_s _work; work_s _work;
uint16_t _max_differential_pressure_pa; float _max_differential_pressure_pa;
bool _sensor_ok; bool _sensor_ok;
int _measure_ticks; int _measure_ticks;
bool _collect_phase; bool _collect_phase;
src/drivers/boards/px4fmu-v1/px4fmu_init.c
+9 -8
View File
@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. * 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 * Keep the SPI2 init optional and conditionally initialize the ADC pins
*/ */
spi2 = up_spiinitialize(2);
if (!spi2) { #ifdef CONFIG_STM32_SPI2
message("[boot] Enabling IN12/13 instead of SPI2\n"); spi2 = up_spiinitialize(2);
/* no SPI2, use pins for ADC */
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
} else {
/* Default SPI2 to 1MHz and de-assert the known chip selects. */ /* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000); SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8); SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); 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 */ /* Get the SPI port for the microSD slot */
src/drivers/ets_airspeed/ets_airspeed.cpp
+1 -1
View File
@@ -180,7 +180,7 @@ ETSAirspeed::collect()
differential_pressure_s report; differential_pressure_s report;
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors); 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.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa; report.max_differential_pressure_pa = _max_differential_pressure_pa;
src/drivers/meas_airspeed/meas_airspeed.cpp
+8 -2
View File
@@ -36,6 +36,7 @@
* @author Lorenz Meier * @author Lorenz Meier
* @author Sarthak Kaingade * @author Sarthak Kaingade
* @author Simon Wilks * @author Simon Wilks
* @author Thomas Gubler
* *
* Driver for the MEAS Spec series connected via I2C. * Driver for the MEAS Spec series connected via I2C.
* *
@@ -76,6 +77,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@@ -184,7 +186,7 @@ MEASAirspeed::collect()
//diff_pres_pa -= _diff_pres_offset; //diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0; int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1]; 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 = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5; dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50; float temperature = ((200 * dT_raw) / 2047) - 50;
@@ -193,7 +195,11 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000 // Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value // 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; struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed). // Track maximum differential pressure measured (so we can work out top speed).
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+2 -3
View File
@@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
float dt = dt_micros / 1000000;
/* lock integral for long intervals */ /* lock integral for long intervals */
if (dt_micros > 500000) 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; _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) { if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+5 -2
View File
@@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _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_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc; 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; _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) { if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
src/lib/external_lgpl/tecs/tecs.h
+3 -1
View File
@@ -54,7 +54,9 @@ public:
_SPE_est(0.0f), _SPE_est(0.0f),
_SKE_est(0.0f), _SKE_est(0.0f),
_SPEdot(0.0f), _SPEdot(0.0f),
_SKEdot(0.0f) { _SKEdot(0.0f),
_vel_dot(0.0f),
_STEdotErrLast(0.0f) {
} }
src/lib/mathlib/math/arm/Matrix.hpp
+3 -3
View File
@@ -121,10 +121,10 @@ public:
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
for (size_t j = 0; j < getCols(); j++) { for (size_t j = 0; j < getCols(); j++) {
float sig; float sig;
int exp; int exponent;
float num = (*this)(i, j); float num = (*this)(i, j);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exponent);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03d ", (double)sig, exponent);
} }
printf("\n"); printf("\n");
src/lib/mathlib/math/arm/Vector.hpp
+3 -3
View File
@@ -109,10 +109,10 @@ public:
inline void print() const { inline void print() const {
for (size_t i = 0; i < getRows(); i++) { for (size_t i = 0; i < getRows(); i++) {
float sig; float sig;
int exp; int exponent;
float num = (*this)(i); float num = (*this)(i);
float2SigExp(num, sig, exp); float2SigExp(num, sig, exponent);
printf("%6.3fe%03.3d,", (double)sig, exp); printf("%6.3fe%03d ", (double)sig, exponent);
} }
printf("\n"); printf("\n");
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+3 -6
View File
@@ -34,7 +34,7 @@
/** /**
* @file KalmanNav.cpp * @file KalmanNav.cpp
* *
* kalman filter navigation code * Kalman filter navigation code
*/ */
#include <poll.h> #include <poll.h>
@@ -228,10 +228,7 @@ void KalmanNav::update()
updateSubscriptions(); updateSubscriptions();
// initialize attitude when sensors online // initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate && if (!_attitudeInitialized && sensorsUpdate) {
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++; if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) { if (_attitudeInitCounter > 100) {
@@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
if (beta > _faultAtt.get()) { if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta); warnx("fault in attitude: beta = %8.4f", (double)beta);
warnx("y:\n"); y.print(); warnx("y:"); y.print();
} }
// update quaternions from euler // update quaternions from euler
src/modules/commander/airspeed_calibration.cpp
+38 -6
View File
@@ -37,12 +37,15 @@
*/ */
#include "airspeed_calibration.h" #include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "commander_helper.h" #include "commander_helper.h"
#include <stdio.h> #include <stdio.h>
#include <fcntl.h>
#include <poll.h> #include <poll.h>
#include <math.h> #include <math.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
static const char *sensor_name = "dpress";
int do_airspeed_calibration(int mavlink_fd) int do_airspeed_calibration(int mavlink_fd)
{ {
/* give directions */ /* 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; const int calibration_count = 2500;
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
int calibration_counter = 0; int calibration_counter = 0;
float diff_pres_offset = 0.0f; 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) { while (calibration_counter < calibration_count) {
/* wait blocking for new data */ /* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
diff_pres_offset += diff_pres.differential_pressure_pa; diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++; 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) { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* 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); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) { if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(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); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) { if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed"); 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); close(diff_pres_sub);
return ERROR; 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); close(diff_pres_sub);
return OK; return OK;
} else { } else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
src/modules/commander/commander.cpp
+7 -10
View File
@@ -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 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 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_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f #define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #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); 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 */ /* Main state machine */
/* make sure we are in preflight state */ /* make sure we are in preflight state */
memset(&status, 0, sizeof(status)); memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) { 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 */ /* update parameters */
orb_check(param_changed_sub, &updated); orb_check(param_changed_sub, &updated);
src/modules/commander/mag_calibration.cpp
+19 -10
View File
@@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd)
/* maximum 500 values */ /* maximum 500 values */
const unsigned int calibration_maxcount = 500; const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter = 0; unsigned int calibration_counter;
struct mag_scale mscale_null = { struct mag_scale mscale_null = {
0.0f, 0.0f,
@@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd)
res = ioctl(fd, MAGIOCCALIBRATE, fd); res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) { if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
} }
} }
close(fd); close(fd);
float *x; float *x = NULL;
float *y; float *y = NULL;
float *z; float *z = NULL;
if (res == OK) { if (res == OK) {
/* allocate memory */ /* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
x = (float *)malloc(sizeof(float) * calibration_maxcount); x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = (float *)malloc(sizeof(float) * calibration_maxcount); y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = (float *)malloc(sizeof(float) * calibration_maxcount); z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) { if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR; res = ERROR;
return res;
} }
} else {
/* exit */
return ERROR;
} }
if (res == OK) { if (res == OK) {
@@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
calibration_counter = 0;
while (hrt_absolute_time() < calibration_deadline && while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) { calibration_counter < calibration_maxcount) {
@@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd)
float sphere_radius; float sphere_radius;
if (res == OK) { if (res == OK) {
/* sphere fit */ /* sphere fit */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
@@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd)
} else { } else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
} }
return res;
} }
return res;
} }
src/modules/fw_att_control/fw_att_control_main.cpp
+35 -32
View File
@@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _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_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.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I"); _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.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _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_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _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, if (isfinite(roll_sp) && isfinite(pitch_sp)) {
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; _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, float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed); lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
/* throttle passed through */ float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _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", /* throttle passed through */
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
// _actuators.control[2], _actuators.control[3]);
/* // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
* Lazily publish the rate setpoint (for analysis, the actuators are published below) // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
* only once available // _actuators.control[2], _actuators.control[3]);
*/
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
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) { rates_sp.timestamp = hrt_absolute_time();
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else { if (_rate_sp_pub > 0) {
/* advertise and publish */ /* publish the attitude setpoint */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); 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 { } else {
src/modules/mavlink/missionlib.c
+1
View File
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include "geo/geo.h"
#include "waypoints.h" #include "waypoints.h"
#include "orb_topics.h" #include "orb_topics.h"
#include "missionlib.h" #include "missionlib.h"
src/modules/sensors/sensors.cpp
+1
View File
@@ -1129,6 +1129,7 @@ Sensors::parameter_update_poll(bool forced)
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor"); warn("WARNING: failed to set scale / offsets for airspeed sensor");
close(fd);
} }
#if 0 #if 0
src/modules/systemlib/airspeed.c
+1 -1
View File
@@ -59,7 +59,7 @@
float calc_indicated_airspeed(float differential_pressure) 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); return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else { } else {
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
src/modules/uORB/topics/differential_pressure.h
+2 -2
View File
@@ -54,8 +54,8 @@
struct differential_pressure_s { struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count; uint64_t error_count;
uint16_t differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor */ float temperature; /**< Temperature provided by sensor */
src/modules/uORB/uORB.cpp
+3 -1
View File
@@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
} else { } else {
SubscriberData *sd = filp_to_sd(filp); SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) if (sd != nullptr) {
hrt_cancel(&sd->update_call);
delete sd; delete sd;
}
} }
return CDev::close(filp); return CDev::close(filp);
src/systemcmds/preflight_check/preflight_check.c
+26 -2
View File
@@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[])
/* ---- ACCEL ---- */ /* ---- ACCEL ---- */
close(fd); close(fd);
fd = open(ACCEL_DEVICE_PATH, 0); fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
ret = ioctl(fd, ACCELIOCSELFTEST, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) { if (ret != OK) {
@@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[])
goto system_eval; goto system_eval;
} }
/* check measurement result range */
struct accel_report acc;
ret = read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
warnx("accel with spurious values");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
/* this is frickin' fatal */
fail_on_error = true;
system_ok = false;
goto system_eval;
}
} else {
warnx("accel read failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
/* this is frickin' fatal */
fail_on_error = true;
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */ /* ---- GYRO ---- */
close(fd); close(fd);
@@ -193,9 +216,10 @@ system_eval:
/* stop alarm */ /* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
/* switch on leds */ /* switch off leds */
led_on(leds, LED_BLUE); led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER); led_on(leds, LED_AMBER);
close(leds);
if (fail_on_error) { if (fail_on_error) {
/* exit with error message */ /* exit with error message */