mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2025-12-11 17:39:01 +08:00
282 lines
6.8 KiB
Bash
282 lines
6.8 KiB
Bash
#!/bin/sh
|
|
|
|
set -e
|
|
|
|
# PX4 commands need the 'px4-' prefix in bash.
|
|
# (px4-alias.sh is expected to be in the PATH)
|
|
# shellcheck disable=SC1091
|
|
. px4-alias.sh
|
|
|
|
#search path for sourcing px4-rc.*
|
|
PATH="$PATH:${R}etc/init.d-posix"
|
|
|
|
#
|
|
# Main SITL startup script
|
|
#
|
|
|
|
# check for ekf2 replay
|
|
# shellcheck disable=SC2154
|
|
if [ "$replay_mode" = "ekf2" ]
|
|
then
|
|
. ${R}etc/init.d-posix/rc.replay
|
|
exit 0
|
|
fi
|
|
|
|
# initialize script variables
|
|
set VEHICLE_TYPE none
|
|
set LOGGER_ARGS ""
|
|
set LOGGER_BUF 1000
|
|
|
|
set RUN_MINIMAL_SHELL no
|
|
|
|
set SYS_AUTOSTART=0
|
|
|
|
if [ "$PX4_SIM_MODEL" = "shell" ]
|
|
then
|
|
set RUN_MINIMAL_SHELL yes
|
|
|
|
elif [ -n "$PX4_SYS_AUTOSTART" ]
|
|
then
|
|
echo "env SYS_AUTOSTART: ${PX4_SYS_AUTOSTART}"
|
|
SYS_AUTOSTART=${PX4_SYS_AUTOSTART}
|
|
|
|
elif [ "$PX4_SIM_MODEL" = "none" ] || [ -z $PX4_SIM_MODEL ]
|
|
then
|
|
# no airframe selected
|
|
SYS_AUTOSTART=0
|
|
|
|
elif [ -n "$PX4_SIM_MODEL" ]
|
|
then
|
|
# Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL})
|
|
# TODO: unify with rc.autostart generation
|
|
# shellcheck disable=SC2012
|
|
REQUESTED_AUTOSTART=$(ls "${R}etc/init.d-posix/airframes" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p')
|
|
if [ -z "$REQUESTED_AUTOSTART" ]; then
|
|
echo "ERROR [init] Unknown model $PX4_SIM_MODEL (not found by name on ${R}etc/init.d-posix/airframes)"
|
|
exit 1
|
|
else
|
|
SYS_AUTOSTART=$REQUESTED_AUTOSTART
|
|
echo "INFO [init] found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART"
|
|
fi
|
|
fi
|
|
|
|
# Load parameters
|
|
set PARAM_FILE parameters.bson
|
|
param select $PARAM_FILE
|
|
|
|
if [ -f $PARAM_FILE ]
|
|
then
|
|
if param load
|
|
then
|
|
echo "[param] Loaded: $PARAM_FILE"
|
|
else
|
|
echo "[param] FAILED loading $PARAM_FILE"
|
|
fi
|
|
fi
|
|
|
|
# exit early when the minimal shell is requested
|
|
[ $RUN_MINIMAL_SHELL = yes ] && exit 0
|
|
|
|
if param compare SYS_AUTOSTART $SYS_AUTOSTART
|
|
then
|
|
set AUTOCNF no
|
|
|
|
elif [ "$SYS_AUTOSTART" -eq 0 ]
|
|
then
|
|
set AUTOCNF no
|
|
else
|
|
set AUTOCNF yes
|
|
param set SYS_AUTOCONFIG 1
|
|
fi
|
|
|
|
if param compare SYS_AUTOCONFIG 1
|
|
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*
|
|
set AUTOCNF yes
|
|
fi
|
|
|
|
# multi-instance setup
|
|
# shellcheck disable=SC2154
|
|
param set MAV_SYS_ID $((px4_instance+1))
|
|
|
|
if [ $AUTOCNF = yes ]
|
|
then
|
|
param set SYS_AUTOSTART $SYS_AUTOSTART
|
|
|
|
param set CAL_ACC0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
|
param set CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
|
|
param set CAL_ACC1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
|
|
param set CAL_GYRO1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
|
|
param set CAL_ACC2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
|
param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
|
|
|
|
param set CAL_MAG0_ID 197388
|
|
param set CAL_MAG1_ID 197644
|
|
|
|
param set SENS_BOARD_X_OFF 0.000001
|
|
param set SENS_DPRES_OFF 0.001
|
|
fi
|
|
|
|
param set-default BAT1_N_CELLS 4
|
|
|
|
param set-default CBRK_AIRSPD_CHK 0
|
|
param set-default CBRK_SUPPLY_CHK 894281
|
|
|
|
# disable check, no CPU load reported on posix yet
|
|
param set-default COM_CPU_MAX -1
|
|
|
|
# Don't require RC calibration and configuration
|
|
param set-default COM_RC_IN_MODE 1
|
|
|
|
# Speedup SITL startup
|
|
param set-default EKF2_REQ_GPS_H 0.5
|
|
|
|
# Multi-EKF
|
|
param set-default EKF2_MULTI_IMU 3
|
|
param set-default SENS_IMU_MODE 0
|
|
param set-default EKF2_MULTI_MAG 2
|
|
param set-default SENS_MAG_MODE 0
|
|
|
|
param set-default IMU_GYRO_FFT_EN 1
|
|
param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets
|
|
|
|
param set-default -s MC_AT_EN 1
|
|
|
|
# By default log from boot until first disarm.
|
|
param set-default SDLOG_MODE 1
|
|
# enable default, estimator replay and vision/avoidance logging profiles
|
|
param set-default SDLOG_PROFILE 131
|
|
param set-default SDLOG_DIRS_MAX 7
|
|
|
|
param set-default TRIG_INTERFACE 3
|
|
|
|
param set-default SYS_FAILURE_EN 1
|
|
|
|
# Adapt timeout parameters if simulation runs faster or slower than realtime.
|
|
if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
|
|
COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
|
|
echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
|
|
param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
|
|
|
|
COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
|
echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
|
|
param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
|
|
|
|
COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
|
echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
|
|
param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
|
|
|
|
COM_OBC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 5.0" | bc)
|
|
echo "COM_OBC_LOSS_T set to $COM_OBC_LOSS_T_LONGER"
|
|
param set COM_OBC_LOSS_T $COM_OBC_LOSS_T_LONGER
|
|
fi
|
|
|
|
# Autostart ID
|
|
autostart_file=''
|
|
# shellcheck disable=SC2231
|
|
for f in ${R}etc/init.d-posix/airframes/"$(param show -q SYS_AUTOSTART)"_*
|
|
do
|
|
filename=$(basename "$f")
|
|
case "$filename" in
|
|
*\.*)
|
|
# ignore files that contain a dot (e.g. <vehicle>.post)
|
|
;;
|
|
*)
|
|
autostart_file="$f"
|
|
;;
|
|
esac
|
|
done
|
|
|
|
if [ -e "$autostart_file" ]
|
|
then
|
|
. "$autostart_file"
|
|
|
|
elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ]
|
|
then
|
|
echo "Error: no autostart file found ($autostart_file)"
|
|
exit 1
|
|
fi
|
|
|
|
#user defined params for instances can be in PATH
|
|
. px4-rc.params
|
|
|
|
dataman start
|
|
|
|
# only start the simulator if not in replay mode, as both control the lockstep time
|
|
if ! replay tryapplyparams
|
|
then
|
|
. px4-rc.simulator
|
|
fi
|
|
|
|
load_mon start
|
|
battery_simulator start
|
|
tone_alarm start
|
|
rc_update start
|
|
manual_control start
|
|
sensors start
|
|
commander start
|
|
|
|
if ! pwm_out_sim start -m sim
|
|
then
|
|
tune_control play error
|
|
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
|
|
|
|
navigator start
|
|
|
|
# Try to start the micrortps_client with UDP transport if module exists
|
|
if px4-micrortps_client status > /dev/null 2>&1
|
|
then
|
|
. px4-rc.rtps
|
|
fi
|
|
|
|
if param greater -s MNT_MODE_IN -1
|
|
then
|
|
gimbal start
|
|
fi
|
|
|
|
if param greater -s TRIG_MODE 0
|
|
then
|
|
camera_trigger start
|
|
camera_feedback start
|
|
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
|
|
|
|
# Payload deliverer module if gripper is enabled
|
|
if param compare -s PD_GRIPPER_EN 1
|
|
then
|
|
payload_deliverer start
|
|
fi
|
|
|
|
#user defined mavlink streams for instances can be in PATH
|
|
. px4-rc.mavlink
|
|
|
|
# execute autostart post script if any
|
|
[ -e "$autostart_file".post ] && . "$autostart_file".post
|
|
|
|
# Run script to start logging
|
|
if param compare SYS_MC_EST_GROUP 2
|
|
then
|
|
set LOGGER_ARGS "-p ekf2_timestamps"
|
|
else
|
|
set LOGGER_ARGS "-p vehicle_attitude"
|
|
fi
|
|
. ${R}etc/init.d/rc.logging
|
|
|
|
mavlink boot_complete
|
|
replay trystart
|