mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 16:17:34 +08:00
José Roberto de Souza
ce180af4ca
The UART3 also have the I2C bus 2 functions so moving GPS to UART7 to have one additional I2C. To keep GPS working is also necessary update the FPGA RTL to version 0xC1 or higher.
1159 lines
21 KiB
Plaintext
1159 lines
21 KiB
Plaintext
#!nsh
|
|
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
|
|
set +e
|
|
# Un comment the line below to help debug scripts by printing a trace of the script commands
|
|
#set -x
|
|
# PX4FMU startup script.
|
|
#
|
|
# NOTE: environment variable references:
|
|
# If the dollar sign ('$') is followed by a left bracket ('{') then the
|
|
# variable name is terminated with the right bracket character ('}').
|
|
# Otherwise, the variable name goes to the end of the argument.
|
|
#
|
|
#
|
|
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
|
|
#
|
|
# UART mapping on FMUv1/2/3/4:
|
|
#
|
|
# UART1 /dev/ttyS0 IO debug
|
|
# USART2 /dev/ttyS1 TELEM1 (flow control)
|
|
# USART3 /dev/ttyS2 TELEM2 (flow control)
|
|
# UART4
|
|
# UART7 CONSOLE
|
|
# UART8 SERIAL4
|
|
#
|
|
#
|
|
# UART mapping on FMUv5:
|
|
#
|
|
# UART1 /dev/ttyS0 GPS
|
|
# USART2 /dev/ttyS1 TELEM1 (flow control)
|
|
# USART3 /dev/ttyS2 TELEM2 (flow control)
|
|
# UART4 /dev/ttyS3 ?
|
|
# USART6 /dev/ttyS4 TELEM3 (flow control)
|
|
# UART7 /dev/ttyS5 ?
|
|
# UART8 /dev/ttyS6 CONSOLE
|
|
|
|
#
|
|
# Mount the procfs.
|
|
#
|
|
mount -t procfs /proc
|
|
|
|
#
|
|
# Start CDC/ACM serial driver
|
|
#
|
|
if sercon
|
|
then
|
|
fi
|
|
|
|
#
|
|
# Default to auto-start mode.
|
|
#
|
|
set MODE autostart
|
|
|
|
set TUNE_ERR ML<<CP4CP4CP4CP4CP4
|
|
set LOG_FILE /fs/microsd/bootlog.txt
|
|
|
|
#
|
|
# Try to mount the microSD card.
|
|
#
|
|
# REBOOTWORK this needs to start after the flight control loop
|
|
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
|
then
|
|
echo "[i] microSD mounted: /fs/microsd"
|
|
if hardfault_log check
|
|
then
|
|
tone_alarm error
|
|
if hardfault_log commit
|
|
then
|
|
hardfault_log reset
|
|
tone_alarm stop
|
|
fi
|
|
else
|
|
# Start playing the startup tune
|
|
tone_alarm start
|
|
fi
|
|
else
|
|
tone_alarm MBAGP
|
|
if mkfatfs /dev/mmcsd0
|
|
then
|
|
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
|
then
|
|
echo "INFO [init] MicroSD card formatted"
|
|
else
|
|
echo "ERROR [init] Format failed"
|
|
tone_alarm MNBG
|
|
set LOG_FILE /dev/null
|
|
fi
|
|
else
|
|
set LOG_FILE /dev/null
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Look for an init script on the microSD card.
|
|
# Disable autostart if the script found.
|
|
#
|
|
set FRC /fs/microsd/etc/rc.txt
|
|
if [ -f $FRC ]
|
|
then
|
|
echo "INFO [init] Executing script: ${FRC}"
|
|
sh $FRC
|
|
set MODE custom
|
|
fi
|
|
unset FRC
|
|
|
|
if [ $MODE == autostart ]
|
|
then
|
|
|
|
#
|
|
# Start the ORB (first app to start)
|
|
#
|
|
uorb start
|
|
|
|
#
|
|
# Load parameters
|
|
#
|
|
set PARAM_FILE /fs/microsd/params
|
|
if mtd start
|
|
then
|
|
set PARAM_FILE /fs/mtd_params
|
|
fi
|
|
|
|
param select $PARAM_FILE
|
|
if param load
|
|
then
|
|
else
|
|
if param reset
|
|
then
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start system state indicator
|
|
#
|
|
if rgbled start
|
|
then
|
|
else
|
|
if blinkm start
|
|
then
|
|
blinkm systemstate
|
|
fi
|
|
fi
|
|
|
|
# Currently unused, but might be useful down the road
|
|
#if pca8574 start
|
|
#then
|
|
#fi
|
|
|
|
#
|
|
# Set AUTOCNF flag to use it in AUTOSTART scripts
|
|
#
|
|
if param compare SYS_AUTOCONFIG 1
|
|
then
|
|
# Wipe out params except RC* and total flight time
|
|
param reset_nostart RC* LND_FLIGHT_T_*
|
|
set AUTOCNF yes
|
|
else
|
|
set AUTOCNF no
|
|
|
|
#
|
|
# Release 1.4.0 transitional support:
|
|
# set to old default if unconfigured.
|
|
# this preserves the previous behaviour
|
|
#
|
|
if param compare BAT_N_CELLS 0
|
|
then
|
|
param set BAT_N_CELLS 3
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Set default values
|
|
#
|
|
set HIL no
|
|
set VEHICLE_TYPE none
|
|
set MIXER none
|
|
set MIXER_AUX none
|
|
set OUTPUT_MODE none
|
|
set PWM_OUT none
|
|
set PWM_RATE p:PWM_RATE
|
|
set PWM_DISARMED p:PWM_DISARMED
|
|
set PWM_MIN p:PWM_MIN
|
|
set PWM_MAX p:PWM_MAX
|
|
set PWM_AUX_OUT none
|
|
set PWM_AUX_RATE none
|
|
set PWM_ACHDIS none
|
|
set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
|
|
set PWM_AUX_MIN p:PWM_AUX_MIN
|
|
set PWM_AUX_MAX p:PWM_AUX_MAX
|
|
set FAILSAFE_AUX none
|
|
set MK_MODE none
|
|
set FMU_MODE pwm
|
|
set AUX_MODE pwm
|
|
set FMU_ARGS ""
|
|
set MAVLINK_F default
|
|
set MAVLINK_COMPANION_DEVICE /dev/ttyS2
|
|
set EXIT_ON_END no
|
|
set MAV_TYPE none
|
|
set FAILSAFE none
|
|
set USE_IO yes
|
|
set LOGGER_BUF 16
|
|
|
|
if param compare SYS_FMU_TASK 1
|
|
then
|
|
set FMU_ARGS "-t"
|
|
fi
|
|
|
|
if param compare SYS_HITL 1
|
|
then
|
|
# Enable HITL mode
|
|
set HIL yes
|
|
fi
|
|
|
|
#
|
|
# Set USE_IO flag
|
|
#
|
|
if param compare SYS_USE_IO 1
|
|
then
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
set USE_IO no
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V5
|
|
then
|
|
set USE_IO no
|
|
set MAVLINK_COMPANION_DEVICE /dev/ttyS3
|
|
fi
|
|
|
|
if ver hwcmp MINDPX_V2
|
|
then
|
|
set USE_IO no
|
|
fi
|
|
|
|
if ver hwcmp CRAZYFLIE
|
|
then
|
|
set USE_IO no
|
|
|
|
if param compare SYS_AUTOSTART 0
|
|
then
|
|
param set SYS_AUTOSTART 4900
|
|
set AUTOCNF yes
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
set USE_IO no
|
|
fi
|
|
|
|
if ver hwcmp AEROCORE2
|
|
then
|
|
set USE_IO no
|
|
fi
|
|
else
|
|
set USE_IO no
|
|
fi
|
|
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
if param compare SYS_AUTOSTART 0
|
|
then
|
|
set AUTOCNF yes
|
|
fi
|
|
|
|
# We don't allow changing AUTOSTART as it doesn't work in
|
|
# other configurations
|
|
param set SYS_AUTOSTART 4070
|
|
fi
|
|
|
|
#
|
|
# Set parameters and env variables for selected AUTOSTART
|
|
#
|
|
if param compare SYS_AUTOSTART 0
|
|
then
|
|
ekf2 start
|
|
else
|
|
sh /etc/init.d/rc.autostart
|
|
fi
|
|
unset MODE
|
|
|
|
#
|
|
# If mount (gimbal) control is enabled and output mode is AUX, set the aux
|
|
# mixer to mount (override the airframe-specific MIXER_AUX setting)
|
|
#
|
|
if param compare MNT_MODE_IN -1
|
|
then
|
|
else
|
|
if param compare MNT_MODE_OUT 0
|
|
then
|
|
set MIXER_AUX mount
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Wipe incompatible settings for boards not having two outputs
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
set MIXER_AUX none
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V5
|
|
then
|
|
set MIXER_AUX none
|
|
fi
|
|
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
set MIXER_AUX none
|
|
fi
|
|
|
|
#
|
|
# Override parameters from user configuration file
|
|
#
|
|
set FCONFIG /fs/microsd/etc/config.txt
|
|
if [ -f $FCONFIG ]
|
|
then
|
|
echo "Custom: ${FCONFIG}"
|
|
sh $FCONFIG
|
|
fi
|
|
unset FCONFIG
|
|
|
|
#
|
|
# If autoconfig parameter was set, reset it and save parameters
|
|
#
|
|
if [ $AUTOCNF == yes ]
|
|
then
|
|
# Disable safety switch by default on Pixracer
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
param set CBRK_IO_SAFETY 22027
|
|
fi
|
|
param set SYS_AUTOCONFIG 0
|
|
fi
|
|
unset AUTOCNF
|
|
|
|
set IO_PRESENT no
|
|
|
|
if [ $USE_IO == yes ]
|
|
then
|
|
#
|
|
# Check if PX4IO present and update firmware if needed
|
|
#
|
|
if [ -f /etc/extras/px4io-v2.bin ]
|
|
then
|
|
set IO_FILE /etc/extras/px4io-v2.bin
|
|
else
|
|
set IO_FILE /etc/extras/px4io-v1.bin
|
|
fi
|
|
|
|
if px4io checkcrc ${IO_FILE}
|
|
then
|
|
echo "[init] PX4IO CRC OK" >> $LOG_FILE
|
|
|
|
set IO_PRESENT yes
|
|
else
|
|
tone_alarm MLL32CP8MB
|
|
|
|
if px4io start
|
|
then
|
|
# try to safe px4 io so motor outputs dont go crazy
|
|
if px4io safety_on
|
|
then
|
|
# success! no-op
|
|
else
|
|
# px4io did not respond to the safety command
|
|
px4io stop
|
|
fi
|
|
fi
|
|
|
|
if px4io forceupdate 14662 ${IO_FILE}
|
|
then
|
|
usleep 10000
|
|
if px4io checkcrc ${IO_FILE}
|
|
then
|
|
echo "PX4IO CRC OK after updating" >> $LOG_FILE
|
|
tone_alarm MLL8CDE
|
|
|
|
set IO_PRESENT yes
|
|
else
|
|
echo "PX4IO update failed" >> $LOG_FILE
|
|
tone_alarm ${TUNE_ERR}
|
|
fi
|
|
else
|
|
echo "PX4IO update failed" >> $LOG_FILE
|
|
tone_alarm ${TUNE_ERR}
|
|
fi
|
|
fi
|
|
unset IO_FILE
|
|
|
|
if [ $IO_PRESENT == no ]
|
|
then
|
|
echo "PX4IO not found" >> $LOG_FILE
|
|
tone_alarm ${TUNE_ERR}
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Set default output if not set
|
|
#
|
|
if [ $OUTPUT_MODE == none ]
|
|
then
|
|
if [ $USE_IO == yes ]
|
|
then
|
|
set OUTPUT_MODE io
|
|
else
|
|
set OUTPUT_MODE fmu
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
|
|
then
|
|
# Need IO for output but it not present, disable output
|
|
set OUTPUT_MODE none
|
|
|
|
# Avoid using ttyS0 for MAVLink on FMUv1
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
set FMU_MODE serial
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == ardrone ]
|
|
then
|
|
set FMU_MODE gpio_serial
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == tap_esc ]
|
|
then
|
|
set FMU_MODE rcin
|
|
fi
|
|
|
|
if [ $HIL == yes ]
|
|
then
|
|
set OUTPUT_MODE hil
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
set FMU_MODE serial
|
|
fi
|
|
else
|
|
gps start
|
|
fi
|
|
|
|
set DATAMAN_OPT ""
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
set DATAMAN_OPT -i
|
|
fi
|
|
if ver hwcmp AEROCORE2
|
|
then
|
|
set DATAMAN_OPT "-f /fs/mtd_dataman"
|
|
fi
|
|
# waypoint storage
|
|
# REBOOTWORK this needs to start in parallel
|
|
if dataman start $DATAMAN_OPT
|
|
then
|
|
fi
|
|
unset DATAMAN_OPT
|
|
|
|
#
|
|
# Sensors System (start before Commander so Preflight checks are properly run)
|
|
#
|
|
if [ $HIL == yes ]
|
|
then
|
|
sensors start -h
|
|
else
|
|
sh /etc/init.d/rc.sensors
|
|
fi
|
|
unset HIL
|
|
|
|
# Needs to be this early for in-air-restarts
|
|
if [ $OUTPUT_MODE == hil ]
|
|
then
|
|
commander start --hil
|
|
else
|
|
commander start
|
|
fi
|
|
|
|
if send_event start
|
|
then
|
|
fi
|
|
|
|
#
|
|
# Start CPU load monitor
|
|
#
|
|
load_mon start
|
|
|
|
#
|
|
# Start primary output
|
|
#
|
|
set TTYS1_BUSY no
|
|
|
|
#
|
|
# Check if UAVCAN is enabled, default to it for ESCs
|
|
#
|
|
if param greater UAVCAN_ENABLE 2
|
|
then
|
|
set OUTPUT_MODE uavcan_esc
|
|
fi
|
|
|
|
# Sensors on the PWM interface bank
|
|
if param compare SENS_EN_LL40LS 1
|
|
then
|
|
# clear pins 5 and 6
|
|
set FMU_MODE pwm4
|
|
set AUX_MODE pwm4
|
|
fi
|
|
if param greater TRIG_MODE 0
|
|
then
|
|
# We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output
|
|
if param compare TRIG_PINS 56
|
|
then
|
|
# clear pins 5 and 6
|
|
set FMU_MODE pwm4
|
|
set AUX_MODE pwm4
|
|
else
|
|
set FMU_MODE none
|
|
set AUX_MODE none
|
|
fi
|
|
camera_trigger start
|
|
|
|
param set CAM_FBACK_MODE 1
|
|
camera_feedback start
|
|
fi
|
|
|
|
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
|
|
if [ $OUTPUT_MODE != none ]
|
|
then
|
|
if [ $OUTPUT_MODE == uavcan_esc ]
|
|
then
|
|
if param compare UAVCAN_ENABLE 0
|
|
then
|
|
echo "OVERRIDING UAVCAN_ENABLE = 3" >> $LOG_FILE
|
|
param set UAVCAN_ENABLE 3
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
|
|
then
|
|
if px4io start
|
|
then
|
|
sh /etc/init.d/rc.io
|
|
else
|
|
echo "PX4IO start failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
|
|
then
|
|
if fmu mode_$FMU_MODE $FMU_ARGS
|
|
then
|
|
else
|
|
echo "FMU start failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
|
then
|
|
set TTYS1_BUSY yes
|
|
fi
|
|
if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
|
|
then
|
|
set TTYS1_BUSY yes
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == mkblctrl ]
|
|
then
|
|
set MKBLCTRL_ARG ""
|
|
if [ $MKBLCTRL_MODE == x ]
|
|
then
|
|
set MKBLCTRL_ARG "-mkmode x"
|
|
fi
|
|
if [ $MKBLCTRL_MODE == + ]
|
|
then
|
|
set MKBLCTRL_ARG "-mkmode +"
|
|
fi
|
|
|
|
if mkblctrl $MKBLCTRL_ARG
|
|
then
|
|
else
|
|
echo "MK start failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
unset MKBLCTRL_ARG
|
|
fi
|
|
unset MK_MODE
|
|
|
|
if [ $OUTPUT_MODE == hil ]
|
|
then
|
|
if pwm_out_sim mode_pwm16
|
|
then
|
|
else
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start IO or FMU for RC PPM input if needed
|
|
#
|
|
if [ $IO_PRESENT == yes ]
|
|
then
|
|
if [ $OUTPUT_MODE != io ]
|
|
then
|
|
if px4io start
|
|
then
|
|
sh /etc/init.d/rc.io
|
|
else
|
|
echo "PX4IO start failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
fi
|
|
else
|
|
if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
|
|
then
|
|
if fmu mode_${FMU_MODE} $FMU_ARGS
|
|
then
|
|
else
|
|
echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_ERR
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
|
|
then
|
|
set TTYS1_BUSY yes
|
|
fi
|
|
if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
|
|
then
|
|
set TTYS1_BUSY yes
|
|
fi
|
|
fi
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
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_F "-r 1200 -d /dev/ttyS0"
|
|
|
|
# Exit from nsh to free port for mavlink
|
|
set EXIT_ON_END yes
|
|
else
|
|
set MAVLINK_F "-r 1200 -f"
|
|
# Avoid using ttyS1 for MAVLink on FMUv4
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
set MAVLINK_F "-r 1200 -d /dev/ttyS1"
|
|
# Start MAVLink on Wifi (ESP8266 port)
|
|
mavlink start -r 20000 -m config -b 921600 -d /dev/ttyS0
|
|
fi
|
|
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
set MAVLINK_F "-r 1200 -d /dev/ttyS3"
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp CRAZYFLIE
|
|
then
|
|
# Avoid using either of the two available serials
|
|
set MAVLINK_F none
|
|
fi
|
|
fi
|
|
|
|
if [ "x$MAVLINK_F" == xnone ]
|
|
then
|
|
else
|
|
mavlink start ${MAVLINK_F}
|
|
fi
|
|
unset MAVLINK_F
|
|
|
|
#
|
|
# MAVLink onboard / TELEM2
|
|
#
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
else
|
|
# XXX We need a better way for runtime eval of shell variables,
|
|
# but this works for now
|
|
if param compare SYS_COMPANION 10
|
|
then
|
|
frsky_telemetry start -d ${MAVLINK_COMPANION_DEVICE}
|
|
fi
|
|
if param compare SYS_COMPANION 20
|
|
then
|
|
syslink start
|
|
mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
|
|
fi
|
|
if param compare SYS_COMPANION 921600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
|
|
fi
|
|
if param compare SYS_COMPANION 57600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m onboard -r 5000 -x -f
|
|
fi
|
|
if param compare SYS_COMPANION 460800
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 460800 -m onboard -r 5000 -x -f
|
|
fi
|
|
if param compare SYS_COMPANION 157600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m osd -r 1000
|
|
fi
|
|
if param compare SYS_COMPANION 257600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m magic -r 5000 -x -f
|
|
fi
|
|
if param compare SYS_COMPANION 319200
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -r 1000
|
|
fi
|
|
if param compare SYS_COMPANION 338400
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -r 1000
|
|
fi
|
|
if param compare SYS_COMPANION 357600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -r 1000
|
|
fi
|
|
if param compare SYS_COMPANION 3115200
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -r 1000
|
|
fi
|
|
if param compare SYS_COMPANION 419200
|
|
then
|
|
iridiumsbd start -d /dev/ttyS2
|
|
mavlink start -d /dev/iridium -b 19200 -m iridium -r 10
|
|
fi
|
|
if param compare SYS_COMPANION 1921600
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -r 20000
|
|
fi
|
|
if param compare SYS_COMPANION 1500000
|
|
then
|
|
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 1500000 -m onboard -r 10000 -x -f
|
|
fi
|
|
fi
|
|
|
|
unset MAVLINK_COMPANION_DEVICE
|
|
|
|
#
|
|
# Starting stuff according to UAVCAN_ENABLE value
|
|
#
|
|
if param greater UAVCAN_ENABLE 0
|
|
then
|
|
if uavcan start
|
|
then
|
|
set LOGGER_BUF 6
|
|
uavcan start fw
|
|
else
|
|
tone_alarm ${TUNE_ERR}
|
|
fi
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
frsky_telemetry start -d /dev/ttyS6
|
|
fi
|
|
|
|
if ver hwcmp MINDPX_V2
|
|
then
|
|
frsky_telemetry start -d /dev/ttyS6
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V2
|
|
then
|
|
# Check for flow sensor - as it is a background task, launch it last
|
|
px4flow start &
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
# Check for flow sensor - as it is a background task, launch it last
|
|
px4flow start &
|
|
fi
|
|
|
|
if ver hwcmp MINDPX_V2
|
|
then
|
|
px4flow start &
|
|
fi
|
|
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
else
|
|
# Start MAVLink
|
|
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
|
|
fi
|
|
|
|
#
|
|
# Logging
|
|
#
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
if sdlog2 start -r 30 -a -b 2 -t
|
|
then
|
|
fi
|
|
else
|
|
if param compare SYS_LOGGER 0
|
|
then
|
|
# check if we should increase logging rate for ekf2 replay message logging
|
|
if param greater EKF2_REC_RPL 0
|
|
then
|
|
if sdlog2 start -r 500 -e -b 18 -t
|
|
then
|
|
fi
|
|
else
|
|
if sdlog2 start -r 100 -a -b 9 -t
|
|
then
|
|
fi
|
|
fi
|
|
else
|
|
set LOGGER_ARGS ""
|
|
#
|
|
# Adjust FMUv5 logging settings
|
|
#
|
|
if ver hwcmp PX4FMU_V5
|
|
then
|
|
set LOGGER_BUF 64
|
|
param set SDLOG_MODE 2
|
|
fi
|
|
if param compare SDLOG_MODE 1
|
|
then
|
|
set LOGGER_ARGS "-e"
|
|
fi
|
|
if param compare SDLOG_MODE 2
|
|
then
|
|
set LOGGER_ARGS "-f"
|
|
fi
|
|
if ver hwcmp AEROFC_V1
|
|
then
|
|
set LOGGER_ARGS "-m mavlink"
|
|
fi
|
|
if logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS}
|
|
then
|
|
fi
|
|
unset LOGGER_BUF
|
|
unset LOGGER_ARGS
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start up ARDrone Motor interface
|
|
#
|
|
if [ $OUTPUT_MODE == ardrone ]
|
|
then
|
|
ardrone_interface start -d /dev/ttyS1
|
|
fi
|
|
|
|
#
|
|
# Fixed wing setup
|
|
#
|
|
if [ $VEHICLE_TYPE == fw ]
|
|
then
|
|
if [ $MIXER == none ]
|
|
then
|
|
# Set default mixer for fixed wing if not defined
|
|
set MIXER AERT
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use MAV_TYPE = 1 (fixed wing) if not defined
|
|
set MAV_TYPE 1
|
|
fi
|
|
|
|
param set MAV_TYPE ${MAV_TYPE}
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard fixedwing apps
|
|
sh /etc/init.d/rc.fw_apps
|
|
fi
|
|
|
|
#
|
|
# Multicopters setup
|
|
#
|
|
if [ $VEHICLE_TYPE == mc ]
|
|
then
|
|
if [ $MIXER == none ]
|
|
then
|
|
echo "Mixer undefined"
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use mixer to detect vehicle type
|
|
if [ $MIXER == quad_x -o $MIXER == quad_+ ]
|
|
then
|
|
set MAV_TYPE 2
|
|
fi
|
|
if [ $MIXER == quad_w -o $MIXER == quad_dc ]
|
|
then
|
|
set MAV_TYPE 2
|
|
fi
|
|
if [ $MIXER == quad_h ]
|
|
then
|
|
set MAV_TYPE 2
|
|
fi
|
|
if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ]
|
|
then
|
|
set MAV_TYPE 15
|
|
fi
|
|
if [ $MIXER == hexa_x -o $MIXER == hexa_+ ]
|
|
then
|
|
set MAV_TYPE 13
|
|
fi
|
|
if [ $MIXER == hexa_cox ]
|
|
then
|
|
set MAV_TYPE 13
|
|
fi
|
|
if [ $MIXER == octo_x -o $MIXER == octo_+ ]
|
|
then
|
|
set MAV_TYPE 14
|
|
fi
|
|
if [ $MIXER == octo_cox -o $MIXER == octo_cox_w ]
|
|
then
|
|
set MAV_TYPE 14
|
|
fi
|
|
if [ $MIXER == coax ]
|
|
then
|
|
set MAV_TYPE 3
|
|
fi
|
|
fi
|
|
|
|
# Still no MAV_TYPE found
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
echo "Unknown MAV_TYPE"
|
|
param set MAV_TYPE 2
|
|
else
|
|
param set MAV_TYPE ${MAV_TYPE}
|
|
fi
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard multicopter apps
|
|
sh /etc/init.d/rc.mc_apps
|
|
fi
|
|
|
|
#
|
|
# VTOL setup
|
|
#
|
|
if [ $VEHICLE_TYPE == vtol ]
|
|
then
|
|
if [ $MIXER == none ]
|
|
then
|
|
echo "VTOL mixer undefined"
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use mixer to detect vehicle type
|
|
if [ $MIXER == caipirinha_vtol ]
|
|
then
|
|
set MAV_TYPE 19
|
|
fi
|
|
if [ $MIXER == firefly6 ]
|
|
then
|
|
set MAV_TYPE 21
|
|
fi
|
|
if [ $MIXER == quad_x_pusher_vtol ]
|
|
then
|
|
set MAV_TYPE 22
|
|
fi
|
|
fi
|
|
|
|
# Still no MAV_TYPE found
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
echo "Unknown MAV_TYPE"
|
|
param set MAV_TYPE 19
|
|
else
|
|
param set MAV_TYPE ${MAV_TYPE}
|
|
fi
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard vtol apps
|
|
sh /etc/init.d/rc.vtol_apps
|
|
fi
|
|
|
|
#
|
|
# UGV setup
|
|
#
|
|
if [ $VEHICLE_TYPE == ugv ]
|
|
then
|
|
if [ $MIXER == none ]
|
|
then
|
|
# Set default mixer for UGV if not defined
|
|
set MIXER stampede
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use MAV_TYPE = 10 (UGV) if not defined
|
|
set MAV_TYPE 10
|
|
fi
|
|
|
|
param set MAV_TYPE ${MAV_TYPE}
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard UGV apps
|
|
sh /etc/init.d/rc.gnd_apps
|
|
fi
|
|
|
|
#
|
|
# For snapdragon, we need a passthrough mode
|
|
# Do not run any mavlink instances since we need the serial port for
|
|
# communication with Snapdragon.
|
|
#
|
|
if [ $VEHICLE_TYPE == passthrough ]
|
|
then
|
|
mavlink stop-all
|
|
commander stop
|
|
|
|
# Stop multicopter attitude controller if it is running, the controls come
|
|
# from Snapdragon.
|
|
if mc_att_control stop
|
|
then
|
|
fi
|
|
|
|
# Start snapdragon interface on serial port.
|
|
if ver hwcmp PX4FMU_V2
|
|
then
|
|
# On Pixfalcon use the standard telemetry port (Telem 1).
|
|
snapdragon_rc_pwm start -d /dev/ttyS1
|
|
px4io start
|
|
fi
|
|
|
|
if ver hwcmp PX4FMU_V4
|
|
then
|
|
# On Pixracer use Telem 2 port (TL2).
|
|
snapdragon_rc_pwm start -d /dev/ttyS2
|
|
fmu mode_pwm4 $FMU_ARGS
|
|
fi
|
|
|
|
pwm failsafe -c 1234 -p 900
|
|
pwm disarmed -c 1234 -p 900
|
|
|
|
# Arm straightaway.
|
|
pwm arm
|
|
# Use 400 Hz PWM on all channels.
|
|
pwm rate -a -r 400
|
|
fi
|
|
|
|
unset MIXER
|
|
unset MAV_TYPE
|
|
unset OUTPUT_MODE
|
|
|
|
#
|
|
# Start the navigator
|
|
#
|
|
navigator start
|
|
|
|
#
|
|
# Generic setup (autostart ID not found)
|
|
#
|
|
if [ $VEHICLE_TYPE == none ]
|
|
then
|
|
echo "No autostart ID found"
|
|
fi
|
|
|
|
# Start any custom addons
|
|
set FEXTRAS /fs/microsd/etc/extras.txt
|
|
if [ -f $FEXTRAS ]
|
|
then
|
|
echo "Addons script: ${FEXTRAS}"
|
|
sh $FEXTRAS
|
|
fi
|
|
unset FEXTRAS
|
|
|
|
if ver hwcmp CRAZYFLIE
|
|
then
|
|
# CF2 shouldn't have an sd card
|
|
else
|
|
|
|
if ver hwcmp AEROCORE2
|
|
then
|
|
# AEROCORE2 shouldn't have an sd card
|
|
else
|
|
|
|
# Run no SD alarm
|
|
if [ $LOG_FILE == /dev/null ]
|
|
then
|
|
# Play SOS
|
|
tone_alarm error
|
|
fi
|
|
|
|
fi
|
|
|
|
fi
|
|
|
|
#
|
|
# Check if we should start a thermal calibration
|
|
# TODO move further up and don't start unnecessary services if we are calibrating
|
|
#
|
|
set TEMP_CALIB_ARGS ""
|
|
if param compare SYS_CAL_GYRO 1
|
|
then
|
|
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -g"
|
|
param set SYS_CAL_GYRO 0
|
|
fi
|
|
if param compare SYS_CAL_ACCEL 1
|
|
then
|
|
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a"
|
|
param set SYS_CAL_ACCEL 0
|
|
fi
|
|
if param compare SYS_CAL_BARO 1
|
|
then
|
|
set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -b"
|
|
param set SYS_CAL_BARO 0
|
|
fi
|
|
if [ "x$TEMP_CALIB_ARGS" != "x" ]
|
|
then
|
|
send_event temperature_calibration ${TEMP_CALIB_ARGS}
|
|
fi
|
|
unset TEMP_CALIB_ARGS
|
|
|
|
# vmount to control mounts such as gimbals, disabled by default.
|
|
if param compare MNT_MODE_IN -1
|
|
then
|
|
else
|
|
if vmount start
|
|
then
|
|
fi
|
|
fi
|
|
|
|
# End of autostart
|
|
fi
|
|
|
|
# There is no further script processing, so we can free some RAM
|
|
# XXX potentially unset all script variables.
|
|
unset TUNE_ERR
|
|
|
|
# Boot is complete, inform MAVLink app(s) that the system is now fully up and running
|
|
mavlink boot_complete
|
|
|
|
if [ $EXIT_ON_END == yes ]
|
|
then
|
|
echo "NSH exit"
|
|
exit
|
|
fi
|
|
unset EXIT_ON_END
|