You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
604 lines
11 KiB
604 lines
11 KiB
#!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 |
|
|
|
# |
|
# 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 |
|
echo "[init] RGB Led" |
|
else |
|
if blinkm start |
|
then |
|
echo "[init] BlinkM" |
|
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 DO_AUTOCONFIG flag to use it in AUTOSTART scripts |
|
# |
|
if param compare SYS_AUTOCONFIG 1 |
|
then |
|
# We can't be sure the defaults haven't changed, so |
|
# if someone requests a re-configuration, we do it |
|
# cleanly from scratch (except autostart / autoconfig) |
|
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 "[init] PX4IO CRC OK" |
|
echo "PX4IO CRC OK" >> $LOG_FILE |
|
|
|
set IO_PRESENT yes |
|
else |
|
echo "[init] Trying to update" |
|
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 "[init] PX4IO CRC OK, update successful" |
|
echo "PX4IO CRC OK after updating" >> $LOG_FILE |
|
tone_alarm MLL8CDE |
|
|
|
set IO_PRESENT yes |
|
else |
|
echo "[init] ERROR: PX4IO update failed" |
|
echo "PX4IO update failed" >> $LOG_FILE |
|
tone_alarm $TUNE_OUT_ERROR |
|
fi |
|
else |
|
echo "[init] ERROR: PX4IO update failed" |
|
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 |
|
|
|
# Try to get an USB console |
|
nshterm /dev/ttyACM0 & |
|
|
|
# |
|
# Start the datamanager (and do not abort boot if it fails) |
|
# |
|
if dataman start |
|
then |
|
fi |
|
|
|
# |
|
# Start the Commander (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 |
|
echo "[init] Use PX4IO PWM as primary output" |
|
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 |
|
echo "[init] Use FMU as primary output" |
|
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 |
|
echo "[init] Use MKBLCTRL as primary output" |
|
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 |
|
echo "[init] Use HIL as primary output" |
|
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 |
|
echo "[init] PX4IO started" |
|
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 |
|
|
|
# |
|
# MAVLink |
|
# |
|
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 |
|
|
|
# |
|
# Start logging in all modes, including HIL |
|
# |
|
sh /etc/init.d/rc.logging |
|
|
|
if [ $GPS == yes ] |
|
then |
|
echo "[init] Start GPS" |
|
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 |
|
|
|
# |
|
# 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
|
|
|