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.
315 lines
9.0 KiB
315 lines
9.0 KiB
#!/bin/sh |
|
|
|
# PX4 commands need the 'px4-' prefix in bash. |
|
# (px4-alias.sh is expected to be in the PATH) |
|
# shellcheck disable=SC1091 |
|
. px4-alias.sh |
|
|
|
SCRIPT_DIR="$(CDPATH='' cd -- "$(dirname -- "$0")" && pwd)" |
|
|
|
# |
|
# 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 AUX_MODE none |
|
set AUX_BANK2 none |
|
set IO_PRESENT no |
|
set MAV_TYPE none |
|
set MIXER none |
|
set MIXER_AUX none |
|
set MIXER_FILE none |
|
set OUTPUT_MODE sim |
|
set EXTRA_MIXER_MODE none |
|
set PWM_OUT none |
|
set SDCARD_MIXERS_PATH etc/mixers |
|
set USE_IO no |
|
set VEHICLE_TYPE none |
|
set LOGGER_ARGS "" |
|
set LOGGER_BUF 1000 |
|
|
|
set RUN_MINIMAL_SHELL no |
|
|
|
# Use the variable set by sitl_run.sh to choose the model settings. |
|
if [ "$PX4_SIM_MODEL" = "shell" ]; then |
|
set RUN_MINIMAL_SHELL yes |
|
else |
|
# 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 "$SCRIPT_DIR/airframes" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p') |
|
if [ -z "$REQUESTED_AUTOSTART" ]; then |
|
echo "Error: Unknown model $PX4_SIM_MODEL (not found by name on $SCRIPT_DIR/airframes)" |
|
exit 1 |
|
else |
|
echo "Info: found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART" |
|
fi |
|
fi |
|
|
|
# Load parameters |
|
set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART" |
|
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 |
|
else |
|
echo "[param] parameter file not found, creating $PARAM_FILE" |
|
fi |
|
|
|
# exit early when the minimal shell is requested |
|
[ $RUN_MINIMAL_SHELL = yes ] && exit 0 |
|
|
|
|
|
# Use environment variable PX4_ESTIMATOR to choose estimator. |
|
if [ "$PX4_ESTIMATOR" = "q" ]; then |
|
param set SYS_MC_EST_GROUP 3 |
|
elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then |
|
param set SYS_MC_EST_GROUP 2 |
|
elif [ "$PX4_ESTIMATOR" = "lpe" ]; then |
|
param set SYS_MC_EST_GROUP 1 |
|
elif [ "$PX4_ESTIMATOR" = "inav" ]; then |
|
param set SYS_MC_EST_GROUP 0 |
|
fi |
|
|
|
if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART |
|
then |
|
set AUTOCNF no |
|
else |
|
set AUTOCNF yes |
|
param set SYS_AUTOCONFIG 1 |
|
fi |
|
|
|
if param compare SYS_AUTOCONFIG 1 |
|
then |
|
set AUTOCNF yes |
|
|
|
# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID |
|
param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT* |
|
fi |
|
|
|
# multi-instance setup |
|
# shellcheck disable=SC2154 |
|
param set MAV_SYS_ID $((px4_instance+1)) |
|
simulator_tcp_port=$((4560+px4_instance)) |
|
udp_offboard_port_local=$((14580+px4_instance)) |
|
udp_offboard_port_remote=$((14540+px4_instance)) |
|
[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps |
|
udp_onboard_payload_port_local=$((14280+px4_instance)) |
|
udp_onboard_payload_port_remote=$((14030+px4_instance)) |
|
udp_onboard_gimbal_port_local=$((13030+px4_instance)) |
|
udp_onboard_gimbal_port_remote=$((13280+px4_instance)) |
|
udp_gcs_port_local=$((18570+px4_instance)) |
|
|
|
if [ $AUTOCNF = yes ] |
|
then |
|
param set SYS_AUTOSTART $REQUESTED_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 |
|
|
|
param set SYS_RESTART_TYPE 2 |
|
fi |
|
|
|
param set-default BAT_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 |
|
|
|
# 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 |
|
|
|
# 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 |
|
echo "Error: no autostart file found ($autostart_file)" |
|
exit 1 |
|
fi |
|
|
|
. "$autostart_file" |
|
|
|
# |
|
# If autoconfig parameter was set, reset it and save parameters. |
|
# |
|
if [ $AUTOCNF = yes ] |
|
then |
|
param set SYS_AUTOCONFIG 0 |
|
fi |
|
|
|
# Simulator IMU data provided at 250 Hz |
|
param set IMU_INTEG_RATE 250 |
|
|
|
dataman start |
|
# only start the simulator if not in replay mode, as both control the lockstep time |
|
if ! replay tryapplyparams |
|
then |
|
# Check if PX4_SIM_HOSTNAME environment variable is empty |
|
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty |
|
# If both are empty use localhost for simulator |
|
if [ -z "${PX4_SIM_HOSTNAME}" ]; then |
|
if [ -z "${PX4_SIM_HOST_ADDR}" ]; then |
|
echo "PX4 SIM HOST: localhost" |
|
simulator start -c $simulator_tcp_port |
|
else |
|
echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" |
|
simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port |
|
fi |
|
else |
|
echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" |
|
simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port |
|
fi |
|
fi |
|
load_mon start |
|
battery_simulator start |
|
tone_alarm start |
|
rc_update start |
|
sensors start |
|
commander start |
|
navigator start |
|
|
|
|
|
if param greater -s MNT_MODE_IN -1 |
|
then |
|
vmount 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 |
|
|
|
# Configure vehicle type specific parameters. |
|
# Note: rc.vehicle_setup is the entry point for rc.interface, |
|
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. |
|
# |
|
. ${R}etc/init.d/rc.vehicle_setup |
|
|
|
if [ -e etc/init.d-posix/rc.mavlink_override ] |
|
then |
|
echo "Running non-default mavlink config rc.mavlink_override" |
|
. ${R}etc/init.d-posix/rc.mavlink_override |
|
else |
|
# GCS link |
|
mavlink start -x -u $udp_gcs_port_local -r 4000000 -f |
|
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local |
|
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local |
|
mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local |
|
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local |
|
|
|
# API/Offboard link |
|
mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote |
|
|
|
# Onboard link to camera |
|
mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote |
|
|
|
# Onboard link to gimbal |
|
mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote |
|
fi |
|
|
|
# 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
|
|
|