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.
227 lines
5.6 KiB
227 lines
5.6 KiB
#!/bin/sh |
|
|
|
# PX4 commands need the 'px4-' prefix in bash. |
|
# (px4-alias.sh is expected to be in the PATH) |
|
# shellcheck disable=SC1091 |
|
source px4-alias.sh |
|
|
|
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[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 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}) |
|
# shellcheck disable=SC2012 #(the file names don't contain spaces) |
|
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 $((1+px4_instance)) |
|
simulator_udp_port=$((14560+px4_instance)) |
|
udp_offboard_port_local=$((14557+px4_instance)) |
|
udp_offboard_port_remote=$((14540+px4_instance)) |
|
udp_gcs_port_local=$((14556+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_ACC0_XOFF 0.01 |
|
param set CAL_ACC0_XSCALE 1.01 |
|
param set CAL_ACC0_YOFF -0.01 |
|
param set CAL_ACC0_YSCALE 1.01 |
|
param set CAL_ACC0_ZOFF 0.01 |
|
param set CAL_ACC0_ZSCALE 1.01 |
|
param set CAL_ACC1_ID 1310728 |
|
param set CAL_ACC1_XOFF 0.01 |
|
param set CAL_GYRO0_ID 2293768 |
|
param set CAL_GYRO0_XOFF 0.01 |
|
param set CAL_MAG0_ID 196616 |
|
param set CAL_MAG0_XOFF 0.01 |
|
param set SENS_DPRES_OFF 0.001 |
|
param set CBRK_AIRSPD_CHK 0 |
|
|
|
param set COM_DISARM_LAND 3 |
|
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_GBIAS_INIT 0.01 |
|
param set EKF2_HGT_MODE 0 |
|
param set EKF2_MAG_TYPE 1 |
|
|
|
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 MIS_TAKEOFF_ALT 2.5 |
|
param set MPC_ALT_MODE 0 |
|
param set MPC_HOLD_MAX_Z 2.0 |
|
param set MPC_Z_VEL_I 0.15 |
|
param set MPC_Z_VEL_P 0.6 |
|
param set NAV_ACC_RAD 2.0 |
|
param set NAV_DLL_ACT 2 |
|
param set RTL_DESCEND_ALT 5.0 |
|
param set RTL_LAND_DELAY 5 |
|
param set RTL_RETURN_ALT 30.0 |
|
|
|
param set SDLOG_DIRS_MAX 7 |
|
param set SENS_BOARD_ROT 0 |
|
param set SENS_BOARD_X_OFF 0.000001 |
|
param set SYS_RESTART_TYPE 2 |
|
|
|
# LPE: GPS only mode |
|
param set LPE_FUSION 145 |
|
|
|
param set WEST_EN 0 |
|
fi |
|
|
|
# Autostart ID |
|
autostart_file_match="etc/init.d-posix/$(param show -q SYS_AUTOSTART)_*" |
|
# shellcheck disable=SC2206 |
|
autostart_files=( $autostart_file_match ) |
|
autostart_file="${autostart_files[0]}" # use first match, but there should really only be one |
|
if [ ! -e "$autostart_file" ]; then |
|
echo "Error: no autostart file found" |
|
exit -1 |
|
fi |
|
|
|
sh "$autostart_file" |
|
|
|
|
|
dataman start |
|
replay tryapplyparams |
|
simulator start -s -u $simulator_udp_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 |
|
param set CAM_FBACK_MODE 1 |
|
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 |
|
|
|
|
|
logger start -e -t -b 1000 |
|
|
|
mavlink boot_complete |
|
replay trystart
|
|
|