mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 02:13:33 +08:00
Julian Oes
ffeeedc310
When simulating with lockstep we can raise the speed by setting the env variable `PX4_SIM_SPEED_FACTOR`. Some inputs like RC, MAVLink heartbeats from a ground station, or offboard controls via MAVLink are still at the normal speed which leads to timeouts getting detected in PX4. To work around this issue we can automatically multiply the timeout parameters by the speed factor.
266 lines
6.4 KiB
Bash
266 lines
6.4 KiB
Bash
#!/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
|
|
sh etc/init.d-posix/rc.replay
|
|
exit 0
|
|
fi
|
|
|
|
# initialize script variables
|
|
set AUX_MODE none
|
|
set IO_PRESENT no
|
|
set LOG_FILE bootlog.txt
|
|
set MAV_TYPE none
|
|
set MIXER none
|
|
set MIXER_AUX none
|
|
set MIXER_FILE none
|
|
set OUTPUT_MODE sim
|
|
set PWM_OUT none
|
|
set SDCARD_MIXERS_PATH etc/mixers
|
|
set USE_IO no
|
|
set VEHICLE_TYPE none
|
|
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" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p')
|
|
if [ -z "$REQUESTED_AUTOSTART" ]; then
|
|
echo "Error: Unknown model '$PX4_SIM_MODEL'"
|
|
exit 1
|
|
fi
|
|
fi
|
|
|
|
# clear bootlog
|
|
[ -f $LOG_FILE ] && rm $LOG_FILE
|
|
|
|
|
|
uorb start
|
|
if [ -f eeprom/parameters ]
|
|
then
|
|
param load
|
|
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" = "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
|
|
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))
|
|
udp_gcs_port_local=$((14570+px4_instance))
|
|
|
|
if [ $AUTOCNF = yes ]
|
|
then
|
|
param set SYS_AUTOSTART $REQUESTED_AUTOSTART
|
|
|
|
param set BAT_N_CELLS 3
|
|
|
|
param set CAL_ACC0_ID 1376264
|
|
param set CAL_ACC1_ID 1310728
|
|
param set CAL_ACC_PRIME 1376264
|
|
|
|
param set CAL_GYRO0_ID 2293768
|
|
param set CAL_GYRO_PRIME 2293768
|
|
|
|
param set CAL_MAG0_ID 196616
|
|
param set CAL_MAG_PRIME 196616
|
|
|
|
param set CBRK_AIRSPD_CHK 0
|
|
|
|
param set COM_DISARM_LAND 0.1
|
|
param set COM_OBL_ACT 2
|
|
param set COM_OBL_RC_ACT 0
|
|
param set COM_OF_LOSS_T 5
|
|
param set COM_RC_IN_MODE 1
|
|
|
|
param set EKF2_AID_MASK 1
|
|
param set EKF2_ANGERR_INIT 0.01
|
|
param set EKF2_HGT_MODE 0
|
|
param set EKF2_GBIAS_INIT 0.01
|
|
|
|
# LPE: GPS only mode
|
|
param set LPE_FUSION 145
|
|
|
|
param set MIS_TAKEOFF_ALT 2.5
|
|
|
|
param set MC_PITCH_P 6
|
|
param set MC_PITCHRATE_P 0.2
|
|
param set MC_ROLL_P 6
|
|
param set MC_ROLLRATE_P 0.2
|
|
|
|
param set MPC_ALT_MODE 0
|
|
param set MPC_HOLD_MAX_Z 2
|
|
param set MPC_Z_VEL_I 0.15
|
|
param set MPC_Z_VEL_P 0.6
|
|
param set MPC_XY_P 0.8
|
|
param set MPC_XY_VEL_P 0.2
|
|
param set MPC_XY_VEL_I 0.02
|
|
param set MPC_XY_VEL_D 0.016
|
|
|
|
param set MPC_JERK_MIN 10
|
|
param set MPC_JERK_MAX 20
|
|
param set MPC_ACC_HOR_MAX 3
|
|
|
|
param set NAV_ACC_RAD 2
|
|
param set NAV_DLL_ACT 2
|
|
|
|
param set RTL_DESCEND_ALT 5
|
|
param set RTL_LAND_DELAY 5
|
|
param set RTL_RETURN_ALT 30
|
|
|
|
# By default log from boot until first disarm.
|
|
param set SDLOG_MODE 1
|
|
# enable default, estimator replay and vision/avoidance logging profiles
|
|
param set SDLOG_PROFILE 131
|
|
param set SDLOG_DIRS_MAX 7
|
|
param set SENS_BOARD_ROT 0
|
|
param set SENS_BOARD_X_OFF 0.000001
|
|
param set SENS_DPRES_OFF 0.001
|
|
param set SYS_RESTART_TYPE 2
|
|
|
|
param set WEST_EN 0
|
|
fi
|
|
|
|
# Adapt timeout parameters if simulation runs faster or slower than realtime.
|
|
if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
|
|
COM_DL_LOSS_T_LONGER=$(($PX4_SIM_SPEED_FACTOR * 10))
|
|
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=$(($PX4_SIM_SPEED_FACTOR * 1))
|
|
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=$(($PX4_SIM_SPEED_FACTOR * 10))
|
|
echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
|
|
param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
|
|
fi
|
|
|
|
# Autostart ID
|
|
autostart_file=''
|
|
for f in etc/init.d-posix/"$(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
|
|
|
|
sh "$autostart_file"
|
|
|
|
|
|
dataman start
|
|
replay tryapplyparams
|
|
simulator start -s -c $simulator_tcp_port
|
|
tone_alarm start
|
|
gyrosim start
|
|
accelsim start
|
|
barosim start
|
|
gpssim start
|
|
sensors start
|
|
commander start
|
|
navigator start
|
|
|
|
if param compare WEST_EN 1
|
|
then
|
|
wind_estimator start
|
|
fi
|
|
|
|
if ! param compare MNT_MODE_IN -1
|
|
then
|
|
vmount start
|
|
fi
|
|
|
|
if param greater TRIG_MODE 0
|
|
then
|
|
camera_trigger start
|
|
camera_feedback start
|
|
fi
|
|
|
|
|
|
if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ]
|
|
then
|
|
if param compare CBRK_AIRSPD_CHK 0
|
|
then
|
|
measairspeedsim start
|
|
fi
|
|
fi
|
|
|
|
# Configure vehicle type specific parameters.
|
|
# Note: rc.vehicle_setup is the entry point for rc.interface,
|
|
# rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.
|
|
#
|
|
sh etc/init.d/rc.vehicle_setup
|
|
|
|
# GCS link
|
|
mavlink start -x -u $udp_gcs_port_local -r 4000000
|
|
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 -m onboard -o $udp_offboard_port_remote
|
|
|
|
# execute autostart post script if any
|
|
[ -e "$autostart_file".post ] && sh "$autostart_file".post
|
|
|
|
# Run script to start logging
|
|
sh etc/init.d/rc.logging
|
|
|
|
mavlink boot_complete
|
|
replay trystart
|