forked from Archive/PX4-Autopilot
619 lines
10 KiB
Plaintext
619 lines
10 KiB
Plaintext
#!nsh
|
|
#
|
|
# PX4FMU startup script.
|
|
|
|
#
|
|
# 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 TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
|
|
|
|
#
|
|
# Try to mount the microSD card.
|
|
#
|
|
echo "[init] 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"
|
|
# 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 ]
|
|
then
|
|
echo "[init] Executing init script: $RC_FILE"
|
|
sh $RC_FILE
|
|
set MODE custom
|
|
else
|
|
echo "[init] Init script not found: $RC_FILE"
|
|
fi
|
|
|
|
# if this is an APM build then there will be a rc.APM script
|
|
# from an EXTERNAL_SCRIPTS build option
|
|
if [ -f /etc/init.d/rc.APM ]
|
|
then
|
|
if sercon
|
|
then
|
|
echo "[init] USB interface connected"
|
|
fi
|
|
|
|
echo "[init] 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"
|
|
|
|
#
|
|
# Start CDC/ACM serial driver
|
|
#
|
|
sercon
|
|
|
|
# Try to get an USB console
|
|
nshterm /dev/ttyACM0 &
|
|
|
|
#
|
|
# 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
|
|
echo "[param] Loaded: $PARAM_FILE"
|
|
else
|
|
echo "[param] FAILED loading $PARAM_FILE"
|
|
if param reset
|
|
then
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start system state indicator
|
|
#
|
|
if rgbled start
|
|
then
|
|
else
|
|
if blinkm start
|
|
then
|
|
blinkm systemstate
|
|
fi
|
|
fi
|
|
|
|
if pca8574 start
|
|
then
|
|
fi
|
|
|
|
#
|
|
# Set default values
|
|
#
|
|
set HIL no
|
|
set VEHICLE_TYPE none
|
|
set MIXER none
|
|
set OUTPUT_MODE none
|
|
set PWM_OUTPUTS none
|
|
set PWM_RATE none
|
|
set PWM_DISARMED none
|
|
set PWM_MIN none
|
|
set PWM_MAX none
|
|
set MKBLCTRL_MODE none
|
|
set FMU_MODE pwm
|
|
set MAVLINK_FLAGS default
|
|
set EXIT_ON_END no
|
|
set MAV_TYPE none
|
|
set LOAD_DEFAULT_APPS yes
|
|
set GPS yes
|
|
set GPS_FAKE no
|
|
set FAILSAFE none
|
|
|
|
#
|
|
# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
|
|
#
|
|
if param compare SYS_AUTOCONFIG 1
|
|
then
|
|
# Wipe out params
|
|
param reset_nostart
|
|
set DO_AUTOCONFIG yes
|
|
else
|
|
set DO_AUTOCONFIG no
|
|
fi
|
|
|
|
#
|
|
# Set USE_IO flag
|
|
#
|
|
if param compare SYS_USE_IO 1
|
|
then
|
|
set USE_IO yes
|
|
else
|
|
set USE_IO no
|
|
fi
|
|
|
|
#
|
|
# Set parameters and env variables for selected AUTOSTART
|
|
#
|
|
if param compare SYS_AUTOSTART 0
|
|
then
|
|
echo "[init] No autostart"
|
|
else
|
|
sh /etc/init.d/rc.autostart
|
|
fi
|
|
|
|
#
|
|
# Override parameters from user configuration file
|
|
#
|
|
if [ -f $CONFIG_FILE ]
|
|
then
|
|
echo "[init] Config: $CONFIG_FILE"
|
|
sh $CONFIG_FILE
|
|
else
|
|
echo "[init] Config not found: $CONFIG_FILE"
|
|
fi
|
|
|
|
#
|
|
# If autoconfig parameter was set, reset it and save parameters
|
|
#
|
|
if [ $DO_AUTOCONFIG == yes ]
|
|
then
|
|
param set SYS_AUTOCONFIG 0
|
|
param save
|
|
fi
|
|
|
|
set IO_PRESENT no
|
|
|
|
if [ $USE_IO == yes ]
|
|
then
|
|
#
|
|
# Check if PX4IO present and update firmware if needed
|
|
#
|
|
if [ -f /etc/extras/px4io-v2_default.bin ]
|
|
then
|
|
set IO_FILE /etc/extras/px4io-v2_default.bin
|
|
else
|
|
set IO_FILE /etc/extras/px4io-v1_default.bin
|
|
fi
|
|
|
|
if px4io checkcrc $IO_FILE
|
|
then
|
|
echo "PX4IO CRC OK" >> $LOG_FILE
|
|
|
|
set IO_PRESENT yes
|
|
else
|
|
echo "PX4IO Trying to update" >> $LOG_FILE
|
|
|
|
tone_alarm MLL32CP8MB
|
|
|
|
if px4io forceupdate 14662 $IO_FILE
|
|
then
|
|
usleep 500000
|
|
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_OUT_ERROR
|
|
fi
|
|
else
|
|
echo "PX4IO update failed" >> $LOG_FILE
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
fi
|
|
fi
|
|
|
|
if [ $IO_PRESENT == no ]
|
|
then
|
|
echo "[init] ERROR: PX4IO not found"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
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
|
|
echo "[init] ERROR: PX4IO not found, disabling output"
|
|
|
|
# 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 [ $HIL == yes ]
|
|
then
|
|
set OUTPUT_MODE hil
|
|
if ver hwcmp PX4FMU_V1
|
|
then
|
|
set FMU_MODE serial
|
|
fi
|
|
fi
|
|
|
|
# waypoint storage
|
|
if dataman start
|
|
then
|
|
fi
|
|
|
|
# Needs to be this early for in-air-restarts
|
|
commander start
|
|
|
|
#
|
|
# Start primary output
|
|
#
|
|
set TTYS1_BUSY no
|
|
|
|
# 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 "[init] OVERRIDING UAVCAN_ENABLE = 1"
|
|
param set UAVCAN_ENABLE 1
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
|
|
then
|
|
if px4io start
|
|
then
|
|
echo "[init] PX4IO started"
|
|
sh /etc/init.d/rc.io
|
|
else
|
|
echo "[init] ERROR: PX4IO start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
fi
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
|
|
then
|
|
if fmu mode_$FMU_MODE
|
|
then
|
|
echo "[init] FMU mode_$FMU_MODE started"
|
|
else
|
|
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
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
|
|
echo "[init] MKBLCTRL started"
|
|
else
|
|
echo "[init] ERROR: MKBLCTRL start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
fi
|
|
|
|
fi
|
|
|
|
if [ $OUTPUT_MODE == hil ]
|
|
then
|
|
if hil mode_port2_pwm8
|
|
then
|
|
echo "[init] HIL output started"
|
|
else
|
|
echo "[init] ERROR: HIL output start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
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 "[init] ERROR: PX4IO start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
fi
|
|
fi
|
|
else
|
|
if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
|
|
then
|
|
if fmu mode_$FMU_MODE
|
|
then
|
|
echo "[init] FMU mode_$FMU_MODE started"
|
|
else
|
|
echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
|
|
tone_alarm $TUNE_OUT_ERROR
|
|
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_FLAGS == 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"
|
|
|
|
# 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"
|
|
fi
|
|
fi
|
|
|
|
mavlink start $MAVLINK_FLAGS
|
|
|
|
#
|
|
# UAVCAN
|
|
#
|
|
sh /etc/init.d/rc.uavcan
|
|
|
|
#
|
|
# Sensors, Logging, GPS
|
|
#
|
|
sh /etc/init.d/rc.sensors
|
|
sh /etc/init.d/rc.logging
|
|
|
|
if [ $GPS == yes ]
|
|
then
|
|
if [ $GPS_FAKE == yes ]
|
|
then
|
|
echo "[init] Faking GPS"
|
|
gps start -f
|
|
else
|
|
gps start
|
|
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
|
|
echo "[init] Vehicle type: FIXED WING"
|
|
|
|
if [ $MIXER == none ]
|
|
then
|
|
# Set default mixer for fixed wing if not defined
|
|
set MIXER FMU_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
|
|
if [ $LOAD_DEFAULT_APPS == yes ]
|
|
then
|
|
sh /etc/init.d/rc.fw_apps
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Multicopters setup
|
|
#
|
|
if [ $VEHICLE_TYPE == mc ]
|
|
then
|
|
echo "[init] Vehicle type: MULTICOPTER"
|
|
|
|
if [ $MIXER == none ]
|
|
then
|
|
echo "Default mixer for multicopter not defined"
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use mixer to detect vehicle type
|
|
if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ]
|
|
then
|
|
set MAV_TYPE 2
|
|
fi
|
|
if [ $MIXER == FMU_quad_w ]
|
|
then
|
|
set MAV_TYPE 2
|
|
fi
|
|
if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
|
|
then
|
|
set MAV_TYPE 13
|
|
fi
|
|
if [ $MIXER == FMU_hexa_cox ]
|
|
then
|
|
set MAV_TYPE 13
|
|
fi
|
|
if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
|
|
then
|
|
set MAV_TYPE 14
|
|
fi
|
|
if [ $MIXER == FMU_octo_cox ]
|
|
then
|
|
set MAV_TYPE 14
|
|
fi
|
|
fi
|
|
|
|
# Still no MAV_TYPE found
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
echo "Unknown MAV_TYPE"
|
|
else
|
|
param set MAV_TYPE $MAV_TYPE
|
|
fi
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard multicopter apps
|
|
if [ $LOAD_DEFAULT_APPS == yes ]
|
|
then
|
|
sh /etc/init.d/rc.mc_apps
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# VTOL setup
|
|
#
|
|
if [ $VEHICLE_TYPE == vtol ]
|
|
then
|
|
echo "[init] Vehicle type: VTOL"
|
|
|
|
if [ $MIXER == none ]
|
|
then
|
|
echo "Default mixer for vtol not defined"
|
|
fi
|
|
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
# Use mixer to detect vehicle type
|
|
if [ $MIXER == FMU_caipirinha_vtol ]
|
|
then
|
|
set MAV_TYPE 19
|
|
fi
|
|
fi
|
|
|
|
# Still no MAV_TYPE found
|
|
if [ $MAV_TYPE == none ]
|
|
then
|
|
echo "Unknown MAV_TYPE"
|
|
else
|
|
param set MAV_TYPE $MAV_TYPE
|
|
fi
|
|
|
|
# Load mixer and configure outputs
|
|
sh /etc/init.d/rc.interface
|
|
|
|
# Start standard vtol apps
|
|
if [ $LOAD_DEFAULT_APPS == yes ]
|
|
then
|
|
sh /etc/init.d/rc.vtol_apps
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start the navigator
|
|
#
|
|
navigator start
|
|
|
|
#
|
|
# Generic setup (autostart ID not found)
|
|
#
|
|
if [ $VEHICLE_TYPE == none ]
|
|
then
|
|
echo "[init] Vehicle type: No autostart ID found"
|
|
|
|
fi
|
|
|
|
# Start any custom addons
|
|
if [ -f $EXTRAS_FILE ]
|
|
then
|
|
echo "[init] Starting addons script: $EXTRAS_FILE"
|
|
sh $EXTRAS_FILE
|
|
else
|
|
echo "[init] No addons script: $EXTRAS_FILE"
|
|
fi
|
|
|
|
if [ $EXIT_ON_END == yes ]
|
|
then
|
|
echo "[init] Exit from nsh"
|
|
exit
|
|
fi
|
|
|
|
# End of autostart
|
|
fi
|