forked from Archive/PX4-Autopilot
561 lines
11 KiB
Bash
561 lines
11 KiB
Bash
#!/bin/sh
|
|
# 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.
|
|
#
|
|
#------------------------------------------------------------------------------
|
|
|
|
#
|
|
# Set default paramter values.
|
|
# Do not add intra word spaces
|
|
# it wastes flash
|
|
#
|
|
set R /
|
|
set FCONFIG /fs/microsd/etc/config.txt
|
|
set FEXTRAS /fs/microsd/etc/extras.txt
|
|
set FRC /fs/microsd/etc/rc.txt
|
|
set IOFW "/etc/extras/px4_io-v2_default.bin"
|
|
set LOGGER_ARGS ""
|
|
set LOGGER_BUF 8
|
|
set PARAM_FILE ""
|
|
set PARAM_BACKUP_FILE ""
|
|
set RC_INPUT_ARGS ""
|
|
set SDCARD_AVAILABLE no
|
|
set SDCARD_EXT_PATH /fs/microsd/ext_autostart
|
|
set SDCARD_FORMAT no
|
|
set STARTUP_TUNE 1
|
|
set VEHICLE_TYPE none
|
|
|
|
#
|
|
# Print full system version.
|
|
#
|
|
ver all
|
|
|
|
#
|
|
# Try to mount the microSD card.
|
|
#
|
|
if [ -b "/dev/mmcsd0" ]
|
|
then
|
|
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
|
then
|
|
if [ -f "/fs/microsd/.format" ]
|
|
then
|
|
echo "INFO [init] format /dev/mmcsd0 requested (/fs/microsd/.format)"
|
|
set SDCARD_FORMAT yes
|
|
rm /fs/microsd/.format
|
|
umount /fs/microsd
|
|
|
|
else
|
|
set SDCARD_AVAILABLE yes
|
|
fi
|
|
fi
|
|
|
|
if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ]
|
|
then
|
|
echo "INFO [init] formatting /dev/mmcsd0"
|
|
set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds)
|
|
|
|
if mkfatfs -F 32 /dev/mmcsd0
|
|
then
|
|
echo "INFO [init] card formatted"
|
|
|
|
if mount -t vfat /dev/mmcsd0 /fs/microsd
|
|
then
|
|
set SDCARD_AVAILABLE yes
|
|
set STARTUP_TUNE 14 # tune 14 = SD_INIT
|
|
else
|
|
echo "ERROR [init] card mount failed"
|
|
fi
|
|
else
|
|
echo "ERROR [init] format failed"
|
|
fi
|
|
fi
|
|
|
|
if [ $SDCARD_AVAILABLE = yes ]
|
|
then
|
|
if hardfault_log check
|
|
then
|
|
set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
|
|
if hardfault_log commit
|
|
then
|
|
hardfault_log reset
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
set PARAM_FILE /fs/microsd/params
|
|
set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson"
|
|
fi
|
|
|
|
#
|
|
# Look for an init script on the microSD card.
|
|
# Disable autostart if the script found.
|
|
#
|
|
if [ -f $FRC ]
|
|
then
|
|
. $FRC
|
|
else
|
|
|
|
#
|
|
# Set the parameter file the board supports params on
|
|
# MTD device.
|
|
#
|
|
if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
|
|
then
|
|
set PARAM_FILE /fs/mtd_params
|
|
fi
|
|
|
|
#
|
|
# Load parameters.
|
|
#
|
|
# if the board has a storage for (factory) calibration data
|
|
if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata
|
|
then
|
|
param load /fs/mtd_caldata
|
|
fi
|
|
|
|
param select $PARAM_FILE
|
|
if ! param import
|
|
then
|
|
echo "ERROR [init] param import failed"
|
|
set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
|
|
|
|
bsondump $PARAM_FILE
|
|
|
|
if [ -d "/fs/microsd" ]
|
|
then
|
|
# try to make a backup copy
|
|
cp $PARAM_FILE /fs/microsd/param_import_fail.bson
|
|
|
|
# try importing from backup file
|
|
if [ -f $PARAM_BACKUP_FILE ]
|
|
then
|
|
echo "[init] importing from parameter backup"
|
|
|
|
# dump current backup file contents for comparison
|
|
bsondump $PARAM_BACKUP_FILE
|
|
|
|
param import $PARAM_BACKUP_FILE
|
|
|
|
# overwrite invalid $PARAM_FILE with backup
|
|
cp $PARAM_BACKUP_FILE $PARAM_FILE
|
|
fi
|
|
|
|
param status
|
|
|
|
dmesg >> /fs/microsd/param_import_fail.txt &
|
|
fi
|
|
fi
|
|
|
|
if [ $SDCARD_AVAILABLE = yes ]
|
|
then
|
|
param select-backup $PARAM_BACKUP_FILE
|
|
fi
|
|
|
|
if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X
|
|
then
|
|
netman update -i eth0
|
|
fi
|
|
|
|
#
|
|
# If the airframe has been previously reset SYS_AUTCONFIG will have been set to 1 and other params will be reset on the next boot.
|
|
#
|
|
if param greater SYS_AUTOCONFIG 0
|
|
then
|
|
# Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID.
|
|
param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
|
|
fi
|
|
|
|
#
|
|
# Optional board architecture defaults: rc.board_arch_defaults
|
|
#
|
|
set BOARD_ARCH_RC_DEFAULTS ${R}etc/init.d/rc.board_arch_defaults
|
|
if [ -f $BOARD_ARCH_RC_DEFAULTS ]
|
|
then
|
|
echo "Board architecture defaults: ${BOARD_ARCH_RC_DEFAULTS}"
|
|
. $BOARD_ARCH_RC_DEFAULTS
|
|
fi
|
|
unset BOARD_ARCH_RC_DEFAULTS
|
|
|
|
#
|
|
# Optional board defaults: rc.board_defaults
|
|
#
|
|
set BOARD_RC_DEFAULTS ${R}etc/init.d/rc.board_defaults
|
|
if [ -f $BOARD_RC_DEFAULTS ]
|
|
then
|
|
echo "Board defaults: ${BOARD_RC_DEFAULTS}"
|
|
. $BOARD_RC_DEFAULTS
|
|
fi
|
|
unset BOARD_RC_DEFAULTS
|
|
|
|
#
|
|
# Set parameters and env variables for selected SYS_AUTOSTART.
|
|
#
|
|
set AUTOSTART_PATH etc/init.d/rc.autostart
|
|
if ! param compare SYS_AUTOSTART 0
|
|
then
|
|
if param greater SYS_AUTOSTART 1000000
|
|
then
|
|
# Use external startup file
|
|
if [ $SDCARD_AVAILABLE = yes ]
|
|
then
|
|
set AUTOSTART_PATH etc/init.d/rc.autostart_ext
|
|
else
|
|
echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
|
|
fi
|
|
fi
|
|
. ${R}$AUTOSTART_PATH
|
|
fi
|
|
unset AUTOSTART_PATH
|
|
|
|
#
|
|
# Start the tone_alarm driver.
|
|
# Needs to be started after the parameters are loaded (for CBRK_BUZZER).
|
|
#
|
|
tone_alarm start
|
|
|
|
#
|
|
# Waypoint storage.
|
|
# REBOOTWORK this needs to start in parallel.
|
|
#
|
|
if param compare -s SYS_DM_BACKEND 1
|
|
then
|
|
dataman start -r
|
|
else
|
|
if param compare SYS_DM_BACKEND 0
|
|
then
|
|
# dataman start default
|
|
dataman start
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Start the socket communication send_event handler.
|
|
#
|
|
send_event start
|
|
|
|
#
|
|
# Start the resource load monitor.
|
|
#
|
|
load_mon start
|
|
|
|
#
|
|
# Start system state indicator.
|
|
#
|
|
rgbled start -X -q
|
|
rgbled_ncp5623c start -X -q
|
|
rgbled_lp5562 start -X -q
|
|
rgbled_is31fl3195 start -X -q
|
|
|
|
#
|
|
# Override parameters from user configuration file.
|
|
#
|
|
if [ -f $FCONFIG ]
|
|
then
|
|
echo "Custom: ${FCONFIG}"
|
|
. $FCONFIG
|
|
fi
|
|
|
|
#
|
|
# Start IO for PWM output or RC input if enabled
|
|
#
|
|
if param compare -s SYS_USE_IO 1
|
|
then
|
|
# Check if PX4IO present and update firmware if needed.
|
|
if [ -f $IOFW ]
|
|
then
|
|
if ! px4io checkcrc ${IOFW}
|
|
then
|
|
# tune Program PX4IO
|
|
tune_control play -t 16 # tune 16 = PROG_PX4IO
|
|
|
|
if px4io update ${IOFW}
|
|
then
|
|
usleep 10000
|
|
tune_control stop
|
|
if px4io checkcrc ${IOFW}
|
|
then
|
|
tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
|
|
else
|
|
tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
|
|
fi
|
|
else
|
|
tune_control stop
|
|
fi
|
|
fi
|
|
fi
|
|
|
|
if ! px4io start
|
|
then
|
|
echo "PX4IO start failed"
|
|
set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# RC update (map raw RC input to calibrate manual control)
|
|
# start before commander
|
|
#
|
|
rc_update start
|
|
manual_control start
|
|
|
|
# Start camera trigger, capture and PPS before pwm_out as they might access
|
|
# pwm pins
|
|
if param greater -s TRIG_MODE 0
|
|
then
|
|
camera_trigger start
|
|
camera_feedback start
|
|
fi
|
|
# PPS capture driver
|
|
if param greater -s PPS_CAP_ENABLE 0
|
|
then
|
|
pps_capture start
|
|
fi
|
|
# Camera capture driver
|
|
if param greater -s CAM_CAP_FBACK 0
|
|
then
|
|
if camera_capture start
|
|
then
|
|
camera_capture on
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# Sensors System (start before Commander so Preflight checks are properly run).
|
|
# Commander needs to be this early for in-air-restarts.
|
|
#
|
|
if param greater SYS_HITL 0
|
|
then
|
|
if ! pwm_out_sim start -m hil
|
|
then
|
|
tune_control play error
|
|
fi
|
|
|
|
sensors start -h
|
|
commander start -h
|
|
# disable GPS
|
|
param set GPS_1_CONFIG 0
|
|
|
|
# start the simulator in hardware if needed
|
|
if param compare SYS_HITL 2
|
|
then
|
|
simulator_sih start
|
|
sensor_baro_sim start
|
|
sensor_mag_sim start
|
|
sensor_gps_sim start
|
|
fi
|
|
|
|
else
|
|
#
|
|
# board sensors: rc.sensors
|
|
#
|
|
set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
|
|
if [ -f $BOARD_RC_SENSORS ]
|
|
then
|
|
echo "Board sensors: ${BOARD_RC_SENSORS}"
|
|
. $BOARD_RC_SENSORS
|
|
fi
|
|
unset BOARD_RC_SENSORS
|
|
|
|
. ${R}etc/init.d/rc.sensors
|
|
|
|
if param compare -s BAT1_SOURCE 2
|
|
then
|
|
esc_battery start
|
|
fi
|
|
|
|
if ! param compare BAT1_SOURCE 1
|
|
then
|
|
battery_status start
|
|
fi
|
|
|
|
sensors start
|
|
commander start
|
|
|
|
dshot start
|
|
pwm_out start
|
|
fi
|
|
|
|
#
|
|
# Configure vehicle type specific parameters.
|
|
# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
|
|
. ${R}etc/init.d/rc.vehicle_setup
|
|
|
|
# Pre-takeoff continuous magnetometer calibration
|
|
if param compare -s MBE_ENABLE 1
|
|
then
|
|
mag_bias_estimator start
|
|
fi
|
|
|
|
#
|
|
# Optional board mavlink streams: rc.board_mavlink
|
|
#
|
|
set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink
|
|
if [ -f $BOARD_RC_MAVLINK ]
|
|
then
|
|
echo "Board mavlink: ${BOARD_RC_MAVLINK}"
|
|
. $BOARD_RC_MAVLINK
|
|
fi
|
|
unset BOARD_RC_MAVLINK
|
|
|
|
#
|
|
# Start UART/Serial device drivers.
|
|
# Note: rc.serial is auto-generated from Tools/serial/generate_config.py
|
|
#
|
|
. ${R}etc/init.d/rc.serial
|
|
|
|
# Must be started after the serial config is read
|
|
rc_input start $RC_INPUT_ARGS
|
|
|
|
#
|
|
# Play the startup tune (if not disabled or there is an error)
|
|
#
|
|
param compare CBRK_BUZZER 782090
|
|
if [ "$?" != "0" -o "$STARTUP_TUNE" != "1" ]
|
|
then
|
|
tune_control play -t $STARTUP_TUNE
|
|
fi
|
|
|
|
#
|
|
# Start the navigator.
|
|
#
|
|
navigator start
|
|
|
|
#
|
|
# Start a thermal calibration if required.
|
|
#
|
|
. ${R}etc/init.d/rc.thermal_cal
|
|
|
|
#
|
|
# Start gimbal to control mounts such as gimbals, disabled by default.
|
|
#
|
|
if param greater -s MNT_MODE_IN -1
|
|
then
|
|
gimbal start
|
|
fi
|
|
|
|
# Blacksheep telemetry
|
|
if param compare -s TEL_BST_EN 1
|
|
then
|
|
bst start -X
|
|
fi
|
|
|
|
if param compare -s IMU_GYRO_FFT_EN 1
|
|
then
|
|
gyro_fft start
|
|
fi
|
|
|
|
if param compare -s IMU_GYRO_CAL_EN 1
|
|
then
|
|
gyro_calibration start
|
|
fi
|
|
|
|
# Check for px4flow sensor
|
|
if param compare -s SENS_EN_PX4FLOW 1
|
|
then
|
|
px4flow start -X &
|
|
fi
|
|
|
|
# Payload deliverer module if gripper is enabled
|
|
if param compare -s PD_GRIPPER_EN 1
|
|
then
|
|
payload_deliverer start
|
|
fi
|
|
|
|
#
|
|
# Optional board supplied extras: rc.board_extras
|
|
#
|
|
set BOARD_RC_EXTRAS ${R}etc/init.d/rc.board_extras
|
|
if [ -f $BOARD_RC_EXTRAS ]
|
|
then
|
|
echo "Board extras: ${BOARD_RC_EXTRAS}"
|
|
. $BOARD_RC_EXTRAS
|
|
fi
|
|
unset BOARD_RC_EXTRAS
|
|
|
|
#
|
|
# Start any custom addons from the sdcard.
|
|
#
|
|
if [ -f $FEXTRAS ]
|
|
then
|
|
echo "Addons script: ${FEXTRAS}"
|
|
. $FEXTRAS
|
|
fi
|
|
|
|
#
|
|
# Start the logger.
|
|
#
|
|
. ${R}etc/init.d/rc.logging
|
|
|
|
#
|
|
# Set additional parameters and env variables for selected AUTOSTART.
|
|
#
|
|
if ! param compare SYS_AUTOSTART 0
|
|
then
|
|
. ${R}etc/init.d/rc.autostart.post
|
|
fi
|
|
|
|
|
|
set BOARD_BOOTLOADER_UPGRADE ${R}etc/init.d/rc.board_bootloader_upgrade
|
|
if [ -f $BOARD_BOOTLOADER_UPGRADE ]
|
|
then
|
|
sh $BOARD_BOOTLOADER_UPGRADE
|
|
fi
|
|
unset BOARD_BOOTLOADER_UPGRADE
|
|
|
|
#
|
|
# Check if UAVCAN is enabled, default to it for ESCs.
|
|
#
|
|
if param greater -s UAVCAN_ENABLE 0
|
|
then
|
|
# Start core UAVCAN module.
|
|
if ! uavcan start
|
|
then
|
|
tune_control play error
|
|
fi
|
|
else
|
|
if param greater -s CYPHAL_ENABLE 0
|
|
then
|
|
cyphal start
|
|
fi
|
|
fi
|
|
|
|
#
|
|
# End of autostart.
|
|
#
|
|
fi
|
|
|
|
#
|
|
# Unset all script parameters to free RAM.
|
|
#
|
|
unset R
|
|
unset FCONFIG
|
|
unset FEXTRAS
|
|
unset FRC
|
|
unset IOFW
|
|
unset LOGGER_ARGS
|
|
unset LOGGER_BUF
|
|
unset PARAM_FILE
|
|
unset PARAM_BACKUP_FILE
|
|
unset RC_INPUT_ARGS
|
|
unset SDCARD_AVAILABLE
|
|
unset SDCARD_EXT_PATH
|
|
unset SDCARD_FORMAT
|
|
unset STARTUP_TUNE
|
|
unset VEHICLE_TYPE
|
|
|
|
#
|
|
# Boot is complete, inform MAVLink app(s) that the system is now fully up and running.
|
|
#
|
|
mavlink boot_complete
|