Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
tumbili
2014-12-15 22:34:01 +01:00
parent 51a7fbeee0 aa40c69853
commit 736f57f436
93 changed files with 3081 additions and 3721 deletions
Download Patch File Download Diff File Expand all files Collapse all files
NuttX
+1 -1
Submodule NuttX updated: ae4b05e2c5...c7b0638592
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+2 -2
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO review MC_YAWRATE_I
param set MC_ROLL_P 8.0
@@ -26,5 +26,5 @@ fi
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_OUT 1234
set PWM_MIN 1200
ROMFS/px4fmu_common/init.d/10016_3dr_iris
+2 -2
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
@@ -29,7 +29,7 @@ fi
set MIXER FMU_quad_w
set PWM_OUTPUTS 1234
set PWM_OUT 1234
set PWM_MIN 1200
set PWM_MAX 1950
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+2 -2
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.mc_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
@@ -31,4 +31,4 @@ set MIXER FMU_quad_w
set PWM_MIN 1210
set PWM_MAX 2100
set PWM_OUTPUTS 1234
set PWM_OUT 1234
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+1 -1
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MIN 12
param set FW_AIRSPD_TRIM 25
ROMFS/px4fmu_common/init.d/11001_hexa_cox
+1 -1
View File
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_cox
# Need to set all 8 channels
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/12001_octo_cox
+1 -1
View File
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_cox
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
+1 -1
View File
@@ -2,4 +2,4 @@
sh /etc/init.d/rc.fw_defaults
set MIXER FMU_AERT
set MIXER skywalker
ROMFS/px4fmu_common/init.d/3030_io_camflyer
+1 -1
View File
@@ -4,5 +4,5 @@ sh /etc/init.d/rc.fw_defaults
set MIXER FMU_Q
# Provide ESC a constant 1000 us pulse while disarmed
set PWM_OUTPUTS 4
set PWM_OUT 4
set PWM_DISARMED 1000
ROMFS/px4fmu_common/init.d/3031_phantom
+2 -2
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MIN 13
param set FW_AIRSPD_TRIM 15
@@ -36,5 +36,5 @@ fi
set MIXER phantom
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_OUT 4
set PWM_DISARMED 1000
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+1 -1
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MIN 15
param set FW_AIRSPD_TRIM 20
ROMFS/px4fmu_common/init.d/3033_wingwing
+2 -2
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set BAT_N_CELLS 2
param set FW_AIRSPD_MAX 15
@@ -44,5 +44,5 @@ fi
set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_OUT 4
set PWM_DISARMED 1000
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
+2 -2
View File
@@ -7,9 +7,9 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO: these are the X5 default parameters, update them to the caipi
param set FW_AIRSPD_MIN 15
ROMFS/px4fmu_common/init.d/4001_quad_x
+1 -1
View File
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_OUT 1234
ROMFS/px4fmu_common/init.d/4008_ardrone
+2 -2
View File
@@ -8,7 +8,7 @@ sh /etc/init.d/rc.mc_defaults
#
# Load default params for this platform
#
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# Set all params here, then disable autoconfig
param set MC_ROLL_P 6.0
@@ -24,7 +24,7 @@ then
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.8
param set BAT_V_SCALING 0.00838095238
fi
ROMFS/px4fmu_common/init.d/4010_dji_f330
+1 -1
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
ROMFS/px4fmu_common/init.d/4011_dji_f450
+1 -1
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
ROMFS/px4fmu_common/init.d/4012_quad_x_can
+1 -1
View File
@@ -7,7 +7,7 @@
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
+1 -1
View File
@@ -9,7 +9,7 @@ echo "HK Micro PCB Quad"
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
ROMFS/px4fmu_common/init.d/5001_quad_+
+1 -1
View File
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_quad_+
set PWM_OUTPUTS 1234
set PWM_OUT 1234
ROMFS/px4fmu_common/init.d/6001_hexa_x
+1 -1
View File
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_x
# Need to set all 8 channels
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/7001_hexa_+
+1 -1
View File
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_+
# Need to set all 8 channels
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/8001_octo_x
+1 -1
View File
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_x
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/9001_octo_+
+1 -1
View File
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults
set MIXER FMU_octo_+
set PWM_OUTPUTS 12345678
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/rc.fw_defaults
+2 -2
View File
@@ -2,7 +2,7 @@
set VEHICLE_TYPE fw
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
#
# Default parameters for FW
@@ -15,4 +15,4 @@ then
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
fi
ROMFS/px4fmu_common/init.d/rc.interface
+15 -14
View File
@@ -8,12 +8,11 @@ then
#
# Load mixer
#
set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
# Use the mixer file from the SD-card if it exists
if [ -f $MIXERSD ]
#Use the mixer file from the SD-card if it exists
if [ -f /fs/microsd/etc/mixers/$MIXER.mix ]
then
set MIXER_FILE $MIXERSD
set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix
else
set MIXER_FILE /etc/mixers/$MIXER.mix
fi
@@ -32,29 +31,31 @@ then
if mixer load $OUTPUT_DEV $MIXER_FILE
then
echo "[init] Mixer loaded: $MIXER_FILE"
echo "[i] Mixer: $MIXER_FILE"
else
echo "[init] Error loading mixer: $MIXER_FILE"
tone_alarm $TUNE_OUT_ERROR
echo "[i] Error loading mixer: $MIXER_FILE"
tone_alarm $TUNE_ERR
fi
unset MIXER_FILE
else
if [ $MIXER != skip ]
then
echo "[init] Mixer not defined"
tone_alarm $TUNE_OUT_ERROR
echo "[i] Mixer not defined"
tone_alarm $TUNE_ERR
fi
fi
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
then
if [ $PWM_OUTPUTS != none ]
if [ $PWM_OUT != none ]
then
#
# Set PWM output frequency
#
if [ $PWM_RATE != none ]
then
pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
pwm rate -c $PWM_OUT -r $PWM_RATE
fi
#
@@ -62,15 +63,15 @@ then
#
if [ $PWM_DISARMED != none ]
then
pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
pwm disarmed -c $PWM_OUT -p $PWM_DISARMED
fi
if [ $PWM_MIN != none ]
then
pwm min -c $PWM_OUTPUTS -p $PWM_MIN
pwm min -c $PWM_OUT -p $PWM_MIN
fi
if [ $PWM_MAX != none ]
then
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
pwm max -c $PWM_OUT -p $PWM_MAX
fi
fi
ROMFS/px4fmu_common/init.d/rc.io
+1 -1
View File
@@ -16,5 +16,5 @@ then
set PX4IO_LIMIT 200
fi
echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
px4io limit $PX4IO_LIMIT
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+1 -1
View File
@@ -2,7 +2,7 @@
set VEHICLE_TYPE mc
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
ROMFS/px4fmu_common/init.d/rc.sensors
+5 -1
View File
@@ -56,7 +56,7 @@ fi
if meas_airspeed start
then
echo "[init] Using MEAS airspeed sensor"
echo "[i] Using MEAS airspeed sensor"
else
if ets_airspeed start
then
@@ -71,6 +71,10 @@ if px4flow start
then
fi
if ll40ls start
then
fi
#
# Start sensors -> preflight_check
#
ROMFS/px4fmu_common/init.d/rc.uavcan
+3 -3
View File
@@ -10,9 +10,9 @@ then
# First sensor publisher to initialize takes lowest instance ID
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
sleep 1
echo "[init] UAVCAN started"
echo "[i] UAVCAN started"
else
echo "[init] ERROR: Could not start UAVCAN"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: Could not start UAVCAN"
tone_alarm $TUNE_ERR
fi
fi
ROMFS/px4fmu_common/init.d/rcS
+89 -70
View File
@@ -1,46 +1,46 @@
#!nsh
#
# PX4FMU startup script.
#
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#
#
# Default to auto-start mode.
#
set MODE autostart
set RC_FILE /fs/microsd/etc/rc.txt
set CONFIG_FILE /fs/microsd/etc/config.txt
set EXTRAS_FILE /fs/microsd/etc/extras.txt
set FRC /fs/microsd/etc/rc.txt
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
set TUNE_ERR ML<<CP4CP4CP4CP4CP4
#
# Try to mount the microSD card.
#
echo "[init] Looking for microSD..."
echo "[i] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
set LOG_FILE /fs/microsd/bootlog.txt
echo "[init] microSD mounted: /fs/microsd"
echo "[i] microSD mounted: /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
set LOG_FILE /dev/null
echo "[init] No microSD card found"
# Play SOS
tone_alarm error
fi
#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $RC_FILE ]
if [ -f $FRC ]
then
echo "[init] Executing init script: $RC_FILE"
sh $RC_FILE
echo "[i] Executing init script: $FRC"
sh $FRC
set MODE custom
else
echo "[init] Init script not found: $RC_FILE"
echo "[i] Init script not found: $FRC"
fi
# if this is an APM build then there will be a rc.APM script
@@ -49,17 +49,17 @@ if [ -f /etc/init.d/rc.APM ]
then
if sercon
then
echo "[init] USB interface connected"
echo "[i] USB interface connected"
fi
echo "[init] Running rc.APM"
echo "[i] Running rc.APM"
# if APM startup is successful then nsh will exit
sh /etc/init.d/rc.APM
fi
if [ $MODE == autostart ]
then
echo "[init] AUTOSTART mode"
echo "[i] AUTOSTART mode"
#
# Start CDC/ACM serial driver
@@ -117,31 +117,31 @@ then
set VEHICLE_TYPE none
set MIXER none
set OUTPUT_MODE none
set PWM_OUTPUTS none
set PWM_OUT none
set PWM_RATE none
set PWM_DISARMED none
set PWM_MIN none
set PWM_MAX none
set MKBLCTRL_MODE none
set MK_MODE none
set FMU_MODE pwm
set MAVLINK_FLAGS default
set MAVLINK_F default
set EXIT_ON_END no
set MAV_TYPE none
set LOAD_DEFAULT_APPS yes
set LOAD_DAPPS yes
set GPS yes
set GPS_FAKE no
set FAILSAFE none
#
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
# Set AUTOCNF flag to use it in AUTOSTART scripts
#
if param compare SYS_AUTOCONFIG 1
then
# Wipe out params
param reset_nostart
set DO_AUTOCONFIG yes
set AUTOCNF yes
else
set DO_AUTOCONFIG no
set AUTOCNF no
fi
#
@@ -159,7 +159,7 @@ then
#
if param compare SYS_AUTOSTART 0
then
echo "[init] No autostart"
echo "[i] No autostart"
else
sh /etc/init.d/rc.autostart
fi
@@ -167,18 +167,19 @@ then
#
# Override parameters from user configuration file
#
if [ -f $CONFIG_FILE ]
if [ -f $FCONFIG ]
then
echo "[init] Config: $CONFIG_FILE"
sh $CONFIG_FILE
echo "[i] Config: $FCONFIG"
sh $FCONFIG
else
echo "[init] Config not found: $CONFIG_FILE"
echo "[i] Config not found: $FCONFIG"
fi
unset FCONFIG
#
# If autoconfig parameter was set, reset it and save parameters
#
if [ $DO_AUTOCONFIG == yes ]
if [ $AUTOCNF == yes ]
then
param set SYS_AUTOCONFIG 0
param save
@@ -219,18 +220,18 @@ then
set IO_PRESENT yes
else
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
tone_alarm $TUNE_ERR
fi
else
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
tone_alarm $TUNE_ERR
fi
fi
if [ $IO_PRESENT == no ]
then
echo "[init] ERROR: PX4IO not found"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: PX4IO not found"
tone_alarm $TUNE_ERR
fi
fi
@@ -251,7 +252,7 @@ then
then
# Need IO for output but it not present, disable output
set OUTPUT_MODE none
echo "[init] ERROR: PX4IO not found, disabling output"
echo "[i] ERROR: PX4IO not found, disabling output"
# Avoid using ttyS0 for MAVLink on FMUv1
if ver hwcmp PX4FMU_V1
@@ -294,7 +295,7 @@ then
then
if param compare UAVCAN_ENABLE 0
then
echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
echo "[i] OVERRIDING UAVCAN_ENABLE = 1"
param set UAVCAN_ENABLE 1
fi
fi
@@ -303,11 +304,11 @@ then
then
if px4io start
then
echo "[init] PX4IO started"
echo "[i] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: PX4IO start failed"
tone_alarm $TUNE_ERR
fi
fi
@@ -315,10 +316,10 @@ then
then
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
echo "[i] FMU mode_$FMU_MODE started"
else
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_ERR
fi
if ver hwcmp PX4FMU_V1
@@ -337,21 +338,21 @@ then
if [ $OUTPUT_MODE == mkblctrl ]
then
set MKBLCTRL_ARG ""
if [ $MKBLCTRL_MODE == x ]
if [ $MK_MODE == x ]
then
set MKBLCTRL_ARG "-mkmode x"
fi
if [ $MKBLCTRL_MODE == + ]
if [ $MK_MODE == + ]
then
set MKBLCTRL_ARG "-mkmode +"
fi
if mkblctrl $MKBLCTRL_ARG
then
echo "[init] MKBLCTRL started"
echo "[i] MK started"
else
echo "[init] ERROR: MKBLCTRL start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: MK start failed"
tone_alarm $TUNE_ERR
fi
fi
@@ -360,10 +361,10 @@ then
then
if hil mode_port2_pwm8
then
echo "[init] HIL output started"
echo "[i] HIL output started"
else
echo "[init] ERROR: HIL output start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: HIL output start failed"
tone_alarm $TUNE_ERR
fi
fi
@@ -376,10 +377,11 @@ then
then
if px4io start
then
echo "[i] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] ERROR: PX4IO start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: PX4IO start failed"
tone_alarm $TUNE_ERR
fi
fi
else
@@ -387,10 +389,10 @@ then
then
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
echo "[i] FMU mode_$FMU_MODE started"
else
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_OUT_ERROR
echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
tone_alarm $TUNE_ERR
fi
if ver hwcmp PX4FMU_V1
@@ -408,23 +410,24 @@ then
fi
fi
if [ $MAVLINK_FLAGS == default ]
if [ $MAVLINK_F == default ]
then
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
if [ $TTYS1_BUSY == yes ]
then
# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
set MAVLINK_F "-r 1000 -d /dev/ttyS0"
# Exit from nsh to free port for mavlink
set EXIT_ON_END yes
else
# Start MAVLink on default port: ttyS1
set MAVLINK_FLAGS "-r 1000"
set MAVLINK_F "-r 1000"
fi
fi
mavlink start $MAVLINK_FLAGS
mavlink start $MAVLINK_F
unset MAVLINK_F
#
# UAVCAN
@@ -439,14 +442,16 @@ then
if [ $GPS == yes ]
then
echo "[i] Start GPS"
if [ $GPS_FAKE == yes ]
then
echo "[init] Faking GPS"
echo "[i] Faking GPS"
gps start -f
else
gps start
fi
fi
unset GPS_FAKE
#
# Start up ARDrone Motor interface
@@ -461,7 +466,7 @@ then
#
if [ $VEHICLE_TYPE == fw ]
then
echo "[init] Vehicle type: FIXED WING"
echo "[i] FIXED WING"
if [ $MIXER == none ]
then
@@ -481,7 +486,7 @@ then
sh /etc/init.d/rc.interface
# Start standard fixedwing apps
if [ $LOAD_DEFAULT_APPS == yes ]
if [ $LOAD_DAPPS == yes ]
then
sh /etc/init.d/rc.fw_apps
fi
@@ -492,11 +497,11 @@ then
#
if [ $VEHICLE_TYPE == mc ]
then
echo "[init] Vehicle type: MULTICOPTER"
echo "[i] MULTICOPTER"
if [ $MIXER == none ]
then
echo "Default mixer for multicopter not defined"
echo "Mixer undefined"
fi
if [ $MAV_TYPE == none ]
@@ -540,7 +545,7 @@ then
sh /etc/init.d/rc.interface
# Start standard multicopter apps
if [ $LOAD_DEFAULT_APPS == yes ]
if [ $LOAD_DAPPS == yes ]
then
sh /etc/init.d/rc.mc_apps
fi
@@ -595,24 +600,38 @@ then
#
if [ $VEHICLE_TYPE == none ]
then
echo "[init] Vehicle type: No autostart ID found"
echo "[i] No autostart ID found"
fi
# Start any custom addons
if [ -f $EXTRAS_FILE ]
if [ -f $FEXTRAS ]
then
echo "[init] Starting addons script: $EXTRAS_FILE"
sh $EXTRAS_FILE
echo "[i] Addons script: $FEXTRAS"
sh $FEXTRAS
else
echo "[init] No addons script: $EXTRAS_FILE"
echo "[i] No addons script: $FEXTRAS"
fi
unset FEXTRAS
if [ $EXIT_ON_END == yes ]
then
echo "[init] Exit from nsh"
echo "Exit from nsh"
exit
fi
unset EXIT_ON_END
# Run no SD alarm last
if [ $LOG_FILE == /dev/null ]
then
echo "[i] No microSD card found"
# Play SOS
tone_alarm error
fi
# End of autostart
fi
# There is no further processing, so we can free some RAM
# XXX potentially unset all script variables.
unset TUNE_ERR
ROMFS/px4fmu_common/mixers/skywalker.mix
+64
View File
@@ -0,0 +1,64 @@
Mixer for Skywalker Airframe
==================================================
This file defines mixers suitable for controlling a fixed wing aircraft with
aileron, rudder, elevator and throttle controls using PX4FMU. The configuration
assumes the aileron servo(s) are connected to PX4FMU servo output 0, the
elevator to output 1, the rudder to output 2 and the throttle to output 3.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
Aileron mixer
-------------
Two scalers total (output, roll).
This mixer assumes that the aileron servos are set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
As there is only one output, if using two servos adjustments to compensate for
differences between the servos must be made mechanically. To obtain the correct
motion using a Y cable, the servos can be positioned reversed from one another.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 -10000 -10000 0 -10000 10000
Elevator mixer
------------
Two scalers total (output, roll).
This mixer assumes that the elevator servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
Rudder mixer
------------
Two scalers total (output, yaw).
This mixer assumes that the rudder servo is set up correctly mechanically;
depending on the actual configuration it may be necessary to reverse the scaling
factors (to reverse the servo movement) and adjust the offset, scaling and
endpoints to suit.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
ROMFS/px4fmu_test/init.d/rc.standalone
+2 -2
View File
@@ -3,11 +3,11 @@
# Flight startup script for PX4FMU standalone configuration.
#
echo "[init] doing standalone PX4FMU startup..."
echo "[i] doing standalone PX4FMU startup..."
#
# Start the ORB
#
uorb start
echo "[init] startup done"
echo "[i] startup done"
ROMFS/px4fmu_test/init.d/rcS
+5 -5
View File
@@ -6,7 +6,7 @@ uorb start
if sercon
then
echo "[init] USB interface connected"
echo "[i] USB interface connected"
# Try to get an USB console
nshterm /dev/ttyACM0 &
@@ -15,14 +15,14 @@ fi
#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
echo "[i] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
echo "[i] card mounted at /fs/microsd"
# Start playing the startup tune
tone_alarm start
else
echo "[init] no microSD card found"
echo "[i] no microSD card found"
# Play SOS
tone_alarm error
fi
@@ -104,4 +104,4 @@ then
else
echo
echo "Some Unit Tests FAILED:${unit_test_failure_list}"
fi
fi
makefiles/config_px4fmu-v2_test.mk
+13 -4
View File
@@ -50,16 +50,25 @@ MODULES += lib/mathlib/math/filter
MODULES += lib/conversion
#
# Modules to test-build, but not useful for test environment
# Example modules to test-build
#
MODULES += examples/flow_position_estimator
MODULES += examples/fixedwing_control
MODULES += examples/hwtest
MODULES += examples/matlab_csv_serial
MODULES += examples/px4_daemon_app
MODULES += examples/px4_mavlink_debug
MODULES += examples/px4_simple_app
#
# Drivers / modules to test build, but not useful for test environment
#
MODULES += modules/attitude_estimator_so3
MODULES += drivers/pca8574
MODULES += examples/flow_position_estimator
#
# Libraries
# Tests
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
src/drivers/ll40ls/ll40ls.cpp
+12 -10
View File
@@ -89,7 +89,7 @@
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
#define LL40LS_MAX_DISTANCE (14.00f)
#define LL40LS_MAX_DISTANCE (60.00f)
#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */
@@ -233,11 +233,11 @@ LL40LS::~LL40LS()
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -263,7 +263,7 @@ LL40LS::init()
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
@@ -314,9 +314,9 @@ LL40LS::probe()
goto ok;
}
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
(unsigned)who_am_i,
LL40LS_WHO_AM_I_REG_VAL,
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
(unsigned)who_am_i,
LL40LS_WHO_AM_I_REG_VAL,
(unsigned)val);
}
@@ -581,6 +581,8 @@ LL40LS::collect()
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
report.valid = 1;
}
@@ -704,7 +706,7 @@ LL40LS::print_info()
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
@@ -969,8 +971,8 @@ ll40ls_main(int argc, char *argv[])
}
}
const char *verb = argv[optind];
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
src/drivers/mb12xx/mb12xx.cpp
+2
View File
@@ -520,6 +520,8 @@ MB12XX::collect()
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it, if we are the primary */
src/drivers/sf0x/sf0x.cpp
+3 -1
View File
@@ -545,7 +545,7 @@ SF0X::collect()
float si_units;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
valid = true;
@@ -564,6 +564,8 @@ SF0X::collect()
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
/* publish it */
src/drivers/trone/trone.cpp
+6 -4
View File
@@ -256,11 +256,11 @@ TRONE::~TRONE()
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
@@ -286,7 +286,7 @@ TRONE::init()
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
@@ -558,8 +558,10 @@ TRONE::collect()
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
src/examples/fixedwing_control/main.c
+22 -131
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 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
@@ -31,6 +30,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file main.c
*
@@ -55,7 +55,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -106,11 +106,9 @@ static void usage(const char *reason);
*
* @param att_sp The current attitude setpoint - the values the system would like to reach.
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param speed_body The velocity of the system. Currently unused.
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
@@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
* @param att The current attitude
* @param att_sp The attitude setpoint. This is the output of the controller
*/
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
/* Variables */
@@ -135,8 +133,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
@@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
actuators->control[1] = pitch_err * p.pitch_p;
}
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
{
@@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
/* calculate heading error */
float yaw_err = att->yaw - bearing;
/* apply control gain */
float roll_command = yaw_err * p.hdng_p;
att_sp->roll_body = yaw_err * p.hdng_p;
/* limit output, this commonly is a tuning parameter, too */
if (att_sp->roll_body < -0.6f) {
@@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct vehicle_global_position_setpoint_s global_sp;
struct position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
/* output structs - this is what is sent to the mixer */
@@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
float speed_body[3] = {0.0f, 0.0f, 0.0f};
/* RC failsafe check */
bool throttle_half_once = false;
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
@@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
/* currently speed in body frame is not used, but here for reference */
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
warnx("Did not get a valid R\n");
}
if (global_sp_updated) {
struct position_setpoint_triplet_s triplet;
orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet);
memcpy(&global_sp, &triplet.current, sizeof(global_sp));
}
if (manual_sp_updated)
@@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.6f) &&
(manual_sp.throttle <= 1.0f)) {
throttle_half_once = true;
if (isfinite(manual_sp.z) &&
(manual_sp.z >= 0.6f) &&
(manual_sp.z <= 1.0f)) {
}
/* get the system status and the flight mode we're in */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* control */
#warning fix this
#if 0
if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
/* simple heading control */
control_heading(&global_pos, &global_sp, &att, &att_sp);
/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
/* simple attitude control */
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost && throttle_half_once) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
#endif
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
@@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
if (verbose) {
warnx("published");
}
}
}
}
src/examples/flow_speed_control/flow_speed_control_main.c
-387
View File
@@ -1,387 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file flow_speed_control.c
*
* Optical flow speed controller
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <math.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_speed_control_params.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
__EXPORT int flow_speed_control_main(int argc, char *argv[]);
/**
* Mainloop of position controller.
*/
static int flow_speed_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn_cmd().
*/
int flow_speed_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
printf("flow speed control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("flow_speed_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 6,
4096,
flow_speed_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop"))
{
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status"))
{
if (thread_running)
printf("\tflow speed control app is running\n");
else
printf("\tflow speed control app not started\n");
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int
flow_speed_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd,"[fsc] started");
uint32_t counter = 0;
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
orb_advert_t att_sp_pub;
bool attitude_setpoint_adverted = false;
/* parameters init*/
struct flow_speed_control_params params;
struct flow_speed_control_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);
/* register the perf counter */
perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
/* wait for first attitude msg to be sure all data are available */
if (sensors_ready)
{
/* polling */
struct pollfd fds[2] = {
{ .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
{ .fd = parameter_update_sub, .events = POLLIN }
};
/* wait for a position update, check for exit condition every 5000 ms */
int ret = poll(fds, 2, 500);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
// printf("[flow speed control] no bodyframe speed setpoints updates\n");
}
else
{
/* parameter update available? */
if (fds[1].revents & POLLIN)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
}
/* only run controller if position/speed changed */
if (fds[0].revents & POLLIN)
{
perf_begin(mc_loop_perf);
/* get a local copy of the armed topic */
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* get a local copy of the control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
/* calc new roll/pitch */
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p;
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
status_changed = true;
/* limit roll and pitch corrections */
if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
{
att_sp.pitch_body = pitch_body;
}
else
{
if(pitch_body > params.limit_pitch)
att_sp.pitch_body = params.limit_pitch;
if(pitch_body < -params.limit_pitch)
att_sp.pitch_body = -params.limit_pitch;
}
if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
{
att_sp.roll_body = roll_body;
}
else
{
if(roll_body > params.limit_roll)
att_sp.roll_body = params.limit_roll;
if(roll_body < -params.limit_roll)
att_sp.roll_body = -params.limit_roll;
}
/* set yaw setpoint forward*/
att_sp.yaw_body = speed_sp.yaw_sp;
/* add trim from parameters */
att_sp.roll_body = att_sp.roll_body + params.trim_roll;
att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
att_sp.thrust = speed_sp.thrust_sp;
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
{
if (attitude_setpoint_adverted)
{
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
else
{
att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
attitude_setpoint_adverted = true;
}
}
else
{
warnx("NaN in flow speed controller!");
}
}
else
{
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
status_changed = false;
/* reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.0f;
att_sp.yaw_body = 0.0f;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
}
}
counter++;
}
else
{
/* sensors not ready waiting for first attitude msg */
/* polling */
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
/* wait for a flow msg, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);
if (ret < 0)
{
/* poll error, count it in perf */
perf_count(mc_err_perf);
}
else if (ret == 0)
{
/* no return value, ignore */
mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
mavlink_log_info(mavlink_fd,"[fsp] initialized.");
}
}
}
}
mavlink_log_info(mavlink_fd,"[fsc] ending now...");
thread_running = false;
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_bodyframe_speed_setpoint_sub);
close(filtered_bottom_flow_sub);
close(armed_sub);
close(control_mode_sub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);
fflush(stdout);
return 0;
}
src/examples/flow_speed_control/flow_speed_control_params.c
-70
View File
@@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file flow_speed_control_params.c
*
*/
#include "flow_speed_control_params.h"
/* controller parameters */
PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f);
PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f);
PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f);
int parameters_init(struct flow_speed_control_param_handles *h)
{
/* PID parameters */
h->speed_p = param_find("FSC_S_P");
h->limit_pitch = param_find("FSC_L_PITCH");
h->limit_roll = param_find("FSC_L_ROLL");
h->trim_roll = param_find("TRIM_ROLL");
h->trim_pitch = param_find("TRIM_PITCH");
return OK;
}
int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p)
{
param_get(h->speed_p, &(p->speed_p));
param_get(h->limit_pitch, &(p->limit_pitch));
param_get(h->limit_roll, &(p->limit_roll));
param_get(h->trim_roll, &(p->trim_roll));
param_get(h->trim_pitch, &(p->trim_pitch));
return OK;
}
src/examples/flow_speed_control/flow_speed_control_params.h
-70
View File
@@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file flow_speed_control_params.h
*
* Parameters for speed controller
*/
#include <systemlib/param/param.h>
struct flow_speed_control_params {
float speed_p;
float limit_pitch;
float limit_roll;
float trim_roll;
float trim_pitch;
};
struct flow_speed_control_param_handles {
param_t speed_p;
param_t limit_pitch;
param_t limit_roll;
param_t trim_roll;
param_t trim_pitch;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct flow_speed_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p);
src/examples/flow_speed_control/module.mk
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build flow speed control
#
MODULE_COMMAND = flow_speed_control
SRCS = flow_speed_control_main.c \
flow_speed_control_params.c
src/examples/hwtest/hwtest.c
+71 -21
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Copyright (c) 2013, 2014 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
@@ -34,7 +33,8 @@
/**
* @file hwtest.c
*
* Simple functional hardware test.
* Simple output test.
* @ref Documentation https://pixhawk.org/dev/examples/write_output
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
@@ -45,30 +45,80 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
__EXPORT int ex_hwtest_main(int argc, char *argv[]);
int ex_hwtest_main(int argc, char *argv[])
{
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
warnx("DO NOT FORGET TO STOP THE COMMANDER APP!");
warnx("(run <commander stop> to do so)");
warnx("usage: http://px4.io/dev/examples/write_output");
int i;
float rcvalue = -1.0f;
hrt_abstime stime;
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
while (true) {
stime = hrt_absolute_time();
while (hrt_absolute_time() - stime < 1000000) {
for (i=0; i<8; i++)
actuators.control[i] = rcvalue;
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
}
warnx("servos set to %.1f", rcvalue);
rcvalue *= -1.0f;
}
struct actuator_armed_s arm;
memset(&arm, 0 , sizeof(arm));
return OK;
arm.timestamp = hrt_absolute_time();
arm.ready_to_arm = true;
arm.armed = true;
orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm);
orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm);
/* read back values to validate */
int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);
if (arm.ready_to_arm && arm.armed) {
warnx("Actuator armed");
} else {
errx(1, "Arming actuators failed");
}
hrt_abstime stime;
int count = 0;
while (count != 36) {
stime = hrt_absolute_time();
while (hrt_absolute_time() - stime < 1000000) {
for (int i = 0; i != 2; i++) {
if (count <= 5) {
actuators.control[i] = -1.0f;
} else if (count <= 10) {
actuators.control[i] = -0.7f;
} else if (count <= 15) {
actuators.control[i] = -0.5f;
} else if (count <= 20) {
actuators.control[i] = -0.3f;
} else if (count <= 25) {
actuators.control[i] = 0.0f;
} else if (count <= 30) {
actuators.control[i] = 0.3f;
} else {
actuators.control[i] = 0.5f;
}
}
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
usleep(10000);
}
warnx("count %i", count);
count++;
}
return OK;
}
src/examples/math_demo/math_demo.cpp
-106
View File
@@ -1,106 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: James Goppert
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file math_demo.cpp
* @author James Goppert
* Demonstration of math library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
/**
* Management function.
*/
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: math_demo {test}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int math_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
usage("unrecognized command");
exit(1);
}
void test()
{
printf("beginning math lib test\n");
using namespace math;
vectorTest();
matrixTest();
vector3Test();
eulerAnglesTest();
quaternionTest();
dcmTest();
}
src/examples/math_demo/module.mk
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2012, 2013 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Mathlib / operations demo application
#
MODULE_COMMAND = math_demo
MODULE_STACKSIZE = 12000
SRCS = math_demo.cpp
src/modules/attitude_estimator_ekf/AttitudeEKF.m
+298
View File
@@ -0,0 +1,298 @@
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
%LQG Postion Estimator and Controller
% Observer:
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
% x[n+1|n] = Ax[n|n] + Bu[n]
%
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
%
%
% Arguments:
% approx_prediction: if 1 then the exponential map is approximated with a
% first order taylor approximation. has at the moment not a big influence
% (just 1st or 2nd order approximation) we should change it to rodriquez
% approximation.
% use_inertia_matrix: set to true if you have the inertia matrix J for your
% quadrotor
% xa_apo_k: old state vectotr
% zFlag: if sensor measurement is available [gyro, acc, mag]
% dt: dt in s
% z: measurements [gyro, acc, mag]
% q_rotSpeed: process noise gyro
% q_rotAcc: process noise gyro acceleration
% q_acc: process noise acceleration
% q_mag: process noise magnetometer
% r_gyro: measurement noise gyro
% r_accel: measurement noise accel
% r_mag: measurement noise mag
% J: moment of inertia matrix
% Output:
% xa_apo: updated state vectotr
% Pa_apo: updated state covariance matrix
% Rot_matrix: rotation matrix
% eulerAngles: euler angles
% debugOutput: not used
%% model specific parameters
% compute once the inverse of the Inertia
persistent Ji;
if isempty(Ji)
Ji=single(inv(J));
end
%% init
persistent x_apo
if(isempty(x_apo))
gyro_init=single([0;0;0]);
gyro_acc_init=single([0;0;0]);
acc_init=single([0;0;-9.81]);
mag_init=single([1;0;0]);
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
end
persistent P_apo
if(isempty(P_apo))
% P_apo = single(eye(NSTATES) * 1000);
P_apo = single(200*ones(12));
end
debugOutput = single(zeros(4,1));
%% copy the states
wx= x_apo(1); % x body angular rate
wy= x_apo(2); % y body angular rate
wz= x_apo(3); % z body angular rate
wax= x_apo(4); % x body angular acceleration
way= x_apo(5); % y body angular acceleration
waz= x_apo(6); % z body angular acceleration
zex= x_apo(7); % x component gravity vector
zey= x_apo(8); % y component gravity vector
zez= x_apo(9); % z component gravity vector
mux= x_apo(10); % x component magnetic field vector
muy= x_apo(11); % y component magnetic field vector
muz= x_apo(12); % z component magnetic field vector
%% prediction section
% compute the apriori state estimate from the previous aposteriori estimate
%body angular accelerations
if (use_inertia_matrix==1)
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
else
wak =[wax;way;waz];
end
%body angular rates
wk =[wx; wy; wz] + dt*wak;
%derivative of the prediction rotation matrix
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
%prediction of the earth z vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
else
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
%precision
end
%prediction of the magnetic vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
else
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
%precision
end
x_apr=[wk;wak;zek;muk];
% compute the apriori error covariance estimate from the previous
%aposteriori estimate
EZ=[0,zez,-zey;
-zez,0,zex;
zey,-zex,0]';
MA=[0,muz,-muy;
-muz,0,mux;
muy,-mux,0]';
E=single(eye(3));
Z=single(zeros(3));
A_lin=[ Z, E, Z, Z
Z, Z, Z, Z
EZ, Z, O, Z
MA, Z, Z, O];
A_lin=eye(12)+A_lin*dt;
%process covariance matrix
persistent Q
if (isempty(Q))
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
q_rotAcc,q_rotAcc,q_rotAcc,...
q_acc,q_acc,q_acc,...
q_mag,q_mag,q_mag]);
end
P_apr=A_lin*P_apo*A_lin'+Q;
%% update
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0,0,0,0;
% 0,r_gyro,0,0,0,0,0,0,0;
% 0,0,r_gyro,0,0,0,0,0,0;
% 0,0,0,r_accel,0,0,0,0,0;
% 0,0,0,0,r_accel,0,0,0,0;
% 0,0,0,0,0,r_accel,0,0,0;
% 0,0,0,0,0,0,r_mag,0,0;
% 0,0,0,0,0,0,0,r_mag,0;
% 0,0,0,0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
%observation matrix
%[zw;ze;zmk];
H_k=[ E, Z, Z, Z;
Z, Z, E, Z;
Z, Z, Z, E];
y_k=z(1:9)-H_k*x_apr;
%S_k=H_k*P_apr*H_k'+R;
S_k=H_k*P_apr*H_k';
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
K_k=(P_apr*H_k'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k)*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
R=[r_gyro,0,0;
0,r_gyro,0;
0,0,r_gyro];
R_v=[r_gyro,r_gyro,r_gyro];
%observation matrix
H_k=[ E, Z, Z, Z];
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_accel,0,0;
% 0,0,0,0,r_accel,0;
% 0,0,0,0,0,r_accel];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, E, Z];
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_mag,0,0;
% 0,0,0,0,r_mag,0;
% 0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, Z, E];
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
x_apo=x_apr;
P_apo=P_apr;
end
end
end
end
%% euler anglels extraction
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
y_n_b=cross(z_n_b,m_n_b);
y_n_b=y_n_b./norm(y_n_b);
x_n_b=(cross(y_n_b,z_n_b));
x_n_b=x_n_b./norm(x_n_b);
xa_apo=x_apo;
Pa_apo=P_apo;
% rotation matrix from earth to body system
Rot_matrix=[x_n_b,y_n_b,z_n_b];
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
theta=-asin(Rot_matrix(1,3));
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
eulerAngles=[phi;theta;psi];
src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
+502
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+27 -8
View File
@@ -38,6 +38,7 @@
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -74,8 +75,7 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "codegen/AttitudeEKF.h"
#include "attitude_estimator_ekf_params.h"
#ifdef __cplusplus
}
@@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
14000,
7200,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
@@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
float debugOutput[4] = { 0.0f };
int overloadcounter = 19;
/* Initialize filter */
attitudeKalmanfilter_initialize();
AttitudeEKF_initialize();
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
@@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
struct attitude_estimator_ekf_params ekf_params = { 0 };
struct attitude_estimator_ekf_param_handles ekf_param_handles;
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
@@ -504,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* Call the estimator */
AttitudeEKF(false, // approx_prediction
(unsigned char)ekf_params.use_moment_inertia,
update_vect,
dt,
z_k,
ekf_params.q[0], // q_rotSpeed,
ekf_params.q[1], // q_rotAcc
ekf_params.q[2], // q_acc
ekf_params.q[3], // q_mag
ekf_params.r[0], // r_gyro
ekf_params.r[1], // r_accel
ekf_params.r[2], // r_mag
ekf_params.moment_inertia_J,
x_aposteriori,
P_aposteriori,
Rot_matrix,
euler,
debugOutput);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {

Some files were not shown because too many files have changed in this diff Show More